Adding script context MODBUS protocol and Eurotherm 2000 series driver

r2886 | dcl | 2010-04-16 09:45:27 +1000 (Fri, 16 Apr 2010) | 2 lines

site_ansto/hardsup/sct_modbusprot.c

@@ -0,0 +1,200 @@
+/** @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 > 9) {
+    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 < 0 || 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) {
+    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);
+  }
+  return 1;
+}
+
+/** @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 AsconReading:
+      ret = ModbusReading(a);
+      return ret;
+      break;
+    default:
+      return AsconStdHandler(a);
+  }
+  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;
+}