sicspsi/spss7.c

/*
 * spss7.c
 *
 * This is an interface between SICS and a S7 Siemens SPS speaking the
 * Fetch/Write protocol via TCP/IP. On initialization, the S7 DB area reserved
 * for communication with SICS is read, parsed and nodes created representing the
 * various data items in there. Suitable callbacks allow to write values into the
 * SPS. Communication is via the device serializer and the genbin protocol handler.
 *
 * copyright: see file COPYRIGHT
 *
 *  Created on: Jul 7, 2010
 *      Author: Mark Koennecke
 */
#include <sics.h>
#include <sicsobj.h>
#include <devser.h>
#include <sicshipadaba.h>

#define MAXDATA 65552 /* 64KB + 16 Byte for headers */

#define NOTLOADED -10
#define NOSWITCHES -20

/*----------------------- private data structure --------------*/
typedef struct {
	DevSer *devser;
	int dbnum;
	int dblength;
	int switchdbnum;
	int switchdblength;
}SPSS7, *pSPSS7;
/*------------------------------------------------------------------*/
typedef struct {
	pSPSS7 sps;
	pHdb spsNode;
	char sendData[MAXDATA];
	char replyData[MAXDATA];
	int toSend;
	int toRead;
	char message[132];
	SConnection *pCon; /* only used when writing */
}S7Action, *pS7Action;
/*------------------------------------------------------------------*/
static int S7ActionMatch(void *a1, void *a2)
{
	if (a1 == a2){
		return 1;
	} else {
		return 0;
	}
}
/*------------------------------------------------------------------*/
static void MakeFetchMessage(pS7Action self, int db, int start, int length)
{
	unsigned short num;

	memset(self->sendData,0,MAXDATA);
	memset(self->replyData,0,MAXDATA);
	self->sendData[0] = 'S';
	self->sendData[1] = '5';
	self->sendData[2] = 0x10;
	self->sendData[3] = 0x01;
	self->sendData[4] = 0x03;
	self->sendData[5] = 0x05;
	self->sendData[6] = 0x03;
	self->sendData[7] = 0x08;
	self->sendData[8] = 0x01;
	self->sendData[9] = db;
	self->sendData[6] = 0x03;
	/*
	 * addresses are in words which is 16 bit = 2 bytes
	 */
	num = htons(start/2);
	memcpy(self->sendData+10,&num,2);
	num = htons(length/2);
	memcpy(self->sendData+12,&num,2);
	self->sendData[14] = 0xFF;
	self->sendData[15] = 0x02;
	self->toSend = 16;
	self->toRead = 16 + length;

	sprintf(self->message,"%lx:%d:%lx:%d", (long) self->sendData, self->toSend,
			(long)self->replyData, self->toRead);
}
/*-------------------------------------------------------------------*/
static void MakeWriteMessage(pS7Action self,
			int start, int length, void *data)
{
	unsigned short num;

	memset(self->sendData,0,MAXDATA);
	memset(self->replyData,0,MAXDATA);
	self->sendData[0] = 'S';
	self->sendData[1] = '5';
	self->sendData[2] = 0x10;
	self->sendData[3] = 0x01;
	self->sendData[4] = 0x03;
	self->sendData[5] = 0x03;
	self->sendData[6] = 0x03;
	self->sendData[7] = 0x08;
	self->sendData[8] = 0x01;
	self->sendData[9] = self->sps->switchdbnum;
	self->sendData[6] = 0x03;
	/*
	 * Start is in bytes, length is in words which is 2 bytes....
	 */
	num = htons(start);
	memcpy(self->sendData+10,&num,2);
	num = htons(length/2);
	memcpy(self->sendData+12,&num,2);
	self->sendData[14] = 0xFF;
	self->sendData[15] = 0x02;
	memcpy(self->sendData+16, data, length);
	self->toSend = 16 + length;
	self->toRead = 16;

	sprintf(self->message,"%lx:%d:%lx:%d", (long) self->sendData, self->toSend,
			(long)self->replyData, self->toRead);
}
/*--------------------------------------------------------------------*/
static int decodeString(char *pPtr, char *string, int maxlen)
{
	int fullength, used;
	fullength = (int)pPtr[0];
	used = (int)pPtr[1];
	if(fullength != maxlen || used > maxlen){
		printf("Correcting DB String error\n");
		fullength = maxlen;
		used = maxlen;
	}
	memset(string,0,fullength+1);
	memcpy(string, pPtr+2, used);
	return fullength + 2;
}
/*------------------------------------------------------------------
 * This is the action handler for writing SPS data
 *-------------------------------------------------------------------*/
