ModbusTcp.cxx

Go to the documentation of this file.
00001 ///
00002 /// ModbusTcp.cxx
00003 ///
00004 /// Driver for accessing MODBUS devices using the MODBUS TCP protocol
00005 /// Konstantin Olchanski, TRIUMF, 2008-2009
00006 ///
00007 /// This driver is implemented using the public MODBUS documentation:
00008 ///
00009 /// http://www.modbus.org/specs.php
00010 /// Modbus_Application_Protocol_V1_1b.pdf
00011 /// Modbus_Messaging_Implementation_Guide_V1_0b.pdf
00012 /// Modbus_over_serial_line_V1_02.pdf
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    // Modbus request encoding:
00210    
00211    buf[0] = 0; // transaction id MSB
00212    buf[1] = 1; // transaction id LSB
00213    buf[2] = 0; // protocol id MSB
00214    buf[3] = 0; // protocol id LSB
00215    buf[4] = 0; // packet length MSB
00216    buf[5] = 6; // packet length LSB
00217    buf[6] = slaveId; // slave id
00218    buf[7] = 3; // function code 3: read registers
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    // Modbus request encoding:
00232    
00233    buf[0] = 0; // transaction id MSB
00234    buf[1] = 1; // transaction id LSB
00235    buf[2] = 0; // protocol id MSB
00236    buf[3] = 0; // protocol id LSB
00237    buf[4] = 0; // packet length MSB
00238    buf[5] = 6; // packet length LSB
00239    buf[6] = slaveId; // slave id
00240    buf[7] = 4; // function code 3: read registers
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    // Modbus request encoding:
00254    
00255    buf[0] = 0; // transaction id MSB
00256    buf[1] = 1; // transaction id LSB
00257    buf[2] = 0; // protocol id MSB
00258    buf[3] = 0; // protocol id LSB
00259    buf[4] = 0; // packet length MSB
00260    buf[5] = 6; // packet length LSB
00261    buf[6] = slaveId; // slave id
00262    buf[7] = 6; // function code 6 write register
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       //int rd = read(fSocket, buf+count, length-count);
00372       //printf("recv rd %d, errno %d (%s)\n", rd, errno, strerror(errno));
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 // end

Midas DOC Version 3.0.0 ---- PSI Stefan Ritt ----
Contributions: Pierre-Andre Amaudruz - Sergio Ballestrero - Suzannah Daviel - Doxygen - Peter Green - Qing Gu - Greg Hackman - Gertjan Hofman - Paul Knowles - Exaos Lee - Rudi Meier - Glenn Moloney - Dave Morris - John M O'Donnell - Konstantin Olchanski - Renee Poutissou - Tamsen Schurman - Andreas Suter - Jan M.Wouters - Piotr Adam Zolnierczuk