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)
    continue; // not for us 
  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("out1"), nb_dio_get("out2")),
        "ibits", mkarray(nb_dio_get("in1"), nb_dio_get("in2")),
        "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 */