529 lines
15 KiB
C
529 lines
15 KiB
C
/*
|
|
* S A F E T Y P L C
|
|
*
|
|
* Douglas Clowes, February 2007
|
|
*
|
|
*/
|
|
|
|
#include <netinet/tcp.h>
|
|
#include <sys/time.h>
|
|
#include <sics.h>
|
|
#include "network.h"
|
|
#include "multichan.h"
|
|
#include "nwatch.h"
|
|
#include "safetyplc.h"
|
|
#include "sicsvar.h"
|
|
|
|
extern int DMC2280MotionControl;
|
|
|
|
#define KEY_ENABLED_BIT (1 << 0)
|
|
#define KEY_DISABLED_BIT (1 << 1)
|
|
#define SEC_OPENED_BIT (1 << 2)
|
|
#define SEC_CLOSED_BIT (1 << 3)
|
|
#define TER_OPENED_BIT (1 << 4)
|
|
#define TER_CLOSED_BIT (1 << 5)
|
|
#define MOTOR_ENABLED_BIT (1 << 6)
|
|
#define MOTOR_DISABLED_BIT (1 << 7)
|
|
#define ACCESS_LOCKED_BIT (1 << 8)
|
|
#define ACCESS_UNLOCKED_BIT (1 << 9)
|
|
#define DC_POWEROK_BIT (1 << 10)
|
|
#define EXIT_INPROGRESS_BIT (1 << 11)
|
|
#define SAFETY_TRIPPED_BIT (1 << 12)
|
|
#define SAFETY_MALFUNCTION_BIT (1 << 13)
|
|
#define TER_OPERATE_BIT (1 << 14)
|
|
#define RELAY_ENABLED_BIT (1 << 15)
|
|
#define INST_READY_BIT (1 << 16)
|
|
#define LAMP_TEST_BIT (1 << 17)
|
|
|
|
#define KEY_BOTH_BITS (KEY_ENABLED_BIT | KEY_DISABLED_BIT)
|
|
#define SEC_BOTH_BITS (SEC_OPENED_BIT | SEC_CLOSED_BIT)
|
|
#define TER_BOTH_BITS (TER_OPENED_BIT | TER_CLOSED_BIT)
|
|
#define MOTOR_BOTH_BITS (MOTOR_ENABLED_BIT | MOTOR_DISABLED_BIT)
|
|
#define ACCESS_BOTH_BITS (ACCESS_LOCKED_BIT | ACCESS_UNLOCKED_BIT)
|
|
|
|
int PLC_UserPriv = 0; /* Internal */
|
|
typedef enum {
|
|
Unknown_low, Invalid_high, Enabled, Disabled,
|
|
Opened, Closed, Locked, Unlocked, True, False,}PLC_STATUS;
|
|
|
|
char *plc_states[] = {
|
|
"Unknown_low", "Invalid_high", "Enabled",
|
|
"Disabled", "Opened", "Closed",
|
|
"Locked", "Unlocked", "True", "False"};
|
|
|
|
typedef enum {
|
|
Key,Secondary,Tertiary,MotionControl,Access,
|
|
DC,Exit,Trip,Fault,Operate,Relay,Ready,}PLC_PARAM;
|
|
|
|
char *plc_parname[] = {
|
|
"plc_key","plc_secondary","plc_tertiary","plc_motioncontrol",
|
|
"plc_access","plc_dc","plc_exit","plc_trip",
|
|
"plc_fault","plc_operate","plc_relay","plc_ready"};
|
|
|
|
typedef struct __SafetyPLCController SafetyPLCController, *pSafetyPLCController;
|
|
|
|
struct __SafetyPLCController {
|
|
pObjectDescriptor pDes;
|
|
pMultiChan mcc; /* associated MultiChan object */
|
|
int iGetOut;
|
|
int iValue;
|
|
int oldValue;
|
|
pNWTimer nw_tmr; /* NetWait timer handle */
|
|
int timeout;
|
|
struct timeval tvSend;
|
|
};
|
|
|
|
typedef struct __command Command, *pCommand;
|
|
typedef int (*CommandCallback)(void* ctx, const char* resp, int resp_len);
|
|
|
|
struct __command {
|
|
pSafetyPLCController plc;
|
|
int cstate;
|
|
int lstate;
|
|
char* out_buf;
|
|
int out_len;
|
|
int out_idx;
|
|
char* inp_buf;
|
|
int inp_len;
|
|
int inp_idx;
|
|
CommandCallback func;
|
|
void* cntx;
|
|
};
|
|
|
|
static int PLC_GetState(void *pData, char *param, PLC_STATUS *retState);
|
|
|
|
static int PLC_Tx(void* ctx)
|
|
{
|
|
int iRet = 1;
|
|
pCommand myCmd = (pCommand) ctx;
|
|
|
|
if (myCmd) {
|
|
gettimeofday(&myCmd->plc->tvSend, NULL); /* refresh */
|
|
iRet = MultiChanWrite(myCmd->plc->mcc, myCmd->out_buf, myCmd->out_len);
|
|
/* TODO handle errors */
|
|
if (iRet < 0) { /* TODO: EOF */
|
|
iRet = MultiChanReconnect(myCmd->plc->mcc);
|
|
if (iRet == 0)
|
|
return 0;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
static int PLC_Rx(void* ctx, int rxchar)
|
|
{
|
|
int iRet = 1;
|
|
pCommand myCmd = (pCommand) ctx;
|
|
|
|
if (rxchar == MCC_TIMEOUT) {
|
|
/* TODO: handle command timeout */
|
|
if (myCmd->func)
|
|
iRet = myCmd->func(myCmd->cntx, NULL, MCC_TIMEOUT);
|
|
free(myCmd->out_buf);
|
|
free(myCmd->inp_buf);
|
|
free(myCmd);
|
|
return MCC_POP_CMD;
|
|
}
|
|
|
|
switch (myCmd->cstate) {
|
|
case 0: /* first character */
|
|
/* normal data */
|
|
myCmd->cstate = 1;
|
|
/* note fallthrough */
|
|
case 1: /* receiving reply */
|
|
if (myCmd->inp_idx < myCmd->inp_len)
|
|
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
|
|
if (rxchar == 0x0D)
|
|
myCmd->cstate = 2;
|
|
break;
|
|
case 2: /* received CR and looking for LF */
|
|
if (myCmd->inp_idx < myCmd->inp_len)
|
|
myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
|
|
if (rxchar == 0x0A) {
|
|
myCmd->cstate = 99;
|
|
/* end of line */
|
|
}
|
|
else
|
|
myCmd->cstate = 1;
|
|
break;
|
|
}
|
|
if (myCmd->cstate == 99) {
|
|
myCmd->inp_buf[myCmd->inp_idx] = '\0';
|
|
if (myCmd->func)
|
|
iRet = myCmd->func(myCmd->cntx, myCmd->inp_buf, myCmd->inp_idx);
|
|
else
|
|
iRet = 0;
|
|
myCmd->cstate = 0;
|
|
myCmd->inp_idx = 0;
|
|
}
|
|
if (iRet == 0) { /* end of command */
|
|
free(myCmd->out_buf);
|
|
free(myCmd->inp_buf);
|
|
free(myCmd);
|
|
return MCC_POP_CMD;
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
static int PLC_SendCmd(pSafetyPLCController self,
|
|
char* command, int cmd_len,
|
|
CommandCallback callback, void* context, int rsp_len)
|
|
{
|
|
pCommand myCmd = NULL;
|
|
|
|
assert(self);
|
|
assert(self->mcc);
|
|
myCmd = (pCommand) malloc(sizeof(Command));
|
|
assert(myCmd);
|
|
memset(myCmd, 0, sizeof(Command));
|
|
myCmd->out_buf = (char*) malloc(cmd_len + 5);
|
|
memcpy(myCmd->out_buf, command, cmd_len);
|
|
myCmd->out_len = cmd_len;
|
|
if (myCmd->out_len < 2 ||
|
|
myCmd->out_buf[myCmd->out_len - 1] != 0x0A ||
|
|
myCmd->out_buf[myCmd->out_len - 2] != 0x0D) {
|
|
myCmd->out_buf[myCmd->out_len++] = 0x0D;
|
|
myCmd->out_buf[myCmd->out_len++] = 0x0A;
|
|
}
|
|
myCmd->out_buf[myCmd->out_len] = '\0';
|
|
myCmd->func = callback;
|
|
myCmd->cntx = context;
|
|
if (rsp_len == 0)
|
|
myCmd->inp_buf = NULL;
|
|
else {
|
|
myCmd->inp_buf = malloc(rsp_len + 1);
|
|
memset(myCmd->inp_buf, 0, rsp_len + 1);
|
|
}
|
|
myCmd->inp_len = rsp_len;
|
|
myCmd->plc = self;
|
|
gettimeofday(&self->tvSend, NULL); /* refresh */
|
|
return MultiChanEnque(self->mcc, myCmd, PLC_Tx, PLC_Rx);
|
|
}
|
|
|
|
static void PLC_Notify(void* context, int event)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) context;
|
|
|
|
switch (event) {
|
|
case MCC_RECONNECT:
|
|
do {
|
|
mkChannel* sock = MultiChanGetSocket(self->mcc);
|
|
int flag = 1;
|
|
setsockopt(sock->sockid, /* socket affected */
|
|
IPPROTO_TCP, /* set option at TCP level */
|
|
TCP_NODELAY, /* name of option */
|
|
(char *) &flag, /* the cast is historical cruft */
|
|
sizeof(int)); /* length of option value */
|
|
return;
|
|
} while (0);
|
|
}
|
|
return;
|
|
}
|
|
|
|
/*
|
|
* \brief GetCallback is the callback for the read command.
|
|
*/
|
|
static int GetCallback(void* ctx, const char* resp, int resp_len)
|
|
{
|
|
int iRet,i;
|
|
unsigned int iRead;
|
|
PLC_STATUS plcState;
|
|
pSicsVariable plcVar=NULL;
|
|
|
|
pSafetyPLCController self = (pSafetyPLCController) ctx;
|
|
if (resp_len < 0) {
|
|
DMC2280MotionControl = -1;
|
|
}
|
|
else {
|
|
iRet = sscanf(resp,"READ %x", &iRead);
|
|
if(iRet != 1) { // Not a number, probably an error response
|
|
self->iValue = 0;
|
|
}
|
|
else {
|
|
if ((iRead & LAMP_TEST_BIT) == 0)
|
|
self->iValue = iRead;
|
|
}
|
|
if ((self->iValue & MOTOR_BOTH_BITS) == 0) /* neither */
|
|
DMC2280MotionControl = -1;
|
|
else if ((self->iValue & MOTOR_BOTH_BITS) == MOTOR_BOTH_BITS) /* both */
|
|
DMC2280MotionControl = -1;
|
|
else if ((self->iValue & MOTOR_ENABLED_BIT)) /* enabled */
|
|
DMC2280MotionControl = 1;
|
|
else /* disabled */
|
|
DMC2280MotionControl = 0;
|
|
}
|
|
if (self->oldValue != self->iValue) {
|
|
for (i=0; i < sizeof(plc_parname)/sizeof(plc_parname[0]); i++) {
|
|
plcVar = (pSicsVariable)FindCommandData(pServ->pSics,plc_parname[i],"SicsVariable");
|
|
PLC_GetState(self,plc_parname[i],&plcState);
|
|
VarSetText(plcVar,plc_states[plcState],PLC_UserPriv);
|
|
}
|
|
}
|
|
|
|
self->oldValue = self->iValue;
|
|
self->iGetOut = 0;
|
|
return 0;
|
|
}
|
|
|
|
static int MyTimerCallback(void* context, int mode)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) context;
|
|
if (self->iGetOut) {
|
|
struct timeval now;
|
|
gettimeofday(&now, NULL);
|
|
/* TODO error handling */
|
|
if (((now.tv_sec - self->tvSend.tv_sec) * 1000
|
|
+ (now.tv_usec / 1000)
|
|
- (self->tvSend.tv_usec / 1000)) < self->timeout)
|
|
return 1;
|
|
DMC2280MotionControl = -1;
|
|
}
|
|
self->iGetOut = 1;
|
|
PLC_SendCmd(self, "READ", 4, GetCallback, self, 132);
|
|
return 1;
|
|
}
|
|
|
|
static int PLC_GetState(void *pData, char *param, PLC_STATUS *retState)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) pData;
|
|
if (strcasecmp(param, plc_parname[Key]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & KEY_BOTH_BITS) == KEY_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
else if (self->iValue & KEY_ENABLED_BIT)
|
|
*retState = Enabled;
|
|
else if (self->iValue & KEY_DISABLED_BIT)
|
|
*retState = Disabled;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Secondary]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & SEC_BOTH_BITS) == SEC_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
if (self->iValue & SEC_OPENED_BIT)
|
|
*retState = Opened;
|
|
else if (self->iValue & SEC_CLOSED_BIT)
|
|
*retState = Closed;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Tertiary]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & TER_BOTH_BITS) == TER_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
if (self->iValue & TER_OPENED_BIT)
|
|
*retState = Opened;
|
|
else if (self->iValue & TER_CLOSED_BIT)
|
|
*retState = Closed;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[MotionControl]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & MOTOR_BOTH_BITS) == MOTOR_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
else if (self->iValue & MOTOR_ENABLED_BIT)
|
|
*retState = Enabled;
|
|
else if (self->iValue & MOTOR_DISABLED_BIT)
|
|
*retState = Disabled;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Access]) == 0) {
|
|
*retState = Unknown_low;
|
|
if ((self->iValue & ACCESS_BOTH_BITS) == ACCESS_BOTH_BITS)
|
|
*retState = Invalid_high;
|
|
else if (self->iValue & ACCESS_LOCKED_BIT)
|
|
*retState = Locked;
|
|
else if (self->iValue & ACCESS_UNLOCKED_BIT)
|
|
*retState = Unlocked;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[DC]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & DC_POWEROK_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Exit]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & EXIT_INPROGRESS_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Trip]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & SAFETY_TRIPPED_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Fault]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & SAFETY_MALFUNCTION_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Operate]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & TER_OPERATE_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Relay]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & RELAY_ENABLED_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, plc_parname[Ready]) == 0) {
|
|
*retState = False;
|
|
if (self->iValue & INST_READY_BIT)
|
|
*retState = True;
|
|
return OKOK;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int PLC_Print(SConnection *pCon, SicsInterp *pSics,
|
|
void *pData, char *name, char *param)
|
|
{
|
|
char line[132];
|
|
PLC_STATUS state;
|
|
|
|
if (PLC_GetState(pData, param, &state) != OKOK) {
|
|
return 0;
|
|
} else {
|
|
snprintf(line, 132, "%s.%s = %s", name, param, plc_states[state]);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
}
|
|
|
|
static int PLC_Action(SConnection *pCon, SicsInterp *pSics,
|
|
void *pData, int argc, char *argv[])
|
|
{
|
|
char line[132];
|
|
pSafetyPLCController self = (pSafetyPLCController) pData;
|
|
if (argc == 1) {
|
|
snprintf(line, 132, "%s.iValue = %06X", argv[0], self->iValue & 0xffffff);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
else if (argc == 2) {
|
|
if (strcasecmp(argv[1], "list") == 0) {
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Key]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Secondary]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Tertiary]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[MotionControl]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Access]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[DC]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Exit]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Trip]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Fault]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Operate]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Relay]);
|
|
PLC_Print(pCon, pSics, pData, argv[0], plc_parname[Ready]);
|
|
return OKOK;
|
|
}
|
|
if (PLC_Print(pCon, pSics, pData, argv[0], argv[1]))
|
|
return OKOK;
|
|
} else if (argc == 3) {
|
|
if (strcasecmp(argv[1], "hattach") == 0) {
|
|
}
|
|
}
|
|
snprintf(line, 132, "%s does not understand %s", argv[0], argv[1]);
|
|
SCWrite(pCon, line, eError);
|
|
return 0;
|
|
}
|
|
|
|
static pSafetyPLCController PLC_Create(const char* pName, int port)
|
|
{
|
|
pSafetyPLCController self = NULL;
|
|
|
|
self = (pSafetyPLCController) malloc(sizeof(SafetyPLCController));
|
|
if (self == NULL)
|
|
return NULL;
|
|
memset(self, 0, sizeof(SafetyPLCController));
|
|
if (MultiChanCreate(pName, &self->mcc) == 0) {
|
|
free(self);
|
|
return NULL;
|
|
}
|
|
MultiChanSetNotify(self->mcc, self, PLC_Notify);
|
|
|
|
self->pDes = CreateDescriptor("SafetyPLC");
|
|
return self;
|
|
}
|
|
|
|
static int PLC_Init(pSafetyPLCController self)
|
|
{
|
|
/* TODO: Init the controller */
|
|
if (self->nw_tmr != NULL)
|
|
NetWatchRemoveTimer(self->nw_tmr);
|
|
NetWatchRegisterTimerPeriodic(&self->nw_tmr,
|
|
1000, 100,
|
|
MyTimerCallback,
|
|
self);
|
|
self->timeout=120000; /* huge */
|
|
return 1;
|
|
}
|
|
|
|
static void PLC_Kill(void* pData)
|
|
{
|
|
pSafetyPLCController self = (pSafetyPLCController) pData;
|
|
if (self->nw_tmr)
|
|
NetWatchRemoveTimer(self->nw_tmr);
|
|
free(self);
|
|
return;
|
|
}
|
|
|
|
int SafetyPLCFactory(SConnection *pCon, SicsInterp *pSics,
|
|
void *pData, int argc, char *argv[])
|
|
{
|
|
pSafetyPLCController pNew = NULL;
|
|
int iRet, status, i;
|
|
char pError[256];
|
|
pSicsVariable plcVar=NULL;
|
|
PLC_STATUS plcState;
|
|
|
|
if(argc < 4)
|
|
{
|
|
SCWrite(pCon,"ERROR: insufficient no of arguments to SafetyPLCFactory",
|
|
eError);
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
create data structure and open port
|
|
*/
|
|
pNew = PLC_Create(argv[2], atoi(argv[3]));
|
|
|
|
if(!pNew)
|
|
{
|
|
SCWrite(pCon,"ERROR: failed to create SafetyPLC in SafetyPLCFactory",eError);
|
|
return 0;
|
|
}
|
|
|
|
status = PLC_Init(pNew);
|
|
if(status != 1)
|
|
{
|
|
sprintf(pError,"ERROR: failed to connect to %s",argv[2]);
|
|
SCWrite(pCon,pError,eError);
|
|
}
|
|
|
|
for (i=0; i < sizeof(plc_parname)/sizeof(plc_parname[0]); i++) {
|
|
plcVar = VarCreate(PLC_UserPriv,veText,plc_parname[i]);
|
|
PLC_GetState(pNew,plc_parname[i],&plcState);
|
|
VarSetText(plcVar,plc_states[plcState],PLC_UserPriv);
|
|
AddCommand(pSics,plc_parname[i],VarWrapper,(KillFunc)VarKill,plcVar);
|
|
}
|
|
/*
|
|
create the command
|
|
*/
|
|
iRet = AddCommand(pSics, argv[1], PLC_Action, PLC_Kill, pNew);
|
|
if(!iRet)
|
|
{
|
|
sprintf(pError,"ERROR: duplicate command %s not created", argv[1]);
|
|
SCWrite(pCon,pError,eError);
|
|
PLC_Kill(pNew);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|