511 lines
14 KiB
C
511 lines
14 KiB
C
/** @file TCP Modbus protocol handler for script-context based controllers.
|
|
*
|
|
*/
|
|
#include <errno.h>
|
|
#include <ascon.h>
|
|
#include <ascon.i>
|
|
#include <dynstring.h>
|
|
#include <stdbool.h>
|
|
#include <math.h>
|
|
#include <fupa.h>
|
|
|
|
#define ERRLEN 128
|
|
static int dbgprintf(char* fmtstr, ...);
|
|
static int dbgprintx(const char* msg, const unsigned char* cp, int blen);
|
|
static unsigned int debug_modbus = 1;
|
|
|
|
/*-------------------------------------------------------------------------*/
|
|
/* IEEE code copied from psi/modbus.c */
|
|
static void double2ieee(double input, 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);
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
|
|
|
|
/*
|
|
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)
|
|
*/
|
|
enum mbDtypes {U16, U32, F32};
|
|
struct cmdPar {
|
|
unsigned int MBAddr, Fcode, SAddr, NumVal, NumBytes, NumRd, 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 };
|
|
static struct cmdPar MBcmd;
|
|
static int BO[4];
|
|
|
|
/** @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]
|
|
*/
|
|
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;
|
|
}
|
|
}
|
|
/** @brief encode modbus request
|
|
*/
|
|
int TCPMBWriteStart(Ascon *a) {
|
|
int ADUlen, PDUlenPlusUID;
|
|
unsigned int i, j;
|
|
char *cmdLine;
|
|
unsigned char ADU[32], ieee[4];
|
|
|
|
|
|
cmdLine = GetCharArray(a->wrBuffer);
|
|
for (i=0; i < 4; i++) BO[i] = i;
|
|
parseCmd(cmdLine, &MBcmd, BO);
|
|
|
|
ADU[0] = 0;
|
|
ADU[1] = 0;
|
|
ADU[2] = 0;
|
|
ADU[3] = 0;
|
|
ADU[6] = MBcmd.MBAddr;
|
|
ADU[7] = MBcmd.Fcode;
|
|
switch (MBcmd.Fcode) {
|
|
case RdCoil:
|
|
case RdHldReg:
|
|
/* TODO Max NumRd = 125 */
|
|
ADU[8] = (MBcmd.SAddr & 0xFF00) >> 8;
|
|
ADU[9] = MBcmd.SAddr & 0xFF;
|
|
ADU[10] = (MBcmd.NumRd & 0xFF00) >> 8;
|
|
ADU[11] = MBcmd.NumRd & 0xFF;
|
|
ADUlen = 12;
|
|
break;
|
|
case WrCoil:
|
|
ADU[8] = (MBcmd.SAddr & 0xFF00) >> 8;
|
|
ADU[9] = MBcmd.SAddr & 0xFF;
|
|
ADU[10] = MBcmd.MBdata[0];
|
|
ADU[11] = 0;
|
|
ADUlen = 12;
|
|
break;
|
|
case WrMRegs:
|
|
ADU[8] = (MBcmd.SAddr & 0xFF00) >> 8;
|
|
ADU[9] = MBcmd.SAddr & 0xFF;
|
|
ADU[10] = (MBcmd.NumWr & 0xFF00) >> 8;
|
|
ADU[11] = MBcmd.NumWr & 0xFF;
|
|
ADU[12] = MBcmd.NumBytes;
|
|
switch (MBcmd.Dtype) {
|
|
case U16:
|
|
for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
|
|
ADU[j + BO[0]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF00) >> 8;
|
|
ADU[j + BO[1]] = (unsigned int)MBcmd.MBdata[i] & 0xFF;
|
|
}
|
|
break;
|
|
case U32:
|
|
for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
|
|
ADU[j + BO[0]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF000000) >> 24;
|
|
ADU[j + BO[1]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF0000) >> 16;
|
|
ADU[j + BO[2]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF00) >> 8;
|
|
ADU[j + BO[3]] = (unsigned int)MBcmd.MBdata[i] & 0xFF;
|
|
}
|
|
break;
|
|
case F32:
|
|
for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
|
|
double2ieee(MBcmd.MBdata[i], ieee);
|
|
ADU[j + BO[0]] = ieee[0];
|
|
ADU[j + BO[1]] = ieee[1];
|
|
ADU[j + BO[2]] = ieee[2];
|
|
ADU[j + BO[3]] = ieee[3];
|
|
}
|
|
break;
|
|
}
|
|
ADUlen = 13 + 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(a->wrBuffer);
|
|
DynStringConcatBytes(a->wrBuffer, ADU, ADUlen);
|
|
a->state = AsconWriting;
|
|
a->wrPos = 0;
|
|
return 1;
|
|
}
|
|
|
|
|
|
/** @brief decode modbus response
|
|
*/
|
|
/* ADU[4] = MSB of DatLen, ADU[5] = LSB of DatLen */
|
|
static int aduLen = 0, RespLen = 0, pduLen = 0, DatLen = 0;
|
|
/* NOTE: Min aduLen is 9. The header (7 bytes) + an Error code and Exception code */
|
|
#define ADUSIZE 17
|
|
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, 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);
|
|
while (ret > 0) {
|
|
a->start = DoubleTime();
|
|
ADU[aduLen++] = chr;
|
|
ret = AsconReadChar(a->fd, &chr);
|
|
}
|
|
|
|
if (RespLen == 0 && aduLen > CODEidx) {
|
|
RespLen = (ADU[LENidx] << 8) + ADU[LENidx+1];
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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[]) {
|
|
char cmdLine[512], msg[512];
|
|
unsigned long ul;
|
|
unsigned char ieee[4];
|
|
double dval;
|
|
int iRet, iNumCmds = 3;
|
|
FuPaResult PaRes;
|
|
FuncTemplate CommandTemplate[] = {
|
|
{"help",0,{0,0}},
|
|
{"ieee2double",1,{FUPATEXT}},
|
|
{"double2ieee",1,{FUPAFLOAT}},
|
|
{NULL}
|
|
};
|
|
|
|
Arg2Text(argc, argv, cmdLine, 511);
|
|
iRet = EvaluateFuPa((pFuncTemplate)&CommandTemplate,iNumCmds,argc-1,&argv[1],&PaRes);
|
|
switch(iRet) {
|
|
case 1:
|
|
ul = 0xFFFFFFFF & strtoul(argv[2],NULL,16);
|
|
ieee[3] = ul & 0xFF;
|
|
ieee[2] = (ul & 0xFF00) >> 8;
|
|
ieee[1] = (ul & 0xFF0000) >> 16;
|
|
ieee[0] = (ul & 0xFF000000) >> 24;
|
|
dval = ieee2double(ieee);
|
|
snprintf(msg, 512, "%s -> %f", argv[1], dval);
|
|
SCWrite(pCon, msg, eValue);
|
|
break;
|
|
case 2:
|
|
break;
|
|
case 0:
|
|
default:
|
|
break;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/** @brief Modbus TCP protocol handler.
|
|
* This handler encodes commands and decodes responses
|
|
* for the Modbus TCP protocol
|
|
*/
|
|
int TCPMBProtHandler(Ascon *a) {
|
|
int ret;
|
|
|
|
switch(a->state){
|
|
case AsconWriteStart:
|
|
ret = TCPMBWriteStart(a);
|
|
return ret;
|
|
break;
|
|
case AsconReadStart:
|
|
a->start = DoubleTime();
|
|
ret = AsconStdHandler(a);
|
|
return ret;
|
|
break;
|
|
case AsconReading:
|
|
ret = TCPMBReading(a);
|
|
return ret;
|
|
break;
|
|
default:
|
|
ret = AsconStdHandler(a);
|
|
return ret;
|
|
break;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
void AddTCPMBProtocol(){
|
|
AsconProtocol *prot = NULL;
|
|
|
|
AddCommand(pServ->pSics, "fermi", TCPMBUtil, NULL, NULL);
|
|
prot = calloc(sizeof(AsconProtocol), 1);
|
|
prot->name = strdup("tcpmodbus");
|
|
prot->init = AsconStdInit;
|
|
prot->handler = TCPMBProtHandler;
|
|
AsconInsertProtocol(prot);
|
|
}
|
|
|
|
#include <stdio.h>
|
|
#include <stdarg.h>
|
|
|
|
static int dbgprintf(char* fmtstr, ...) {
|
|
if (debug_modbus > 0) {
|
|
FILE* fp = NULL;
|
|
int ret = 0;
|
|
fp = fopen("/tmp/fermiskfMB.txt", "a");
|
|
if (fp != NULL) {
|
|
va_list ap;
|
|
va_start(ap, fmtstr);
|
|
ret = vfprintf(fp, fmtstr, ap);
|
|
va_end(ap);
|
|
fclose(fp);
|
|
}
|
|
return ret;
|
|
}
|
|
return 0;
|
|
}
|
|
static int dbgprintx(const char* msg, const unsigned char* cp, int blen) {
|
|
if (debug_modbus > 0) {
|
|
char tmp[128];
|
|
int i, j;
|
|
const char hex[] = "0123456789ABCDEF";
|
|
for (i = 0, j = 0; i < blen && j < 126; ++i) {
|
|
tmp[j++] = hex[(cp[i] >> 4) & 0xF];
|
|
tmp[j++] = hex[(cp[i] ) & 0xF];
|
|
}
|
|
tmp[j++] = '\0';
|
|
return dbgprintf("%s: %s\n", msg, tmp);
|
|
}
|
|
return 0;
|
|
}
|