Readded poldi chopper stuff
A new astriumnet protocol driver A poldifold which solves the quadrant issue with the new chopper
This commit is contained in:
313
astriumnet.c
Normal file
313
astriumnet.c
Normal file
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user