linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bugfixes in drivers

Browse Source
This commit is contained in:
zolliker
2011-08-29 11:55:12 +00:00
parent b0fca0f96f
commit 01ecc51a4a
7 changed files with 259 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

197
binprot.c
View File

@@ -48,14 +48,21 @@
* 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] opf 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)
*/
typedef enum {intType, hexType, floatType,
skipType, codeType, crcType, dumpType, errorType} BinDataType;
// dumpType, errorType must be the last items
typedef enum {modbusCrc, kellerCrc, syconCrc} CrcAlgorithm;
typedef struct {
char *crcAlgorithm;
CrcAlgorithm crcAlgorithm;
pDynString inp;
char *nextFmt;
pDynString result;
@@ -63,16 +70,20 @@ typedef struct {
BinDataType type;
long iValue;
int dumpFrom;
int syconState;
unsigned char chksum;
} BinPrivate;
/*----------------------------------------------------------------------------*/
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 */
/** 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;
@@ -154,6 +165,42 @@ void BinError(Ascon *a, char *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);
}
/*----------------------------------------------------------------------------*/
int BinHandler(Ascon *a) {
int res;
@@ -192,14 +239,17 @@ int BinHandler(Ascon *a) {
if (strcasecmp(item, "/") == 0) {
break;
} else if (strcasecmp(item, "crc") == 0) {
if (strcasecmp(p->crcAlgorithm, "keller-crc") == 0) {
switch (p->crcAlgorithm) {
case kellerCrc:
crc = calc_crc(GetCharArray(dyn), l);
DynStringConcatChar(dyn, crc / 256);
DynStringConcatChar(dyn, crc % 256);
} else { /* modbus-crc */
break;
case modbusCrc:
crc = calc_crc(GetCharArray(dyn), l);
DynStringConcatChar(dyn, crc / 256);
DynStringConcatChar(dyn, crc % 256);
DynStringConcatChar(dyn, crc / 256);
break;
}
} else if (strncasecmp(item, "int", 3) == 0) {
sscanf(item + 3, "%d", &size);
@@ -238,6 +288,7 @@ int BinHandler(Ascon *a) {
return 1;
}
DynStringConcatChar(dyn, (iValue & 255));
break;
case floatType:
res = sscanf(item, "%lf", &fValue);
if (res != 1) {
@@ -255,6 +306,10 @@ int BinHandler(Ascon *a) {
}
}
if (p->crcAlgorithm == syconCrc) {
BinToSycon(dyn);
}
p->nextFmt = str + pos;
p->type = hexType; /* initialize to anything < dumpType */
do {
@@ -274,6 +329,7 @@ int BinHandler(Ascon *a) {
DynStringClear(p->result);
p->type = hexType; /* initialize to anything < dumpType */
BinReadItem(a);
p->syconState = 0;
break;
case AsconReading:
res = AsconBaseHandler(a);
@@ -283,12 +339,12 @@ int BinHandler(Ascon *a) {
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);
}
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;
@@ -297,12 +353,58 @@ int BinHandler(Ascon *a) {
}
/* exchange buffers */
dyn = a->rdBuffer;
a->rdBuffer = p->result;
p->result = dyn;
a->rdBuffer = p->result;
p->result = dyn;
return 1;
}
return res;
}
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:
@@ -311,16 +413,16 @@ int BinHandler(Ascon *a) {
} else {
DynStringCopy(a->errmsg, "BINERR: ");
p->expectedChars = 2;
p->type = errorType;
p->dumpFrom = 0;
p->type = errorType;
p->dumpFrom = 0;
/* skip to end */
p->nextFmt = "";
p->nextFmt = "";
}
break;
case errorType:
if (p->expectedChars > 1) {
p->expectedChars--;
snprintf(item, sizeof item, "error %d / ", (a->lastChar & 255));
snprintf(item, sizeof item, "error %d / ", (a->lastChar & 255));
DynStringCopy(p->result, item);
}
break;
@@ -344,7 +446,7 @@ int BinHandler(Ascon *a) {
DynStringConcat(p->result, item);
}
break;
case floatType:
case floatType: /* does not work with sycon when stuffed */
p->expectedChars--;
if (p->expectedChars <= 0) {
str = GetCharArray(a->rdBuffer) + GetDynStringLength(a->rdBuffer) - 4;
@@ -358,14 +460,28 @@ int BinHandler(Ascon *a) {
if (p->expectedChars <= 0) {
str = GetCharArray(a->rdBuffer);
l = GetDynStringLength(a->rdBuffer);
if (strcasecmp(p->crcAlgorithm, "keller-crc") == 0) {
switch (p->crcAlgorithm) {
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 kellerCrc:
i = str[l-2];
str[l-2] = str[l-1];
str[l-1] = i;
/* fall through */
case modbusCrc:
if (calc_crc(str, l) != 0) {
DynStringConcat(p->result, "badCRC ");
}
}
if (calc_crc(str, l) != 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;
}
}
if (p->expectedChars <= 0) {
@@ -376,11 +492,10 @@ int BinHandler(Ascon *a) {
return AsconBaseHandler(a);
}
void BinPrivateKill(void *pVoid) {
static void BinPrivateKill(void *pVoid) {
BinPrivate *p = pVoid;
DeleteDynString(p->inp);
DeleteDynString(p->result);
free(p->crcAlgorithm);
free(p);
}
@@ -388,18 +503,30 @@ static int BinInit(Ascon * a, SConnection * con, int argc, char *argv[])
{
BinPrivate *p;
/* argv[2] may be modbus-crc (default) or keller-crc */
/* argv[2] may be modbus-crc (default), keller-crc or sycon-crc */
if (argc < 2) {
return 0;
}
p = calloc(sizeof(*p), 1);
a->hostport = strdup(argv[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], "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);
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 {