1 /* 2 * FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU. 3 * Copyright (c) 2006 Christian Walter <[email protected]> 4 * All rights reserved. 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions 8 * are met: 9 * 1. Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 2. Redistributions in binary form must reproduce the above copyright 12 * notice, this list of conditions and the following disclaimer in the 13 * documentation and/or other materials provided with the distribution. 14 * 3. The name of the author may not be used to endorse or promote products 15 * derived from this software without specific prior written permission. 16 * 17 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 * 28 * File: $Id: mbfuncholding.c,v 1.12 2007/02/18 23:48:22 wolti Exp $ 29 */ 30 31 /* ----------------------- System includes ----------------------------------*/ 32 #include "stdlib.h" 33 #include "string.h" 34 35 /* ----------------------- Platform includes --------------------------------*/ 36 #include "port.h" 37 38 /* ----------------------- Modbus includes ----------------------------------*/ 39 #include "mb.h" 40 #include "mbframe.h" 41 #include "mbproto.h" 42 #include "mbconfig.h" 43 44 /* ----------------------- Defines ------------------------------------------*/ 45 #define MB_PDU_FUNC_READ_ADDR_OFF ( MB_PDU_DATA_OFF + 0) 46 #define MB_PDU_FUNC_READ_REGCNT_OFF ( MB_PDU_DATA_OFF + 2 ) 47 #define MB_PDU_FUNC_READ_SIZE ( 4 ) 48 #define MB_PDU_FUNC_READ_REGCNT_MAX ( 0x007D ) 49 50 #define MB_PDU_FUNC_WRITE_ADDR_OFF ( MB_PDU_DATA_OFF + 0) 51 #define MB_PDU_FUNC_WRITE_VALUE_OFF ( MB_PDU_DATA_OFF + 2 ) 52 #define MB_PDU_FUNC_WRITE_SIZE ( 4 ) 53 54 #define MB_PDU_FUNC_WRITE_MUL_ADDR_OFF ( MB_PDU_DATA_OFF + 0 ) 55 #define MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF ( MB_PDU_DATA_OFF + 2 ) 56 #define MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF ( MB_PDU_DATA_OFF + 4 ) 57 #define MB_PDU_FUNC_WRITE_MUL_VALUES_OFF ( MB_PDU_DATA_OFF + 5 ) 58 #define MB_PDU_FUNC_WRITE_MUL_SIZE_MIN ( 5 ) 59 #define MB_PDU_FUNC_WRITE_MUL_REGCNT_MAX ( 0x0078 ) 60 61 #define MB_PDU_FUNC_READWRITE_READ_ADDR_OFF ( MB_PDU_DATA_OFF + 0 ) 62 #define MB_PDU_FUNC_READWRITE_READ_REGCNT_OFF ( MB_PDU_DATA_OFF + 2 ) 63 #define MB_PDU_FUNC_READWRITE_WRITE_ADDR_OFF ( MB_PDU_DATA_OFF + 4 ) 64 #define MB_PDU_FUNC_READWRITE_WRITE_REGCNT_OFF ( MB_PDU_DATA_OFF + 6 ) 65 #define MB_PDU_FUNC_READWRITE_BYTECNT_OFF ( MB_PDU_DATA_OFF + 8 ) 66 #define MB_PDU_FUNC_READWRITE_WRITE_VALUES_OFF ( MB_PDU_DATA_OFF + 9 ) 67 #define MB_PDU_FUNC_READWRITE_SIZE_MIN ( 9 ) 68 69 /* ----------------------- Static functions ---------------------------------*/ 70 eMBException prveMBError2Exception( eMBErrorCode eErrorCode ); 71 72 /* ----------------------- Start implementation -----------------------------*/ 73 74 #if MB_FUNC_WRITE_HOLDING_ENABLED > 0 75 76 eMBException 77 eMBFuncWriteHoldingRegister( UCHAR * pucFrame, USHORT * usLen ) 78 { 79 USHORT usRegAddress; 80 eMBException eStatus = MB_EX_NONE; 81 eMBErrorCode eRegStatus; 82 83 if( *usLen == ( MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN ) ) 84 { 85 usRegAddress = ( USHORT )( pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF] << 8 ); 86 usRegAddress |= ( USHORT )( pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF + 1] ); 87 usRegAddress++; 88 89 /* Make callback to update the value. */ 90 eRegStatus = eMBRegHoldingCB( &pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF], 91 usRegAddress, 1, MB_REG_WRITE ); 92 93 /* If an error occured convert it into a Modbus exception. */ 94 if( eRegStatus != MB_ENOERR ) 95 { 96 eStatus = prveMBError2Exception( eRegStatus ); 97 } 98 } 99 else 100 { 101 /* Can't be a valid request because the length is incorrect. */ 102 eStatus = MB_EX_ILLEGAL_DATA_VALUE; 103 } 104 return eStatus; 105 } 106 #endif 107 108 #if MB_FUNC_WRITE_MULTIPLE_HOLDING_ENABLED > 0 109 eMBException 110 eMBFuncWriteMultipleHoldingRegister( UCHAR * pucFrame, USHORT * usLen ) 111 { 112 USHORT usRegAddress; 113 USHORT usRegCount; 114 UCHAR ucRegByteCount; 115 116 eMBException eStatus = MB_EX_NONE; 117 eMBErrorCode eRegStatus; 118 119 if( *usLen >= ( MB_PDU_FUNC_WRITE_MUL_SIZE_MIN + MB_PDU_SIZE_MIN ) ) 120 { 121 usRegAddress = ( USHORT )( pucFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF] << 8 ); 122 usRegAddress |= ( USHORT )( pucFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF + 1] ); 123 usRegAddress++; 124 125 usRegCount = ( USHORT )( pucFrame[MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF] << 8 ); 126 usRegCount |= ( USHORT )( pucFrame[MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF + 1] ); 127 128 ucRegByteCount = pucFrame[MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF]; 129 130 if( ( usRegCount >= 1 ) && 131 ( usRegCount <= MB_PDU_FUNC_WRITE_MUL_REGCNT_MAX ) && 132 ( ucRegByteCount == ( UCHAR ) ( 2 * usRegCount ) ) ) 133 { 134 /* Make callback to update the register values. */ 135 eRegStatus = 136 eMBRegHoldingCB( &pucFrame[MB_PDU_FUNC_WRITE_MUL_VALUES_OFF], 137 usRegAddress, usRegCount, MB_REG_WRITE ); 138 139 /* If an error occured convert it into a Modbus exception. */ 140 if( eRegStatus != MB_ENOERR ) 141 { 142 eStatus = prveMBError2Exception( eRegStatus ); 143 } 144 else 145 { 146 /* The response contains the function code, the starting 147 * address and the quantity of registers. We reuse the 148 * old values in the buffer because they are still valid. 149 */ 150 *usLen = MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF; 151 } 152 } 153 else 154 { 155 eStatus = MB_EX_ILLEGAL_DATA_VALUE; 156 } 157 } 158 else 159 { 160 /* Can't be a valid request because the length is incorrect. */ 161 eStatus = MB_EX_ILLEGAL_DATA_VALUE; 162 } 163 return eStatus; 164 } 165 #endif 166 167 #if MB_FUNC_READ_HOLDING_ENABLED > 0 168 169 eMBException 170 eMBFuncReadHoldingRegister( UCHAR * pucFrame, USHORT * usLen ) 171 { 172 USHORT usRegAddress; 173 USHORT usRegCount; 174 UCHAR *pucFrameCur; 175 176 eMBException eStatus = MB_EX_NONE; 177 eMBErrorCode eRegStatus; 178 179 if( *usLen == ( MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN ) ) 180 { 181 usRegAddress = ( USHORT )( pucFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8 ); 182 usRegAddress |= ( USHORT )( pucFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1] ); 183 usRegAddress++; 184 185 usRegCount = ( USHORT )( pucFrame[MB_PDU_FUNC_READ_REGCNT_OFF] << 8 ); 186 usRegCount |= ( USHORT )( pucFrame[MB_PDU_FUNC_READ_REGCNT_OFF + 1] ); 187 188 /* Check if the number of registers to read is valid. If not 189 * return Modbus illegal data value exception. 190 */ 191 if( ( usRegCount >= 1 ) && ( usRegCount <= MB_PDU_FUNC_READ_REGCNT_MAX ) ) 192 { 193 /* Set the current PDU data pointer to the beginning. */ 194 pucFrameCur = &pucFrame[MB_PDU_FUNC_OFF]; 195 *usLen = MB_PDU_FUNC_OFF; 196 197 /* First byte contains the function code. */ 198 *pucFrameCur++ = MB_FUNC_READ_HOLDING_REGISTER; 199 *usLen += 1; 200 201 /* Second byte in the response contain the number of bytes. */ 202 *pucFrameCur++ = ( UCHAR ) ( usRegCount * 2 ); 203 *usLen += 1; 204 205 /* Make callback to fill the buffer. */ 206 eRegStatus = eMBRegHoldingCB( pucFrameCur, usRegAddress, usRegCount, MB_REG_READ ); 207 /* If an error occured convert it into a Modbus exception. */ 208 if( eRegStatus != MB_ENOERR ) 209 { 210 eStatus = prveMBError2Exception( eRegStatus ); 211 } 212 else 213 { 214 *usLen += usRegCount * 2; 215 } 216 } 217 else 218 { 219 eStatus = MB_EX_ILLEGAL_DATA_VALUE; 220 } 221 } 222 else 223 { 224 /* Can't be a valid request because the length is incorrect. */ 225 eStatus = MB_EX_ILLEGAL_DATA_VALUE; 226 } 227 return eStatus; 228 } 229 230 #endif 231 232 #if MB_FUNC_READWRITE_HOLDING_ENABLED > 0 233 234 eMBException 235 eMBFuncReadWriteMultipleHoldingRegister( UCHAR * pucFrame, USHORT * usLen ) 236 { 237 USHORT usRegReadAddress; 238 USHORT usRegReadCount; 239 USHORT usRegWriteAddress; 240 USHORT usRegWriteCount; 241 UCHAR ucRegWriteByteCount; 242 UCHAR *pucFrameCur; 243 244 eMBException eStatus = MB_EX_NONE; 245 eMBErrorCode eRegStatus; 246 247 if( *usLen >= ( MB_PDU_FUNC_READWRITE_SIZE_MIN + MB_PDU_SIZE_MIN ) ) 248 { 249 usRegReadAddress = ( USHORT )( pucFrame[MB_PDU_FUNC_READWRITE_READ_ADDR_OFF] << 8U ); 250 usRegReadAddress |= ( USHORT )( pucFrame[MB_PDU_FUNC_READWRITE_READ_ADDR_OFF + 1] ); 251 usRegReadAddress++; 252 253 usRegReadCount = ( USHORT )( pucFrame[MB_PDU_FUNC_READWRITE_READ_REGCNT_OFF] << 8U ); 254 usRegReadCount |= ( USHORT )( pucFrame[MB_PDU_FUNC_READWRITE_READ_REGCNT_OFF + 1] ); 255 256 usRegWriteAddress = ( USHORT )( pucFrame[MB_PDU_FUNC_READWRITE_WRITE_ADDR_OFF] << 8U ); 257 usRegWriteAddress |= ( USHORT )( pucFrame[MB_PDU_FUNC_READWRITE_WRITE_ADDR_OFF + 1] ); 258 usRegWriteAddress++; 259 260 usRegWriteCount = ( USHORT )( pucFrame[MB_PDU_FUNC_READWRITE_WRITE_REGCNT_OFF] << 8U ); 261 usRegWriteCount |= ( USHORT )( pucFrame[MB_PDU_FUNC_READWRITE_WRITE_REGCNT_OFF + 1] ); 262 263 ucRegWriteByteCount = pucFrame[MB_PDU_FUNC_READWRITE_BYTECNT_OFF]; 264 265 if( ( usRegReadCount >= 1 ) && ( usRegReadCount <= 0x7D ) && 266 ( usRegWriteCount >= 1 ) && ( usRegWriteCount <= 0x79 ) && 267 ( ( 2 * usRegWriteCount ) == ucRegWriteByteCount ) ) 268 { 269 /* Make callback to update the register values. */ 270 eRegStatus = eMBRegHoldingCB( &pucFrame[MB_PDU_FUNC_READWRITE_WRITE_VALUES_OFF], 271 usRegWriteAddress, usRegWriteCount, MB_REG_WRITE ); 272 273 if( eRegStatus == MB_ENOERR ) 274 { 275 /* Set the current PDU data pointer to the beginning. */ 276 pucFrameCur = &pucFrame[MB_PDU_FUNC_OFF]; 277 *usLen = MB_PDU_FUNC_OFF; 278 279 /* First byte contains the function code. */ 280 *pucFrameCur++ = MB_FUNC_READWRITE_MULTIPLE_REGISTERS; 281 *usLen += 1; 282 283 /* Second byte in the response contain the number of bytes. */ 284 *pucFrameCur++ = ( UCHAR ) ( usRegReadCount * 2 ); 285 *usLen += 1; 286 287 /* Make the read callback. */ 288 eRegStatus = 289 eMBRegHoldingCB( pucFrameCur, usRegReadAddress, usRegReadCount, MB_REG_READ ); 290 if( eRegStatus == MB_ENOERR ) 291 { 292 *usLen += 2 * usRegReadCount; 293 } 294 } 295 if( eRegStatus != MB_ENOERR ) 296 { 297 eStatus = prveMBError2Exception( eRegStatus ); 298 } 299 } 300 else 301 { 302 eStatus = MB_EX_ILLEGAL_DATA_VALUE; 303 } 304 } 305 return eStatus; 306 } 307 308 #endif 309