Added Modbus Protocol
This commit is contained in:
Douglas Clowes
@ -1,6 +1,7 @@
|
|||||||
#include "loader_asyncprotocol.h"
|
#include "loader_asyncprotocol.h"
|
||||||
#include "huber_asyncprotocol.h"
|
#include "huber_asyncprotocol.h"
|
||||||
#include "knauer_asyncprotocol.h"
|
#include "knauer_asyncprotocol.h"
|
||||||
|
#include "modbus_asyncprotocol.h"
|
||||||
#include "omron_asyncprotocol.h"
|
#include "omron_asyncprotocol.h"
|
||||||
#include "sics.h"
|
#include "sics.h"
|
||||||
|
|
||||||
@ -8,5 +9,6 @@ void LOADERInitProtocol(SicsInterp *pSics)
|
|||||||
{
|
{
|
||||||
HUBERInitProtocol(pSics);
|
HUBERInitProtocol(pSics);
|
||||||
KNAUERInitProtocol(pSics);
|
KNAUERInitProtocol(pSics);
|
||||||
|
MODBUSInitProtocol(pSics);
|
||||||
OMRONInitProtocol(pSics);
|
OMRONInitProtocol(pSics);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -38,6 +38,7 @@ HOBJ += aqp_opalstatus.o
|
|||||||
HOBJ += omron_asyncprotocol.o
|
HOBJ += omron_asyncprotocol.o
|
||||||
HOBJ += huber_asyncprotocol.o
|
HOBJ += huber_asyncprotocol.o
|
||||||
HOBJ += knauer_asyncprotocol.o
|
HOBJ += knauer_asyncprotocol.o
|
||||||
|
HOBJ += modbus_asyncprotocol.o
|
||||||
HOBJ += loader_asyncprotocol.o
|
HOBJ += loader_asyncprotocol.o
|
||||||
|
|
||||||
libhlib.a: $(HOBJ)
|
libhlib.a: $(HOBJ)
|
||||||
|
|||||||
718
site_ansto/hardsup/modbus_asyncprotocol.c
Normal file
718
site_ansto/hardsup/modbus_asyncprotocol.c
Normal file
@ -0,0 +1,718 @@
|
|||||||
|
#include "modbus_asyncprotocol.h"
|
||||||
|
#include "asyncprotocol.h"
|
||||||
|
#include "asyncqueue.h"
|
||||||
|
#include "ascon.i"
|
||||||
|
#include "dynstring.h"
|
||||||
|
#include <errno.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
5
site_ansto/hardsup/modbus_asyncprotocol.h
Normal file
5
site_ansto/hardsup/modbus_asyncprotocol.h
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
#ifndef MODBUS_ASYNCPROTOCOL_H
|
||||||
|
#define MODBUS_ASYNCPROTOCOL_H
|
||||||
|
#include "sics.h"
|
||||||
|
void MODBUSInitProtocol(SicsInterp *pSics);
|
||||||
|
#endif
|
||||||
Reference in New Issue
Block a user