sics/site_ansto/hardsup/sct_modbusprot.c

/** @file Modbus protocol handler for script-context based controllers.
*
*/
#include <errno.h>
#include <ascon.h>
#include <ascon.i>
#include <dynstring.h>

static int dbgprintf(char* fmtstr, ...);
static int dbgprintx(const char* msg, const unsigned char* cp, int blen);
static unsigned int debug_modbus = 0;

/** @brief encode modbus request
* TODO: clean me up
*/
int ModbusWriteStart(Ascon *a) {
  unsigned int dev, cmd, reg;
  int len = GetDynStringLength(a->wrBuffer);
  char* buff = NULL;
  char temp[32];
  char* endp = NULL;
  int idx = 6;
  temp[0] = 0; /* transaction id */
  temp[1] = 0;
  temp[2] = 0; /* protocol id */
  temp[3] = 0;

  DynStringConcatChar(a->wrBuffer, 0);
  buff = GetCharArray(a->wrBuffer);
  dbgprintf("modbus-wr:%s\n", buff);
  dev = strtoul(buff, &endp, 10);
  if (endp == buff || dev < 1 || dev > 39) {
    dbgprintf("modbus-er: Bad device id: %d from %s\n", dev, buff);
    a->state = AsconIdle;
    AsconError(a, "Bad device id", 0);
    return 0;
  }
  temp[idx++] = dev;
  buff = endp + 1;
  cmd = strtoul(buff, &endp, 10);
  if (endp == buff || (cmd != 3 && cmd != 16)) { /* read/write registers */
    dbgprintf("modbus-er: Bad command id: %d from %s\n", cmd, buff);
    a->state = AsconIdle;
    AsconError(a, "Bad command id", 0);
    return 0;
  }
  temp[idx++] = cmd;
  buff = endp + 1;
  reg = strtoul(buff, &endp, 10);
  if (endp == buff || reg < 1 || reg > 65535) {
    dbgprintf("modbus-er: Bad register id: %d from %s\n", reg, buff);
    a->state = AsconIdle;
    AsconError(a, "Bad register id", 0);
    return 0;
  }
  temp[idx++] = (reg >> 8) & 0xFF;
  temp[idx++] = reg & 0xFF;
  temp[idx++] = 0; /* word count msbyte */
  temp[idx++] = 1; /* word count lsbyte */
  if (cmd == 16) { /* write registers */
    buff = endp + 1;
    unsigned int val;
    val = strtoul(buff, &endp, 10);
    if (endp == buff || val > 65535) {
      dbgprintf("modbus-er: Bad value: %d from %s\n", val, buff);
      a->state = AsconIdle;
      AsconError(a, "Bad value", 0);
      return 0;
    }
    temp[idx++] = 2; /* byte count */
    temp[idx++] = (val >> 8) & 0xFF;
    temp[idx++] = val & 0xFF;
  }
  len = idx - 6;
  temp[4] = len >> 8;   /* length msbyte */
  temp[5] = len & 0xFF; /* length lsbyte */

  if (debug_modbus > 0) {
    dbgprintx("modbus-xo", (unsigned char*)temp, idx);
  }
  DynStringReplaceWithLen(a->wrBuffer, temp, 0, idx);
  a->state = AsconWriting;
  a->wrPos = 0;
  return 1;
}

/** @brief decode modbus response
* TODO: clean me up
*/
int ModbusReading(Ascon *a) {
  int ret, blen, rlen;
  char chr = '\0';
  unsigned char* cp = NULL;

  ret = AsconReadChar(a->fd, &chr);
  while (ret > 0) {
    a->start = DoubleTime();

    DynStringConcatChar(a->rdBuffer, chr);
    cp = (unsigned char*) GetCharArray(a->rdBuffer);
    blen = GetDynStringLength(a->rdBuffer);
    if (debug_modbus > 0) {
      dbgprintx("modbus-xi", cp, blen);
    }
    if (blen >= 6) {
      int mlen = (cp[4] << 8) + cp[5];
      if (blen - 6 >= mlen) {
        char temp[64];
        if (cp[7] == 3 && cp[8] == 2) {
          rlen = snprintf(temp, 64, "%d", (((unsigned int)cp[9]) << 8) + (unsigned int)cp[10]);
        }
        else if (cp[7] == 16 && cp[11] == 1) {
          rlen = snprintf(temp, 64, "OK");
        }
        else if (((unsigned int)cp[7]) == 0x83) {
          rlen = snprintf(temp, 64, "ASCERR:%02x:%d", cp[7], cp[8]);
        }
        else if (((unsigned int)cp[7]) == 0x90) {
          rlen = snprintf(temp, 64, "ASCERR:%02x:%d", cp[7], cp[8]);
        }
        else {
          rlen = snprintf(temp, 64, "ASCERR:%02x:%d", cp[7], cp[8]);
        }
        if (debug_modbus > 0) {
          dbgprintx("modbus-xi", cp, blen);
        }
        dbgprintf("modbus-rd:%s\n", temp);
        DynStringReplaceWithLen(a->rdBuffer, temp, 0, rlen);
        a->state = AsconReadDone;
        return 1;
      }
    }
    ret = AsconReadChar(a->fd, &chr);
  }
  if (ret < 0) {
    AsconError(a, "AsconReadChar failed:", errno);
    return 0;
  }
  if (a->state == AsconReadDone) {
    DynStringConcatChar(a->rdBuffer, '\0');
  } else {
    if (a->timeout > 0) {
      if (DoubleTime() - a->start > a->timeout) {
        AsconError(a, "read timeout", 0);
        a->state = AsconTimeout;
      }
    }
  }
  return 0;
}

/** @brief Modbus TCP protocol handler.
* This handler encodes commands and decodes responses
* for the Modbus TCP protocol
*/
int ModbusProtHandler(Ascon *a) {
  int ret;

  switch(a->state){
    case AsconWriteStart:
      ret = ModbusWriteStart(a);
      return ret;
      break;
    case AsconReadStart:
      a->start = DoubleTime();
      ret = AsconStdHandler(a);
      return ret;
      break;
    case AsconReading:
      ret = ModbusReading(a);
      return ret;
      break;
    default:
      ret = AsconStdHandler(a);
      return ret;
      break;
  }
  return 1;
}

void AddModbusProtocoll(){
	AsconProtocol *prot = NULL;
	
	prot = calloc(sizeof(AsconProtocol), 1);
	prot->name = strdup("modbus");
	prot->init = AsconStdInit;
	prot->handler = ModbusProtHandler;
	AsconInsertProtocol(prot);
}

#include <stdio.h>
#include <stdarg.h>

static int dbgprintf(char* fmtstr, ...) {
  if (debug_modbus > 0) {
    FILE* fp = NULL;
    int ret = 0;
    fp = fopen("/tmp/modbus.txt", "a");
    if (fp != NULL) {
      va_list ap;
      va_start(ap, fmtstr);
      ret = vfprintf(fp, fmtstr, ap);
      va_end(ap);
      fclose(fp);
    }
    return ret;
  }
  return 0;
}
static int dbgprintx(const char* msg, const unsigned char* cp, int blen) {
  if (debug_modbus > 0) {
    char tmp[128];
    int i, j;
    const char hex[] = "0123456789ABCDEF";
    for (i = 0, j = 0; i < blen && j < 126; ++i) {
      tmp[j++] = hex[(cp[i] >> 4) & 0xF];
      tmp[j++] = hex[(cp[i]     ) & 0xF];
    }
    tmp[j++] = '\0';
    return dbgprintf("%s: %s\n", msg, tmp);
  }
  return 0;
}