00001
00024
00025
00026 #include <string.h>
00027
00028 #include "modbus_defs.h"
00029 #include "modbus_utils.h"
00030
00031 static unsigned char CRCHi[] = {
00032 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
00033 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
00034 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
00035 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
00036 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
00037 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
00038 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
00039 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
00040 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
00041 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
00042 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
00043 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
00044 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
00045 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
00046 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
00047 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
00048 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
00049 0x40
00050 };
00051 static unsigned char CRCLo[] = {
00052 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
00053 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
00054 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
00055 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
00056 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
00057 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
00058 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
00059 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
00060 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
00061 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
00062 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
00063 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
00064 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
00065 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
00066 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
00067 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
00068 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
00069 0x40
00070 };
00071
00078
00079 u16 CRC16( const u8 *buf, int len)
00080 {
00081 unsigned char hi=0xFF;
00082 unsigned char lo=0xFF;
00083 unsigned short index;
00084
00085 if ( buf == 0 || len <= 0 )
00086 return(MB_ERROR_PARAMETER);
00087
00088 while ( len-- ) {
00089 index = lo ^ (u8)*buf++;
00090 lo = hi ^ CRCHi[index];
00091 hi = CRCLo[index];
00092 }
00093
00094 index = lo << 8;
00095 index |= hi;
00096 return(index);
00097 }
00098
00099
00106
00107 u8 LRC( const u8 *buf, int len)
00108 {
00109 unsigned char ch=0;
00110
00111 if ( len <= 0 || buf == 0 )
00112 return(MB_ERROR_PARAMETER);
00113
00114 while ( len-- )
00115 ch += (unsigned char)*buf++;
00116
00117 return(ch);
00118 }
00119
00120
00131
00132 int MBASCIIToData( u8 *buf, const u8 *data, int len)
00133 {
00134 int i= 0;
00135 u8 ch1, ch2;
00136
00137
00138 if ( len & 1 )
00139 return( MB_ERROR_LENGTH);
00140 if ( len <=1 || len >= MB_ASCII_ADU_MAX_LENGTH )
00141 return( MB_ERROR_LENGTH);
00142
00143 for ( i=0; i<len; i+=2 )
00144 {
00145 ch2 = 0;
00146 ch1 = *data++;
00147 if ( ch1 >= '0' && ch1 <= '9' )
00148 ch1 -= '0';
00149 else if ( ch1 >= 'A' && ch1 <= 'F' )
00150 ch1 = ch1 - 'A' + 10;
00151 else
00152 return(MB_ERROR_FORMAT);
00153 ch2 = ch1 << 4;
00154 ch1 = *data++;
00155 if ( ch1 >= '0' && ch1 <= '9' )
00156 ch1 -= '0';
00157 else if ( ch1 >= 'A' && ch1 <= 'F' )
00158 ch1 = ch1 - 'A' + 10;
00159 else
00160 return(MB_ERROR_FORMAT);
00161 ch2 |= ch1;
00162 *buf++ = ch2;
00163 }
00164
00165 return(len/2);
00166 }
00167
00168
00176
00177 int MBDataToASCII( u8 *buf, const u8 *data, int len)
00178 {
00179 unsigned char lrc= 0;
00180 int i= 0, j= 0;
00181
00182 for( i= 0; i< len; i++)
00183 {
00184 u8 ch= ((data[ i] & 0xF0) >> 4);
00185 if( ch >= 0 && ch <= 9)
00186 ch+= '0';
00187 else
00188 ch+= ('A'- 10);
00189 buf[ j++]= ch;
00190
00191 ch= (data[ i] & 0x0F);
00192 if( ch >= 0 && ch <= 9)
00193 ch+= '0';
00194 else
00195 ch+= ('A'-10);
00196 buf[ j++]= ch;
00197 }
00198
00199 return j;
00200 }
00201
00202
00213
00214 int MBMakeADU( u8 *buf, int protocol, u8 address, const u8 *pdu, int len, u16 tid)
00215 {
00216 u8 tmp[ MB_ASCII_ADU_MAX_LENGTH];
00217 u8 lrc;
00218 u16 crc;
00219 int i= 0, tmp_len= 0, buf_len= 0;
00220
00221 switch( protocol)
00222 {
00223 case MB_ASCII_PROTOCOL:
00224 buf[ 0]= ':';
00225
00226 tmp[ 0]= address;
00227 tmp_len++;
00228
00229 bcopy( (const char *)pdu, (char *)(tmp+ tmp_len), len);
00230 tmp_len+= len;
00231
00232 lrc= LRC( tmp, tmp_len);
00233 tmp[ tmp_len++]= lrc;
00234
00235 buf_len= MBDataToASCII( buf+ 1, tmp, tmp_len);
00236 buf_len++;
00237
00238 buf[ buf_len++]= 0xd;
00239 buf[ buf_len++]= 0xa;
00240 break;
00241 case MB_RTU_PROTOCOL:
00242 buf[ 0]= address;
00243 buf_len++;
00244
00245 bcopy( (const char *)pdu, (char *)(buf+ 1), len);
00246 buf_len+= len;
00247
00248 crc= CRC16( buf, buf_len);
00249 buf[ buf_len++]= crc >> 8;
00250 buf[ buf_len++]= crc & 0x00FF;
00251 break;
00252 case MB_TCP_PROTOCOL:
00253 buf[ 0]= tid >> 8;
00254 buf[ 1]= tid & 0x00FF;
00255
00256 buf[ 2]= 0;
00257 buf[ 3]= 0;
00258
00259 buf[ 4]= (len+1) >> 8;
00260 buf[ 5]= (len+1) & 0x00FF;
00261
00262 buf[ 6]= address;
00263
00264 bcopy( (const char *)pdu, (char *)(buf+ 7), len);
00265 buf_len= 7+ len;
00266 break;
00267 default:
00268 return MB_ERROR_PROTOCOL;
00269 }
00270
00271 return buf_len;
00272 }
00273
00274
00282
00283 int MBGetPDU( u8 *buf, u8 *adu, int len)
00284 {
00285 int minus= 2;
00286 if( adu[ 0] == ':')
00287 minus= 1;
00288 bcopy( (const char *)(adu+1), (char *)buf, len- 1- minus);
00289
00290 return len- 1- minus;
00291 }
00292
00293
00302
00303 int MBMakePDU( u8 *buf, u8 function, u8 *data, int len)
00304 {
00305 int ret_len= 0;
00306
00307 buf[ 0]= function;
00308 bcopy( (const char *)data, (char *)(buf+ 1), len);
00309 ret_len= len+ 1;
00310
00311 return ret_len;
00312 }
00313
00314
00320
00321 u8 MBGetAddress( const u8 *adu)
00322 {
00323 return adu[ 0];
00324 }
00325
00326
00332
00333 u8 MBGetFunctionCode( const u8 *pdu)
00334 {
00335 return pdu[ 0];
00336 }
00337
00338
00339
00340
00348
00349 int MBReadDecreteInputs(u8 *buf, u16 startdec, u16 no)
00350 {
00351 int len= 0;
00352 buf[ len]= MBF_READ_DECRETE_INPUTS;
00353 len++;
00354 PUTU16( buf+ len, startdec);
00355 len+= 2;
00356 PUTU16( buf+ len, no);
00357 len+= 2;
00358
00359 return len;
00360 }
00361
00362
00370
00371 int MBReadCoils(u8 *buf, u16 startcoils, u16 no)
00372 {
00373 int len= 0;
00374 buf[ len]= MBF_READ_COILS;
00375 len++;
00376 PUTU16( buf+ len, startcoils);
00377 len+= 2;
00378 PUTU16( buf+ len, no);
00379 len+= 2;
00380
00381 return len;
00382 }
00383
00384
00392
00393 int MBWriteSingleCoil(u8 *buf, u16 coilreg, u16 onoff)
00394 {
00395 int len= 0;
00396 buf[ len]= MBF_WRITE_SINGLE_COIL;
00397 len++;
00398 PUTU16( buf+ len, coilreg);
00399 len+= 2;
00400 PUTU16( buf+ len, onoff);
00401 len+= 2;
00402
00403 return len;
00404 }
00405
00406
00416
00417 int MBWriteMultipleCoils( u8 *buf, u16 startcoils, u16 no, u8 count, u8 *data)
00418 {
00419 int len= 0;
00420 buf[ len]= MBF_WRITE_MULTIPLE_COILS;
00421 len++;
00422 PUTU16( buf+ len, startcoils);
00423 len+= 2;
00424 PUTU16( buf+ len, no);
00425 len+= 2;
00426 buf[ len]= count;
00427 len++;
00428 if( count & 0x07)
00429 count = ( count >> 3)+ 1;
00430 else
00431 count >>= 3;
00432 bcopy( (const char *)data, (char *)(buf+ len), count);
00433 len+= count;
00434
00435 return len;
00436 }
00437
00438
00446
00447 int MBReadInputRegisters(u8 *buf, u16 startreg, u16 no)
00448 {
00449 int len= 0;
00450 buf[ len]= MBF_READ_INPUT_REGISTERS;
00451 len++;
00452 PUTU16( buf+ len, startreg);
00453 len+= 2;
00454 PUTU16( buf+ len, no);
00455 len+= 2;
00456
00457 return len;
00458 }
00459
00460
00468
00469 int MBReadHoldingRegisters(u8 *buf, u16 startreg, u16 no)
00470 {
00471 int len= 0;
00472 buf[ len]= MBF_READ_HOLDING_REGISTERS;
00473 len++;
00474 PUTU16( buf+ len, startreg);
00475 len+= 2;
00476 PUTU16( buf+ len, no);
00477 len+= 2;
00478
00479 return len;
00480 }
00481
00482
00490
00491 int MBWriteSingleRegister(u8 *buf, u16 devicereg, u16 value)
00492 {
00493 int len= 0;
00494 buf[ len]= MBF_WRITE_SINGLE_REGISTER;
00495 len++;
00496 PUTU16( buf+ len, devicereg);
00497 len+= 2;
00498 PUTU16( buf+ len, value);
00499 len+= 2;
00500
00501 return len;
00502 }
00503
00504
00514
00515 int MBWriteMultipleRegisters(u8 *buf, u16 startreg, u16 noreg, u8 count, u16 *value)
00516 {
00517 int len= 0, i= 0;
00518 buf[ len]= MBF_WRITE_MULTIPLE_REGISTERS;
00519 len++;
00520 PUTU16( buf+ len, startreg);
00521 len+= 2;
00522 PUTU16( buf+ len, noreg);
00523 len+= 2;
00524 buf[ len]= count;
00525 len++;
00526
00527 for( i= 0; i< count/2; i++, len+= 2)
00528 PUTU16( buf+len, value[ i]);
00529
00530
00531 return len;
00532 }
00533
00534
00546
00547 int MBReadWriteMultipleRegisters(u8 *buf, u16 rsreg, u16 rno, u16 wsreg, u16 wno, u8 count, u16 *wv)
00548 {
00549 int len= 0, i= 0;
00550 buf[ len]= MBF_READ_WRITE_MULTIPLE_REGISTERS;
00551 len++;
00552 PUTU16( buf+ len, rsreg);
00553 len+= 2;
00554 PUTU16( buf+ len, rno);
00555 len+= 2;
00556 PUTU16( buf+ len, wsreg);
00557 len+= 2;
00558 PUTU16( buf+ len, wno);
00559 len+= 2;
00560 buf[ len++]= count;
00561
00562 for( i= 0; i< wno/2; i++, len+= 2)
00563 PUTU16( buf+len, wv[ i]);
00564
00565 return len;
00566 }
00567
00568
00577
00578 int MBMaskWriteRegister(u8 *buf, u16 reg, u16 andmask, u16 ormask)
00579 {
00580 int len= 0;
00581 buf[ len]= MBF_MASK_WRITE_REGISTER;
00582 len++;
00583 PUTU16( buf+ len, reg);
00584 len+= 2;
00585 PUTU16( buf+ len, andmask);
00586 len+= 2;
00587 PUTU16( buf+ len, ormask);
00588 len+= 2;
00589
00590 return len;
00591 }
00592
00593
00600
00601 int MBReadFIFOQueue(u8 *buf, u16 FIFOAddr)
00602 {
00603 int len= 0;
00604 buf[ len]= MBF_READ_FIFO_QUEUE;
00605 len++;
00606 PUTU16( buf+ len, FIFOAddr);
00607 len+= 2;
00608
00609 return len;
00610 }
00611
00612
00613
00614
00615
00621
00622 int MBReadExceptionStatus(u8 *buf)
00623 {
00624 int len= 0;
00625 buf[ len]= MBF_READ_EXCEPTION_STATUS;
00626 len++;
00627
00628 return len;
00629 }
00630
00631
00639
00640 int MBDiagnostic(u8 *buf, u16 subfunc, u16 data)
00641 {
00642 int len= 0;
00643 buf[ len]= MBF_DIAGNOSTIC;
00644 len++;
00645 PUTU16( buf+ len, subfunc);
00646 len+= 2;
00647 PUTU16( buf+ len, data);
00648 len+= 2;
00649
00650 return len;
00651 }
00652
00653
00659
00660 int MBGetCommEventCounter(u8 *buf)
00661 {
00662 int len= 0;
00663 buf[ len]= MBF_GET_COMM_EVENT_COUNTER;
00664 len++;
00665
00666 return len;
00667 }
00668
00669
00675
00676 int MBGetCommEventLog(u8 *buf)
00677 {
00678 int len= 0;
00679 buf[ len]= MBF_GET_COMM_EVENT_LOG;
00680 len++;
00681
00682 return len;
00683 }
00684
00685
00691
00692 int MBReportSlaveID(u8 *buf)
00693 {
00694 int len= 0;
00695 buf[ len]= MBF_REPORT_SLAVE_ID;
00696 len++;
00697
00698 return len;
00699 }
00700
00701
00702
00710
00711 int MBResponseReadDecreteInputs(u8 *buf, u8 no, u8 *value)
00712 {
00713 int len= 0, i= 0;
00714 buf[ len]= MBF_READ_DECRETE_INPUTS;
00715 len++;
00716 buf[ len]= no;
00717 len++;
00718 if( no & 0x07)
00719 no= (no >> 3) + 1;
00720 else
00721 no>>= 3;
00722
00723 for( i= 0; i< no/2; i++, len+= 2)
00724 PUTU16( buf+len, value[ i]);
00725
00726
00727 return len;
00728 }
00729
00730
00738
00739 int MBResponseReadCoils(u8 *buf, u8 no, u8 *value)
00740 {
00741 int len= 0, i= 0;
00742 buf[ len]= MBF_READ_COILS;
00743 len++;
00744 buf[ len]= no;
00745 len++;
00746 if( no & 0x07)
00747 no= (no >> 3) + 1;
00748 else
00749 no>>= 3;
00750
00751 for( i= 0; i< no/2; i++, len+= 2)
00752 PUTU16( buf+len, value[ i]);
00753
00754
00755 return len;
00756 }
00757
00758
00766
00767 int MBResponseReadInputRegisters(u8 *buf, u8 no, u16 *value)
00768 {
00769 int len= 0, i= 0;
00770 buf[ len]= MBF_READ_INPUT_REGISTERS;
00771 len++;
00772 buf[ len]= no;
00773 len++;
00774
00775 for( i= 0; i< no/2; i++, len+= 2)
00776 PUTU16( buf+len, value[ i]);
00777
00778 return len;
00779 }
00780
00781
00789
00790 int MBResponseReadHoldingRegisters(u8 *buf, u8 no, u16 *value)
00791 {
00792 int len= 0, i= 0;
00793 buf[ len]= MBF_READ_HOLDING_REGISTERS;
00794 len++;
00795 buf[ len]= no;
00796 len++;
00797
00798 for( i= 0; i< no/2; i++, len+= 2)
00799 PUTU16( buf+len, value[ i]);
00800
00801 return len;
00802 }
00803
00804
00812
00813 int MBResponseWriteMultipleRegisters(u8 *buf, u16 address, u16 no)
00814 {
00815 int len= 0;
00816 buf[ len]= MBF_WRITE_MULTIPLE_REGISTERS;
00817 len++;
00818 PUTU16( buf+len, address);
00819 len+= 2;
00820 PUTU16( buf+len, no);
00821 len+= 2;
00822
00823 return len;
00824 }
00825
00826
00834
00835 int MBResponseWriteSingleCoil(u8 *buf, u16 address, u16 value)
00836 {
00837 int len= 0;
00838 buf[ len]= MBF_WRITE_SINGLE_COIL;
00839 len++;
00840 PUTU16( buf+len, address);
00841 len+= 2;
00842 PUTU16( buf+len, value);
00843 len+= 2;
00844
00845 return len;
00846 }
00847
00848
00856
00857 int MBResponseWriteSingleRegister(u8 *buf, u16 address, u16 value)
00858 {
00859 int len= 0;
00860 buf[ len]= MBF_WRITE_SINGLE_REGISTER;
00861 len++;
00862 PUTU16( buf+len, address);
00863 len+= 2;
00864 PUTU16( buf+len, value);
00865 len+= 2;
00866
00867 return len;
00868 }
00869
00870
00879
00880 int MBResponseReadFIFOQueue(u8 *buf, u16 no, u16 count, u16 *value)
00881 {
00882 int len= 0, i= 0;
00883 buf[ len]= MBF_READ_FIFO_QUEUE;
00884 len++;
00885 PUTU16( buf+len, no);
00886 len+= 2;
00887 PUTU16( buf+len, count);
00888 len+= 2;
00889
00890 for( i= 0; i< (no- 2)/2; i++, len+= 2)
00891 PUTU16( buf+len, value[ i]);
00892
00893 return len;
00894 }
00895
00896
00903
00904 int MBResponseReadExceptionStatus(u8 *buf, u8 status)
00905 {
00906 int len= 0;
00907 buf[ len]= MBF_READ_EXCEPTION_STATUS;
00908 len++;
00909 buf[ len]= status;
00910 len++;
00911
00912 return len;
00913 }
00914
00915
00923
00924 int MBResponseDiagnostic(u8 *buf, u16 subfunc, u16 data)
00925 {
00926 int len= 0;
00927 buf[ len]= MBF_DIAGNOSTIC;
00928 len++;
00929 PUTU16( buf+len, subfunc);
00930 len+= 2;
00931 PUTU16( buf+len, data);
00932 len+= 2;
00933
00934 return len;
00935 }
00936
00937
00945
00946 int MBResponseGetCommEventCounter(u8 *buf, u16 status, u16 eventcount)
00947 {
00948 int len= 0;
00949 buf[ len]= MBF_GET_COMM_EVENT_COUNTER;
00950 len++;
00951 PUTU16( buf+len, status);
00952 len+= 2;
00953 PUTU16( buf+len, eventcount);
00954 len+= 2;
00955
00956 return len;
00957 }
00958
00959
00970
00971 int MBResponseGetCommEventLog(u8 *buf, u8 no, u16 status, u16 eventcount, u16 messagecount, u8 *events)
00972 {
00973 int len= 0;
00974 buf[ len]= MBF_GET_COMM_EVENT_LOG;
00975 len++;
00976 buf[ len]= no;
00977 len++;
00978 PUTU16( buf+len, status);
00979 len+= 2;
00980 PUTU16( buf+len, eventcount);
00981 len+= 2;
00982 PUTU16( buf+len, messagecount);
00983 len+= 2;
00984
00985 bcopy( (const char *)events, (char *)(buf+ len), no- 6);
00986
00987 return len;
00988 }
00989
00990
00991
00999
01000 int MBGetReadDecreteInputs(u8 *pdu, u16 *startdec, u16 *no)
01001 {
01002 if( pdu[ 0] != MBF_READ_DECRETE_INPUTS)
01003 return MB_ERROR_FUNCTION;
01004
01005 GETU16( *startdec, pdu+ 1);
01006 GETU16( *no, pdu+ 3);
01007
01008 return MB_OK;
01009 }
01010
01011
01019
01020 int MBGetReadCoils(u8 *pdu, u16 *startcoils, u16 *no)
01021 {
01022 if( pdu[ 0] != MBF_READ_COILS)
01023 return MB_ERROR_FUNCTION;
01024
01025 GETU16( *startcoils, pdu+ 1);
01026 GETU16( *no, pdu+ 3);
01027
01028 return MB_OK;
01029 }
01030
01031
01039
01040 int MBGetWriteSingleCoil(u8 *pdu, u16 *coilreg, u16 *onoff)
01041 {
01042 if( pdu[ 0] != MBF_WRITE_SINGLE_COIL)
01043 return MB_ERROR_FUNCTION;
01044
01045 GETU16( *coilreg, pdu+ 1);
01046 GETU16( *onoff, pdu+ 3);
01047
01048 return MB_OK;
01049 }
01050
01051
01059
01060 int MBGetWriteMultipleCoils(u8 *pdu, u16 *startcoils, u16 *no)
01061 {
01062 if( pdu[ 0] != MBF_WRITE_MULTIPLE_COILS)
01063 return MB_ERROR_FUNCTION;
01064
01065 GETU16( *startcoils, pdu+ 1);
01066 GETU16( *no, pdu+ 3);
01067
01068 return MB_OK;
01069 }
01070
01071
01079
01080 int MBGetReadInputRegisters(u8 *pdu, u16 *startreg, u16 *no)
01081 {
01082 if( pdu[ 0] != MBF_READ_INPUT_REGISTERS)
01083 return MB_ERROR_FUNCTION;
01084
01085 GETU16( *startreg, pdu+ 1);
01086 GETU16( *no, pdu+ 3);
01087
01088 return MB_OK;
01089 }
01090
01091
01099
01100 int MBGetReadHoldingRegisters(u8 *pdu, u16 *startreg, u16 *no)
01101 {
01102 if( pdu[ 0] != MBF_READ_HOLDING_REGISTERS)
01103 return MB_ERROR_FUNCTION;
01104
01105 GETU16( *startreg, pdu+ 1);
01106 GETU16( *no, pdu+ 3);
01107
01108 return MB_OK;
01109 }
01110
01111
01119
01120 int MBGetWriteSingleRegister(u8 *pdu, u16 *devicereg, u16 *value)
01121 {
01122 if( pdu[ 0] != MBF_WRITE_SINGLE_REGISTER)
01123 return MB_ERROR_FUNCTION;
01124
01125 GETU16( *devicereg, pdu+ 1);
01126 GETU16( *value, pdu+ 3);
01127
01128 return MB_OK;
01129 }
01130
01131
01141
01142 int MBGetWriteMultipleRegisters(u8 *pdu, u16 *startreg, u16 *noreg, u8 *count, u16 *value)
01143 {
01144 int i= 0;
01145 if( pdu[ 0] != MBF_WRITE_MULTIPLE_REGISTERS)
01146 return MB_ERROR_FUNCTION;
01147
01148 GETU16( *startreg, pdu+ 1);
01149 GETU16( *noreg, pdu+ 3);
01150 *count= pdu[ 5];
01151
01152 for( i= 0; i< (*count)/2; i++)
01153 GETU16( value[ i], pdu+ 6+ i* 2);
01154
01155 return MB_OK;
01156 }
01157
01158
01170
01171 int MBGetReadWriteMultipleRegisters(u8 *pdu, u16 *rsreg, u16 *rno, u16 *wsreg, u16 *wno, u8 *count, u16 *wv)
01172 {
01173 int i= 0;
01174 if( pdu[ 0] != MBF_READ_WRITE_MULTIPLE_REGISTERS)
01175 return MB_ERROR_FUNCTION;
01176
01177 GETU16( *rsreg, pdu+ 1);
01178 GETU16( *rno, pdu+ 3);
01179 GETU16( *wsreg, pdu+ 5);
01180 GETU16( *wno, pdu+ 7);
01181 *count= pdu[ 9];
01182 for( i= 0; i< (*count)/2; i++)
01183 GETU16( wv[ i], pdu+ 10+ i*2 );
01184
01185 return MB_OK;
01186 }
01187
01188
01197
01198 int MBGetMaskWriteRegister(u8 *pdu, u16 *reg, u16 *andmask, u16 *ormask)
01199 {
01200 if( pdu[ 0] != MBF_MASK_WRITE_REGISTER)
01201 return MB_ERROR_FUNCTION;
01202
01203 GETU16( *reg, pdu+ 1);
01204 GETU16( *andmask, pdu+ 3);
01205 GETU16( *ormask, pdu+ 5);
01206
01207 return MB_OK;
01208 }
01209
01210
01217
01218 int MBGetReadFIFOQueue(u8 *pdu, u16 *FIFOAddr)
01219 {
01220 if( pdu[ 0] != MBF_READ_FIFO_QUEUE)
01221 return MB_ERROR_FUNCTION;
01222
01223 GETU16( *FIFOAddr, pdu+ 1);
01224
01225 return MB_OK;
01226 }
01227
01228
01236
01237 int MBGetDiagnostic(u8 *pdu, u16 *subfunc, u16 *data)
01238 {
01239 if( pdu[ 0] != MBF_DIAGNOSTIC)
01240 return MB_ERROR_FUNCTION;
01241
01242 GETU16( *subfunc, pdu+ 1);
01243 GETU16( *data, pdu+ 3);
01244
01245 return MB_OK;
01246 }
01247
01248
01249
01257
01258 int MBGetResponseReadDecreteInputs(u8 *pdu, u8 *no, u8 *value)
01259 {
01260 int i= 0;
01261 if( pdu[ 0] != MBF_READ_DECRETE_INPUTS)
01262 return MB_ERROR_FUNCTION;
01263
01264 *no= pdu[ 1];
01265 for( i= 0; i< *no; i++)
01266 value[ i]= pdu[ 2+ i];
01267
01268 return MB_OK;
01269 }
01270
01271
01279
01280 int MBGetResponseReadCoils(u8 *pdu, u8 *no, u8 *value)
01281 {
01282 int i= 0;
01283 if( pdu[ 0] != MBF_READ_COILS)
01284 return MB_ERROR_FUNCTION;
01285
01286 *no= pdu[ 1];
01287 for( i= 0; i< *no; i++)
01288 value[ i]= pdu[ 2+ i];
01289
01290 return MB_OK;
01291 }
01292
01293
01301
01302 int MBGetResponseReadInputRegisters(u8 *pdu, u8 *no, u16 *value)
01303 {
01304 int i= 0;
01305 if( pdu[ 0] != MBF_READ_INPUT_REGISTERS)
01306 return MB_ERROR_FUNCTION;
01307
01308 *no= pdu[ 1];
01309 for( i= 0; i< *no/2; i++)
01310 GETU16( value[ i], pdu+ 2+ i*2);
01311
01312 return MB_OK;
01313 }
01314
01315
01323
01324 int MBGetResponseReadHoldingRegisters(u8 *pdu, u8 *no, u16 *value)
01325 {
01326 int i= 0;
01327 if( pdu[ 0] != MBF_READ_HOLDING_REGISTERS)
01328 return MB_ERROR_FUNCTION;
01329
01330 *no= pdu[ 1];
01331 for( i= 0; i< *no/2; i++)
01332 GETU16( value[ i], ( pdu+ 2+ i*2));
01333
01334 return MB_OK;
01335 }
01336
01337
01345
01346 int MBGetResponseWriteMultipleRegisters(u8 *pdu, u16 *address, u16 *no)
01347 {
01348 if( pdu[ 0] != MBF_WRITE_MULTIPLE_REGISTERS)
01349 return MB_ERROR_FUNCTION;
01350
01351 GETU16( *address, pdu+ 1);
01352 GETU16( *no, pdu+ 3);
01353
01354 return MB_OK;
01355 }
01356
01357
01365
01366 int MBGetResponseWriteSingleCoil(u8 *pdu, u16 *address, u16 *value)
01367 {
01368 if( pdu[ 0] != MBF_WRITE_SINGLE_COIL)
01369 return MB_ERROR_FUNCTION;
01370
01371 GETU16( *address, pdu+ 1);
01372 GETU16( *value, pdu+ 3);
01373
01374 return MB_OK;
01375 }
01376
01377
01385
01386 int MBGetResponseWriteMultipleCoils(u8 *pdu, u16 *address, u16 *value)
01387 {
01388 if( pdu[ 0] != MBF_WRITE_MULTIPLE_COILS)
01389 return MB_ERROR_FUNCTION;
01390
01391 GETU16( *address, pdu+ 1);
01392 GETU16( *value, pdu+ 3);
01393
01394 return MB_OK;
01395 }
01396
01397
01405
01406 int MBGetResponseWriteSingleRegister(u8 *pdu, u16 *address, u16 *value)
01407 {
01408 if( pdu[ 0] != MBF_WRITE_SINGLE_REGISTER)
01409 return MB_ERROR_FUNCTION;
01410
01411 GETU16( *address, pdu+ 1);
01412 GETU16( *value, pdu+ 3);
01413
01414 return MB_OK;
01415 }
01416
01417
01425
01426 int MBGetResponseReadFIFOQueue(u8 *pdu, u16 *count, u16 *value)
01427 {
01428 u16 i= 0, no= 0;
01429 if( pdu[ 0] != MBF_READ_FIFO_QUEUE)
01430 return MB_ERROR_FUNCTION;
01431
01432 GETU16( no, pdu+ 1);
01433 GETU16( *count, pdu+ 3);
01434 for( i= 0; i< no/2; i++)
01435 GETU16( value[ i], ( pdu+ 5+ i*2));
01436
01437 return MB_OK;
01438 }
01439
01440
01447
01448 int MBGetResponseReadExceptionStatus(u8 *pdu, u8 *status)
01449 {
01450 if( pdu[ 0] != MBF_READ_EXCEPTION_STATUS)
01451 return MB_ERROR_FUNCTION;
01452
01453 *status= pdu[ 1];
01454
01455 return MB_OK;
01456 }
01457
01458
01466
01467 int MBGetResponseDiagnostic(u8 *pdu, u16 *subfunc, u16 *data)
01468 {
01469 if( pdu[ 0] != MBF_DIAGNOSTIC)
01470 return MB_ERROR_FUNCTION;
01471
01472 GETU16( *subfunc, pdu+ 1);
01473 GETU16( *data, pdu+ 3);
01474
01475 return MB_OK;
01476 }
01477
01478
01486
01487 int MBGetResponseGetCommEventCounter(u8 *pdu, u16 *status, u16 *eventcount)
01488 {
01489 if( pdu[ 0] != MBF_GET_COMM_EVENT_COUNTER)
01490 return MB_ERROR_FUNCTION;
01491
01492 GETU16( *status, pdu+ 1);
01493 GETU16( *eventcount, pdu+ 3);
01494
01495 return MB_OK;
01496 }
01497
01498
01508
01509 int MBGetResponseGetCommEventLog(u8 *pdu, u16 *status, u16 *eventcount, u16 *messagecount, u8 *events)
01510 {
01511 u8 no;
01512 if( pdu[ 0] != MBF_GET_COMM_EVENT_LOG)
01513 return MB_ERROR_FUNCTION;
01514
01515 no= pdu[ 1];
01516 GETU16( *status, pdu+ 2);
01517 GETU16( *eventcount, pdu+ 4);
01518 GETU16( *messagecount, pdu+ 6);
01519
01520 bcopy( (const char *)(pdu+ 8), (char *)events, no- 6);
01521
01522 return MB_OK;
01523 }
01524
01525
01533
01534 int MBGetResponseReadWriteMultipleRegisters(u8 *pdu, u8 *no, u16 *value)
01535 {
01536 int i= 0;
01537 if( pdu[ 0] != MBF_READ_WRITE_MULTIPLE_REGISTERS)
01538 return MB_ERROR_FUNCTION;
01539
01540 *no= pdu[ 1];
01541 for( i= 0; i< *no/2; i++)
01542 GETU16( value[ i], pdu+ 2+ i*2);
01543
01544 return MB_OK;
01545 }
01546
01547
01556
01557 int MBGetResponseMaskWriteRegister(u8 *pdu, u16 *reg, u16 *andmask, u16 *ormask)
01558 {
01559 if( pdu[ 0] != MBF_MASK_WRITE_REGISTER)
01560 return MB_ERROR_FUNCTION;
01561
01562 GETU16( *reg, pdu+ 1);
01563 GETU16( *andmask, pdu+ 3);
01564 GETU16( *ormask, pdu+ 5);
01565
01566 return MB_OK;
01567 }
01568
01569
01577
01578 int MBGetResponseReportSlaveID(u8 *pdu, u8 *slave_id, u8 *status)
01579 {
01580 if( pdu[ 0] != MBF_REPORT_SLAVE_ID)
01581 return MB_ERROR_FUNCTION;
01582
01583 *slave_id= pdu[ 2];
01584 *status= pdu[ 3];
01585
01586 return MB_OK;
01587 }