Merge branch 'poldichopper'

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 <errno.h>
+#include <math.h>
+#include <ascon.h>
+#include <ascon.i>
+#include <stptok.h>
+#include <dynstring.h>
+#include <mxml.h>
+
+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 <?XML=1.0 bla bla ?> 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,"<?xml version=\"1.0\" encoding=\"utf-8\"?>");
+  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), "<Hash>") == 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,"<AuthToken>");
+    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,"<AuthReq>");
+      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),"</Msg>") != 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);
+}
+

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
 

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 <sics.h>
+#include <sicshipadaba.h>
+#include <stptok.h>
+
+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;
+}

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);