|
rllib
1
|
00001 /*************************************************************************** 00002 rlmodbus.cpp - description 00003 ------------------- 00004 begin : Tue Mar 13 2003 00005 copyright : (C) 2003 by Rainer Lehrig 00006 email : lehrig@t-online.de 00007 ***************************************************************************/ 00008 00009 /*************************************************************************** 00010 * * 00011 * This library is free software; you can redistribute it and/or modify * 00012 * it under the terms of the GNU LESSER GENERAL PUBLIC LICENSE as * 00013 * published by the Free Software Foundation * 00014 * * 00015 ***************************************************************************/ 00016 #include "rlmodbus.h" 00017 #include <stdio.h> 00018 #include <string.h> 00019 00020 rlModbus::rlModbus(long max_telegram_length, int _mode, char end_delimitor) 00021 { 00022 if(max_telegram_length < 256) max_telegram_length = 256; 00023 tel = new unsigned char[max_telegram_length]; 00024 maxtel = max_telegram_length; 00025 mode = _mode; 00026 delimitor = end_delimitor; 00027 s = NULL; 00028 tty = NULL; 00029 autoreconnectSocket = 1; 00030 } 00031 00032 rlModbus::~rlModbus() 00033 { 00034 if(tel != NULL) delete [] tel; 00035 } 00036 00037 int rlModbus::data2int(const unsigned char *data) 00038 { 00039 return (data[0]*256)+data[1]; 00040 } 00041 00042 int rlModbus::int2data(int val, unsigned char *data) 00043 { 00044 data[0] = (unsigned char) val / 256; 00045 data[1] = (unsigned char) val & 0x0ff; 00046 return 0; 00047 } 00048 00049 int rlModbus::intsize() 00050 { 00051 return 2; 00052 } 00053 00054 int rlModbus::write(int slave, int function, const unsigned char *data, int datalen, int *transactionID) 00055 { 00056 int len,i; 00057 00058 if(slave < 0 || slave > 255) return MODBUS_ERROR; 00059 len = 0; 00060 if(mode == MODBUS_ASCII) 00061 { 00062 tel[len++] = ':'; 00063 sprintf((char *) &tel[len], "%02X", slave); len += 2; 00064 sprintf((char *) &tel[len], "%02X", function); len += 2; 00065 for(i=0; i<datalen; i++) 00066 { 00067 sprintf((char *) &tel[len], "%02X",(int) data[i]); len += 2; 00068 if((len+4) > maxtel) return MODBUS_ERROR; 00069 } 00070 insertLRC(len); len += 2; 00071 tel[len++] = 0x0d; 00072 if(delimitor == 0x0a) tel[len++] = 0x0a; 00073 } 00074 else if(mode == MODBUS_RTU) 00075 { 00076 if(s != NULL) 00077 { 00078 if(transactionID == NULL) 00079 { 00080 tel[len++] = 0; // bytes 0,1 Transaction ID. Not important. Usually zero when making a request, the server will copy them faithfully into the response. 00081 tel[len++] = 0; 00082 } 00083 else 00084 { 00085 tel[len++] = ((*transactionID) & 0xFF00) / 256; // bytes 0,1 Transaction ID. 00086 tel[len++] = (*transactionID) & 0xFF; 00087 } 00088 tel[len++] = 0; // bytes 2,3 Protocol number. Must be zero. 00089 tel[len++] = 0; 00090 tel[len++] = 0; // byte 4 Length (upper byte). Since all requests will be less than 256 bytes in length (!), this will always be zero. 00091 tel[len++] = 2+datalen; // byte 5 Length (lower byte). Equal to the number of bytes which follow 00092 } 00093 tel[len++] = (unsigned char) slave; 00094 tel[len++] = (unsigned char) function; 00095 for(i=0; i<datalen; i++) 00096 { 00097 tel[len++] = data[i]; 00098 if((len+2) > maxtel) return MODBUS_ERROR; 00099 } 00100 insertCRC(len); len += 2; 00101 } 00102 else return MODBUS_ERROR; 00103 00104 if(s != NULL) 00105 { 00106 if(s->isConnected() == 0) 00107 { 00108 if(autoreconnectSocket) s->connect(); 00109 if(s->isConnected() == 0) return MODBUS_ERROR; 00110 } 00111 if(s->write(tel,len-2) < 0) return MODBUS_ERROR; // don't send LRC or CRC 00112 } 00113 else if(tty != NULL) 00114 { 00115 if(tty->writeBlock(tel,len) < 0) return MODBUS_ERROR; 00116 } 00117 else return MODBUS_ERROR; 00118 return MODBUS_SUCCESS; 00119 } 00120 00121 int rlModbus::request(int slave, int function, int start_adr, int num_register) 00122 { 00123 unsigned char data[4]; 00124 00125 data[0] = (unsigned char) ( start_adr / 256 ); 00126 data[1] = (unsigned char) ( start_adr & 0x0ff ); 00127 data[2] = (unsigned char) ( num_register / 256 ); 00128 data[3] = (unsigned char) ( num_register & 0x0ff ); 00129 return write(slave, function, data, 4); 00130 } 00131 00132 int rlModbus::response(int *slave, int *function, unsigned char *data, int timeout) 00133 { 00134 unsigned char *telptr; 00135 int ret,len,byte_count,idata,i,itel,val; 00136 00137 len = 0; 00138 if(mode != MODBUS_ASCII && mode != MODBUS_RTU) return MODBUS_ERROR; 00139 if(s != NULL) 00140 { 00141 if(s->isConnected() == 0) return MODBUS_ERROR; 00142 if(mode == MODBUS_RTU) 00143 { 00144 if(s->read((char *) tel, 6, timeout) <= 0) return MODBUS_ERROR; 00145 // bytes 0,1 Transaction ID faithfully copied from the request message 00146 // bytes 2,3 Protocol number always zero 00147 // byte 4 Response length (upper byte) Always zero 00148 // byte 5 Response length (lower byte). Equal to the number of bytes which follow 00149 // Here comes the normal Modus telegram 00150 if(s->read((char *) tel, 2, timeout) <= 0) return MODBUS_ERROR; 00151 *slave = tel[0]; 00152 *function = tel[1]; 00153 switch(*function) 00154 { 00155 case ReadCoilStatus: 00156 case ReadInputStatus: 00157 case ReadHoldingRegisters: 00158 case ReadInputRegisters: 00159 case FetchCommEventLog: 00160 case ReportSlaveID: 00161 case ReadGeneralReference: 00162 case WriteGeneralReference: 00163 case ReadWrite4XRegisters: 00164 if(s->read((char *) tel, 1, timeout) <= 0) return MODBUS_ERROR; 00165 byte_count = tel[0]; 00166 if(s->read((char *) data, byte_count, timeout) <= 0) return MODBUS_ERROR; 00167 return byte_count; 00168 case ForceSingleCoil: 00169 case PresetSingleRegister: 00170 case FetchCommEventCtr: 00171 case ForceMultipleCoils: 00172 case PresetMultipleRegs: 00173 byte_count = 4; 00174 if(s->read((char *) data, byte_count, timeout) <= 0) return MODBUS_ERROR; 00175 return byte_count; 00176 case ReadExceptionStatus: 00177 byte_count = 1; 00178 if(s->read((char *) data, byte_count, timeout) <= 0) return MODBUS_ERROR; 00179 return byte_count; 00180 case MaskWrite4XRegisters: 00181 byte_count = 6; 00182 if(s->read((char *) data, byte_count, timeout) <= 0) return MODBUS_ERROR; 00183 return byte_count; 00184 case ReadFifoQueue: 00185 if(s->read((char *) tel, 2, timeout) <= 0) return MODBUS_ERROR; 00186 byte_count = tel[0]*256 + tel[1]; 00187 if(s->read((char *) data, byte_count, timeout) <= 0) return MODBUS_ERROR; 00188 return byte_count; 00189 default: 00190 return MODBUS_ERROR; 00191 } 00192 } 00193 } 00194 else if(tty != NULL) 00195 { 00196 if(mode == MODBUS_ASCII) 00197 { 00198 //printf("modbus ascii\n"); 00199 for(i=0; i<maxtel; i++) 00200 { 00201 ret = tty->select(timeout); 00202 if(ret == 0) return MODBUS_ERROR; 00203 //printf("readChar\n"); 00204 itel = tty->readChar(); 00205 if(itel < 0) return MODBUS_ERROR; 00206 tel[i] = (unsigned char) itel; 00207 if(tel[i] == 0x0d && delimitor != 0x0a) break; 00208 if(tel[i] == 0x0a) break; 00209 } 00210 tel[i] = '\0'; 00211 telptr = (unsigned char *) strchr((const char *) tel,':'); 00212 if(telptr == NULL) return MODBUS_ERROR; 00213 len++; 00214 sscanf((char *) &telptr[len],"%02X",slave); len += 2; 00215 sscanf((char *) &telptr[len],"%02X",function); len += 2; 00216 switch(*function) 00217 { 00218 case ReadCoilStatus: 00219 case ReadInputStatus: 00220 case ReadHoldingRegisters: 00221 case ReadInputRegisters: 00222 case FetchCommEventLog: 00223 case ReportSlaveID: 00224 case ReadGeneralReference: 00225 case WriteGeneralReference: 00226 case ReadWrite4XRegisters: 00227 sscanf((char *) &telptr[len],"%02X",&byte_count); len += 2; 00228 for(idata=0; idata<byte_count; idata++) 00229 { 00230 sscanf((const char *) &telptr[len], "%02X", &val); len += 2; 00231 data[idata] = val; 00232 } 00233 data[idata] = 0x0ff; // terminator, this data can't come over modbus 00234 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00235 return byte_count; 00236 case ForceSingleCoil: 00237 case PresetSingleRegister: 00238 case FetchCommEventCtr: 00239 case ForceMultipleCoils: 00240 case PresetMultipleRegs: 00241 byte_count = 8; 00242 for(idata=0; idata<(byte_count/2); idata++) 00243 { 00244 data[idata] = buf2int_ascii(&telptr[len]); len += 2; 00245 } 00246 data[idata] = 0x0ff; // terminator, this data can't come over modbus 00247 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00248 return byte_count; 00249 case ReadExceptionStatus: 00250 byte_count = 2; 00251 for(idata=0; idata<(byte_count/2); idata++) 00252 { 00253 data[idata] = buf2int_ascii(&telptr[len]); len += 2; 00254 } 00255 data[idata] = 0x0ff; // terminator, this data can't come over modbus 00256 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00257 return byte_count; 00258 case MaskWrite4XRegisters: 00259 byte_count = 12; 00260 for(idata=0; idata<(byte_count/2); idata++) 00261 { 00262 data[idata] = buf2int_ascii(&telptr[len]); len += 2; 00263 } 00264 data[idata] = 0x0ff; // terminator, this data can't come over modbus 00265 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00266 return byte_count; 00267 case ReadFifoQueue: 00268 sscanf((char *) &telptr[len],"%04X",&byte_count); len += 4; 00269 for(idata=0; idata<(byte_count/2); idata++) 00270 { 00271 data[idata] = buf2int_ascii(&telptr[len]); len += 2; 00272 } 00273 data[idata] = 0x0ff; // terminator, this data can't come over modbus 00274 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00275 return byte_count; 00276 break; 00277 default: 00278 return MODBUS_ERROR; 00279 } 00280 } 00281 else if(mode == MODBUS_RTU) 00282 { 00283 ret = tty->select(timeout); 00284 if(ret == 0) return MODBUS_ERROR; 00285 if(tty->readBlock(tel, 2) <= 0) return MODBUS_ERROR; 00286 *slave = tel[len++]; 00287 *function = tel[len++]; 00288 switch(*function) 00289 { 00290 case ReadCoilStatus: 00291 case ReadInputStatus: 00292 case ReadHoldingRegisters: 00293 case ReadInputRegisters: 00294 case FetchCommEventLog: 00295 case ReportSlaveID: 00296 case ReadGeneralReference: 00297 case WriteGeneralReference: 00298 case ReadWrite4XRegisters: 00299 if(tty->select(timeout) == 0) return MODBUS_ERROR; 00300 if(tty->readBlock(&tel[len], 1) <= 0) return MODBUS_ERROR; 00301 byte_count = tel[len++]; 00302 if(tty->select(timeout) == 0) return MODBUS_ERROR; 00303 if(tty->readBlock(data, byte_count+2) <= 0) return MODBUS_ERROR; 00304 memcpy(&tel[len],data,byte_count+2); len += byte_count + 2; 00305 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00306 return byte_count; 00307 case ForceSingleCoil: 00308 case PresetSingleRegister: 00309 case FetchCommEventCtr: 00310 case ForceMultipleCoils: 00311 case PresetMultipleRegs: 00312 byte_count = 4; 00313 if(tty->select(timeout) == 0) return MODBUS_ERROR; 00314 if(tty->readBlock(data, byte_count+2) <= 0) return MODBUS_ERROR; 00315 memcpy(&tel[len],data,byte_count+2); len += byte_count + 2; 00316 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00317 return byte_count; 00318 case ReadExceptionStatus: 00319 byte_count = 1; 00320 if(tty->select(timeout) == 0) return MODBUS_ERROR; 00321 if(tty->readBlock(data, byte_count+2) <= 0) return MODBUS_ERROR; 00322 memcpy(&tel[len],data,byte_count+2); len += byte_count + 2; 00323 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00324 return byte_count; 00325 case MaskWrite4XRegisters: 00326 byte_count = 6; 00327 if(tty->select(timeout) == 0) return MODBUS_ERROR; 00328 if(tty->readBlock(data, byte_count+2) <= 0) return MODBUS_ERROR; 00329 memcpy(&tel[len],data,byte_count+2); len += byte_count + 2; 00330 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00331 return byte_count; 00332 case ReadFifoQueue: 00333 if(tty->select(timeout) == 0) return MODBUS_ERROR; 00334 if(tty->readBlock(&tel[len], 2) <= 0) return MODBUS_ERROR; 00335 byte_count = tel[len]*256 + tel[len+1]; len += 2; 00336 if(tty->select(timeout) == 0) return MODBUS_ERROR; 00337 if(tty->readBlock(data, byte_count+2) <= 0) return MODBUS_ERROR; 00338 memcpy(&tel[len],data,byte_count+2); len += byte_count + 2; 00339 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00340 return byte_count; 00341 default: 00342 return MODBUS_ERROR; 00343 } 00344 } 00345 } 00346 else return MODBUS_ERROR; 00347 return MODBUS_SUCCESS; 00348 } 00349 00350 int rlModbus::readRequest(int *slave, int *function, unsigned char *data, int timeout, int *transactionID) 00351 { 00352 unsigned char *telptr; 00353 int ret,len,byte_count,i,itel,val; 00354 00355 len = 0; 00356 if(mode != MODBUS_ASCII && mode != MODBUS_RTU) return MODBUS_ERROR; 00357 if(s != NULL) 00358 { 00359 if(s->isConnected() == 0) return MODBUS_ERROR; 00360 if(mode == MODBUS_RTU) 00361 { 00362 if(s->read((char *) tel, 6, timeout) <= 0) return MODBUS_ERROR; 00363 if(transactionID != NULL) *transactionID = tel[0] * 256 + tel[1]; // return transactionID 00364 // bytes 0,1 Transaction ID faithfully copied from the request message 00365 // bytes 2,3 Protocol number always zero 00366 // byte 4 Response length (upper byte) Always zero 00367 // byte 5 Response length (lower byte). Equal to the number of bytes which follow 00368 // Here comes the normal Modus telegram 00369 if(s->read((char *) tel, 2, timeout) <= 0) return MODBUS_ERROR; 00370 *slave = tel[0]; 00371 *function = tel[1]; 00372 switch(*function) 00373 { 00374 case ReadCoilStatus: 00375 case ReadInputStatus: 00376 case ReadHoldingRegisters: 00377 case ReadInputRegisters: 00378 case ForceSingleCoil: 00379 case PresetSingleRegister: 00380 if(s->read((char *) data, 4, timeout) <= 0) return MODBUS_ERROR; 00381 return 4; 00382 case ReadExceptionStatus: 00383 case FetchCommEventCtr: 00384 case FetchCommEventLog: 00385 case ReportSlaveID: 00386 return 0; 00387 case ForceMultipleCoils: 00388 case PresetMultipleRegs: 00389 if(s->read((char *) data, 5, timeout) <= 0) return MODBUS_ERROR; 00390 byte_count = data[4]; 00391 if(s->read((char *) &data[4], byte_count, timeout) <= 0) return MODBUS_ERROR; 00392 return 4+byte_count; 00393 case ReadGeneralReference: 00394 case WriteGeneralReference: 00395 if(s->read((char *) data, 1, timeout) <= 0) return MODBUS_ERROR; 00396 byte_count = data[0]; 00397 if(s->read((char *) data, byte_count, timeout) <= 0) return MODBUS_ERROR; 00398 return byte_count; 00399 case MaskWrite4XRegisters: 00400 if(s->read((char *) data, 6, timeout) <= 0) return MODBUS_ERROR; 00401 return 6; 00402 case ReadWrite4XRegisters: 00403 if(s->read((char *) data, 9, timeout) <= 0) return MODBUS_ERROR; 00404 byte_count = data[8]; 00405 if(s->read((char *) &data[8], byte_count, timeout) <= 0) return MODBUS_ERROR; 00406 return 8+byte_count; 00407 case ReadFifoQueue: 00408 if(s->read((char *) data, 2, timeout) <= 0) return MODBUS_ERROR; 00409 return 2; 00410 default: 00411 return MODBUS_ERROR; 00412 } 00413 } 00414 } 00415 else if(tty != NULL) 00416 { 00417 if(mode == MODBUS_ASCII) 00418 { 00419 //printf("modbus ascii\n"); 00420 for(i=0; i<maxtel; i++) 00421 { 00422 ret = tty->select(timeout); 00423 if(ret == 0) return MODBUS_ERROR; 00424 //printf("readChar\n"); 00425 itel = tty->readChar(); 00426 if(itel < 0) return MODBUS_ERROR; 00427 tel[i] = (unsigned char) itel; 00428 if(tel[i] == 0x0d && delimitor != 0x0a) break; 00429 if(tel[i] == 0x0a) break; 00430 } 00431 tel[i] = '\0'; 00432 telptr = (unsigned char *) strchr((const char *) tel,':'); 00433 if(telptr == NULL) return MODBUS_ERROR; 00434 len++; 00435 sscanf((char *) &telptr[len],"%02X",slave); len += 2; 00436 sscanf((char *) &telptr[len],"%02X",function); len += 2; 00437 switch(*function) 00438 { 00439 case ReadCoilStatus: 00440 case ReadInputStatus: 00441 case ReadHoldingRegisters: 00442 case ReadInputRegisters: 00443 case ForceSingleCoil: 00444 case PresetSingleRegister: 00445 sscanf((char *) &telptr[len],"%02X",&val); data[0] = (unsigned char) val; len += 2; 00446 sscanf((char *) &telptr[len],"%02X",&val); data[1] = (unsigned char) val; len += 2; 00447 sscanf((char *) &telptr[len],"%02X",&val); data[2] = (unsigned char) val; len += 2; 00448 sscanf((char *) &telptr[len],"%02X",&val); data[3] = (unsigned char) val; len += 2; 00449 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00450 return 4; 00451 case ReadExceptionStatus: 00452 case FetchCommEventCtr: 00453 case FetchCommEventLog: 00454 case ReportSlaveID: 00455 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00456 return 0; 00457 case ForceMultipleCoils: 00458 case PresetMultipleRegs: 00459 sscanf((char *) &telptr[len],"%02X",&val); data[0] = (unsigned char) val; len += 2; 00460 sscanf((char *) &telptr[len],"%02X",&val); data[1] = (unsigned char) val; len += 2; 00461 sscanf((char *) &telptr[len],"%02X",&val); data[2] = (unsigned char) val; len += 2; 00462 sscanf((char *) &telptr[len],"%02X",&val); data[3] = (unsigned char) val; len += 2; 00463 sscanf((char *) &telptr[len],"%02X",&byte_count); len += 2; 00464 for(i=0; i<byte_count; i++) 00465 { 00466 sscanf((char *) &telptr[len],"%02X",&val); data[4+i] = (unsigned char) val; len += 2; 00467 } 00468 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00469 return 4+byte_count; 00470 case ReadGeneralReference: 00471 case WriteGeneralReference: 00472 sscanf((char *) &telptr[len],"%02X",&byte_count); len += 2; 00473 for(i=0; i<byte_count; i++) 00474 { 00475 sscanf((char *) &telptr[len],"%02X",&val); data[i] = (unsigned char) val; len += 2; 00476 } 00477 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00478 return byte_count; 00479 case MaskWrite4XRegisters: 00480 sscanf((char *) &telptr[len],"%02X",&val); data[0] = (unsigned char) val; len += 2; 00481 sscanf((char *) &telptr[len],"%02X",&val); data[1] = (unsigned char) val; len += 2; 00482 sscanf((char *) &telptr[len],"%02X",&val); data[2] = (unsigned char) val; len += 2; 00483 sscanf((char *) &telptr[len],"%02X",&val); data[3] = (unsigned char) val; len += 2; 00484 sscanf((char *) &telptr[len],"%02X",&val); data[4] = (unsigned char) val; len += 2; 00485 sscanf((char *) &telptr[len],"%02X",&val); data[5] = (unsigned char) val; len += 2; 00486 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00487 return 6; 00488 case ReadWrite4XRegisters: 00489 sscanf((char *) &telptr[len],"%02X",&val); data[0] = (unsigned char) val; len += 2; 00490 sscanf((char *) &telptr[len],"%02X",&val); data[1] = (unsigned char) val; len += 2; 00491 sscanf((char *) &telptr[len],"%02X",&val); data[2] = (unsigned char) val; len += 2; 00492 sscanf((char *) &telptr[len],"%02X",&val); data[3] = (unsigned char) val; len += 2; 00493 sscanf((char *) &telptr[len],"%02X",&val); data[4] = (unsigned char) val; len += 2; 00494 sscanf((char *) &telptr[len],"%02X",&val); data[5] = (unsigned char) val; len += 2; 00495 sscanf((char *) &telptr[len],"%02X",&val); data[6] = (unsigned char) val; len += 2; 00496 sscanf((char *) &telptr[len],"%02X",&val); data[7] = (unsigned char) val; len += 2; 00497 sscanf((char *) &telptr[len],"%02X",&byte_count); len += 2; 00498 for(i=0; i<byte_count; i++) 00499 { 00500 sscanf((char *) &telptr[len],"%02X",&val); data[8+i] = (unsigned char) val; len += 2; 00501 } 00502 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00503 return 8+byte_count; 00504 case ReadFifoQueue: 00505 sscanf((char *) &telptr[len],"%02X",&val); data[0] = (unsigned char) val; len += 2; 00506 sscanf((char *) &telptr[len],"%02X",&val); data[1] = (unsigned char) val; len += 2; 00507 if(LRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00508 return 2; 00509 default: 00510 return MODBUS_ERROR; 00511 } 00512 } 00513 else if(mode == MODBUS_RTU) 00514 { 00515 ret = tty->select(timeout); 00516 if(ret == 0) return MODBUS_ERROR; 00517 if(tty->readBlock(tel, 2) <= 0) return MODBUS_ERROR; 00518 *slave = tel[len++]; 00519 *function = tel[len++]; 00520 ret = tty->select(timeout); 00521 if(ret == 0) return MODBUS_ERROR; 00522 switch(*function) 00523 { 00524 case ReadCoilStatus: 00525 case ReadInputStatus: 00526 case ReadHoldingRegisters: 00527 case ReadInputRegisters: 00528 case ForceSingleCoil: 00529 case PresetSingleRegister: 00530 if(tty->readBlock(data, 4+2) <= 0) return MODBUS_ERROR; 00531 memcpy(&tel[len],data,4+2); len += 4+2; 00532 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00533 return 4; 00534 case ReadExceptionStatus: 00535 case FetchCommEventCtr: 00536 case FetchCommEventLog: 00537 case ReportSlaveID: 00538 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00539 return 0; 00540 case ForceMultipleCoils: 00541 case PresetMultipleRegs: 00542 if(tty->readBlock(data, 5) <= 0) return MODBUS_ERROR; 00543 memcpy(&tel[len],data,5); len += 5; 00544 byte_count = data[4]; 00545 if(tty->readBlock(&data[4], byte_count+2) <= 0) return MODBUS_ERROR; 00546 memcpy(&tel[len],data,byte_count+2); len += byte_count+2; 00547 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00548 return 4+byte_count; 00549 case ReadGeneralReference: 00550 case WriteGeneralReference: 00551 if(tty->readBlock(data, 1) <= 0) return MODBUS_ERROR; 00552 memcpy(&tel[len],data,1); len++; 00553 byte_count = data[0]; 00554 if(tty->readBlock(data, byte_count+2) <= 0) return MODBUS_ERROR; 00555 memcpy(&tel[len],data,byte_count+2); len += byte_count+2; 00556 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00557 return byte_count; 00558 case MaskWrite4XRegisters: 00559 if(tty->readBlock(data, 6+2) <= 0) return MODBUS_ERROR; 00560 memcpy(&tel[len],data,6+2); len += 6+2; 00561 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00562 return 6; 00563 case ReadWrite4XRegisters: 00564 if(tty->readBlock(data, 9) <= 0) return MODBUS_ERROR; 00565 memcpy(&tel[len],data,9); len += 9; 00566 byte_count = data[8]; 00567 if(tty->readBlock(&data[8], byte_count+2) <= 0) return MODBUS_ERROR; 00568 memcpy(&tel[len],data,byte_count+2); len += byte_count+2; 00569 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00570 return 8+byte_count; 00571 case ReadFifoQueue: 00572 if(tty->readBlock(data, 2+2) <= 0) return MODBUS_ERROR; 00573 memcpy(&tel[len],data,2+2); len += 2+2; 00574 if(CRCerror(len) == 1) return MODBUS_CHECKSUM_ERROR; 00575 return 2; 00576 default: 00577 return MODBUS_ERROR; 00578 } 00579 } 00580 } 00581 else return MODBUS_ERROR; 00582 return MODBUS_SUCCESS; 00583 } 00584 00585 void rlModbus::registerSocket(rlSocket *socket) 00586 { 00587 tty = NULL; 00588 s = socket; 00589 } 00590 00591 void rlModbus::registerSerial(rlSerial *serial) 00592 { 00593 s = NULL; 00594 tty = serial; 00595 } 00596 00597 int rlModbus::buf2int_rtu(unsigned char *buf) 00598 { 00599 return (buf[0]*256 + buf[1]); 00600 } 00601 00602 void rlModbus::int2buf_rtu(int i, unsigned char *buf) 00603 { 00604 int high, low; 00605 00606 high = i / 256; 00607 low = i & 0x0ff; 00608 buf[0] = (unsigned char) high; 00609 buf[1] = (unsigned char) low; 00610 } 00611 00612 int rlModbus::buf2int_ascii(unsigned char *buf) 00613 { 00614 int val; 00615 00616 sscanf((char *) buf,"%04X",&val); 00617 return val; 00618 } 00619 00620 void rlModbus::int2buf_ascii(int i, unsigned char *buf) 00621 { 00622 sprintf((char *) buf,"%04X",i); 00623 } 00624 00625 void rlModbus::insertLRC(int len) 00626 { 00627 unsigned char lrc; 00628 int i,high,low,val; 00629 00630 if(len < 0) return; 00631 lrc = 0; 00632 for(i=1; i<len; i+=2) // exclude starting ':' and trailing <CR><LF> 00633 { 00634 sscanf((const char *) &tel[i], "%1X", &high); 00635 sscanf((const char *) &tel[i+1], "%1X", &low); 00636 val = high*16 + low; 00637 lrc += val; 00638 } 00639 lrc = ((unsigned char)(-((char) lrc))); 00640 sprintf((char *) &tel[len],"%02X",(unsigned int) lrc); 00641 } 00642 00643 int rlModbus::LRCerror(int len) 00644 { 00645 unsigned char *cptr; 00646 unsigned char lrc; 00647 int i,high,low,val; 00648 00649 if(len < 0) return 1; 00650 tel[maxtel-1] = '\0'; 00651 cptr = (unsigned char *) strchr((char *)tel,':'); 00652 if(cptr == NULL) return 1; 00653 cptr++; 00654 lrc = 0; 00655 for(i=1; i<len+2; i+=2) // exclude starting ':' and trailing <CR><LF> 00656 { // len is without lrc -> len+2 00657 sscanf((const char *) cptr++, "%1X", &high); 00658 sscanf((const char *) cptr++, "%1X", &low); 00659 val = high*16 + low; 00660 lrc += val; 00661 } 00662 if(lrc == 0) return 0; // lrc ok 00663 return 1; // lrc error 00664 } 00665 00666 /* Table of CRC values for high-order byte */ 00667 static const unsigned char array_crc_low[] = 00668 { 00669 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00670 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00671 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 00672 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 00673 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 00674 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 00675 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 00676 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00677 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00678 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 00679 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 00680 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 00681 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00682 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 00683 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 00684 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 00685 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00686 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00687 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 00688 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00689 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 00690 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 00691 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 00692 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 00693 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 00694 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 00695 }; 00696 00697 /* Table of CRC values for low-order byte */ 00698 static const unsigned char array_crc_high[] = 00699 { 00700 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 00701 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 00702 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 00703 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 00704 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 00705 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 00706 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 00707 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 00708 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 00709 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 00710 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 00711 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 00712 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 00713 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 00714 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 00715 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 00716 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 00717 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 00718 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 00719 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 00720 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 00721 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 00722 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 00723 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 00724 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 00725 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 00726 }; 00727 00728 void rlModbus::insertCRC(int len) 00729 { 00730 unsigned char crc_high, crc_low; 00731 unsigned index; 00732 int i; 00733 00734 if(len < 0) return; 00735 crc_high = crc_low = 0xff; 00736 for(i=0; i<len; i++) 00737 { 00738 index = crc_low ^ tel[i]; 00739 crc_low = crc_high ^ array_crc_low[index]; 00740 crc_high = array_crc_high[index]; 00741 } 00742 tel[len] = crc_low; 00743 tel[len+1] = crc_high; 00744 } 00745 00746 int rlModbus::CRCerror(int len) 00747 { 00748 unsigned char crc_high, crc_low; 00749 unsigned index; 00750 int i; 00751 00752 if(len < 2) return 1; 00753 crc_high = crc_low = 0xff; 00754 for(i=0; i<len-2; i++) 00755 { 00756 index = crc_low ^ tel[i]; 00757 crc_low = crc_high ^ array_crc_low[index]; 00758 crc_high = array_crc_high[index]; 00759 } 00760 if(crc_low != tel[len-2]) return 1; 00761 if(crc_high != tel[len-1]) return 1; 00762 return 0; 00763 }
1.7.5.1