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) { 
    cmd = r[7]; // command
    switch (cmd) {
      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;
    }   
 
    switch (cmd) { // reply for all commands
      case 1: // read coil status
      case 2: // read input status
      case 5: // write to a single coil
        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;
        }
        break;
    }
}   
 
/* not reached */