Modbus Tcp Server

// mbserver.c V2.1 1/18/01
// example multi-session Modbus/TCP server supporting class 0 commands

// This program should work under UNIX (as C or C++) or Win32 (as C or C++)

// the symbol WIN32 will be defined if compiling for Windows. Assume if it is not
// set that we are compiling for some form of UNIX

// V2.0 1/14/00 added 10 second idle timeout option
// V2.1 1/18/01 timeout was not being reset on successful traffic
// V2.2 7/27/01 defaults for EXE set to sessions = 100, listen() backlog set to same, timeout on

#ifndef WIN32

// various flavors of UNIX may spread their include files in different ways
// the set below is correct for Redhat Linux 5.1 and 6.1 and Ultrix V4.3A

#include  // printf
#include  // errno
#include  // close
#include  // timeval
#include  // socket bind listen accept recv send
#include  // FIONBIO
#include  // sockaddr_in sockaddr INADDR_ANY

typedef int SOCKET;
const int INVALID_SOCKET=(~(int)0);

// the following fake out the Winsock-specific routines

typedef struct WSAData { int w;} WSADATA;
int WSAStartup(int v, WSADATA *pw) { return 0;}
int WSACleanup() {}
int WSAGetLastError() { return (errno);}
int closesocket(SOCKET s) { close(s); }
int ioctlsocket(SOCKET s, long cmd, unsigned long *valp) { ioctl(s, cmd, valp); }
#define WSAEWOULDBLOCK EWOULDBLOCK

#else // must be WIN32

#include 
#include  // printf
#include  // time, difftime

#endif



///
// configuration settings
///

#define numSockets 100	/* number of concurrent server sessions */
#define num4xRegs 10000	/* number of words in the 'register table' maintained by this server */
#define IDLE_TIMEOUT 1	/* set if the session idle timeout to be enforced */

///
// data structure definitions
///

// maintain a data structure per session,into which can be stored partial msgs

struct fragMsg 
{
    int fragLen;                // length of request assembled so far
    unsigned char fragBuf[261]; // request so far assembled
};

///
// global data definition
///

struct fragMsg frag[numSockets];
time_t openTime[numSockets];

// make a 'state table' to read and write into

unsigned short reg4x[num4xRegs];

///
// utility routines
///


// extract a 16-bit word from an incoming Modbus message

unsigned short getWord(unsigned char b[], unsigned i) 
{
    return (((unsigned short)(b[i])) << 8) | b[i+1];
}

// write a 16-bit word to an outgoing Modbus message

void putWord(unsigned char b[], unsigned i, unsigned short w) 
{
    b[i] = (unsigned char)(w >> 8);
    b[i+1] = (unsigned char)(w & 0xff);
}

///
// process legitimate Modbus/TCP requests
///

int processMsg(unsigned char b[],   // message buffer, starting with prefix
               unsigned len)        // length of incoming messsage
                                    // returns length of response
{

    // if you wish to make your processing dependent upon unit identifier
    // use b[6]. Many PLC devices will ignore this field, and most clients
    // default value to zero. However gateways or specialized programs can
    // use the unit number to indicate what type of precessing is desired

    unsigned i;

    // handle the function codes 3 and 16

    switch(b[7]) 
    {
        case 3:     // read registers
                    // request   03 rr rr nn nn
                    // response  03 bb da ta ......
        {
            unsigned regNo = getWord(b, 8);
            unsigned regCount = getWord(b, 10);
            if (len != 12) 
            {
                // exception 3 - bad structure to message
                b[7] |= 0x80;
                b[8] = 3;
                b[5] = 3; // length
                break;
            }
            if (regCount < 1 || regCount > 125 || 
				regNo >= num4xRegs || (regCount + regNo) > num4xRegs) 
            {
                // exception 2 - bad register number or length
                b[7] |= 0x80;
                b[8] = 2;
                b[5] = 3; // length
                break;
            }
            // OK so prepare the 'OK response'
            b[8] = 2 * regCount;
            b[5] = b[8] + 3;
            for (i=0;i 100 || 
				regNo >= num4xRegs || (regCount + regNo) > num4xRegs) 
            {
                // exception 2 - bad register number or length
                b[7] |= 0x80;
                b[8] = 2;
                b[5] = 3; // length
                break;
            }
            // OK so process the data
            for (i=0;i= numSockets) 
                {
                    // nowhere to put it
                    printf("connection abandoned - maximum concurrent sessions reached\n");
                    closesocket(cs);
                }

                // set socket non-blocking just in case anything happens which might make the 
                // recv() operation block later on. This should not be necessary.

                if (ioctlsocket (cs, FIONBIO, &nbiotrue)) 
                {
                    printf("ioctlsocket - error %d\n", WSAGetLastError());
                }

            }
        }

        // any socket level work?

        for (si=0;sifragBuf;

            if (thisFrag->fragLen < 6) 
            {
                // don't know the length yet, just read the prefix
                i = recv(cs, (char *)&thisFrag->fragBuf[thisFrag->fragLen], 6 - thisFrag->fragLen, 0);
                if (i <= 0) 
                {
                    // this session has been closed or damaged at the remote end
                    // this may be a normal condition

                    // remove session from active list
                    csa[si] = INVALID_SOCKET;
                    // close connection
                    closesocket(cs);
                    continue;
                }
                thisFrag->fragLen += i;

                // unfortunately, we are not sure if there are any more bytes
                // so continue this processing on the next cycle
                continue;
            }

            if (ibuf[2] != 0 || ibuf[3] != 0 || ibuf[4] != 0 || ibuf[5] < 2) 
            {
                // this is not legitimate Modbus/TCP
                // possibly your client is very confused
                // close down the connection

                // remove session from active list
                csa[si] = INVALID_SOCKET;
                printf("bad MB/TCP protocol - closing\n");
                // close connection
                closesocket(cs);
                continue;
            }

            // the real length is in ibuf[5]

            if (thisFrag->fragLen < 6+ibuf[5]) 
            {
                i = recv(cs, (char *)&thisFrag->fragBuf[thisFrag->fragLen], 6 + ibuf[5] - thisFrag->fragLen, 0);
                if (i <= 0) 
                {
                    // this session has been closed or damaged at the remote end
                    printf("session closed with partial request outstanding\n", i);
                    // remove session from active list
                    csa[si] = INVALID_SOCKET;
                    // close connection
                    closesocket(cs);
                    continue;
                }
                thisFrag->fragLen += i;
            }
            if (thisFrag->fragLen < 6+ibuf[5]) 
            {
                // still waiting for completion of the message
                continue;
            }

            // if we get here, the message is complete and it looks like MB/TCP

            // process the incoming request, generating a response
            // note that there is no requirement to keep track of which connection
            // the request was received on - you must only use the same one for 
            // sending the response

            i = processMsg(ibuf, thisFrag->fragLen);

            i = send(cs, (char *)ibuf, i, 0);

            thisFrag->fragLen = 0;

        }
        // note that this outer loop will run forever unless cancelled
        // and that if it is cancelled, you must close outstanding sockets
        // and call WSACleanup()
    }
}