00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include <stdio.h>
00016 #include <unistd.h>
00017
00018 #include <sys/types.h>
00019 #include <sys/socket.h>
00020 #include <netinet/in.h>
00021
00022 #include <string.h>
00023 #include <stdlib.h>
00024 #include <netdb.h>
00025 #include <fcntl.h>
00026
00027 #include <errno.h>
00028 #include <assert.h>
00029
00030 #include "ModbusTcp.h"
00031 #include "midas.h"
00032
00033 ModbusTcp::ModbusTcp()
00034 {
00035 fTrace = false;
00036 fSocket = -1;
00037 fReadTimeout_sec = 10;
00038 };
00039
00040 int ModbusTcp::WriteRegister(int slaveId, int ireg, int value)
00041 {
00042 int f = ireg/1000000;
00043 int r = ireg%1000000;
00044
00045 if (f==0 || f==MODBUS_FUNC3 || f==MODBUS_FUNC4)
00046 f = MODBUS_FUNC6;
00047
00048 return WriteRegister(slaveId, f, r, value);
00049 }
00050
00051 int ModbusTcp::WriteRegister(int slaveId, int func, int ireg, int value)
00052 {
00053 assert(func == 6);
00054 assert(ireg>0);
00055 assert(ireg<=0x10000);
00056
00057 int status = Function6(slaveId, ireg-1, value);
00058
00059 if (status < 0)
00060 return -1;
00061
00062 char buf[10000];
00063
00064 int expected = 12;
00065
00066 int rd = Read(buf, expected);
00067
00068 if (rd < 0)
00069 return -1;
00070
00071 if (0) {
00072 for (int i=0; i<rd; i++)
00073 printf(" %02x", buf[i]&0xFF);
00074 printf("\n");
00075 }
00076
00077 if (buf[7] != 6) {
00078 cm_msg(MERROR, "ModbusTcp::WriteRegister", "reg %d, modbus error code 0x%x, exception code 0x%x", ireg, 0xFF&buf[7], 0xFF&buf[8]);
00079 return -1;
00080 }
00081
00082 if (rd != expected) {
00083 cm_msg(MERROR, "ModbusTcp::WriteRegister", "reg %d, bad modbus packet length %d, expected %d", ireg, rd, expected);
00084 return -1;
00085 }
00086
00087 return SUCCESS;
00088 }
00089
00090 int ModbusTcp::ReadRegister(int slaveId, int ireg)
00091 {
00092 int f = ireg/1000000;
00093 int r = ireg%1000000;
00094 if (f==0)
00095 f = MODBUS_FUNC3;
00096 return ReadRegister(slaveId, f, r);
00097 }
00098
00099 int ModbusTcp::ReadRegister(int slaveId, int func, int ireg)
00100 {
00101 assert(func==3 || func==4);
00102 assert(ireg>0);
00103 assert(ireg<=0x10000);
00104
00105 int status;
00106
00107 if (func==3)
00108 status = Function3(slaveId, ireg-1, 1);
00109 else if (func==4)
00110 status = Function4(slaveId, ireg-1, 1);
00111 else
00112 assert(!"cannot happen!");
00113 if (status < 0)
00114 return -1;
00115
00116 char buf[10000];
00117
00118 int expected = 11;
00119
00120 int rd = Read(buf, expected);
00121 if (rd < 0)
00122 return -1;
00123
00124 if (0) {
00125 for (int i=0; i<rd; i++)
00126 printf(" %02x", buf[i]&0xFF);
00127 printf("\n");
00128 }
00129
00130 if (buf[7] != func) {
00131 cm_msg(MERROR, "ModbusTcp::ReadRegister", "reg %d, modbus error code 0x%x, exception code 0x%x", ireg, 0xFF&buf[7], 0xFF&buf[8]);
00132 return -1;
00133 }
00134
00135 if (buf[8] != 2 || rd != expected) {
00136 cm_msg(MERROR, "ModbusTcp::ReadRegister", "reg %d, bad modbus packet length %d, received %d, expected %d", ireg, buf[8], rd, expected);
00137 return -1;
00138 }
00139
00140 uint16_t v16 = (buf[9]&0xFF)<<8 | (buf[10]&0xFF);
00141
00142 return v16;
00143 }
00144
00145 int ModbusTcp::ReadRegisters(int slaveId, int ireg, int numReg, uint16_t data[])
00146 {
00147 int f = ireg/1000000;
00148 int r = ireg%1000000;
00149 if (f==0)
00150 f = MODBUS_FUNC3;
00151 return ReadRegisters(slaveId, f, r, numReg, data);
00152 }
00153
00154 int ModbusTcp::ReadRegisters(int slaveId, int func, int ireg, int numReg, uint16_t data[])
00155 {
00156 assert(func==3 || func==4);
00157 assert(ireg>0);
00158 assert(ireg+numReg<0x10000);
00159
00160 for (int i=0; i<numReg; i++)
00161 data[i] = 0;
00162
00163 int status;
00164
00165 if (func==3)
00166 status = Function3(slaveId, ireg-1, numReg);
00167 else if (func==4)
00168 status = Function4(slaveId, ireg-1, numReg);
00169 else
00170 assert(!"cannot happen!");
00171
00172 if (status < 0)
00173 return -1;
00174
00175 char buf[10000];
00176
00177 int expected = 9+numReg*2;
00178
00179 int rd = Read(buf, expected);
00180 if (rd < 0)
00181 return -1;
00182
00183 if (0) {
00184 for (int i=8; i<rd; i++)
00185 printf(" %02x", buf[i]&0xFF);
00186 printf("\n");
00187 }
00188
00189 if (buf[7] != func) {
00190 cm_msg(MERROR, "ModbusTcp::ReadRegisters", "reg %d, modbus error code 0x%x, exception code 0x%x", ireg, buf[7], buf[8]);
00191 return -1;
00192 }
00193
00194 if (buf[8] != 0xFF&(numReg*2) || rd != expected) {
00195 cm_msg(MERROR, "ModbusTcp::ReadRegisters", "reg %d, bad modbus packet length %d, received %d, expected %d", ireg, 0xFF&buf[8], rd, expected);
00196 return -1;
00197 }
00198
00199 for (int i=0; i<numReg; i++)
00200 data[i] = (buf[9+i*2]&0xFF)<<8 | (buf[10+i*2]&0xFF);
00201
00202 return SUCCESS;
00203 }
00204
00205 int ModbusTcp::Function3(int slaveId, int firstReg, int numReg)
00206 {
00207 char buf[12];
00208
00209
00210
00211 buf[0] = 0;
00212 buf[1] = 1;
00213 buf[2] = 0;
00214 buf[3] = 0;
00215 buf[4] = 0;
00216 buf[5] = 6;
00217 buf[6] = slaveId;
00218 buf[7] = 3;
00219 buf[8] = (firstReg & 0xFF00)>>8;
00220 buf[9] = (firstReg & 0x00FF);
00221 buf[10] = ( numReg & 0xFF00)>>8;
00222 buf[11] = ( numReg & 0x00FF);
00223
00224 return Write(buf, 12);
00225 }
00226
00227 int ModbusTcp::Function4(int slaveId, int firstReg, int numReg)
00228 {
00229 char buf[12];
00230
00231
00232
00233 buf[0] = 0;
00234 buf[1] = 1;
00235 buf[2] = 0;
00236 buf[3] = 0;
00237 buf[4] = 0;
00238 buf[5] = 6;
00239 buf[6] = slaveId;
00240 buf[7] = 4;
00241 buf[8] = (firstReg & 0xFF00)>>8;
00242 buf[9] = (firstReg & 0x00FF);
00243 buf[10] = ( numReg & 0xFF00)>>8;
00244 buf[11] = ( numReg & 0x00FF);
00245
00246 return Write(buf, 12);
00247 }
00248
00249 int ModbusTcp::Function6(int slaveId, int ireg, int value)
00250 {
00251 char buf[12];
00252
00253
00254
00255 buf[0] = 0;
00256 buf[1] = 1;
00257 buf[2] = 0;
00258 buf[3] = 0;
00259 buf[4] = 0;
00260 buf[5] = 6;
00261 buf[6] = slaveId;
00262 buf[7] = 6;
00263 buf[8] = ( ireg & 0xFF00)>>8;
00264 buf[9] = ( ireg & 0x00FF);
00265 buf[10] = (value & 0xFF00)>>8;
00266 buf[11] = (value & 0x00FF);
00267
00268 return Write(buf, 12);
00269 }
00270
00271 int ModbusTcp::Connect(const char *addr)
00272 {
00273 char laddr[256];
00274 strlcpy(laddr, addr, sizeof(laddr));
00275 char* s = strchr(laddr,':');
00276 if (!s) {
00277 cm_msg(MERROR, "ModbusTcp::Connect", "Invalid address \'%s\': no \':\', should look like \'hostname:tcpport\'", laddr);
00278 return -1;
00279 }
00280
00281 *s = 0;
00282
00283 int port = atoi(s+1);
00284 if (port == 0) {
00285 cm_msg(MERROR, "ModbusTcp::Connect", "Invalid address: \'%s\', tcp port number is zero", laddr);
00286 return -1;
00287 }
00288
00289 struct hostent *ph = gethostbyname(laddr);
00290 if (ph == NULL) {
00291 cm_msg(MERROR, "ModbusTcp::Connect", "Cannot resolve IP address for \'%s\', h_errno %d (%s)", laddr, h_errno, hstrerror(h_errno));
00292 return -1;
00293 }
00294
00295 int fd = socket (AF_INET, SOCK_STREAM, 0);
00296 if (fd < 0) {
00297 cm_msg(MERROR, "ModbusTcp::Connect", "Cannot create TCP socket, socket(AF_INET, SOCK_STREAM) errno %d (%s)", errno, strerror(errno));
00298 return -1;
00299 }
00300
00301 struct sockaddr_in inaddr;
00302
00303 memset(&inaddr, 0, sizeof(inaddr));
00304 inaddr.sin_family = AF_INET;
00305 inaddr.sin_port = htons(port);
00306 memcpy((char *) &inaddr.sin_addr, ph->h_addr, 4);
00307
00308 cm_msg(MINFO, "ModbusTcp::Connect", "Connecting to \"%s\" port %d", laddr, port);
00309
00310 int status = connect(fd, (sockaddr*)&inaddr, sizeof(inaddr));
00311 if (status == -1) {
00312 cm_msg(MERROR, "ModbusTcp::Connect", "Cannot connect to %s:%d, connect() errno %d (%s)", laddr, port, errno, strerror(errno));
00313 return -1;
00314 }
00315
00316 fSocket = fd;
00317
00318 return SUCCESS;
00319 }
00320
00321 int ModbusTcp::Disconnect()
00322 {
00323 if (fSocket < 0)
00324 return SUCCESS;
00325
00326 close(fSocket);
00327 fSocket = -1;
00328 return SUCCESS;
00329 }
00330
00331 int ModbusTcp::Write(const char* buf, int length)
00332 {
00333 if (1) {
00334 int count = 0;
00335 while (1) {
00336 char buf[256];
00337 int rd = recv(fSocket, buf, sizeof(buf), MSG_DONTWAIT);
00338 if (rd <= 0)
00339 break;
00340 count += rd;
00341 }
00342
00343 if (count > 0)
00344 cm_msg(MINFO, "ModbusTcp::Write", "Flushed %d bytes", count);
00345 }
00346
00347 if (fTrace) {
00348 printf("Writing %p+%d to socket %d: 0x", buf, length, fSocket);
00349 for (int i=0; i<length; i++)
00350 printf(" %02x", buf[i]&0xFF);
00351 printf("\n");
00352 }
00353
00354 int wr = send(fSocket, buf, length, 0);
00355
00356 if (wr != length) {
00357 cm_msg(MERROR, "ModbusTcp::Write", "TCP I/O error, send() returned %d, errno %d (%s)", wr, errno, strerror(errno));
00358 return -1;
00359 }
00360
00361 return wr;
00362 }
00363
00364 int ModbusTcp::Read(char* buf, int length)
00365 {
00366 time_t t0 = time(NULL);
00367 int count = 0;
00368
00369 while (count < length) {
00370 int rd = recv(fSocket, buf+count, length-count, MSG_DONTWAIT);
00371
00372
00373
00374 if (rd == -1 && errno == EAGAIN) {
00375 if (time(NULL)-t0 > fReadTimeout_sec) {
00376 cm_msg(MERROR, "ModbusTcp::Read", "TCP connection timeout");
00377 return count;
00378 }
00379 ss_sleep(100);
00380 continue;
00381 } else if (rd == 0) {
00382 cm_msg(MERROR, "ModbusTcp::Read", "TCP connection unexpectedly closed");
00383 return -1;
00384 } else if (rd < 0) {
00385 cm_msg(MERROR, "ModbusTcp::Read", "TCP I/O error, read() returned %d, errno %d (%s)", rd, errno, strerror(errno));
00386 return -1;
00387 }
00388
00389 count += rd;
00390 }
00391
00392 if (fTrace) {
00393 printf("ModbusTcp::Read: received %d bytes: 0x", count);
00394
00395 for (int i=0; i<count; i++)
00396 printf(" %02x", buf[i]&0xFF);
00397 printf("\n");
00398 }
00399
00400 return count;
00401 }
00402
00403