866 lines
24 KiB
C
866 lines
24 KiB
C
#include <ctype.h>
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
#include "ascon.h"
|
|
#include "ascon.i"
|
|
#include "dynstring.h"
|
|
#include "cnvrt.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<o> changing the format to 4 byte ieee float
|
|
* order <o>:
|
|
* 0: normal (may be omitted)
|
|
* 1: exchange bytes within words
|
|
* 2: exchange words
|
|
* 3: invert order
|
|
* str<n> string nul terminated or with exactly n characters
|
|
* (encode space by underscore)
|
|
* crc<t><i> send crc
|
|
* <t> is the type of crc
|
|
* (m: modbus, k: keller, r: rs_port, s: sycon, c: checksum)
|
|
* <i> index from where to start counting (default 0)
|
|
*
|
|
* and formatItem is one of the follwing
|
|
*
|
|
* a number (decimal format): returned byte must match (else error)
|
|
* 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<o> convert 4 bytes from ieee float (<o> see above)
|
|
* str<n> string (length n or nul terminated)
|
|
* (spaces are encoded by underscore)
|
|
* dump read all bytes in read buffer as hex (only for tests)
|
|
* crc<t><i> check crc (if wrong, "badCRC" is added to the response)
|
|
* <t> is the type of crc
|
|
* (m: modbus, k: keller, r: rs_port, s: sycon, c: checksum)
|
|
* <i> index from where to start counting (default 0)
|
|
* 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
|
|
*
|
|
* different crc's might be used (argv[2] of BinInit):
|
|
* modbus-crc: CRC-16-IBM, order: lo,hi
|
|
* keller-crc: CRC-16-IBM, order: hi,lo
|
|
* sycon-crc: sort of mod 256 checksum, byte stuffing included (no float allowed)
|
|
* chksum-crc: simple 8bit checksum
|
|
*/
|
|
|
|
#define SIMULATE_ERROR 0
|
|
static int simulate_error = 0;
|
|
|
|
typedef enum {intType, hexType, floatType, strType,
|
|
skipType, codeType, checkType, crcType,
|
|
dumpType, endType, errorType} BinDataType; /* items on this line stay at end */
|
|
|
|
typedef enum {modbusCrc, kellerCrc, rsportCrc, syconCrc, chksumCrc} CrcAlgorithm;
|
|
|
|
typedef struct {
|
|
CrcAlgorithm crcAlgorithm;
|
|
CrcAlgorithm algo;
|
|
int crcStart;
|
|
pDynString inp;
|
|
char *nextFmt; /* position of next format token */
|
|
char *readFmt; /* position of first read format */
|
|
pDynString result;
|
|
int expectedChars;
|
|
BinDataType type;
|
|
int order;
|
|
int done;
|
|
long iValue;
|
|
int dumpFrom;
|
|
int syconState;
|
|
unsigned char chksum;
|
|
pDynString data;
|
|
int try_again_count;
|
|
int try_again_max;
|
|
} BinPrivate;
|
|
|
|
#define MAXDUMP 999
|
|
#define NUL_TERMINATED 9999
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static int parseAlgo(char *inp, BinPrivate *p) {
|
|
int index = 0;
|
|
|
|
p->algo = p->crcAlgorithm;
|
|
switch (*inp) {
|
|
case 'm': p->algo = modbusCrc; break;
|
|
case 'k': p->algo = kellerCrc; break;
|
|
case 'r': p->algo = rsportCrc; break;
|
|
case 's': p->algo = syconCrc; break;
|
|
case 'c': p->algo = chksumCrc; break;
|
|
}
|
|
while (*inp > '9') inp++;
|
|
sscanf(inp, "%d", &index);
|
|
return index;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
static int calc_crc(char *inp, int inpLen)
|
|
{
|
|
/** CRC-16-IBM Algorithm
|
|
* crc calculation:
|
|
* returns the 16bit CRC value of a message
|
|
* crc check:
|
|
* returns 0 when the crc appended to the message (lo byte first)
|
|
* is correct
|
|
*/
|
|
|
|
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;
|
|
}
|
|
/*----------------------------------------------------------------------------*/
|
|
static int calc_crc8(char *inp, int inpLen)
|
|
{
|
|
/** CRC-8 (x8+x5+x4+1) Algorithm
|
|
* crc calculation:
|
|
* returns the 8bit CRC value of a message
|
|
* crc check:
|
|
* returns 0 when the crc appended to the message is correct
|
|
*/
|
|
|
|
unsigned char crc = 0;
|
|
unsigned char data;
|
|
int n;
|
|
|
|
while (inpLen--) {
|
|
data = *(unsigned char *) inp;
|
|
for (n = 0; n < 8; n++) {
|
|
if ((crc ^ data) & 1) {
|
|
crc ^= 0x18;
|
|
crc >>= 1;
|
|
crc |= 0x80;
|
|
} else {
|
|
crc >>= 1;
|
|
crc &= 0x7f;
|
|
}
|
|
data >>= 1;
|
|
}
|
|
inp++;
|
|
}
|
|
return crc;
|
|
}
|
|
/*----------------------------------------------------------------------------*/
|
|
static int calc_chksum(char *inp, int inpLen)
|
|
{
|
|
/** simple 8bit checksum
|
|
*/
|
|
|
|
unsigned char crc = 0;
|
|
unsigned char data;
|
|
int n;
|
|
|
|
while (inpLen--) {
|
|
data = *(unsigned char *) inp;
|
|
crc += data;
|
|
inp++;
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
int BinGetItem(char *str, char *item, size_t item_size) {
|
|
int cntcurly = 0;
|
|
char *p, *res;
|
|
int l;
|
|
|
|
p = str;
|
|
while (*p == ' ') p++;
|
|
if (*p == '{') {
|
|
cntcurly++;
|
|
p++;
|
|
}
|
|
if (!*p) return -1;
|
|
res = p;
|
|
while (*p) {
|
|
if (*p == '{') {
|
|
cntcurly++;
|
|
} else if (*p == '}') {
|
|
cntcurly--;
|
|
if (cntcurly == 0) {
|
|
l = p - str - 1;
|
|
break;
|
|
}
|
|
} else if (*p == ' ' && cntcurly == 0) {
|
|
break;
|
|
}
|
|
p++;
|
|
}
|
|
l = p - res;
|
|
if (l >= item_size) {
|
|
l = item_size - 1;
|
|
}
|
|
strncpy(item, res, l);
|
|
item[l] = '\0';
|
|
return p - str;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
int BinReadItem(Ascon *a) {
|
|
BinPrivate *p = a->private;
|
|
char item[32];
|
|
int valen;
|
|
int size;
|
|
CrcAlgorithm algo;
|
|
|
|
valen = BinGetItem(p->nextFmt, item, sizeof item);
|
|
if (valen < 0) {
|
|
if (p->type < dumpType) {
|
|
p->dumpFrom = GetDynStringLength(a->rdBuffer);
|
|
p->type = endType;
|
|
}
|
|
p->expectedChars = MAXDUMP;
|
|
return 0;
|
|
}
|
|
|
|
if (strncasecmp(item, "crc", 3) == 0) {
|
|
p->type = crcType;
|
|
p->expectedChars = 2;
|
|
p->crcStart = parseAlgo(item + 3, p);
|
|
if (p->algo == chksumCrc) {
|
|
p->expectedChars = 1;
|
|
} else {
|
|
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 (strncasecmp(item, "float", 5) == 0) {
|
|
p->order = 0;
|
|
sscanf(item + 5, "%d", &p->order);
|
|
p->expectedChars = 4;
|
|
p->type = floatType;
|
|
DynStringClear(p->data);
|
|
} else if (strncasecmp(item, "str", 3) == 0) {
|
|
size = 1;
|
|
sscanf(item + 3, "%d", &size);
|
|
if (size == 0) {
|
|
size = NUL_TERMINATED;
|
|
}
|
|
p->expectedChars = size;
|
|
p->type = strType;
|
|
p->done = 0;
|
|
DynStringClear(p->data);
|
|
} else if (strcasecmp(item, "hex") == 0) {
|
|
p->expectedChars = 1;
|
|
p->type = hexType;
|
|
} else if (strcasecmp(item, "code") == 0) {
|
|
p->type = codeType;
|
|
} else if (strcasecmp(item, "dump") == 0) {
|
|
p->dumpFrom = GetDynStringLength(a->rdBuffer);
|
|
p->type = dumpType;
|
|
} else if (strncasecmp(item, "skip", 4) == 0) {
|
|
size = 1;
|
|
sscanf(item + 4, "%d", &size);
|
|
p->expectedChars = size;
|
|
p->type = skipType;
|
|
} else {
|
|
p->iValue = 0;
|
|
if (strncasecmp(item, "0x", 2) == 0) {
|
|
if (1 != sscanf(item + 2, "%lx", &p->iValue)) {
|
|
return -1;
|
|
}
|
|
} else {
|
|
if (1 != sscanf(item, "%ld", &p->iValue)) {
|
|
return -1;
|
|
}
|
|
}
|
|
p->expectedChars = 1;
|
|
p->type = checkType;
|
|
}
|
|
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;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void BinToSycon(pDynString dyn) {
|
|
char *str = strdup(GetCharArray(dyn));
|
|
int l = GetDynStringLength(dyn);
|
|
unsigned char sum, byte;
|
|
int i;
|
|
|
|
DynStringClear(dyn);
|
|
DynStringConcat(dyn, "\x02\x10\x80"); /* STX ADDR CMD */
|
|
sum = 0x10 + 0x80;
|
|
for (i=0; i<l; i++) {
|
|
byte = str[i];
|
|
sum += byte;
|
|
switch (byte) {
|
|
case 0x02:
|
|
DynStringConcatChar(dyn, 0x07);
|
|
DynStringConcatChar(dyn, '0');
|
|
break;
|
|
case 0x0d:
|
|
DynStringConcatChar(dyn, 0x07);
|
|
DynStringConcatChar(dyn, '1');
|
|
break;
|
|
case 0x07:
|
|
DynStringConcatChar(dyn, 0x07);
|
|
DynStringConcatChar(dyn, '2');
|
|
break;
|
|
default:
|
|
DynStringConcatChar(dyn, byte);
|
|
}
|
|
}
|
|
DynStringConcatChar(dyn, 0x30 + (sum / 16));
|
|
DynStringConcatChar(dyn, 0x30 + (sum % 16));
|
|
DynStringConcatChar(dyn, 0x0d);
|
|
free(str);
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void BinShuffle(char *input, int order) {
|
|
/* order may be 0: no shuffle, 1: change within words, 2: change words, 3: invert*/
|
|
char ch;
|
|
|
|
switch (order) {
|
|
case 1:
|
|
ch = input[0]; input[0] = input[1]; input[1] = ch;
|
|
ch = input[2]; input[2] = input[3]; input[3] = ch;
|
|
break;
|
|
case 2:
|
|
ch = input[0]; input[0] = input[2]; input[2] = ch;
|
|
ch = input[1]; input[1] = input[3]; input[3] = ch;
|
|
break;
|
|
case 3:
|
|
ch = input[0]; input[0] = input[3]; input[3] = ch;
|
|
ch = input[1]; input[1] = input[2]; input[2] = ch;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void BinResetWrite(Ascon *a) {
|
|
BinPrivate *p = a->private;
|
|
p->nextFmt = p->readFmt;
|
|
a->wrPos = 0;
|
|
a->lineCount = 0;
|
|
a->state = AsconWriting;
|
|
}
|
|
/*----------------------------------------------------------------------------*/
|
|
int BinHandler(Ascon *a) {
|
|
int res;
|
|
char *str;
|
|
int len;
|
|
int i, l, ls, pos;
|
|
int start;
|
|
unsigned int crc;
|
|
pDynString dyn;
|
|
char item[32];
|
|
char data[8];
|
|
int size;
|
|
int order;
|
|
int valen;
|
|
BinDataType type;
|
|
long iValue;
|
|
uint32_t uValue;
|
|
double fValue;
|
|
BinPrivate *p = a->private;
|
|
|
|
switch (a->state) {
|
|
case AsconWriteStart:
|
|
/* exchange buffers */
|
|
dyn = p->inp;
|
|
p->inp = a->wrBuffer;
|
|
p->try_again_count = p->try_again_max;
|
|
a->wrBuffer = dyn;
|
|
DynStringClear(dyn);
|
|
str = GetCharArray(p->inp);
|
|
len = GetDynStringLength(p->inp);
|
|
l = 0;
|
|
type = intType;
|
|
for (pos = 0; pos < len; ) {
|
|
valen = BinGetItem(str + pos, item, sizeof item);
|
|
if (valen < 0) {
|
|
BinError(a, "missing '/'");
|
|
return 1;
|
|
}
|
|
pos += valen;
|
|
if (strcasecmp(item, "/") == 0) {
|
|
break;
|
|
} else if (strncasecmp(item, "crc", 3) == 0) {
|
|
start = parseAlgo(item + 3, p);
|
|
switch (p->algo) {
|
|
case kellerCrc:
|
|
crc = calc_crc(GetCharArray(dyn)+start, l-start);
|
|
DynStringConcatChar(dyn, crc / 256);
|
|
DynStringConcatChar(dyn, crc % 256);
|
|
break;
|
|
case modbusCrc:
|
|
crc = calc_crc(GetCharArray(dyn)+start, l-start);
|
|
DynStringConcatChar(dyn, crc % 256);
|
|
DynStringConcatChar(dyn, crc / 256);
|
|
break;
|
|
case chksumCrc:
|
|
crc = calc_chksum(GetCharArray(dyn)+start, l-start);
|
|
DynStringConcatChar(dyn, crc % 256);
|
|
break;
|
|
case rsportCrc:
|
|
crc = calc_crc8(GetCharArray(dyn)+start, l-start);
|
|
DynStringConcatChar(dyn, crc);
|
|
break;
|
|
}
|
|
} else if (strncasecmp(item, "int", 3) == 0) {
|
|
size = 1;
|
|
sscanf(item + 3, "%d", &size);
|
|
type = intType;
|
|
} else if (strcasecmp(item, "hex") == 0) {
|
|
type = hexType;
|
|
} else if (strncasecmp(item, "float", 5) == 0) {
|
|
order = 0;
|
|
sscanf(item + 5, "%d", &order);
|
|
type = floatType;
|
|
} else if (strncasecmp(item, "str", 3) == 0) {
|
|
size = NUL_TERMINATED;
|
|
sscanf(item + 3, "%d", &size);
|
|
type = strType;
|
|
} else {
|
|
switch (type) {
|
|
case intType:
|
|
res = sscanf(item, "%ld", &iValue);
|
|
if (res != 1) {
|
|
BinError(a, "invalid integer");
|
|
return 1;
|
|
}
|
|
if (iValue < 0) {
|
|
printf("%ld\n", iValue);
|
|
}
|
|
uValue = iValue;
|
|
for (i = size - 1; i >= 0; i--) {
|
|
if (i < sizeof data) {
|
|
data[i] = uValue % 256;
|
|
}
|
|
uValue >>= 8;
|
|
}
|
|
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));
|
|
l += 1;
|
|
break;
|
|
case floatType:
|
|
res = sscanf(item, "%lf", &fValue);
|
|
if (res != 1) {
|
|
BinError(a, "invalid float");
|
|
return 1;
|
|
}
|
|
double2ieee(fValue, data);
|
|
BinShuffle(data, order);
|
|
DynStringConcatBytes(dyn, data, 4);
|
|
l += 4;
|
|
break;
|
|
case strType:
|
|
ls = strlen(item);
|
|
if (ls > size) {
|
|
item[size] = '\0';
|
|
}
|
|
DynStringConcat(dyn, item);
|
|
if (size == NUL_TERMINATED) {
|
|
DynStringConcatChar(dyn, 0);
|
|
l += ls + 1;
|
|
} else {
|
|
for (i = ls; i < size; i++) {
|
|
DynStringConcatChar(dyn, 0);
|
|
}
|
|
l += size;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (p->crcAlgorithm == syconCrc) {
|
|
BinToSycon(dyn);
|
|
}
|
|
|
|
p->readFmt = str + pos;
|
|
p->nextFmt = p->readFmt;
|
|
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);
|
|
BinResetWrite(a);
|
|
if (GetDynStringLength(a->wrBuffer) == 0) {
|
|
/* prevent to swallow "garbage" */
|
|
a->state = AsconWriteDone;
|
|
}
|
|
return 1;
|
|
case AsconReadStart:
|
|
DynStringClear(p->result);
|
|
p->type = hexType; /* initialize to anything < dumpType */
|
|
BinReadItem(a);
|
|
p->syconState = 0;
|
|
break;
|
|
case AsconReading:
|
|
res = AsconBaseHandler(a);
|
|
if (a->state == AsconTimeout && p->try_again_count > 0) {
|
|
p->try_again_count--;
|
|
DynStringCopy(a->errmsg, "no response");
|
|
BinResetWrite(a);
|
|
return 0;
|
|
}
|
|
if (res == 0) {
|
|
if (GetDynStringLength(a->rdBuffer) == 0) {
|
|
/* wait for the first byte - or timeout */
|
|
return 0;
|
|
}
|
|
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 == endType && p->dumpFrom < l) {
|
|
DynStringCopy(a->errmsg, "BINERR: superflous chars ");
|
|
DynStringConcat(a->errmsg, GetCharArray(p->result));
|
|
a->state = AsconFailed;
|
|
} else if (p->type == errorType) {
|
|
DynStringConcat(a->errmsg, GetCharArray(p->result));
|
|
a->state = AsconFailed;
|
|
} else {
|
|
if (p->try_again_count != p->try_again_max) {
|
|
Log(INFO, "com", "sock0:recovered from (%s) after: %s", GetCharArray(a->errmsg), GetCharArray(p->inp));
|
|
}
|
|
a->state = AsconReadDone;
|
|
}
|
|
/* exchange buffers */
|
|
dyn = a->rdBuffer;
|
|
a->rdBuffer = p->result;
|
|
p->result = dyn;
|
|
if (a->state == AsconFailed && p->try_again_count > 0) {
|
|
p->try_again_count--;
|
|
BinResetWrite(a);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
if (p->crcAlgorithm == syconCrc) {
|
|
switch (p->syconState) {
|
|
case 0: /* wait for STX */
|
|
if (a->lastChar == 0x02) {
|
|
p->syconState = 1;
|
|
p->chksum = 0;
|
|
}
|
|
return res;
|
|
case 1: /* skip address */
|
|
case 2: /* skip cmd_rsp */
|
|
p->syconState++;
|
|
p->chksum += a->lastChar;
|
|
return res;
|
|
case 3: /* detect stuffed bytes */
|
|
if (a->lastChar == 0x07) {
|
|
p->syconState = 4;
|
|
return res;
|
|
}
|
|
p->chksum += a->lastChar;
|
|
break;
|
|
case 4: /* last byte was 0x07 */
|
|
switch (a->lastChar) {
|
|
case '0':
|
|
a->lastChar = 0x02;
|
|
break;
|
|
case '1':
|
|
a->lastChar = 0x0d;
|
|
break;
|
|
case '2':
|
|
a->lastChar = 0x07;
|
|
break;
|
|
}
|
|
p->chksum += a->lastChar;
|
|
p->syconState = 3;
|
|
break;
|
|
case 5: /* expect 0x0d */
|
|
if (a->lastChar != 0x0d) {
|
|
DynStringConcat(p->result, "noCR ");
|
|
}
|
|
p->syconState = 6;
|
|
p->dumpFrom++; /* skip for dump */
|
|
case 6: /* skip everything else */
|
|
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 endType:
|
|
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, "%d ", (int32_t)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--;
|
|
DynStringConcatChar(p->data, a->lastChar);
|
|
if (p->expectedChars <= 0) {
|
|
char *shuffle = GetCharArray(p->data);
|
|
BinShuffle(shuffle, p->order);
|
|
fValue = ieee2double(shuffle);
|
|
snprintf(item, sizeof item, "%.7g ", fValue);
|
|
DynStringConcat(p->result, item);
|
|
}
|
|
break;
|
|
case strType:
|
|
if (a->lastChar) {
|
|
if (!p->done) {
|
|
DynStringConcatChar(p->data, a->lastChar);
|
|
}
|
|
} else {
|
|
p->done = 1;
|
|
if (p->expectedChars == NUL_TERMINATED) {
|
|
DynStringConcat(p->result, GetCharArray(p->data));
|
|
p->expectedChars = 0;
|
|
break;
|
|
}
|
|
}
|
|
p->expectedChars--;
|
|
if (p->expectedChars <= 0) {
|
|
DynStringConcat(p->result, GetCharArray(p->data));
|
|
break;
|
|
}
|
|
break;
|
|
case crcType:
|
|
p->expectedChars--;
|
|
if (p->expectedChars <= 0) {
|
|
str = GetCharArray(a->rdBuffer);
|
|
l = GetDynStringLength(a->rdBuffer);
|
|
switch (p->algo) {
|
|
case syconCrc: /* ignore crc check */
|
|
/* subtract CRC char (undo the addition) and subtract crc higher four bits */
|
|
p->chksum -= a->lastChar + (a->lastChar & 0x0f);
|
|
p->syconState = 5;
|
|
if (p->chksum != 0) {
|
|
DynStringConcat(p->result, "badCRC ");
|
|
}
|
|
break;
|
|
case chksumCrc:
|
|
if (calc_chksum(str+p->crcStart, l-1-p->crcStart) != (unsigned char)str[l-1]) {
|
|
DynStringConcat(p->result, "badCRC ");
|
|
}
|
|
break;
|
|
case kellerCrc:
|
|
i = str[l-2];
|
|
str[l-2] = str[l-1];
|
|
str[l-1] = i;
|
|
/* fall through */
|
|
case modbusCrc:
|
|
case rsportCrc:
|
|
if (calc_crc(str+p->crcStart, l-p->crcStart) != 0) {
|
|
DynStringConcat(p->result, "badCRC ");
|
|
}
|
|
}
|
|
} else if (p->crcAlgorithm == syconCrc) {
|
|
/* subtract CRC char (undo the addition) and subtract crc higher four bits */
|
|
p->chksum -= a->lastChar + (a->lastChar & 0x0f) * 16;
|
|
}
|
|
break;
|
|
case checkType:
|
|
p->expectedChars--;
|
|
if ((unsigned char)a->lastChar != p->iValue) {
|
|
i = GetDynStringLength(a->rdBuffer) - 1;
|
|
snprintf(item, sizeof item, "BINERR: [%d]==%2.2x != %2.2x ", i, (unsigned char)a->lastChar, (unsigned char)p->iValue);
|
|
DynStringCopy(a->errmsg, item);
|
|
p->type = errorType;
|
|
p->dumpFrom = 0;
|
|
/* skip to end */
|
|
p->nextFmt = "";
|
|
}
|
|
#if SIMULATE_ERROR > 0
|
|
if (p->try_again_max > 0 && simulate_error++ > SIMULATE_ERROR) {
|
|
DynStringCopy(a->errmsg, "simulate ");
|
|
p->type = errorType;
|
|
p->dumpFrom = 0;
|
|
/* skip to end */
|
|
p->nextFmt = "";
|
|
simulate_error = 0;
|
|
}
|
|
#endif
|
|
}
|
|
if (p->expectedChars <= 0) {
|
|
BinReadItem(a);
|
|
}
|
|
return res;
|
|
}
|
|
return AsconBaseHandler(a);
|
|
}
|
|
|
|
static void BinPrivateKill(void *pVoid) {
|
|
BinPrivate *p = pVoid;
|
|
DeleteDynString(p->inp);
|
|
DeleteDynString(p->result);
|
|
DeleteDynString(p->data);
|
|
free(p);
|
|
}
|
|
|
|
static int BinInit(Ascon * a, SConnection * con, int argc, char *argv[])
|
|
{
|
|
BinPrivate *p;
|
|
|
|
/* args: 2 <protocol>, 3 <timeout>, 4 <try_again_max>
|
|
/* argv[2] may be modbus-crc (default), keller-crc, rsport-crc or sycon-crc */
|
|
|
|
if (argc < 2) {
|
|
return 0;
|
|
}
|
|
p = calloc(sizeof(*p), 1);
|
|
a->private = p;
|
|
a->killPrivate = BinPrivateKill;
|
|
p->crcAlgorithm = modbusCrc;
|
|
if (argc > 2 && strcmp(argv[2], "") != 0) {
|
|
if (strcasecmp(argv[2], "keller-crc") == 0) {
|
|
p->crcAlgorithm = kellerCrc;
|
|
} else if (strcasecmp(argv[2], "sycon-crc") == 0) {
|
|
p->crcAlgorithm = syconCrc;
|
|
} else if (strcasecmp(argv[2], "rsport-crc") == 0) {
|
|
p->crcAlgorithm = rsportCrc;
|
|
} else if (strcasecmp(argv[2], "chksum-crc") == 0) {
|
|
p->crcAlgorithm = chksumCrc;
|
|
} else if (strcasecmp(argv[2], "modbus-crc") != 0) {
|
|
SCPrintf(con, eError, "ERROR: unknown crc-algorithm %s", argv[2]);
|
|
a->private = NULL;
|
|
free(p);
|
|
return 0;
|
|
}
|
|
}
|
|
a->hostport = strdup(argv[1]);
|
|
p->inp = CreateDynString(60,63);
|
|
p->result = CreateDynString(60,63);
|
|
p->data = CreateDynString(60,63);
|
|
if (argc > 3) {
|
|
a->timeout = atof(argv[3]);
|
|
} else {
|
|
a->timeout = 2.0; /* sec */
|
|
}
|
|
if (argc > 4) {
|
|
p->try_again_max = atoi(argv[4]);
|
|
} else {
|
|
p->try_again_max = 0;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------*/
|
|
void AddBinProtocol()
|
|
{
|
|
static AsconProtocol binprot;
|
|
binprot.name = "bin";
|
|
binprot.handler = BinHandler;
|
|
binprot.init = BinInit;
|
|
AsconInsertProtocol(&binprot);
|
|
}
|