fd150f35a2
could be read on initialization. - The new SLS magnet driver for the TCP/IP concentrator now successfully writes data
308 lines
8.1 KiB
C
308 lines
8.1 KiB
C
/*
|
|
* slsecho.c
|
|
* This is a protocol adapter for yet another driver for the
|
|
* SLS DPC magnet controller. This one is for the shiny, silvery
|
|
* little box with TCP/IP, 8 fibre optic magnet couplers and
|
|
* two wires for power. The current assumption is that this
|
|
* device communicates with 64 byte message going back and forth,
|
|
* 8 bytes for each channel. Where the first two seem to be unused
|
|
* and the last 6 being the DSP codes as described elsewhere.
|
|
*
|
|
* The expected input for this is:
|
|
*
|
|
* channel:mode:code:val:convertflag
|
|
*
|
|
* with mode being either r or w and convertflag being one of
|
|
* none,read,write
|
|
*
|
|
* There is an endianness issue here: swapDataBytes may or not be
|
|
* required according to the platform used.
|
|
*
|
|
* Created on: Mar 18, 2010
|
|
* Author: Mark Koennecke
|
|
*/
|
|
#include <math.h>
|
|
#include <errno.h>
|
|
#include <ascon.h>
|
|
#include <ascon.i>
|
|
#include <stptok.h>
|
|
|
|
#define NONE 0
|
|
#define READ 1
|
|
#define WRITE 2
|
|
|
|
typedef struct {
|
|
char sendbuf[64];
|
|
char readbuf[64];
|
|
int convertflag; /* which conversions need to be done */
|
|
int channel; /* magnet channel number, starting at 0 */
|
|
} SLSEcho, *pSLSEcho;
|
|
|
|
/*
|
|
packet header codes
|
|
*/
|
|
#define DSPWRITE 0x80
|
|
#define DSPREAD 0x0
|
|
|
|
/*-------------------------------------------------------------------------
|
|
Code for data conversion from Lukas Tanner. The ULONG is system dependent,
|
|
it MUST describe a 32 bit value.
|
|
-------------------------------------------------------------------------*/
|
|
#define ULONG int
|
|
/***********************************************************************/
|
|
static ULONG double2DSPfloat(double input)
|
|
/***********************************************************************/
|
|
/* Konvertiert eine normale double Variable in ein 32bit DSP Float Format. */
|
|
{
|
|
ULONG output;
|
|
double mantissa;
|
|
int exponent;
|
|
|
|
if (input == 0) {
|
|
output = 0;
|
|
} else {
|
|
mantissa = 2 * (frexp(fabs(input), &exponent));
|
|
mantissa = ldexp(mantissa, 23);
|
|
exponent = exponent - 1 + 127;
|
|
if (exponent < 0) {
|
|
exponent = 0;
|
|
} else if (exponent > 0xFE) {
|
|
exponent = 0xFE;
|
|
}
|
|
exponent = exponent << 23;
|
|
output = ((ULONG) mantissa) & 0x7FFFFF;
|
|
output = output | ((ULONG) (exponent));
|
|
if (input < 0) {
|
|
output = output | 0x80000000;
|
|
}
|
|
}
|
|
return output;
|
|
}
|
|
|
|
/***********************************************************************/
|
|
static double DSPfloat2double(ULONG input)
|
|
/***********************************************************************/
|
|
/* Konvertiert eine Variable inder ein 32bit DSP Float Wert abgelegt ist,
|
|
in ein normales double Format
|
|
32bit IEEE Float => SEEEEEEEEMMMMMMMMMMMMMMMMMMMMMMM; S = Sign Bit
|
|
E = Exponent
|
|
M = Mantissa (23)*/
|
|
{
|
|
double output;
|
|
if ((input & 0x7FFFFF) == 0 && ((input >> 23) & 0xFF) == 0) {
|
|
output = 0;
|
|
} else {
|
|
output = ldexp(input & 0x7FFFFF, -23) + 1;
|
|
output = output * pow(-1, ((input >> 31) & 1));
|
|
output = output * ldexp(1, (((input >> 23) & 0xFF) - 127));
|
|
}
|
|
return output;
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static void swapDataBytes(char *pMessage)
|
|
{
|
|
char tmp[4];
|
|
|
|
memcpy(tmp,pMessage+4,4);
|
|
pMessage[4] = tmp[3];
|
|
pMessage[5] = tmp[2];
|
|
pMessage[6] = tmp[1];
|
|
pMessage[7] = tmp[0];
|
|
}
|
|
/*-------------------------------------------------------------------------*/
|
|
static int EchoPack(Ascon *a)
|
|
{
|
|
pSLSEcho echo = NULL;
|
|
char *pMessage = NULL, *pPtr = NULL;
|
|
char token[20], parseString[255];
|
|
int code, val;
|
|
double dval;
|
|
|
|
echo = a->private;
|
|
assert(echo != NULL);
|
|
|
|
memset(echo->sendbuf,0,sizeof(echo->sendbuf));
|
|
memset(echo->readbuf,0,sizeof(echo->readbuf));
|
|
|
|
assert(GetDynStringLength(a->wrBuffer) < 255);
|
|
memset(parseString,0,sizeof(parseString));
|
|
strcpy(parseString,GetCharArray(a->wrBuffer));
|
|
pPtr = parseString;
|
|
|
|
/* read num */
|
|
pPtr = stptok(pPtr,token,sizeof(token),":");
|
|
echo->channel = atoi(token);
|
|
pMessage = echo->sendbuf + echo->channel*8;
|
|
|
|
/* read mode */
|
|
pPtr = stptok(pPtr,token,sizeof(token),":");
|
|
if(token[0] == 'r'){
|
|
pMessage[1] = DSPREAD;
|
|
} else if(token[0] == 'w'){
|
|
pMessage[1] = DSPWRITE;
|
|
}else if(token[0] == 's'){
|
|
pMessage[1] = 0x2;
|
|
} else {
|
|
a->state = AsconFailed;
|
|
DynStringConcat(a->errmsg,"ERROR: bad mode token ");
|
|
DynStringConcat(a->errmsg,token);
|
|
return 0;
|
|
}
|
|
|
|
/* read code */
|
|
pPtr = stptok(pPtr,token,sizeof(token),":");
|
|
sscanf(token,"%x", &code);
|
|
pMessage[3] = code;
|
|
|
|
/* read value */
|
|
pPtr = stptok(pPtr,token,sizeof(token),":");
|
|
dval = atof(token);
|
|
val = dval;
|
|
|
|
/* read convertflag */
|
|
pPtr = stptok(pPtr,token,sizeof(token),":");
|
|
if(strcmp(token,"none") == 0){
|
|
echo->convertflag = NONE;
|
|
} else if(strcmp(token,"read") == 0){
|
|
echo->convertflag = READ;
|
|
} else if(strcmp(token,"write") == 0){
|
|
echo->convertflag = WRITE;
|
|
} else {
|
|
a->state = AsconFailed;
|
|
DynStringConcat(a->errmsg,"ERROR: bad convertflag ");
|
|
DynStringConcat(a->errmsg,token);
|
|
return 0;
|
|
}
|
|
if(echo->convertflag == WRITE){
|
|
val = double2DSPfloat(dval);
|
|
}
|
|
memcpy(pMessage+4,&val,4);
|
|
swapDataBytes(pMessage);
|
|
|
|
/* printf("Sending code: 0x%x, val = %d\n", code, val); */
|
|
return 1;
|
|
}
|
|
/*-------------------------------------------------------------------*/
|
|
static int EchoUnpack(Ascon *a)
|
|
{
|
|
pSLSEcho echo = a->private;
|
|
char *pMessage = NULL, printbuf[30];
|
|
int val;
|
|
unsigned char code = 0;
|
|
double dval;
|
|
|
|
|
|
DynStringClear(a->rdBuffer);
|
|
pMessage = echo->readbuf + echo->channel*8;
|
|
|
|
code = pMessage[3];
|
|
snprintf(printbuf,sizeof(printbuf),"%x", code);
|
|
DynStringConcat(a->rdBuffer,printbuf);
|
|
DynStringConcatChar(a->rdBuffer,':');
|
|
|
|
swapDataBytes(pMessage);
|
|
memcpy(&val,pMessage+4, 4);
|
|
dval = (double)val;
|
|
|
|
if(echo->convertflag == READ){
|
|
dval = DSPfloat2double(val);
|
|
}
|
|
snprintf(printbuf,sizeof(printbuf),"%f", dval);
|
|
DynStringConcat(a->rdBuffer,printbuf);
|
|
/* printf("Reading code: 0x%x, val = %d\n", code, val);*/
|
|
return 1;
|
|
}
|
|
/*----------------------------------------------------------------------*/
|
|
static int SLSEchoHandler(Ascon *a)
|
|
{
|
|
pSLSEcho echo = a->private;
|
|
int written, read;
|
|
char c;
|
|
|
|
assert(echo != NULL);
|
|
|
|
switch(a->state){
|
|
case AsconWriteStart:
|
|
EchoPack(a);
|
|
a->wrPos = 0;
|
|
a->state = AsconWriting;
|
|
break;
|
|
case AsconWriting:
|
|
AsconReadGarbage(a->fd);
|
|
written = send(a->fd, echo->sendbuf+a->wrPos,64-a->wrPos, 0);
|
|
if(written > 0){
|
|
a->wrPos += written;
|
|
if(a->wrPos >= 64){
|
|
a->state = AsconWriteDone;
|
|
break;
|
|
}
|
|
} else if (written == 0) {
|
|
errno = ECONNRESET;
|
|
return ASCON_DISCONNECTED;
|
|
} else {
|
|
return ASCON_SEND_ERROR;
|
|
}
|
|
break;
|
|
case AsconReadStart:
|
|
a->start = DoubleTime();
|
|
a->state = AsconReading;
|
|
a->wrPos = 0; /* reusing wrPos for character count received*/
|
|
break;
|
|
case AsconReading:
|
|
read = AsconReadChar(a->fd,&c);
|
|
if (read <= 0) {
|
|
if (read < 0) {
|
|
AsconError(a, "ASC5", errno);
|
|
return 0;
|
|
}
|
|
if (a->timeout > 0) {
|
|
if (DoubleTime() - a->start > a->timeout) {
|
|
AsconError(a, "no response", 0);
|
|
a->state = AsconTimeout;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
echo->readbuf[a->wrPos] = c;
|
|
a->wrPos++;
|
|
if(a->wrPos >= 64){
|
|
a->state = AsconReadDone;
|
|
EchoUnpack(a);
|
|
}
|
|
break;
|
|
default:
|
|
return AsconStdHandler(a);
|
|
}
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
static int SLSEchoInit(Ascon * a, SConnection * con, int argc, char *argv[])
|
|
{
|
|
a->fd = -1;
|
|
a->state = AsconConnectStart;
|
|
a->reconnectInterval = 10;
|
|
a->hostport = strdup(argv[1]);
|
|
if (argc > 2) {
|
|
a->timeout = atof(argv[2]);
|
|
} else {
|
|
a->timeout = 2.0; /* sec */
|
|
}
|
|
a->private = malloc(sizeof(SLSEcho));
|
|
if(a->private == NULL){
|
|
return 0;
|
|
}
|
|
a->killPrivate = free;
|
|
return 1;
|
|
}
|
|
/*------------------------------------------------------------------------*/
|
|
void AddSLSEchoProtocoll()
|
|
{
|
|
AsconProtocol *prot = NULL;
|
|
|
|
prot = calloc(sizeof(AsconProtocol), 1);
|
|
prot->name = strdup("slsecho");
|
|
prot->init = SLSEchoInit;
|
|
prot->handler = SLSEchoHandler;
|
|
AsconInsertProtocol(prot);
|
|
}
|