Access to the digital I/Os via Modbus TCP

/* DESC: This script can be used to access to the digital I/Os via Modbus TCP
 * Copyright (C) 2015 NetModule AG, Switzerland
 * supports Read Coil Status (FC01), Read Input Status (FC02) and Force Single Coil (FC05)
 *   Mapping: 
 *     Coil(digital I/O): 1=out1, 2=out2
 *     Input(digital In): 1=in1, 2=in2	
 */
 
TCP_PORT    = 1502;  // sdk port must be higher than 1024
SLAVEID     = 7;
 
if ((fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0) { 
    printf("Unable to open socket\n");
    exit(1); 
} 
if (bind(fd, TCP_PORT, "") < 0) {
    printf("Unable to bind port %d\n", TCP_PORT); 
    exit(2); 
} 
if (listen(fd, 1) < 0) {
    printf("Unable to listen\n"); 
    exit(3); 
}
 
cfd = -1; 
for(;;) {
    if (cfd == -1)
    {
        if ((cfd = accept(fd)) < 0) {
            printf("accept failed\n");
            continue;
        }
        if (nb_modbus_register(cfd, MODBUS_TYPE_TCP) < 0) { 
            printf("Unable to register client (%s)\n", nb_modbus_last_error());
            close(cfd); 
            cfd = -1;
            continue;
        }
    }
    if ((r = nb_modbus_receive(cfd)) == NULL) {
        printf("Unable to receive request (%s)\n", nb_modbus_last_error());
        nb_modbus_unregister(cfd); 
        close(cfd); 
        cfd = -1; 
        continue;
    } 
    if (r[6] == SLAVEID) { 
        switch (r[7]) { // command
            case 1: // read coil status
            case 2: // read input status
                break; // no write action, only reply needed
            case 5: // write to a single coil
                addr = r[8]<<8 + r[9];
                data = r[10]<<8 + r[11];
                switch (addr) { //switch on address 
                    case 0:
                        if (data == 0)
                            nb_dio_set("out1", 0);
                        else
                            nb_dio_set("out1", 1);
                        break;
                    case 1:
                        if (data == 0)
                            nb_dio_set("out2", 0);
                        else
                            nb_dio_set("out2", 1);
                        break;
                }
                break;
        }   
        // reply for all commands
        resp = mkstruct(
            "bits", mkarray(nb_dio_get("in1"), nb_dio_get("in2")),
            "ibits", mkarray(nb_dio_get("out1"), nb_dio_get("out2")),
            "regs", mkarray(),
            "iregs", mkarray()
        );
        if (nb_modbus_reply(cfd, r, resp) < 0)
        {
            printf("Unable to reply%s)\n", nb_modbus_last_error());
            nb_modbus_unregister(cfd); 
            close(cfd); 
            cfd = -1; 
            continue;
        } 
    }
}   
 
/* not reached */