Main Page | Alphabetical List | Data Structures | Directories | File List | Data Fields | Globals

modbus_utils.c

Go to the documentation of this file.
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         /* to do length error check */
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;          // length of address + pdu
00260                         buf[ 5]= (len+1) & 0x00FF;      // length of address + pdu
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 // Master function
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);            // don't put ++ in the macro
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);          // don't put ++ in the macro
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);             // don't put ++ in the macro
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);          // don't put ++ in the macro
00423         len+= 2;
00424         PUTU16( buf+ len, no);
00425         len+= 2;
00426         buf[ len]= count;
00427         len++;
00428         if( count & 0x07)                       // test if no could be divied by 8
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);            // don't put ++ in the macro
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);            // don't put ++ in the macro
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);           // don't put ++ in the macro
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);            // don't put ++ in the macro
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);               // don't put ++ in the macro
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);                 // don't put ++ in the macro
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);            // don't put ++ in the macro
00607         len+= 2;
00608 
00609         return len;
00610 }
00611 
00612 // MBF_READ_FILE_RECORD                 0x14
00613 // MBF_WRITE_FILE_RECORD                0x15
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);             // don't put ++ in the macro
00646         len+= 2;
00647         PUTU16( buf+ len, data);                // all data of subfunc is 16bits long
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 // Response function
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)                  // test if no could be divied by 8
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)                  // test if no could be divied by 8
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 // Modbus get request value from PDU
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 // Modbus get response value from PDU
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 }

Generated on Thu Oct 6 09:13:41 2005 for Example Modbus Library by  doxygen 1.4.4