478 lines
12 KiB
C
478 lines
12 KiB
C
#include <ctype.h>
|
|
#include <math.h>
|
|
#include "ascon.h"
|
|
#include "ascon.i"
|
|
#include "dynstring.h"
|
|
|
|
/*
|
|
* this is a (scriptcontext) general binary protocol driver
|
|
*
|
|
* Markus Zolliker Aug 2010
|
|
*
|
|
* conversion to and from binary
|
|
*
|
|
* Syntax:
|
|
* space separated items, the command first, a slash and then the read format
|
|
*
|
|
* commandItem ... / formatItem ...
|
|
*
|
|
* where a commandItem one of the following
|
|
*
|
|
* a number converted according to the choosen format, i1 is default
|
|
* int<n> changing the format to <n> byte integers
|
|
* hex changing the format to hexadecimal (1 byte at a time)
|
|
* float changing the format to 4 byte ieee float
|
|
* crc send crc
|
|
*
|
|
* and formatItem is one of the follwing
|
|
*
|
|
* skip skip one byte
|
|
* skip<n> skip <n> bytes
|
|
* code returned function code, when bit7 is set, the response is
|
|
* recogized as an error message and the response is dumped
|
|
* the result
|
|
* int<n> convert <n> bytes to integer (most significant byte first)
|
|
* and append the (decimal coded) number to the result
|
|
* if <n> is omitted, <n> = 1 is assumed
|
|
* hex convert 1 byte to hexadecimal coded integer
|
|
* and append it to the response
|
|
* float convert 4 bytes from ieee float
|
|
* and append the number to the response
|
|
* crc check crc (if wrong, "badCRC" is added to the response)
|
|
*
|
|
* multiple items in the response are space separated
|
|
*
|
|
* Usage example: Modbus read (float)
|
|
* command: address 250, function 3, start-address 8, size 2, crc
|
|
* response: address, function, byte-count, float, crc
|
|
*
|
|
* sct send 250 3 int2 10 2 crc / skip code skip float crc
|
|
*/
|
|
|
|
typedef enum {intType, hexType, floatType,
|
|
skipType, codeType, crcType, dumpType, errorType} BinDataType;
|
|
// dumpType, errorType must be the last items
|
|
|
|
typedef struct {
|
|
char *crcAlgorithm;
|
|
pDynString inp;
|
|
char *nextFmt;
|
|
pDynString result;
|
|
int expectedChars;
|
|
BinDataType type;
|
|
long iValue;
|
|
int dumpFrom;
|
|
} BinPrivate;
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
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);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static int calc_crc(char *inp, int inpLen)
|
|
{
|
|
|
|
/* CRC runs cyclic Redundancy Check Algorithm on input inp */
|
|
/* Returns value of 16 bit CRC after completion and */
|
|
/* always adds 2 crc bytes to message */
|
|
/* returns 0 if incoming message has correct CRC */
|
|
|
|
unsigned int crc = 0xffff;
|
|
unsigned int next;
|
|
int carry;
|
|
int n;
|
|
|
|
while (inpLen--) {
|
|
next = *(unsigned char *) inp;
|
|
crc ^= next;
|
|
for (n = 0; n < 8; n++) {
|
|
carry = crc & 1;
|
|
crc >>= 1;
|
|
if (carry) {
|
|
crc ^= 0xA001;
|
|
}
|
|
}
|
|
inp++;
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
int BinReadItem(Ascon *a) {
|
|
BinPrivate *p = a->private;
|
|
char item[32];
|
|
int valen;
|
|
int size;
|
|
|
|
if (sscanf(p->nextFmt, "%30s%n", item, &valen) <= 0) {
|
|
if (p->type < dumpType) {
|
|
p->dumpFrom = GetDynStringLength(a->rdBuffer);
|
|
p->type= dumpType;
|
|
}
|
|
p->expectedChars = 999;
|
|
return 0;
|
|
}
|
|
|
|
if (strcasecmp(item, "crc") == 0) {
|
|
p->type = crcType;
|
|
p->expectedChars = 2;
|
|
p->nextFmt += valen;
|
|
return 1;
|
|
} else if (strncasecmp(item, "int", 3) == 0) {
|
|
size = 1;
|
|
sscanf(item + 3, "%d", &size);
|
|
p->expectedChars = size;
|
|
p->type = intType;
|
|
p->iValue = 0;
|
|
} else if (strcasecmp(item, "float") == 0) {
|
|
p->expectedChars = 4;
|
|
p->type = floatType;
|
|
} else if (strcasecmp(item, "hex") == 0) {
|
|
p->expectedChars = 1;
|
|
p->type = hexType;
|
|
} else if (strcasecmp(item, "code") == 0) {
|
|
p->type = codeType;
|
|
} else if (strncasecmp(item, "skip", 4) == 0) {
|
|
size = 1;
|
|
sscanf(item + 4, "%d", &size);
|
|
p->expectedChars = size;
|
|
p->type = skipType;
|
|
} else {
|
|
return -1;
|
|
}
|
|
p->nextFmt += valen;
|
|
return 1;
|
|
}
|
|
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void BinError(Ascon *a, char *text) {
|
|
BinPrivate *p = a->private;
|
|
|
|
p->type= errorType;
|
|
p->dumpFrom = 0;
|
|
p->expectedChars = 1;
|
|
DynStringCopy(a->errmsg, "BINERR: ");
|
|
DynStringConcat(a->errmsg, text);
|
|
a->state = AsconFailed;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
int BinHandler(Ascon *a) {
|
|
int res;
|
|
char *str;
|
|
int len;
|
|
int i, l, pos;
|
|
unsigned int crc;
|
|
pDynString dyn;
|
|
char item[32];
|
|
char data[8];
|
|
int size;
|
|
int valen;
|
|
BinDataType type;
|
|
long iValue;
|
|
double fValue;
|
|
BinPrivate *p = a->private;
|
|
|
|
switch (a->state) {
|
|
case AsconWriteStart:
|
|
/* exchange buffers */
|
|
dyn = p->inp;
|
|
p->inp = a->wrBuffer;
|
|
a->wrBuffer = dyn;
|
|
DynStringClear(dyn);
|
|
str = GetCharArray(p->inp);
|
|
len = GetDynStringLength(p->inp);
|
|
l = 0;
|
|
size = 1;
|
|
type = intType;
|
|
for (pos = 0; pos < len; ) {
|
|
if (sscanf(str + pos, "%30s%n", item, &valen) <= 0) {
|
|
BinError(a, "missing '/'");
|
|
return 1;
|
|
}
|
|
pos += valen;
|
|
if (strcasecmp(item, "/") == 0) {
|
|
break;
|
|
} else if (strcasecmp(item, "crc") == 0) {
|
|
if (strcasecmp(p->crcAlgorithm, "keller-crc") == 0) {
|
|
crc = calc_crc(GetCharArray(dyn), l);
|
|
DynStringConcatChar(dyn, crc / 256);
|
|
DynStringConcatChar(dyn, crc % 256);
|
|
} else { /* modbus-crc */
|
|
crc = calc_crc(GetCharArray(dyn), l);
|
|
DynStringConcatChar(dyn, crc / 256);
|
|
DynStringConcatChar(dyn, crc % 256);
|
|
}
|
|
} else if (strncasecmp(item, "int", 3) == 0) {
|
|
sscanf(item + 3, "%d", &size);
|
|
type = intType;
|
|
} else if (strcasecmp(item, "hex") == 0) {
|
|
type = hexType;
|
|
} else if (strcasecmp(item, "float") == 0) {
|
|
type = floatType;
|
|
} else {
|
|
switch (type) {
|
|
case intType:
|
|
res = sscanf(item, "%ld", &iValue);
|
|
if (res != 1) {
|
|
BinError(a, "invalid integer");
|
|
return 1;
|
|
}
|
|
for (i = size - 1; i >= 0; i--) {
|
|
if (i < sizeof data) {
|
|
data[i] = iValue % 256;
|
|
}
|
|
iValue /= 256;
|
|
}
|
|
for (i = 0; i < size; i++) {
|
|
if (i >= sizeof data) {
|
|
DynStringConcatChar(dyn, 0);
|
|
} else {
|
|
DynStringConcatChar(dyn, data[i]);
|
|
}
|
|
}
|
|
l += size;
|
|
break;
|
|
case hexType:
|
|
res = sscanf(item, "%lx", &iValue);
|
|
if (res != 1) {
|
|
BinError(a, "invalid hex. integer");
|
|
return 1;
|
|
}
|
|
DynStringConcatChar(dyn, (iValue & 255));
|
|
case floatType:
|
|
res = sscanf(item, "%lf", &fValue);
|
|
if (res != 1) {
|
|
BinError(a, "invalid float");
|
|
return 1;
|
|
}
|
|
double2ieee(fValue, data);
|
|
DynStringConcatChar(dyn, data[0]);
|
|
DynStringConcatChar(dyn, data[1]);
|
|
DynStringConcatChar(dyn, data[2]);
|
|
DynStringConcatChar(dyn, data[3]);
|
|
l += 4;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
p->nextFmt = str + pos;
|
|
p->type = hexType; /* initialize to anything < dumpType */
|
|
do {
|
|
res = BinReadItem(a);
|
|
if (res < 0) {
|
|
BinError(a, "illegal return format: ");
|
|
DynStringConcat(a->errmsg, p->nextFmt);
|
|
return 1;
|
|
}
|
|
} while (res == 1);
|
|
p->nextFmt = str + pos;
|
|
a->wrPos = 0;
|
|
a->state = AsconWriting;
|
|
a->lineCount = 0;
|
|
return 1;
|
|
case AsconReadStart:
|
|
DynStringClear(p->result);
|
|
p->type = hexType; /* initialize to anything < dumpType */
|
|
BinReadItem(a);
|
|
break;
|
|
case AsconReading:
|
|
res = AsconBaseHandler(a);
|
|
if (res == 0) {
|
|
if (GetDynStringLength(a->rdBuffer) == 0) {
|
|
/* wait for the first byte - or timeout */
|
|
return res;
|
|
}
|
|
if (p->type >= dumpType) {
|
|
l = GetDynStringLength(a->rdBuffer);
|
|
str = GetCharArray(a->rdBuffer);
|
|
for (i = p->dumpFrom; i < l; i++) {
|
|
snprintf(item, sizeof item, "%2.2x ", (str[i] & 255));
|
|
DynStringConcat(p->result, item);
|
|
}
|
|
if (p->type == errorType) {
|
|
DynStringConcat(a->errmsg, GetCharArray(p->result));
|
|
a->state = AsconFailed;
|
|
} else {
|
|
a->state = AsconReadDone;
|
|
}
|
|
/* exchange buffers */
|
|
dyn = a->rdBuffer;
|
|
a->rdBuffer = p->result;
|
|
p->result = dyn;
|
|
return 1;
|
|
}
|
|
return res;
|
|
}
|
|
/* convert according to type */
|
|
switch (p->type) {
|
|
case codeType:
|
|
if ((a->lastChar & 255) < 128) {
|
|
p->expectedChars = 0;
|
|
} else {
|
|
DynStringCopy(a->errmsg, "BINERR: ");
|
|
p->expectedChars = 2;
|
|
p->type = errorType;
|
|
p->dumpFrom = 0;
|
|
/* skip to end */
|
|
p->nextFmt = "";
|
|
}
|
|
break;
|
|
case errorType:
|
|
if (p->expectedChars > 1) {
|
|
p->expectedChars--;
|
|
snprintf(item, sizeof item, "error %d / ", (a->lastChar & 255));
|
|
DynStringCopy(p->result, item);
|
|
}
|
|
break;
|
|
case dumpType:
|
|
break;
|
|
case skipType:
|
|
p->expectedChars--;
|
|
break;
|
|
case intType:
|
|
p->expectedChars--;
|
|
p->iValue = p->iValue * 256 + (a->lastChar & 255);
|
|
if (p->expectedChars <= 0) {
|
|
snprintf(item, sizeof item, "%ld ", p->iValue);
|
|
DynStringConcat(p->result, item);
|
|
}
|
|
break;
|
|
case hexType:
|
|
p->expectedChars--;
|
|
if (p->expectedChars <= 0) {
|
|
snprintf(item, sizeof item, "%2.2x ", (a->lastChar & 255));
|
|
DynStringConcat(p->result, item);
|
|
}
|
|
break;
|
|
case floatType:
|
|
p->expectedChars--;
|
|
if (p->expectedChars <= 0) {
|
|
str = GetCharArray(a->rdBuffer) + GetDynStringLength(a->rdBuffer) - 4;
|
|
fValue = ieee2double(str);
|
|
snprintf(item, sizeof item, "%.7g ", fValue);
|
|
DynStringConcat(p->result, item);
|
|
}
|
|
break;
|
|
case crcType:
|
|
p->expectedChars--;
|
|
if (p->expectedChars <= 0) {
|
|
str = GetCharArray(a->rdBuffer);
|
|
l = GetDynStringLength(a->rdBuffer);
|
|
if (strcasecmp(p->crcAlgorithm, "keller-crc") == 0) {
|
|
i = str[l-2];
|
|
str[l-2] = str[l-1];
|
|
str[l-1] = i;
|
|
}
|
|
if (calc_crc(str, l) != 0) {
|
|
DynStringConcat(p->result, "badCRC ");
|
|
}
|
|
}
|
|
}
|
|
if (p->expectedChars <= 0) {
|
|
BinReadItem(a);
|
|
}
|
|
return res;
|
|
}
|
|
return AsconBaseHandler(a);
|
|
}
|
|
|
|
void BinPrivateKill(void *pVoid) {
|
|
BinPrivate *p = pVoid;
|
|
DeleteDynString(p->inp);
|
|
DeleteDynString(p->result);
|
|
free(p->crcAlgorithm);
|
|
free(p);
|
|
}
|
|
|
|
static int BinInit(Ascon * a, SConnection * con, int argc, char *argv[])
|
|
{
|
|
BinPrivate *p;
|
|
|
|
/* argv[2] may be modbus-crc (default) or keller-crc */
|
|
|
|
p = calloc(sizeof(*p), 1);
|
|
a->hostport = strdup(argv[1]);
|
|
a->private = p;
|
|
p->inp = CreateDynString(60,63);
|
|
p->result = CreateDynString(60,63);
|
|
if (argc > 2 && strcmp(argv[2], "") != 0) {
|
|
p->crcAlgorithm = strdup(argv[2]);
|
|
} else {
|
|
p->crcAlgorithm = strdup("modbus-crc");
|
|
}
|
|
if (argc > 3) {
|
|
a->timeout = atof(argv[3]);
|
|
} else {
|
|
a->timeout = 2.0; /* sec */
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void AddBinProtocol()
|
|
{
|
|
static AsconProtocol binprot;
|
|
binprot.name = "bin";
|
|
binprot.handler = BinHandler;
|
|
binprot.init = BinInit;
|
|
AsconInsertProtocol(&binprot);
|
|
}
|