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
 *
 *   Commands:   
 *     Read Coil Status (FC01)
 *     Read Input Status (FC02)
 *     Read Input Register (FC04)
 *     Force Single Coil (FC05)
 *     Write Multiple Coils (FC15)
 *        
 *   Mapping: 
 *     Coil status(digital I/O): 1=out1, 2=out2
 *     Input satus(digital In): 1=in1, 2=in2  
 *     Input register : 1=temperature 2=mobile 1 rsrp
 *
 */
 
TCP_PORT    = 1502;  // sdk port must be higher than 1024
SLAVEID     = 7;
 
int set_io(int adr, int dat)
{
  ret = 0;
  switch (adr) { //switch on address 
    case 0:
      if (dat == 0)
        nb_dio_set("out1", 0);
      else
        nb_dio_set("out1", 1);
      break;
    case 1:
      if (dat == 0)
        nb_dio_set("out2", 0);
      else
        nb_dio_set("out2", 1);
      break;
    default:
    ret = -1;
    break;    
    }
  return ret;
}
 
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) { // write commands 
    case 5: // write to a single coil
      addr = r[8]<<8 + r[9];
      data = r[10]<<8 + r[11];
      if (set_io(addr, data) == -1)
        continue; // error
      break;
    case 15: // write to multiple coils
      addr = r[8]<<8 + r[9];
      cnt = r[10]<<8 + r[11];
      for (i = 0; i < cnt; i ++)
        if (set_io(addr+i, (int) (r[13+i/8] & 1<<(i%256))) == -1)
          continue; // error
      break;
  }   
 
  switch (cmd) { // reply for all commands
    case 1: // read coil status
    case 2: // read input status
    case 5: // write to a single coil
    case 4: // read input register
    case 15: // write to multiple coils
      syst = nb_status("system");
      wwan = nb_status("wwan");
      resp = mkstruct(
        "bits", mkarray(nb_dio_get("out1"), nb_dio_get("out2")),
        "ibits", mkarray(nb_dio_get("in1"), nb_dio_get("in2")),
        "regs", mkarray(0,0),
        "iregs", mkarray((int)syst.TEMPERATURE, (int)wwan.MOBILE1_RSRP)
      );
      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 */