static char *S7WriteHandler(void *actionData, char *reply, int comerror)
{
	pS7Action self = (pS7Action)actionData;
	char message[132];
	SConnection *pCon = NULL;

	if(reply == NULL){
		return self->message;
	}

	pCon = self->pCon;
	if(self->replyData[8] != 0){
		snprintf(message,132,"ERROR: code %d when writing to S7/%s",
				self->replyData[8], GetHipadabaPath(self->spsNode));
		if(pCon != NULL){
			SCWrite(pCon,message,eError);
			printf("Write Shit %d happened\n", self->replyData[8]);
		} else {
			printf("%s\n", message);
		}
	} else {
		if(pCon != NULL){
			SCSendOK(pCon);
		}
	}

	return NULL;
}
/*--------------------------------------------------------------------*/
static void UpdateSPSDataBase(pS7Action self)
{
	char *pPtr;
    char name[15], unit[9], description[25], reference[11], error[50], alarms[10];
    unsigned char type, alarm;
    unsigned short val;
    int ival;
    unsigned char bval, b2;
    float fval;
    pHdb node = NULL;
    hdbValue hdbVal;

    pPtr = self->replyData + 16 + 2;
	while(pPtr - self->replyData < self->toRead){
		type = pPtr[0];
		alarm = pPtr[1];
		pPtr+= 2;
		pPtr += decodeString(pPtr, name,14);
		pPtr += decodeString(pPtr, unit,8);
		pPtr += decodeString(pPtr, description,24);
		pPtr += decodeString(pPtr, reference,10);
		node = GetHipadabaNode(self->spsNode,name);
		if(node == NULL){
			printf("Something very fishy is happening here: did Roman change the SPS layout under our feet?\n");
			continue;
		}
		switch (type) {
		case 1:
		case 4:
			memcpy(&bval,pPtr,1);
			/*
			memcpy(&b2,pPtr+1,1);
			printf("Bytes: %d %d\n", bval, b2);
			*/
			hdbVal = MakeHdbInt(bval);
			UpdateHipadabaPar(node,hdbVal,NULL);
			pPtr += 2;
			break;
		case 2:
			memcpy(&ival,pPtr,4);
			hdbVal = MakeHdbInt(ntohl(ival));
			UpdateHipadabaPar(node,hdbVal,NULL);
			pPtr += 4;
			break;
		case 3:
			memcpy(&ival,pPtr+2,4);
			ival = htonl(ival);
			memcpy(&fval,&ival,4);
			hdbVal = MakeHdbFloat(fval);
			UpdateHipadabaPar(node,hdbVal,NULL);
			pPtr += 6;
			break;
		}
		if(alarm == 0){
			SetHdbProperty(node,"geterror", NULL);
		} else {
			snprintf(error,50,"Alarm %d on par",alarm);
			SetHdbProperty(node,"geterror", error);
		}
		snprintf(alarms,sizeof(alarms),"%d",alarm);
		SetHdbProperty(node,"alarm", alarms);
	}
}
/*------------------------------------------------------------------
 * This is the action handler for updating the SPS data in SICS
 *-------------------------------------------------------------------*/
