#include #include #include #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 changing the format to byte integers * hex changing the format to hexadecimal (1 byte at a time) * float changing the format to 4 byte ieee float * order : * 0: normal (may be omitted) * 1: exchange bytes within words * 2: exchange words * 3: invert order * str string nul terminated or with exactly n characters * (encode space by underscore) * crc send crc * is the type of crc * (m: modbus, k: keller, r: rs_port, s: sycon, c: checksum) * 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 skip 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 convert bytes to integer (most significant byte first) * and append the (decimal coded) number to the result * if is omitted, = 1 is assumed * hex convert 1 byte to hexadecimal coded integer * and append it to the response * float convert 4 bytes from ieee float ( see above) * str string (length n or nul terminated) * (spaces are encoded by underscore) * dump read all bytes in read buffer as hex (only for tests) * crc check crc (if wrong, "badCRC" is added to the response) * is the type of crc * (m: modbus, k: keller, r: rs_port, s: sycon, c: checksum) * 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; iprivate; 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 , 3 , 4 /* 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); }