diff --git a/site_ansto/hardsup/loader_asyncprotocol.c b/site_ansto/hardsup/loader_asyncprotocol.c index cb1c7441..9dd4816a 100644 --- a/site_ansto/hardsup/loader_asyncprotocol.c +++ b/site_ansto/hardsup/loader_asyncprotocol.c @@ -1,6 +1,7 @@ #include "loader_asyncprotocol.h" #include "huber_asyncprotocol.h" #include "knauer_asyncprotocol.h" +#include "modbus_asyncprotocol.h" #include "omron_asyncprotocol.h" #include "sics.h" @@ -8,5 +9,6 @@ void LOADERInitProtocol(SicsInterp *pSics) { HUBERInitProtocol(pSics); KNAUERInitProtocol(pSics); + MODBUSInitProtocol(pSics); OMRONInitProtocol(pSics); } diff --git a/site_ansto/hardsup/makefile b/site_ansto/hardsup/makefile index dcac1541..aef194cb 100644 --- a/site_ansto/hardsup/makefile +++ b/site_ansto/hardsup/makefile @@ -38,6 +38,7 @@ HOBJ += aqp_opalstatus.o HOBJ += omron_asyncprotocol.o HOBJ += huber_asyncprotocol.o HOBJ += knauer_asyncprotocol.o +HOBJ += modbus_asyncprotocol.o HOBJ += loader_asyncprotocol.o libhlib.a: $(HOBJ) diff --git a/site_ansto/hardsup/modbus_asyncprotocol.c b/site_ansto/hardsup/modbus_asyncprotocol.c new file mode 100644 index 00000000..13cdec68 --- /dev/null +++ b/site_ansto/hardsup/modbus_asyncprotocol.c @@ -0,0 +1,718 @@ +#include "modbus_asyncprotocol.h" +#include "asyncprotocol.h" +#include "asyncqueue.h" +#include "ascon.i" +#include "dynstring.h" +#include +#include +#include + +#define PROTOCOL_CONTINUE 0 +#define PROTOCOL_COMPLETE 1 +#define PROTOCOL_ERROR (-1) + +/* Sample Messages + * structure: + * STX + * data + * ETX + * BCC +*/ +#define PROTOCOL_NAME "MODBUS_AP" +#define PROTOCOL_INIT MODBUSInitProtocol +#define ADUSIZE 17 + +/* + * MODBUS DataTypes + */ +enum mbDtypes {U16, U32, F32}; + +/* + * NumVal = number of values to read or write, + * ie number of coils, input/outputs or registers. + * Number of bytes = NumVal * (number of bytes in Dtype) + */ +struct cmdPar { + unsigned int MBAddr; /**< MODBUS Address */ + unsigned int Fcode; /**< Function Code */ + unsigned int SAddr; + unsigned int NumVal; /**< Number of Values */ + unsigned int NumBytes; + unsigned int NumRd; + unsigned int NumWr; + enum mbDtypes Dtype; + double MBdata[2000]; +}; + +enum MBFCODES { RdCoil=1, RdInp=2, RdHldReg=3, RdInpReg=4, WrCoil=5, WrReg=6, WrMCoils=15, WrMRegs=16 }; + +typedef struct modbus_private_t Private, *pPrivate; +struct modbus_private_t { + struct cmdPar MBcmd; + int BO[4]; + int aduLen; + int RespLen; + int DatLen; + unsigned char ADU[ADUSIZE]; /* Allows upto 8 bytes if data */ + pDynString rdBuffer; +}; + +/* + * Protocol Private data block + */ +typedef struct proto_private_t { + int state; /**< protocol state machine */ + int len; /**< length from the protocol */ + pDynString wrBuffer; /**< transmitted message */ + pDynString rxBuffer; /**< received message */ + Private mb_priv; +} ProtoPrivate; + +static ProtoPrivate *makeProtoPrivate() +{ + ProtoPrivate *priv = calloc(sizeof(ProtoPrivate), 1); + priv->wrBuffer = CreateDynString(100, 100); + priv->rxBuffer = CreateDynString(100, 100); + return priv; +} + +static ProtoPrivate *Proto_KillPrivate(ProtoPrivate *priv) +{ + if (priv) { + if (priv->rxBuffer) { + DeleteDynString(priv->rxBuffer); + priv->rxBuffer = NULL; + } + free(priv); + } + return NULL; +} + +/* + * Protocol Specific Support Functions + */ + +/* IEEE code copied from psi/modbus.c */ +static void double2ieee(double input, unsigned 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 { + /* frexp -> 0.f * 2^23 */ + /* 2^24 * fract instead of 2^23 + * because we get 0.(f/2) not 1.f from frexp and exponent = e + 1 + */ + 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(unsigned char ieee[4]) { + + /* IEEE 32 bit floating number to double + * (denormalized numbers are considered as zero) + */ + + long mantissa; + double output, norm = 1; + int exponent, bias = 127; + + mantissa = ((ieee[1] << 16) & 0x7F0000) + | ((ieee[2] << 8) & 0xFF00) + | ((ieee[3] ) & 0xFF); + + exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */ + if (exponent == 255) { + if (mantissa == 0) { + /*TODO INF */ + } else { + /*TODO NAN */ + } + } + if (exponent == 0) { + if (mantissa == 0) { + return 0.0; + } else { + bias = 126; + norm = 0; + } + } + output = ldexp(mantissa, -23) + norm; + if (ieee[0] & 0x80) { + output = -output; + } + return output * ldexp(1, exponent - bias); +} +/*-------------------------------------------------------------------------*/ + +/** @brief Parse a command string of the form "MBAddr:Fcode:SAddr:Dtype:MBdata" +Dtype U16, U32, F32 with optional byte order array eg U32[3,2,1,0] +*/ +static void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) { + int len; + unsigned int i; + char *nextPar, dataType[4]; + double dVal; + + MBC->MBAddr = strtoul(cmdLine, &nextPar, 0); + cmdLine = nextPar+1; + MBC->Fcode = strtoul(cmdLine, &nextPar, 0); + cmdLine = nextPar+1; + MBC->SAddr = strtoul(cmdLine, &nextPar, 0); + cmdLine = nextPar+1; + if (MBC->Fcode != WrCoil) { + MBC->NumVal = strtod(cmdLine, &nextPar); + cmdLine = nextPar + 1; + strncpy(dataType,cmdLine,3); + dataType[3] = '\0'; + cmdLine += 3; + nextPar = cmdLine; + if (strcmp(dataType, "U16") == 0) { + MBC->Dtype = U16; + } else if (strcmp(dataType, "U32") == 0) { + MBC->Dtype = U32; + } else if (strcmp(dataType, "F32") == 0) { + MBC->Dtype = F32; + } + if (*cmdLine == '[') { + cmdLine++; + BO[0] = strtol(cmdLine, &nextPar, 10); + cmdLine = nextPar + 1; + BO[1] = strtol(cmdLine, &nextPar, 10); + cmdLine = nextPar + 1; + BO[2] = strtol(cmdLine, &nextPar, 10); + cmdLine = nextPar + 1; + BO[3] = strtol(cmdLine, &nextPar, 10); + } + cmdLine = nextPar + 1; + } + switch (MBC->Fcode) { + case RdCoil: + /* Coils are numbered from 1 but addressed from 0 */ + MBC->SAddr--; + MBC->NumBytes = MBC->NumVal; + MBC->NumRd = MBC->NumVal; + break; + case WrCoil: + MBC->SAddr--; + dVal = strtod(cmdLine, &nextPar); + if (dVal == 1) { + MBC->MBdata[0] = 255; + } else if (dVal == 0) { + MBC->MBdata[0] = 0; + } else { + /* TODO ERROR */ + } + break; + case RdHldReg: + if (MBC->Dtype == F32 || MBC->Dtype == U32) { + MBC->NumRd = MBC->NumVal * 2; + } else if (MBC->Dtype == U16) { + MBC->NumRd = MBC->NumVal; + } else { + /* TODO ERROR */ + } + break; + case WrMRegs: + for (i=0; i < MBC->NumVal; i++) { + MBC->MBdata[i] = strtod(cmdLine, &nextPar); + cmdLine = nextPar + 1; + } + if (MBC->Dtype == F32 || MBC->Dtype == U32) { + MBC->NumWr = MBC->NumVal * 2; + MBC->NumBytes = MBC->NumVal * 4; + } else if (MBC->Dtype == U16) { + MBC->NumWr = MBC->NumVal; + MBC->NumBytes = MBC->NumVal * 2; + } + break; + } +} + +static int ModbusOutput(pPrivate myPriv, pDynString wrBuffer, pDynString send_buffer) { + int ADUlen, PDUlenPlusUID; + unsigned int i, j; + char *cmdLine; + unsigned char ADU[32], ieee[4]; + + cmdLine = GetCharArray(wrBuffer); + for (i=0; i < 4; i++) + myPriv->BO[i] = i; + parseCmd(cmdLine, &myPriv->MBcmd, myPriv->BO); + + ADU[0] = 0; + ADU[1] = 0; + ADU[2] = 0; + ADU[3] = 0; + ADU[6] = myPriv->MBcmd.MBAddr; + ADU[7] = myPriv->MBcmd.Fcode; + switch (myPriv->MBcmd.Fcode) { + case RdCoil: + case RdHldReg: + /* TODO Max NumRd = 125 */ + ADU[8] = (myPriv->MBcmd.SAddr & 0xFF00) >> 8; + ADU[9] = myPriv->MBcmd.SAddr & 0xFF; + ADU[10] = (myPriv->MBcmd.NumRd & 0xFF00) >> 8; + ADU[11] = myPriv->MBcmd.NumRd & 0xFF; + ADUlen = 12; + break; + case WrCoil: + ADU[8] = (myPriv->MBcmd.SAddr & 0xFF00) >> 8; + ADU[9] = myPriv->MBcmd.SAddr & 0xFF; + ADU[10] = myPriv->MBcmd.MBdata[0]; + ADU[11] = 0; + ADUlen = 12; + break; + case WrMRegs: + ADU[8] = (myPriv->MBcmd.SAddr & 0xFF00) >> 8; + ADU[9] = myPriv->MBcmd.SAddr & 0xFF; + ADU[10] = (myPriv->MBcmd.NumWr & 0xFF00) >> 8; + ADU[11] = myPriv->MBcmd.NumWr & 0xFF; + ADU[12] = myPriv->MBcmd.NumBytes; + switch (myPriv->MBcmd.Dtype) { + case U16: + for (i=0, j=13; i < myPriv->MBcmd.NumVal; i++, j+=4) { + ADU[j + myPriv->BO[0]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF00) >> 8; + ADU[j + myPriv->BO[1]] = (unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF; + } + break; + case U32: + for (i=0, j=13; i < myPriv->MBcmd.NumVal; i++, j+=4) { + ADU[j + myPriv->BO[0]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF000000) >> 24; + ADU[j + myPriv->BO[1]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF0000) >> 16; + ADU[j + myPriv->BO[2]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF00) >> 8; + ADU[j + myPriv->BO[3]] = (unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF; + } + break; + case F32: + for (i=0, j=13; i < myPriv->MBcmd.NumVal; i++, j+=4) { + double2ieee(myPriv->MBcmd.MBdata[i], ieee); + ADU[j + myPriv->BO[0]] = ieee[0]; + ADU[j + myPriv->BO[1]] = ieee[1]; + ADU[j + myPriv->BO[2]] = ieee[2]; + ADU[j + myPriv->BO[3]] = ieee[3]; + } + break; + } + ADUlen = 13 + myPriv->MBcmd.NumBytes; + break; + default: + // Not Implemented + break; + } + /* Length field in MBAP includes byte 7 of the MBAP, so Length = ADU length - 6. */ + PDUlenPlusUID = ADUlen - 6; + ADU[4] = (PDUlenPlusUID & 0xFF00) >> 8; + ADU[5] = PDUlenPlusUID & 0xFF; + DynStringClear(send_buffer); + DynStringConcatBytes(send_buffer, (char *) ADU, ADUlen); + return 1; +} + + +/** @brief decode modbus response +*/ +/* ADU[4] = MSB of DatLen, ADU[5] = LSB of DatLen */ + +/* NOTE: Min aduLen is 9. The header (7 bytes) + an Error code and Exception code */ +static const int TIDidx=0; +static const int PIDidx=2; +static const int LENidx=4; +static const int UIDidx=6; +static const int CODEidx=7; +static const int DATLENidx=8; +static const int DATAidx=9; + +static void StartIn(pPrivate myPriv) { + myPriv->aduLen = 0; + myPriv->RespLen = 0; + myPriv->DatLen = 0; +} + +static int ModbusInput(pPrivate myPriv, unsigned char chr, pDynString rdBuffer) { + int i, byteCnt; + unsigned char ieee[4]; + double dVal; + long int liVal; + + if (myPriv->aduLen == 0) + DynStringClear(rdBuffer); + + myPriv->ADU[myPriv->aduLen++] = chr; + if (myPriv->RespLen == 0) { + if (myPriv->aduLen >= UIDidx) { + myPriv->RespLen = (myPriv->ADU[LENidx] << 8) + myPriv->ADU[LENidx+1]; + if (myPriv->RespLen <= 0) { + DynStringReplace(rdBuffer, "ASCERR: Length field exception", 0); + return PROTOCOL_ERROR; + } + } else { + return PROTOCOL_CONTINUE; + } + } + if (myPriv->aduLen < (6 + myPriv->RespLen)) { + return PROTOCOL_CONTINUE; + } + + if (myPriv->ADU[CODEidx] > 0x80) { + /* snprintf(errMsg, ERRLEN, "TCPMODBUS: Function code %d exception %d:", ADU[CODEidx] & 0x0F, ADU[8] &0x0F); */ + DynStringReplace(rdBuffer, "ASCERR: Function code exception", 0); + return PROTOCOL_ERROR; + } else if (myPriv->ADU[CODEidx] != myPriv->MBcmd.Fcode) { + /* snprintf(errMsg, ERRLEN, "TCPMODBUS: Requested function %d but got response for %d:", MBcmd.Fcode, ADU[CODEidx]); */ + DynStringReplace(rdBuffer, "ASCERR: Function code response exception", 0); + return PROTOCOL_ERROR; + } else { + char temp[65]; + DynStringClear(rdBuffer); + switch (myPriv->MBcmd.Fcode) { + case RdCoil: + byteCnt = myPriv->ADU[8]; + for (i=0; i < byteCnt; i++) { + snprintf(temp, 64, "%d", myPriv->ADU[9+i]); + DynStringConcat(rdBuffer, temp); + DynStringConcatChar(rdBuffer, ' '); + } + break; + case RdHldReg: + byteCnt = myPriv->ADU[8]; + switch (myPriv->MBcmd.Dtype) { + case U16: + for (i=0; i < byteCnt/2; i++) { + ieee[ myPriv->BO[0] ] = myPriv->ADU[9+i*2]; + ieee[ myPriv->BO[1] ] = myPriv->ADU[10+i*2]; + liVal = (ieee[0] << 8) | ieee[1]; + snprintf(temp, 64, "%ld", liVal); + DynStringConcat(rdBuffer, temp); + DynStringConcatChar(rdBuffer, ' '); + } + break; + case U32: + case F32: + for (i=0; i < byteCnt/4; i++) { + ieee[ myPriv->BO[0] ] = myPriv->ADU[9+i*4]; + ieee[ myPriv->BO[1] ] = myPriv->ADU[10+i*4]; + ieee[ myPriv->BO[2] ] = myPriv->ADU[11+i*4]; + ieee[ myPriv->BO[3] ] = myPriv->ADU[12+i*4]; + if (myPriv->MBcmd.Dtype == F32) { + dVal = ieee2double(ieee); + snprintf(temp, 64, "%g", dVal); + } else if (myPriv->MBcmd.Dtype == U32) { + liVal = (ieee[0] << 24) | (ieee[1] << 16) | (ieee[2] << 8) | ieee[3]; + snprintf(temp, 64, "%ld", liVal); + } + DynStringConcat(rdBuffer, temp); + DynStringConcatChar(rdBuffer, ' '); + } + break; + default: + break; + } + break; + case WrCoil: + if (myPriv->ADU[7] == myPriv->MBcmd.Fcode && (myPriv->ADU[8] * 256 | myPriv->ADU[9]) == myPriv->MBcmd.SAddr && + myPriv->ADU[10] == myPriv->MBcmd.MBdata[0]) { + DynStringReplace(rdBuffer, "OK", 0); + } else { + DynStringReplace(rdBuffer, "ASCERR: Response doesn't match set request", 0); + return PROTOCOL_ERROR; + } + break; + case WrMRegs: + if (myPriv->ADU[7] == myPriv->MBcmd.Fcode && (myPriv->ADU[8] * 256 | myPriv->ADU[9]) == myPriv->MBcmd.SAddr && + (myPriv->ADU[10] * 256 | myPriv->ADU[11]) == myPriv->MBcmd.NumWr) { + DynStringReplace(rdBuffer, "OK", 0); + } else { + DynStringReplace(rdBuffer, "ASCERR: Response doesn't match set request", 0); + return PROTOCOL_ERROR; + } + break; + default: + break; + } + } + myPriv->aduLen = 0, myPriv->DatLen = 0, myPriv->RespLen = 0; + memset(myPriv->ADU, 0, ADUSIZE); + return PROTOCOL_COMPLETE; +} + +/* + * Protocol Prepare Output + */ +static int Proto_Prepare(ProtoPrivate *priv, pDynString wrBuffer) +{ + ModbusOutput(&priv->mb_priv, wrBuffer, wrBuffer); + StartIn(&priv->mb_priv); + return PROTOCOL_COMPLETE; +} + +/* + * Protocol receive character - characater by character + */ +static int Proto_RxChar(ProtoPrivate *priv, int rxchar) +{ + enum RX_STATE { + RX_START=0, + RX_TEXT=1, + RX_LAST=2, /* BCC */ + RX_STOP=99 + }; + int iRet; + rxchar &= 0xFF; + iRet = ModbusInput(&priv->mb_priv, rxchar, priv->rxBuffer); + if (iRet == PROTOCOL_COMPLETE) + return PROTOCOL_COMPLETE; + if (iRet == PROTOCOL_CONTINUE) + return PROTOCOL_CONTINUE; + return PROTOCOL_ERROR; +} + +/* + * AsyncProtocol handling + * ====================== + */ + +static void Async_KillPrivate(pAsyncTxn pTxn) +{ + Proto_KillPrivate((ProtoPrivate *) pTxn->proto_private); + pTxn->proto_private = NULL; +} + +/* + * AsyncProtocol Receive Character + */ +static int Async_Rx(pAsyncProtocol p, pAsyncTxn pTxn, int rxchar) { + int iRet, str_len; + ProtoPrivate *priv = (ProtoPrivate *) pTxn->proto_private; + + iRet = Proto_RxChar(priv, rxchar); + /* + * Keep inp_buf and inp_idx up-to-date after each character + */ + str_len = GetDynStringLength(priv->rxBuffer); + if (str_len > pTxn->inp_idx && pTxn->inp_idx < pTxn->inp_len) { + int xfr_len; + char *tgt = &pTxn->inp_buf[pTxn->inp_idx]; + char *loc = &GetCharArray(priv->rxBuffer)[pTxn->inp_idx]; + if (str_len > pTxn->inp_len) + xfr_len = pTxn->inp_len - pTxn->inp_idx; + else + xfr_len = str_len - pTxn->inp_idx; + memcpy(tgt, loc, xfr_len); + pTxn->inp_idx += xfr_len; + } + + if (iRet == PROTOCOL_CONTINUE) { + return 1; /* Keep Going */ + } + + if (iRet == PROTOCOL_ERROR) { + return -1; /* Error condition */ + } + + /* Message Complete */ + return AQU_POP_CMD; +} + +/* + * AsyncProtocol Event callback + */ +static int Async_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) { + if (event == AQU_TIMEOUT) { + /* handle command timeout */ + pTxn->txn_status = ATX_TIMEOUT; + return AQU_POP_CMD; + } + return AQU_POP_CMD; +} + +/* + * AsyncProtocol Prepare Transaction + */ +static int Async_PrepareTxn(pAsyncProtocol p, pAsyncTxn pTxn, const char* cmd, int cmd_len, int rsp_len) { + ProtoPrivate *priv; + priv = makeProtoPrivate(); + if (priv == NULL) { + SICSLogWrite("ERROR: Out of memory in Async_PrepareTxn", eError); + return 0; + } + priv->state = 0; + priv->len = 0; + DynStringConcatBytes(priv->wrBuffer, (char *) cmd, cmd_len); + Proto_Prepare(priv, priv->wrBuffer); + pTxn->out_len = GetDynStringLength(priv->wrBuffer); + pTxn->out_buf = (char*) malloc(pTxn->out_len); + if (pTxn->out_buf == NULL) { + SICSLogPrintf(eError, "ERROR: Out of memory in %s:%s", __FILE__, __FUNCTION__); + Proto_KillPrivate(priv); + return 0; + } + pTxn->proto_private = priv; + pTxn->kill_private = Async_KillPrivate; + memcpy(pTxn->out_buf, GetCharArray(priv->wrBuffer), pTxn->out_len); + + return 1; +} + +/* + * Ascon Protocol handling + * ======================= + */ + +static void Ascon_KillPrivate(void *priv) +{ + Proto_KillPrivate((ProtoPrivate *) priv); +} + +/* + * Ascon Protocol WriteStart + */ +static int Ascon_Prepare(Ascon *a) { + ProtoPrivate *priv = (ProtoPrivate *) a->private; + Proto_Prepare(priv, a->wrBuffer); + a->wrPos = 0; + a->state = AsconWriting; + return 1; +} + +/* + * Ascon Protocol Read Poll + */ +static int Ascon_Rx(Ascon *a) { + int ret, status; + char chr = '\0'; + ProtoPrivate *priv = (ProtoPrivate *) a->private; + + ret = AsconReadChar(a->fd, &chr); + while (ret > 0) { + a->start = DoubleTime(); + status = Proto_RxChar(priv, chr); + if (GetDynStringLength(priv->rxBuffer) > GetDynStringLength(a->rdBuffer)) { + int len_rd = GetDynStringLength(a->rdBuffer); + int len_rx = GetDynStringLength(priv->rxBuffer); + char *loc = &GetCharArray(priv->rxBuffer)[len_rd]; + DynStringConcatBytes(a->rdBuffer, loc, len_rx - len_rd); + } + if (status > 0) { /* Complete */ + a->state = AsconReadDone; + return 1; + } + else if (status < 0) { /* Error */ + AsconError(a, "Protocol Input Error:", status); + /*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */ + a->lastReconnect = DoubleTime(); + return 1; + } + ret = AsconReadChar(a->fd, &chr); + } + if (ret < 0) { + AsconError(a, "AsconReadChar failed:", errno); + return 1; + } + if (a->state != AsconReadDone) { + if (a->timeout > 0) { + if (DoubleTime() - a->start > a->timeout) { + AsconError(a, "read timeout", 0); + a->state = AsconTimeout; + } + } + } + return 1; +} + +/* + * Ascon Protocol Poll Loop + */ +static int AsconProtHandler(Ascon *a) { + ProtoPrivate *priv = (ProtoPrivate *) a->private; + int ret; + + switch(a->state){ + case AsconWriteStart: + ret = Ascon_Prepare(a); + return ret; + case AsconReadStart: + DynStringClear(priv->rxBuffer); + a->start = DoubleTime(); + priv->state = 0; + priv->len = 0; + ret = AsconStdHandler(a); + return ret; + case AsconReading: + ret = Ascon_Rx(a); + return ret; + default: + ret = AsconStdHandler(a); + return ret; + } + return 1; +} + +/* + * Ascon Protocol Connection Init + */ +static int AsconInit(Ascon *a, SConnection *con, int argc, char *argv[]) +{ + int iRet; + ProtoPrivate *priv; + + iRet = AsconStdInit(a, con, argc, argv); + priv = makeProtoPrivate(); + a->private = priv; + a->killPrivate = Ascon_KillPrivate; + return iRet; +} + +static AsyncProtocol *My_Async_Protocol = NULL; +static AsconProtocol *My_Ascon_Protocol = NULL; + +/* + * Protocol Initialisation + */ +void PROTOCOL_INIT(SicsInterp *pSics) { + if (My_Async_Protocol == NULL) { + AsyncProtocol *prot; + prot = AsyncProtocolCreate(pSics, PROTOCOL_NAME, NULL, NULL); + prot->sendCommand = NULL; + prot->handleInput = Async_Rx; + prot->handleEvent = Async_Ev; + prot->prepareTxn = Async_PrepareTxn; + My_Async_Protocol = prot; + } + + if (My_Ascon_Protocol == NULL) { + AsconProtocol *prot; + prot = calloc(sizeof(AsconProtocol), 1); + prot->name = strdup(PROTOCOL_NAME); + prot->init = AsconInit; + prot->handler = AsconProtHandler; + AsconInsertProtocol(prot); + My_Ascon_Protocol = prot; + } +} + diff --git a/site_ansto/hardsup/modbus_asyncprotocol.h b/site_ansto/hardsup/modbus_asyncprotocol.h new file mode 100644 index 00000000..fa1d05c5 --- /dev/null +++ b/site_ansto/hardsup/modbus_asyncprotocol.h @@ -0,0 +1,5 @@ +#ifndef MODBUS_ASYNCPROTOCOL_H +#define MODBUS_ASYNCPROTOCOL_H +#include "sics.h" +void MODBUSInitProtocol(SicsInterp *pSics); +#endif