331 lines
8.8 KiB
C
331 lines
8.8 KiB
C
/** @file Modbus protocol handler for script-context based controllers.
|
|
*
|
|
*/
|
|
#include <errno.h>
|
|
#include <ascon.h>
|
|
#include <ascon.i>
|
|
#include <dynstring.h>
|
|
#include <stdbool.h>
|
|
#include <math.h>
|
|
|
|
static int dbgprintf(char* fmtstr, ...);
|
|
static int dbgprintx(const char* msg, const unsigned char* cp, int blen);
|
|
static unsigned int debug_modbus = 1;
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
/* IEEE code copied from psi/modbus.c */
|
|
static void double2ieee(double input, char ieee[4]) {
|
|
|
|
/* convert double to IEEE 32 bit floating number (denormalized numbers are considered as zero) */
|
|
|
|
long mantissa;
|
|
int exponent;
|
|
|
|
if (input == 0) {
|
|
ieee[0]= 0;
|
|
ieee[1]= 0;
|
|
ieee[2]= 0;
|
|
ieee[3]= 0;
|
|
} else {
|
|
mantissa = 0x1000000 * (frexp(fabs(input), &exponent));
|
|
exponent = exponent - 1 + 127;
|
|
if (exponent < 0) {
|
|
exponent = 0;
|
|
} else if (exponent > 0xFE) {
|
|
exponent = 0xFE;
|
|
}
|
|
if (input < 0) {
|
|
ieee[0] = 0x80 | (exponent >> 1);
|
|
} else {
|
|
ieee[0] = exponent >> 1;
|
|
}
|
|
ieee[1] = (exponent & 1) << 7 | ((mantissa & 0x7F0000) >> 16);
|
|
ieee[2] = (mantissa & 0xFF00) >> 8;
|
|
ieee[3] = mantissa & 0xFF;
|
|
}
|
|
return;
|
|
}
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
static double ieee2double(char ieee[4]) {
|
|
|
|
/* IEEE 32 bit floating number to double (denormalized numbers are considered as zero) */
|
|
|
|
long mantissa;
|
|
double output;
|
|
int exponent;
|
|
|
|
mantissa = ((ieee[1] << 16) & 0x7FFFFF)
|
|
| ((ieee[2] << 8) & 0xFF00)
|
|
| ((ieee[3] ) & 0xFF);
|
|
|
|
exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */
|
|
if (exponent == 0 && mantissa == 0) {
|
|
return 0.0;
|
|
}
|
|
output = ldexp(mantissa, -23) + 1.0;
|
|
if (ieee[0] & 0x80) {
|
|
output = -output;
|
|
}
|
|
return output * ldexp(1, exponent - 127);
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
|
|
/** @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) {
|
|
struct { char v[4]; } ieee; /* big endian */
|
|
double val = strtod(buff, &endp);
|
|
if (endp == buff) {
|
|
dbgprintf("modbus-er: Bad value: %f from %s\n", val, buff);
|
|
a->state = AsconIdle;
|
|
AsconError(a, "Bad value", 0);
|
|
return 0;
|
|
}
|
|
double2ieee(val, ieee.v);
|
|
dbgprintf("strtod:%f\n", val);
|
|
dbgprintx("double2ieee", (unsigned char*)ieee.v, 4);
|
|
temp[idx++] = 4; /* byte count */
|
|
temp[idx++] = ieee.v[2]; /* watlow RUI specific byte ordering */
|
|
temp[idx++] = ieee.v[3];
|
|
temp[idx++] = ieee.v[0];
|
|
temp[idx++] = ieee.v[1];
|
|
} 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);
|
|
}
|
|
DynStringClear(a->wrBuffer);
|
|
DynStringConcatBytes(a->wrBuffer, temp, 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) {
|
|
struct { char v[4]; } ieee;
|
|
double val;
|
|
ieee.v[2] = cp[9]; /* watlow RUI specific byte ordering */
|
|
ieee.v[3] = cp[10];
|
|
ieee.v[0] = cp[11];
|
|
ieee.v[1] = cp[12];
|
|
val = ieee2double(ieee.v);
|
|
dbgprintx("rawhex", (unsigned char*)&cp[9], 4);
|
|
dbgprintf("ieee2double:%f\n", val);
|
|
rlen = snprintf(temp, 64, "%g", 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);
|
|
DynStringClear(a->rdBuffer);
|
|
DynStringConcatBytes(a->rdBuffer, temp, 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;
|
|
}
|