From 1079f6e702c761f77f895f08fc41e51237bf0d2b Mon Sep 17 00:00:00 2001 From: Ferdi Franceschini Date: Wed, 13 Mar 2013 19:06:57 +1100 Subject: [PATCH] Always return 0 from the AsconReading state to make sure we exit the AsconTask(). Since the PSI update the AsconTask() function will loop forever unless you return 0. --- site_ansto/hardsup/sct_tcpmodbus.c | 199 ++++++++++++++--------------- 1 file changed, 95 insertions(+), 104 deletions(-) diff --git a/site_ansto/hardsup/sct_tcpmodbus.c b/site_ansto/hardsup/sct_tcpmodbus.c index ab981991..14804676 100644 --- a/site_ansto/hardsup/sct_tcpmodbus.c +++ b/site_ansto/hardsup/sct_tcpmodbus.c @@ -280,132 +280,123 @@ static int aduLen = 0, RespLen = 0, pduLen = 0, DatLen = 0; static const int TIDidx=0, PIDidx=2, LENidx=4, UIDidx=6, CODEidx=7, DATLENidx=8, DATAidx=9; static unsigned char ADU[ADUSIZE]; /* Allows upto 8 bytes if data */ int TCPMBReading(Ascon *a) { - const int MBAPLen = 7; - int i, ret, rlen, byteCnt, allRead = 0; + const int MBAPLen = 7, UIDpos=6; + int i, ret, rlen, byteCnt; char chr, temp[64], errMsg[ERRLEN]; unsigned char ieee[4]; double dval; long int lival; ret = AsconReadChar(a->fd, &chr); - /* TODO Check for timeout while reading and call AsconError() - Eg if (DoubleTime() > a->lastReconnect + a->reconnectInterval) then AsconError - */ while (ret > 0) { a->start = DoubleTime(); ADU[aduLen++] = chr; ret = AsconReadChar(a->fd, &chr); } - if (ret < 0) { - AsconError(a, "AsconReadChar failed:", errno); - return 1; - } + if (RespLen == 0 && aduLen > CODEidx) { RespLen = (ADU[LENidx] << 8) + ADU[LENidx+1]; } - if (allRead == 0 && aduLen >= (6 + RespLen)) { - /* all of response has been read */ - /* FIXME If the Length field (ie RespLen) is wrong then we loop forever waiting for a response. - Can this be detected automatically (eg timeout) and reset? - */ - allRead = 1; - } - if (allRead) { - if (ADU[CODEidx] > 0x80) { - a->state = AsconReadDone; - snprintf(errMsg, ERRLEN, "TCPMODBUS: Function code %d exception %d:", ADU[CODEidx] & 0x0F, ADU[8] &0x0F); - AsconError(a, errMsg, 0); - /*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */ - a->lastReconnect = DoubleTime(); - } else if (ADU[CODEidx] != MBcmd.Fcode) { - a->state = AsconReadDone; - snprintf(errMsg, ERRLEN, "TCPMODBUS: Requested function %d but got response for %d:", MBcmd.Fcode, ADU[CODEidx]); - AsconError(a, errMsg, 0); - /*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */ - a->lastReconnect = DoubleTime(); - } else { - DynStringClear(a->rdBuffer); - switch (MBcmd.Fcode) { - case RdCoil: - byteCnt = ADU[8]; - for (i=0; i < byteCnt; i++) { - rlen = snprintf(temp, 64, "%ld", ADU[9+i]); - DynStringConcat(a->rdBuffer, temp); - DynStringConcatChar(a->rdBuffer, ' '); - } - a->state = AsconReadDone; - break; - case RdHldReg: - byteCnt = ADU[8]; - switch (MBcmd.Dtype) { - case U16: - for (i=0; i < byteCnt/2; i++) { - ieee[ BO[0] ] = ADU[9+i*2]; - ieee[ BO[1] ] = ADU[10+i*2]; - lival = (ieee[0] << 8) | ieee[1]; - rlen = snprintf(temp, 64, "%ld", lival); - DynStringConcat(a->rdBuffer, temp); - DynStringConcatChar(a->rdBuffer, ' '); - } - break; - case U32: - case F32: - for (i=0; i < byteCnt/4; i++) { - ieee[ BO[0] ] = ADU[9+i*4]; - ieee[ BO[1] ] = ADU[10+i*4]; - ieee[ BO[2] ] = ADU[11+i*4]; - ieee[ BO[3] ] = ADU[12+i*4]; - if (MBcmd.Dtype == F32) { - dval = ieee2double(ieee); - rlen = snprintf(temp, 64, "%g", dval); - } else if (MBcmd.Dtype == U32) { - lival = (ieee[0] << 24) | (ieee[1] << 16) | (ieee[2] << 8) | ieee[3]; - rlen = snprintf(temp, 64, "%ld", lival); - } - DynStringConcat(a->rdBuffer, temp); - DynStringConcatChar(a->rdBuffer, ' '); - } - break; - default: - break; - } - a->state = AsconReadDone; - break; - case WrCoil: - if (ADU[7] == MBcmd.Fcode && (ADU[8] << 8 | ADU[9]) == MBcmd.SAddr && - ADU[10] == MBcmd.MBdata[0]) { - DynStringReplace(a->rdBuffer, "OK", 0); - } else { - DynStringReplace(a->rdBuffer, "ASCERR: Response doesn't match set request", 0); - } - a->state = AsconReadDone; - break; - case WrMRegs: - if (ADU[7] == MBcmd.Fcode && (ADU[8] << 8 | ADU[9]) == MBcmd.SAddr && - (ADU[10] << 8 | ADU[11]) == MBcmd.NumWr) { - DynStringReplace(a->rdBuffer, "OK", 0); - } else { - DynStringReplace(a->rdBuffer, "ASCERR: Response doesn't match set request", 0); - } - a->state = AsconReadDone; - break; - default: - break; - } - } - aduLen = 0, DatLen = 0, RespLen = 0; - memset(ADU, 0, ADUSIZE); - if (a->state != AsconReadDone) { + + if (aduLen < (UIDpos + RespLen)) { + if (ret == 0) { if (a->timeout > 0) { if (DoubleTime() - a->start > a->timeout) { AsconError(a, "read timeout", 0); a->state = AsconTimeout; } } + } else if (ret < 0) { + AsconError(a, "AsconReadChar failed:", errno); } - + return 0; } - return 1; + + if (ADU[CODEidx] > 0x80) { + a->state = AsconReadDone; + snprintf(errMsg, ERRLEN, "TCPMODBUS: Function code %d exception %d:", ADU[CODEidx] & 0x0F, ADU[8] &0x0F); + AsconError(a, errMsg, 0); + /*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */ + a->lastReconnect = DoubleTime(); + } else if (ADU[CODEidx] != MBcmd.Fcode) { + a->state = AsconReadDone; + snprintf(errMsg, ERRLEN, "TCPMODBUS: Requested function %d but got response for %d:", MBcmd.Fcode, ADU[CODEidx]); + AsconError(a, errMsg, 0); + /*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */ + a->lastReconnect = DoubleTime(); + } else { + DynStringClear(a->rdBuffer); + switch (MBcmd.Fcode) { + case RdCoil: + byteCnt = ADU[8]; + for (i=0; i < byteCnt; i++) { + rlen = snprintf(temp, 64, "%ld", ADU[9+i]); + DynStringConcat(a->rdBuffer, temp); + DynStringConcatChar(a->rdBuffer, ' '); + } + a->state = AsconReadDone; + break; + case RdHldReg: + byteCnt = ADU[8]; + switch (MBcmd.Dtype) { + case U16: + for (i=0; i < byteCnt/2; i++) { + ieee[ BO[0] ] = ADU[9+i*2]; + ieee[ BO[1] ] = ADU[10+i*2]; + lival = (ieee[0] << 8) | ieee[1]; + rlen = snprintf(temp, 64, "%ld", lival); + DynStringConcat(a->rdBuffer, temp); + DynStringConcatChar(a->rdBuffer, ' '); + } + break; + case U32: + case F32: + for (i=0; i < byteCnt/4; i++) { + ieee[ BO[0] ] = ADU[9+i*4]; + ieee[ BO[1] ] = ADU[10+i*4]; + ieee[ BO[2] ] = ADU[11+i*4]; + ieee[ BO[3] ] = ADU[12+i*4]; + if (MBcmd.Dtype == F32) { + dval = ieee2double(ieee); + rlen = snprintf(temp, 64, "%g", dval); + } else if (MBcmd.Dtype == U32) { + lival = (ieee[0] << 24) | (ieee[1] << 16) | (ieee[2] << 8) | ieee[3]; + rlen = snprintf(temp, 64, "%ld", lival); + } + DynStringConcat(a->rdBuffer, temp); + DynStringConcatChar(a->rdBuffer, ' '); + } + break; + default: + break; + } + a->state = AsconReadDone; + break; + case WrCoil: + if (ADU[7] == MBcmd.Fcode && (ADU[8] << 8 | ADU[9]) == MBcmd.SAddr && + ADU[10] == MBcmd.MBdata[0]) { + DynStringReplace(a->rdBuffer, "OK", 0); + } else { + DynStringReplace(a->rdBuffer, "ASCERR: Response doesn't match set request", 0); + } + a->state = AsconReadDone; + break; + case WrMRegs: + if (ADU[7] == MBcmd.Fcode && (ADU[8] << 8 | ADU[9]) == MBcmd.SAddr && + (ADU[10] << 8 | ADU[11]) == MBcmd.NumWr) { + DynStringReplace(a->rdBuffer, "OK", 0); + } else { + DynStringReplace(a->rdBuffer, "ASCERR: Response doesn't match set request", 0); + } + a->state = AsconReadDone; + break; + default: + break; + } + } + aduLen = 0, DatLen = 0, RespLen = 0; + memset(ADU, 0, ADUSIZE); + return 0; } int TCPMBUtil(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) {