static char *S7UpdateHandler(void *actionData, char *reply, int comerror)
{
	pS7Action self = (pS7Action)actionData;

	if(reply == NULL){
		if(self->sps->dblength > 0){
		  MakeFetchMessage(self,self->sps->dbnum, 0,self->sps->dblength);
		  return self->message;
		} else {
			return NULL;
		}
	}

	UpdateSPSDataBase(self);
	return NULL;
}
/*-------------------------------------------------------------------*/
static hdbCallbackReturn S7WriteCallback(pHdb currentNode,
		void *userData, pHdbMessage message)
{
	pS7Action writeAction = NULL, updateAction = NULL;
	pSPSS7 self = (pSPSS7)userData;
	pHdbDataMessage mm = NULL;
	SConnection *pCon = NULL;
	int offset, type, length, idata;
	float fval;
	short sdata;
	unsigned char bdata;
	char prop[50];

	mm = GetHdbSetMessage(message);
	if(mm != NULL){
		writeAction = calloc(1,sizeof(S7Action));
		if(writeAction == NULL && mm->callData != NULL){
			pCon = (SConnection *)mm->callData;
			SCWrite(pCon, "ERROR: out of memory in S7WriteCallback", eError);
			return hdbContinue;
		}
		writeAction->sps = self;
		writeAction->pCon = mm->callData;
		writeAction->spsNode = currentNode;
		GetHdbProperty(currentNode,"offset",prop,50);
		offset = atoi(prop);
		GetHdbProperty(currentNode,"type",prop,50);
		type = atoi(prop);
		switch(type){
		case 1:
		case 4:
			sdata = (short)mm->v->v.intValue;
			/* sdata = htons(sdata); */
			MakeWriteMessage(writeAction,offset,2,&sdata);
			break;
		case 2:
			idata = mm->v->v.intValue;
			idata = htonl(idata);
			MakeWriteMessage(writeAction,offset,4,&idata);
			break;
		case 3:
				fval = (float)mm->v->v.doubleValue;
				memcpy(&idata,&fval,4);
				idata = htonl(idata);
				MakeWriteMessage(writeAction,offset,4,&idata);
				break;
		default:
				assert(0);
		}
		DevQueue(self->devser, writeAction, WritePRIO,
				S7WriteHandler, S7ActionMatch, free, NULL);

		updateAction = calloc(1,sizeof(S7Action));
		if(updateAction != NULL){
			updateAction->sps = self;
			updateAction->spsNode = currentNode->mama->mama;
			DevQueue(self->devser, updateAction, ProgressPRIO,
					S7UpdateHandler, S7ActionMatch, free, NULL);
		}
	}

	return hdbContinue;
}
/*--------------------------------------------------------------------*/
static void InitializeSPSDataBase(pS7Action self, pHdb parent)
{
	char *pPtr;
    char name[15], unit[9], description[25], reference[11], num[11], error[50], alarms[10];
    unsigned char type, alarm;
    short val;
    int ival;
    float fval;
    unsigned char bval;
    pHdb node = NULL;
    hdbValue hdbVal;

    pPtr = self->replyData + 16 + 2;
	while(pPtr - self->replyData < self->toRead){
		type = pPtr[0];
		alarm = pPtr[1];
		pPtr+= 2;
		pPtr += decodeString(pPtr, name,14);
		pPtr += decodeString(pPtr, unit,8);
		pPtr += decodeString(pPtr, description,24);
		pPtr += decodeString(pPtr, reference,10);
		switch (type) {
		case 1:
		case 4:
			node = MakeHipadabaNode(name,HIPINT,1);
			memcpy(&bval,pPtr+1,1);
			hdbVal = MakeHdbInt(bval);
			UpdateHipadabaPar(node,hdbVal,NULL);
			snprintf(num,10,"%ld", (long)(pPtr - self->replyData - 16));
			SetHdbProperty(node,"offset",num);
			pPtr += 2;
			break;
		case 2:
			node = MakeHipadabaNode(name,HIPINT,1);
			memcpy(&ival,pPtr,4);
			hdbVal = MakeHdbInt(ntohl(ival));
			UpdateHipadabaPar(node,hdbVal,NULL);
			snprintf(num,10,"%ld", (long)(pPtr - self->replyData - 16));
			SetHdbProperty(node,"offset",num);
			pPtr += 4;
			break;
		case 3:
			node = MakeHipadabaNode(name,HIPFLOAT,1);
			memcpy(&ival,pPtr+2,4);
			ival = htonl(ival);
			memcpy(&fval,&ival,4);
			hdbVal = MakeHdbFloat(fval);
			UpdateHipadabaPar(node,hdbVal,NULL);
			snprintf(num,10,"%ld", (long)(pPtr - self->replyData + 2 - 16));
			SetHdbProperty(node,"offset",num);
			pPtr += 6;
			break;
		}
		snprintf(num,10,"%d",type);
		SetHdbProperty(node,"type", num);
		if(alarm != 0){
			snprintf(error,10,"Alarm %d on par",alarm);
			SetHdbProperty(node,"geterror", error);
		}
		snprintf(alarms,sizeof(alarms),"%d",alarm);
		SetHdbProperty(node,"alarm", alarms);
		SetHdbProperty(node,"unit", unit);
		SetHdbProperty(node,"description", description);
		SetHdbProperty(node,"reference", reference);
		AddHipadabaChild(parent,node, NULL);
		printf("Found parameter %s\n", name);
	}
}
/*------------------------------------------------------------------
 * This is the action handler for doing the initialisation of
 * The SPS
 *-------------------------------------------------------------------*/
