478 lines
13 KiB
C
478 lines
13 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"
|
|
|
|
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)
|
|
|
|
typedef struct __SafetyPLCController SafetyPLCController, *pSafetyPLCController;
|
|
|
|
struct __SafetyPLCController {
|
|
pObjectDescriptor pDes;
|
|
pMultiChan mcc; /* associated MultiChan object */
|
|
int iGetOut;
|
|
int iValue;
|
|
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_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;
|
|
|
|
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 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;
|
|
unsigned int iRead;
|
|
pSafetyPLCController self = (pSafetyPLCController) ctx;
|
|
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;
|
|
|
|
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_Print(SConnection *pCon, SicsInterp *pSics,
|
|
void *pData, char *name, char *param)
|
|
{
|
|
char line[132];
|
|
pSafetyPLCController self = (pSafetyPLCController) pData;
|
|
if (strcasecmp(param, "key") == 0) {
|
|
char* state = "unknown(low)";
|
|
if ((self->iValue & KEY_BOTH_BITS) == KEY_BOTH_BITS)
|
|
state = "invalid(high)";
|
|
else if (self->iValue & KEY_ENABLED_BIT)
|
|
state = "enabled";
|
|
else if (self->iValue & KEY_DISABLED_BIT)
|
|
state = "disabled";
|
|
snprintf(line, 132, "%s.Key = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "secondary") == 0) {
|
|
char* state = "unknown(low)";
|
|
if ((self->iValue & SEC_BOTH_BITS) == SEC_BOTH_BITS)
|
|
state = "invalid(high)";
|
|
if (self->iValue & SEC_OPENED_BIT)
|
|
state = "opened";
|
|
else if (self->iValue & SEC_CLOSED_BIT)
|
|
state = "closed";
|
|
snprintf(line, 132, "%s.Secondary = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "tertiary") == 0) {
|
|
char* state = "unknown(low)";
|
|
if ((self->iValue & TER_BOTH_BITS) == TER_BOTH_BITS)
|
|
state = "invalid(high)";
|
|
if (self->iValue & TER_OPENED_BIT)
|
|
state = "opened";
|
|
else if (self->iValue & TER_CLOSED_BIT)
|
|
state = "closed";
|
|
snprintf(line, 132, "%s.Tertiary = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "motioncontrol") == 0) {
|
|
char* state = "unknown(low)";
|
|
if ((self->iValue & MOTOR_BOTH_BITS) == MOTOR_BOTH_BITS)
|
|
state = "invalid(high)";
|
|
else if (self->iValue & MOTOR_ENABLED_BIT)
|
|
state = "enabled";
|
|
else if (self->iValue & MOTOR_DISABLED_BIT)
|
|
state = "disabled";
|
|
snprintf(line, 132, "%s.MotionControl = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "access") == 0) {
|
|
char* state = "unknown(low)";
|
|
if ((self->iValue & ACCESS_BOTH_BITS) == ACCESS_BOTH_BITS)
|
|
state = "invalid(high)";
|
|
else if (self->iValue & ACCESS_LOCKED_BIT)
|
|
state = "locked";
|
|
else if (self->iValue & ACCESS_UNLOCKED_BIT)
|
|
state = "unlocked";
|
|
snprintf(line, 132, "%s.Access = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "dc") == 0) {
|
|
char* state = "false";
|
|
if (self->iValue & DC_POWEROK_BIT)
|
|
state = "true";
|
|
snprintf(line, 132, "%s.DC = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "exit") == 0) {
|
|
char* state = "false";
|
|
if (self->iValue & EXIT_INPROGRESS_BIT)
|
|
state = "true";
|
|
snprintf(line, 132, "%s.Exit = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "trip") == 0) {
|
|
char* state = "false";
|
|
if (self->iValue & SAFETY_TRIPPED_BIT)
|
|
state = "true";
|
|
snprintf(line, 132, "%s.Trip = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "fault") == 0) {
|
|
char* state = "false";
|
|
if (self->iValue & SAFETY_MALFUNCTION_BIT)
|
|
state = "true";
|
|
snprintf(line, 132, "%s.Fault = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "operate") == 0) {
|
|
char* state = "false";
|
|
if (self->iValue & TER_OPERATE_BIT)
|
|
state = "true";
|
|
snprintf(line, 132, "%s.Operate = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "relay") == 0) {
|
|
char* state = "false";
|
|
if (self->iValue & RELAY_ENABLED_BIT)
|
|
state = "true";
|
|
snprintf(line, 132, "%s.Relay = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
if (strcasecmp(param, "ready") == 0) {
|
|
char* state = "false";
|
|
if (self->iValue & INST_READY_BIT)
|
|
state = "true";
|
|
snprintf(line, 132, "%s.Ready = %s", name, state);
|
|
SCWrite(pCon, line, eStatus);
|
|
return OKOK;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
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 (strcmp(argv[1], "list") == 0) {
|
|
PLC_Print(pCon, pSics, pData, argv[0], "key");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "secondary");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "tertiary");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "motioncontrol");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "access");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "dc");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "exit");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "trip");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "fault");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "operate");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "relay");
|
|
PLC_Print(pCon, pSics, pData, argv[0], "ready");
|
|
return OKOK;
|
|
}
|
|
if (PLC_Print(pCon, pSics, pData, argv[0], argv[1]))
|
|
return OKOK;
|
|
}
|
|
/* TODO: handle private stuff */
|
|
return MultiChanAction(pCon, pSics, self->mcc, argc, argv);
|
|
}
|
|
|
|
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=30000;
|
|
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;
|
|
char pError[256];
|
|
|
|
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);
|
|
}
|
|
|
|
/*
|
|
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;
|
|
}
|