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 <stdio.h> // printf
#include <errno.h> // errno
#include <unistd.h> // close
#include <sys/time.h> // timeval
#include <sys/socket.h> // socket bind listen accept recv send
#include <sys/ioctl.h> // FIONBIO
#include <netinet/in.h> // 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 <winsock.h>
#include <stdio.h> // printf
#include <time.h> // 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<regCount;i++) 
            {
                putWord(b, 9+i+i, reg4x[i + regNo]);
            }
        }
        break;

        case 16:    // write registers
                    // request 
        {
            unsigned regNo = getWord(b, 8);
            unsigned regCount = getWord(b, 10);
            if (len != 13 + regCount + regCount || 
            b[12] != regCount + regCount) 
            {
                // exception 3 - bad structure to message
                b[7] |= 0x80;
                b[8] = 3;
                b[5] = 3; // length
                break;
            }
            if (regCount < 1 || regCount > 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<regCount;i++) 
            {
                reg4x[i + regNo] = getWord(b, 13+i+i);
            }
            // and the OK response is a copy of the request to byte 11
            b[5] = 6;
        }
        break;

        default:
        // generate exception 1 - unknown function code
        b[7] |= 0x80;
        b[8] = 1;
        b[5] = 3; // length
        break;
    }

    // return the total size of the MB/TCP response
    // notice that bytes 0-4 and 6 will be identical to those of the request

    return 6+b[5];
}

///
// main entry point for the program
///

int main(int argc, char **argv) // arguments ignored for now
{
    int i;
    SOCKET csa[numSockets];
    SOCKET s;
    struct sockaddr_in server;
    static WSADATA wd;

    unsigned long nbiotrue = 1;

    printf("mbserver V2.2 7/27/01 Reference Class 0 Modbus/TCP Server\n"
			"sessions = %u, registers = %u, idle timeout = %s\n", 
			numSockets, num4xRegs, IDLE_TIMEOUT?"true":"false");

    // initialize WinSock
    if (WSAStartup(0x0101, &wd))
    {
        printf("cannot initialize WinSock\n");
        return 1;
    }
    // set up an array of socket descriptors, initially set to INVALID_SOCKET (not in use)
    // and initialize the fragment buffer

    for (i=0;i<numSockets;i++) 
    {
        csa[i] = INVALID_SOCKET;
        frag[i].fragLen = 0;
    }

    // set up listen socket

    s = socket(PF_INET, SOCK_STREAM, 0);
    server.sin_family = AF_INET;
    server.sin_port = htons(502); // ASA standard port
    server.sin_addr.s_addr = INADDR_ANY;

    i = bind(s, (struct sockaddr *)&server, sizeof(struct sockaddr_in));
    if (i<0)
    {
        printf("bind - error %d\n",WSAGetLastError());
        closesocket(s);
        WSACleanup();
        return 1;
    }

    // set socket non-blocking in case a client has second thoughts about
    // establishing a connection later on. In this case the accept() will return
    // with an error rather than with a socket.

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

    i = listen(s, numSockets);
    if (i<0)
    {
        printf("listen - error %d\n",WSAGetLastError());
        closesocket(s);
        WSACleanup();
        return 1;
    }

    // at this point, be prepared to handle incoming requests on socket s
    // by doing accept() and processing them independently

    for (;;) 
    {

        SOCKET cs;
        fd_set fds;
        int si;
        struct timeval tv;

        // set up a 1 second timeout to allow for (future) background activity

        tv.tv_sec = 1;
        tv.tv_usec = 0;

        // be prepared for incoming messages on the listen port and any active ports

        FD_ZERO(&fds);

        // wait for incoming connection
        FD_SET(s, &fds);

        // and add any Modbus sessions
        for (i=0;i<numSockets;i++) 
        {
            if (csa[i] != INVALID_SOCKET) 
            {
                cs = csa[i];
                FD_SET(cs, &fds);
            }
        }

        i = select(32, &fds, NULL, NULL, &tv); // read

        // note that the fd_set will have been updated to select only those sockets 
        // requiring attention right now

        if (i<0)
        {
            printf("select - error %d\n",WSAGetLastError());
            closesocket(s);
            WSACleanup();
            return 1;
        }

        // any listen work?

        if (FD_ISSET(s, &fds)) 
        {
            printf("new connection received\n");
            cs = accept(s,NULL,0);
            if (cs<0)
            {
                int e = WSAGetLastError();
                if (e != WSAEWOULDBLOCK) 
                {
                    printf("accept - error %d\n",e);
                    closesocket(s);
                    WSACleanup();
                    return 1;
                } else 
                {
                    // the connection is no longer pending so it must have been
                    // abandoned by the client
                }
            } else 
            {

                // add the newly opened socket to the list. If nowhere to put it, throw it 
                // away

                for (i=0;i<numSockets;i++) 
                {
                    if (csa[i] == INVALID_SOCKET) 
                    {
                        csa[i] = cs;
                        frag[i].fragLen = 0;
						openTime[i] = time(NULL);
                        break;
                    }
                }

                if (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;si<numSockets;si++) 
        {
            struct fragMsg *thisFrag;
            unsigned char *ibuf;

            cs = csa[si];

            if (cs == INVALID_SOCKET) 
            {
                // nothing to do right now on this one
                continue;
            }

#if IDLE_TIMEOUT
			if (10 <= difftime(time(NULL), openTime[si])) 
			{
				// the 10 second idle timer has expired. Close the incoming session
				printf("closing session %d because idle time expired\n");
                // remove session from active list
                csa[si] = INVALID_SOCKET;
                // close connection
                closesocket(cs);
                continue; // abandon any further processing on this session
			}
#endif // IDLE_TIMEOUT

            if (!FD_ISSET(cs, &fds)) 
            {
                // nothing to do right now on this one
                continue;
            }

#if IDLE_TIMEOUT
			// ags 1/18/01
			// update timeout so that shhutdown only occurs after 10 sec of IDLE
			// and not arbitrarily 10 sec from session open!
			openTime[si] = time(NULL);
#endif // IDLE_TIMEOUT

            cs = csa[si];

            // account for any fragment outstanding from a previous cycle
            // (this stupidity would not be necessary if all clients would send
            // their messages in one piece)

            thisFrag = &frag[si];
            ibuf = thisFrag->fragBuf;

            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()
    }
}