static char *S7InitHandler(void *actionData, char *reply, int comerror)
{
	pS7Action self = (pS7Action)actionData;
	short dblength;
	pHdb node = NULL;

	/*
	 * Start: read length of the database
	 */
	if(reply == NULL){
		self->sps->dblength  = 0;
		MakeFetchMessage(self,self->sps->dbnum, 0,2);
		return self->message;
	}

	/*
	 * we are reading the database length
	 */
	if(self->sps->dblength == 0){
		memcpy(&dblength, self->replyData+16,2);
		self->sps->dblength = ntohs(dblength);
		MakeFetchMessage(self,self->sps->dbnum, 0,ntohs(dblength));
		return self->message;
	} else {
		memcpy(&dblength, self->replyData+16,2);
		dblength = ntohs(dblength);
		InitializeSPSDataBase(self,self->spsNode);
		node = GetHipadabaNode(self->spsNode,"init");
		if(node != NULL){
			UpdateHipadabaPar(node,MakeHdbInt(1), NULL);
		}
		return NULL;
	}
}
/*-------------------------------------------------------------------------*/
static void InitializeSwitches(pS7Action self)
{
	pHdb switches = NULL, node = NULL;
	int i;
	char name[20];

	switches = GetHipadabaNode(self->spsNode,"switches");
	assert(switches != NULL);

	/*
	 * TODO: Add write callbacks to switches
	 */
	node = switches->child;
	while(node != NULL){
		AppendHipadabaCallback(node,
			MakeHipadabaCallback(S7WriteCallback,self->sps,NULL));
		node = node->next;
	}

	node = GetHipadabaNode(self->spsNode,"init");
	if(node != NULL){
		UpdateHipadabaPar(node,MakeHdbInt(1), NULL);
	}
}
/*-------------------------------------------------------------------------*/
static char *S7InitSwitchHandler(void *actionData, char *reply, int comerror)
{
	pS7Action self = (pS7Action)actionData;
	short dblength;
    pHdb parent = NULL;

	/*
	 * Start: read length of the database
	 */
	if(reply == NULL && self->sps->switchdblength == NOTLOADED){
		MakeFetchMessage(self,self->sps->switchdbnum, 0,2);
		return self->message;
	}

	/*
	 * we are reading the database length
	 */
	if(self->sps->switchdblength == NOTLOADED){
		memcpy(&dblength, self->replyData+16,2);
		self->sps->switchdblength = ntohs(dblength);
		if(self->sps->switchdblength == 0){
		  self->sps->switchdblength = NOSWITCHES;
		  return NULL;
		}
		MakeFetchMessage(self,self->sps->switchdbnum,
				0,ntohs(dblength));
		return self->message;
	} else {
		parent = GetHipadabaNode(self->spsNode,"switches");
		assert(parent != NULL);
		InitializeSPSDataBase(self,parent);
		InitializeSwitches(self);
		return NULL;
	}
}
/*-------------------------------------------------------------------*/
static void KillS7Action(void *data)
{
  if(data != NULL){
    free(data);
  }
}
/*===================================================================*/
static int S7UpdateCmd(pSICSOBJ ccmd, SConnection * con,
                        Hdb * cmdNode, Hdb * par[], int nPar)
{
	pSPSS7 self = (pSPSS7)ccmd->pPrivate;
	pS7Action updateAction = NULL;

	updateAction = calloc(1,sizeof(S7Action));
	if(updateAction == NULL){
		SCWrite(con,"ERROR: out of memory in S7 update", eError);
		return 0;
	}
	updateAction->sps = self;
	updateAction->spsNode = ccmd->objectNode;
	DevQueue(self->devser, updateAction, ProgressPRIO,
			S7UpdateHandler, S7ActionMatch, KillS7Action, NULL);
	SCSendOK(con);
	return 1;
}
/*---------------------------------------------------------------------*/
static void KillSPSS7(void *data)
{
	pSPSS7 self = (pSPSS7)data;
	if(self == NULL){
		return;
	}
	if(self->devser != NULL){
		DevKill(self->devser);
	}
	free(self);
}
/*--------------------------------------------------------------------*/
int MakeSPSS7(SConnection * con, SicsInterp * sics,
                             void *object, int argc, char *argv[])
{
	pSPSS7 self = NULL;
	pSICSOBJ pNew = NULL;
	char *devArgs[3];
	pS7Action initAction = NULL, updateAction = NULL,
			initSwitchAction = NULL;

	int status;

	if(argc < 5){
		SCWrite(con,"ERROR: not enough arguments for MakeSPSS7", eError);
		return 0;
	}

	self = calloc(1,sizeof(SPSS7));
	if(self == NULL){
		SCWrite(con,"ERROR: out of memory in MakeSPSS7", eError);
		return 0;
	}

	devArgs[0] = strdup("genbin");
	devArgs[1] = strdup(argv[4]);

	self->devser = DevMake(con,2,devArgs);
	if(self->devser == NULL){
		return 0;
	}
	free(devArgs[0]);
	free(devArgs[1]);

	self->dbnum = atoi(argv[2]);
	self->switchdbnum = atoi(argv[3]);
	self->switchdblength = NOTLOADED;

	pNew = MakeSICSOBJv(argv[1],"SPS-S7", HIPNONE, usInternal);
	if(pNew == NULL){
		SCWrite(con,"ERROR: out of memory in MakeSPSS7", eError);
		return 0;
	}
	pNew->pPrivate = self;
	pNew->KillPrivate = KillSPSS7;

	status = AddCommand(sics,
	            argv[1], InterInvokeSICSOBJ, KillSICSOBJ, pNew);
	if (status != 1) {
		KillSICSOBJ(pNew);
	    SCPrintf(con, eError, "ERROR: failed create duplicate command %s",
	             argv[1]);
	    return 0;
	}

	AddSICSHdbPar(pNew->objectNode,
	                      "update", usSpy, MakeSICSFunc(S7UpdateCmd));

	AddHipadabaChild(pNew->objectNode,
				MakeSICSROPar("init",MakeHdbInt(0)),NULL);

	AddHipadabaChild(pNew->objectNode,
			MakeHipadabaNode("switches",HIPNONE,0),NULL);


	initAction = calloc(1,sizeof(S7Action));
	initSwitchAction = calloc(1,sizeof(S7Action));
	updateAction = calloc(1,sizeof(S7Action));
	if(initAction == NULL || updateAction == NULL || initSwitchAction == NULL){
		SCWrite(con,"ERROR: out of memory in MakeSPSS7", eError);
		return 0;
	}
	initAction->sps = self;
	initAction->spsNode = pNew->objectNode;
	DevQueue(self->devser, initAction, WritePRIO,
			S7InitHandler, S7ActionMatch, free, NULL);

	initSwitchAction->sps = self;
	initSwitchAction->spsNode = pNew->objectNode;
	DevQueue(self->devser, initSwitchAction, WritePRIO,
			S7InitSwitchHandler, S7ActionMatch, KillS7Action, NULL);

	updateAction->sps = self;
	updateAction->spsNode = pNew->objectNode;
	DevSchedule(self->devser, updateAction,
			ReadPRIO, 60.,
			S7UpdateHandler, S7ActionMatch, KillS7Action, NULL);

	return 1;
}