diff --git a/astriumnet.c b/astriumnet.c new file mode 100644 index 0000000..878e659 --- /dev/null +++ b/astriumnet.c @@ -0,0 +1,313 @@ +/** + * astriumnet protocoll. This is the protocoll understood by the newest version + * of Astrium choppers with the Beckhoff server backend. There are a number of + * peculaties here: + * + * On connecting there is an authorisation exchange. This, the client, has to send a + * challenge. The server responds with a randam hash code. Then a password is encrypted + * with the hashcode and sent to the server. Who, hopefully, will respond with an AuthOK + * message. + * + * Furtheron the message exchange happens in an XML like format. But implemented in a + * slightly strange way in that everything is done through attributes. + * + * For more information, see the documentation by Astrium. + * + * copyright: see file COPYRIGHT + * + * Mark Koennecke, April 2014 + */ +#include +#include +#include +#include +#include +#include +#include + +extern char *trim(char *); + +typedef struct { + char password[132]; + int authState; + pDynString buffer; +}Astriumnet, *pAstriumnet; + +#define AUTHSEND 4 +#define AUTHSTART 0 +#define WAITHASH 1 +#define WAITOK 2 +#define AUTHOK 3 +/*-----------------------------------------------------------------------*/ +const char *AstriumWS(mxml_node_t* node, int where) +{ + /* + * do not want whitespace in there anywhere + */ + return NULL; +} +/*-----------------------------------------------------------------------*/ +static void encodeXML(Ascon *a) +{ + char *pPtr, *equal, *close; + mxml_node_t *root, *msg, *sys, *param; + char token[132]; + + pPtr = GetCharArray(a->wrBuffer); + root = mxmlNewXML("1.0"); + msg = mxmlNewElement(root,"Msg"); + + /* + * The first element must be the system name + */ + pPtr = stptok(pPtr,token,sizeof(token),":"); + sys = mxmlNewElement(msg,"SysName"); + mxmlNewText(sys,0,token); + + param = mxmlNewElement(msg,"Param"); + while((pPtr = stptok(pPtr,token,sizeof(token),":")) != NULL){ + equal = strchr(token,(int)'='); + if(equal != NULL){ + *equal = '\0'; + equal++; + mxmlElementSetAttr(param,token,equal); + } else { + mxmlElementSetAttr(param,token,NULL); + } + } + + mxmlSetWrapMargin(0); + pPtr = mxmlSaveAllocString(root,AstriumWS); + /* + * Skip the string + */ + close = strchr(pPtr,(int)'>'); + DynStringCopy(a->wrBuffer,"#CRQ#"); + DynStringConcat(a->wrBuffer,trim(close+1)); + free(pPtr); + mxmlDelete(root); +} +/*-----------------------------------------------------------------------*/ +static int decodeXML(Ascon *a) +{ + pDynString xmlBuffer; + mxml_node_t *root, *param; + mxml_attr_t attr; + + int i; + + /* + * prepend the XML prefix to make mxml a happy library + */ + xmlBuffer = CreateDynString(128,128); + DynStringCopy(xmlBuffer,""); + DynStringConcat(xmlBuffer,GetCharArray(a->rdBuffer)+5); + + root = mxmlLoadString(NULL,GetCharArray(xmlBuffer),MXML_TEXT_CALLBACK); + if(root == NULL){ + AsconError(a,"Astrium returned invalid XML",0); + return 0; + } + + DynStringClear(a->rdBuffer); + param = mxmlFindElement(root,root,"Param",NULL,NULL,MXML_DESCEND); + if(param == NULL){ + AsconError(a,"No Param node found",0); + return 0; + } + + for(i = 0; i < param->value.element.num_attrs; i++){ + attr = param->value.element.attrs[i]; + DynStringConcat(a->rdBuffer,attr.name); + if(attr.value != NULL){ + DynStringConcat(a->rdBuffer,"="); + DynStringConcat(a->rdBuffer,attr.value); + } + DynStringConcat(a->rdBuffer,":"); + } + DeleteDynString(xmlBuffer); + mxmlDelete(root); + + return 1; +} +/*-----------------------------------------------------------------------*/ +static long calculateToken(char *password, long hash) +{ + long tmp; + int i; + + tmp = hash ^ 0x80AA80AA; + for(i = 0; i < strlen(password); i++){ + tmp += password[i] * (long)pow(2,i); + tmp -= password[i] * (long)pow(2,31-i); + } + return tmp ^ hash; +} +/*------------------------------------------------------------------------*/ +static int doWaitHash(Ascon *a) +{ + pAstriumnet priv = NULL; + int ret; + char chr; + long hash; + char token[50]; + + priv = (pAstriumnet)a->private; + ret = AsconReadChar(a->fd,&chr); + if(ret < 0){ + AsconError(a, "ASC5", errno); + return 0; + } + if(ret == 1) { + DynStringConcatChar(a->rdBuffer,chr); + } + if(GetDynStringLength(a->rdBuffer) >= 23) { + if(strstr(GetCharArray(a->rdBuffer), "") == NULL){ + AsconError(a,"Invalid response when trying to get hash",0); + return 0; + } + hash = atol(GetCharArray(a->rdBuffer)+6); + hash = calculateToken(priv->password,hash); + DynStringCopy(priv->buffer,""); + snprintf(token,sizeof(token),"%ld",hash); + DynStringConcat(priv->buffer,token); + ret = AsconWriteChars(a->fd, GetCharArray(priv->buffer), + GetDynStringLength(priv->buffer)); + if (ret < 0) { + AsconError(a, "ASC4", errno); /* sets state to AsconFailed */ + return 0; + } + DynStringClear(a->rdBuffer); + priv->authState = WAITOK; + } + return 1; +} +/*------------------------------------------------------------------------*/ +static int doWaitOK(Ascon *a) +{ + pAstriumnet priv = NULL; + int ret; + char chr; + + priv = (pAstriumnet)a->private; + ret = AsconReadChar(a->fd,&chr); + if(ret < 0){ + AsconError(a, "ASC5", errno); + return 0; + } + if(ret == 1) { + DynStringConcatChar(a->rdBuffer,chr); + } + if(strchr(GetCharArray(a->rdBuffer),(int)'>') != NULL) { + if(strstr(GetCharArray(a->rdBuffer),"AuthOK") == NULL){ + AsconError(a,"Token authorisation failed",0); + return 0; + } + priv->authState = AUTHOK; + a->state = AsconConnectDone; + } + return 1; +} + +/*------------------------------------------------------------------------*/ +static int AstriumnetHandler(Ascon *a) +{ + pAstriumnet priv = NULL; + int ret; + char chr; + + priv = (pAstriumnet)a->private; + + switch(a->state){ + case AsconConnectStart: + AsconBaseHandler(a); + if(a->state == AsconConnecting){ + DynStringCopy(priv->buffer,""); + priv->authState = AUTHSEND; + return 1; + } else { + return 0; + } + break; + case AsconConnecting: + if(priv->authState == AUTHSEND){ + ret = AsconWriteChars(a->fd, GetCharArray(priv->buffer), + GetDynStringLength(priv->buffer)); + if (ret < 0) { + if(errno == EINPROGRESS){ + return 1; + } + AsconError(a, "ASC4", errno); /* sets state to AsconFailed */ + return 0; + } + priv->authState = WAITHASH; + DynStringClear(a->rdBuffer); + return 1; + } else if(priv->authState == WAITHASH){ + return doWaitHash(a); + } else if(priv->authState == WAITOK){ + return doWaitOK(a); + } else { + AsconError(a,"Invalid authentication state",0); + return 0; + } + break; + case AsconWriteStart: + encodeXML(a); + a->state = AsconWriting; + return 1; + break; + case AsconReading: + ret = AsconReadChar(a->fd,&chr); + if(ret < 0){ + AsconError(a, "ASC5", errno); + return 0; + } + if(ret == 1) { + DynStringConcatChar(a->rdBuffer,chr); + } + if(strstr(GetCharArray(a->rdBuffer),"") != NULL) { + decodeXML(a); + a->state = AsconReadDone; + } + return 1; + break; + } + return AsconBaseHandler(a); +} +/*------------------------------------------------------------------------*/ +static int AstriumnetInit(Ascon * a, SConnection * con, int argc, char *argv[]) +{ + pAstriumnet priv = NULL; + + if(argc < 3){ + SCPrintf(con,eError,"ERROR: missing password argument to Astriumnet"); + return 0; + } + priv = calloc(sizeof(Astriumnet), 1); + a->fd = -1; + a->state = AsconConnectStart; + a->reconnectInterval = 10; + a->hostport = strdup(argv[1]); + a->private = priv; + a->killPrivate = free; + strncpy(priv->password,argv[2],sizeof(priv->password)); + priv->authState = AUTHSTART; + priv->buffer = CreateDynString(128,128); + if(priv->buffer == NULL){ + return 0; + } + return 1; +} +/*------------------------------------------------------------------------*/ +void AddAstriumnetProtocoll() +{ + AsconProtocol *prot = NULL; + + prot = calloc(sizeof(AsconProtocol), 1); + prot->name = strdup("astriumnet"); + prot->init = AstriumnetInit; + prot->handler = AstriumnetHandler; + AsconInsertProtocol(prot); +} + diff --git a/make_gen b/make_gen index 50bb9d3..c57cf98 100644 --- a/make_gen +++ b/make_gen @@ -25,7 +25,7 @@ OBJ=psi.o buffer.o ruli.o sps.o pimotor.o charbychar.o\ rebin.o sanslirebin.o lmd200.o slsvme.o julprot.o sinqhttpprot.o \ pmacprot.o pfeifferprot.o termprot.o phytron.o autowin.o eigera2.o \ tclClock.o tclDate.o tclUnixTime.o jvlprot.o \ - eigermono.o sputterprot.o zwickroll.o + eigermono.o sputterprot.o zwickroll.o astriumnet.o poldifold.o .SECONDARY.: sanslirebin.c diff --git a/poldifold.c b/poldifold.c new file mode 100644 index 0000000..d984c64 --- /dev/null +++ b/poldifold.c @@ -0,0 +1,98 @@ +/** + * With the new POLDI chopper there is only one chopper start signal but 4 equivalent + * quadrants on the chopper. This leads to the fact that the same pattern is repeated + * four times throughout the time intervall. This code now folds this onto one again. + * Yet another case where a shortcoming of the HW has to be resolved in software. + * + * copyright: see file COPYRIGHT + * + * Mark Koennecke, April 2014 + */ +#include +#include +#include + +static int PoldiFold (pSConnection pCon, pSicsInterp pInter, void + *pData, int argc, char *argv[]) +{ + pHdb source, target, dim; + int *sourceData, *targetData, *sourceCurrent, *targetCurrent; + unsigned int nDet, nTof, nSourceTof; + int offset[4], i, j, k; + char num[20], *pPtr; + + if(argc < 4) { + SCWrite(pCon,"ERROR: not enough arguments to poldifold",eError); + return 0; + } + + /* + reading and checking arguments + */ + source = FindHdbNode(NULL,argv[1],pCon); + target = FindHdbNode(NULL,argv[2],pCon); + if(source == NULL || target == NULL){ + SCPrintf(pCon,eError,"ERROR: source %s or target %s path invalid", + argv[1], argv[2]); + return 0; + } + + pPtr = argv[3]; + for(i = 0; i < 4; i++){ + pPtr = stptok(pPtr,num,sizeof(num),","); + if(pPtr == NULL){ + SCWrite(pCon,"ERROR: not enough values in the offset list",eError); + return 0; + } + } + pPtr = stptok(pPtr,num,sizeof(num),","); + if(pPtr == NULL){ + SCWrite(pCon,"ERROR: not enough values in the offset list",eError); + return 0; + } + nTof = atoi(num); + + dim = FindHdbNode(NULL,"../dim",pCon); + if(dim == NULL){ + SCPrintf(pCon,eError,"ERROR: failed to find dimensions beneath %s, no HM?", + argv[1]); + return 0; + } + nDet = dim->value.v.intArray[0]; + nSourceTof = dim->value.v.intArray[1]; + + /* + ensure enough space to write + */ + if(target->value.arrayLength != nDet*nTof){ + free(target->value.v.intArray); + target->value.v.intArray = malloc(nDet*nTof*sizeof(int)); + if(target->value.v.intArray == NULL){ + SCWrite(pCon,"ERROR: out of memory in poldifold",eError); + return 0; + } + target->value.arrayLength = nDet*nTof; + } + sourceData = source->value.v.intArray; + targetData = target->value.v.intArray; + memset(targetData,0,nDet*nTof*sizeof(int)); + + for(i = 0; i < nDet; i++){ + sourceCurrent = sourceData + i*nSourceTof; + targetCurrent = targetData + i*nTof; + for(j = 0; j < 4; j++){ + for(k = 0; k < nTof; k++){ + targetCurrent[k] += sourceCurrent[offset[j] + k]; + } + } + } + SCSendOK(pCon); + return 1; +} +/*-----------------------------------------------------------------*/ +int MakePoldiFold (pSConnection pCon, pSicsInterp pInter, void + *pData, int argc, char *argv[]) +{ + AddCommand(pInter, "poldifold", PoldiFold, NULL, NULL); + return 1; +} diff --git a/psi.c b/psi.c index fe8d640..725109a 100644 --- a/psi.c +++ b/psi.c @@ -60,6 +60,7 @@ void SiteInit(void) /* * SICS specific Asynchronous I/O protocols */ + INIT(AddAstriumnetProtocoll); INIT(AddJulChoProtocoll); INIT(AddHttpProtocoll); INIT(AddHttpOptProtocoll); @@ -111,6 +112,7 @@ static void AddPsiCommands(SicsInterp * pInter) SCMD("MakeFocusAverager", MakeFA); SCMD("MakeLMD200", MakeLMD200); SCMD("MakePIMotor", PIMotorFactory); + SCMD("MakePoldiFold", MakePoldiFold); SCMD("MakePSDFrame", MakeFrameFunc); SCMD("MakeRitaFix", MakeRitaFix); SCMD("MakeRitaWin", MakeRitaWin);