Mark Koennecke
7d986b1a9f
- The astrium chopper controller requires floats with a . and retuns them with a , . Accomodated for this quirk
347 lines
9.0 KiB
C
347 lines
9.0 KiB
C
/**
|
|
* 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 random 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;
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
void ptToKomma(char *txt)
|
|
{
|
|
int i;
|
|
|
|
for(i = 0; i < strlen(txt); i++){
|
|
if(txt[i] == '.'){
|
|
txt[i] = ',';
|
|
}
|
|
}
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
void kommaToPt(char *txt)
|
|
{
|
|
int i;
|
|
|
|
for(i = 0; i < strlen(txt); i++){
|
|
if(txt[i] == ','){
|
|
txt[i] = '.';
|
|
}
|
|
}
|
|
}
|
|
/*-----------------------------------------------------------------------*/
|
|
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++;
|
|
/* ptToKomma(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,"=");
|
|
kommaToPt(attr.value);
|
|
DynStringConcat(a->rdBuffer,attr.value);
|
|
}
|
|
DynStringConcat(a->rdBuffer,":");
|
|
}
|
|
DeleteDynString(xmlBuffer);
|
|
mxmlDelete(root);
|
|
|
|
return 1;
|
|
}
|
|
/*-----------------------------------------------------------------------
|
|
This code and the function below is sensitive to int sizes. The hash must
|
|
be a 64bit int. This is long on OSX and long long on SL6 32 bit. Setting
|
|
this to int64_t as defined in stdint.h does not solve the problem because
|
|
of the conversions to and from strings involved.
|
|
------------------------------------------------------------------------- */
|
|
static long long calculateToken(char *password, long long hash)
|
|
{
|
|
long long tmp;
|
|
int i;
|
|
|
|
assert(sizeof(hash) == 8);
|
|
|
|
tmp = hash ^ 0x80AA80AA;
|
|
for(i = 0; i < strlen(password); i++){
|
|
tmp += password[i] * (long long)pow(2,i);
|
|
tmp -= password[i] * (long long)pow(2,31-i);
|
|
}
|
|
return tmp ^ hash;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int doWaitHash(Ascon *a)
|
|
{
|
|
pAstriumnet priv = NULL;
|
|
int ret;
|
|
char chr;
|
|
long long hash;
|
|
char token[50], *pPtr, *endptr;
|
|
|
|
priv = (pAstriumnet)a->private;
|
|
ret = AsconReadChar(a->fd,&chr);
|
|
if(ret < 0){
|
|
AsconError(a, "ASC5", errno);
|
|
return 0;
|
|
}
|
|
if(ret == 1 && (int)chr != 0x16) {
|
|
DynStringConcatChar(a->rdBuffer,chr);
|
|
}
|
|
if(strstr(GetCharArray(a->rdBuffer),"</Hash>") != NULL ) {
|
|
pPtr = GetCharArray(a->rdBuffer);
|
|
pPtr += 6;
|
|
endptr = strchr(pPtr,(int)'<');
|
|
*endptr = '\0';
|
|
hash = strtoll(pPtr, &endptr,0);
|
|
hash = calculateToken(priv->password,hash);
|
|
DynStringCopy(priv->buffer,"<AuthToken>");
|
|
snprintf(token,sizeof(token),"%lld",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 && (int)chr != 0x16) {
|
|
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;
|
|
a->wrPos = 0;
|
|
return 1;
|
|
break;
|
|
case AsconReading:
|
|
ret = AsconReadChar(a->fd,&chr);
|
|
if(ret < 0){
|
|
AsconError(a, "ASC5", errno);
|
|
return 0;
|
|
}
|
|
if(ret == 1 && (int)chr != 0x16) {
|
|
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);
|
|
}
|
|
|