/** @file Modbus protocol handler for script-context based controllers. * */ #include #include #include #include #include static int dbgprintf(char* fmtstr, ...); static int dbgprintx(const char* msg, const unsigned char* cp, int blen); static unsigned int debug_modbus = 1; /** @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; bool do_float = false; 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 && cmd != 1003 && cmd != 1016)) { /* 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; } if (cmd > 1000) { cmd %= 1000; do_float = true; } else { do_float = false; } temp[idx++] = cmd; buff = endp + 1; reg = strtoul(buff, &endp, 10); if (endp == buff || 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 */ if (do_float) temp[idx++] = 2; /* word count lsbyte */ else temp[idx++] = 1; /* word count lsbyte */ if (cmd == 16) { /* write registers */ buff = endp + 1; if (do_float) { union { unsigned char v[4]; float val; } u; u.val = strtof(buff, &endp); if (endp == buff) { dbgprintf("modbus-er: Bad value: %f from %s\n", u.val, buff); a->state = AsconIdle; AsconError(a, "Bad value", 0); return 0; } temp[idx++] = 4; /* byte count */ temp[idx++] = u.v[1]; temp[idx++] = u.v[0]; temp[idx++] = u.v[3]; temp[idx++] = u.v[2]; } else { 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] == 3 && cp[8] == 4) { union { unsigned char v[4]; float val; } u; u.v[1] = cp[9]; u.v[0] = cp[10]; u.v[3] = cp[11]; u.v[2] = cp[12]; rlen = snprintf(temp, 64, "%g", u.val); } else if (cp[7] == 16 && cp[11] == 1) { rlen = snprintf(temp, 64, "OK int"); } else if (cp[7] == 16 && cp[11] == 2) { rlen = snprintf(temp, 64, "OK float"); } 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 #include 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; }