sicspsi/jvlprot.c

/*
 * jvlprot.c
 *
 * This is a protocol driver for the JVL integrated stepper motors.
 * They talk on a RS485 bus, half-duplex, 19220 Baud, 8N1. They use
 * a weird binary protocol which is documented in the accompanying
 * manual.
 *
 *  Created on: May 7, 2012
 *      Author: koennecke
 */

#include <errno.h>
#include <time.h>
#include <ascon.h>
#include <ascon.i>
#include <stptok.h>

typedef struct {
	unsigned char wrBuffer[30];
	unsigned char rdBuffer[30];
	int toWrite, toRead;
	unsigned char *wrPtr, *rdPtr;
	int reading;
	time_t start;
}JVL, *pJVL;
/*------------------------------------------------------------*/
static int validResponse(pJVL priv)
{
  if(priv->rdPtr - priv->rdBuffer > 3){
    if((priv->rdBuffer[0] == 0x52 && priv->rdBuffer[1] == 0x52 && priv->rdBuffer[2] == 0x52) ||
       (priv->rdBuffer[0] == 0x11 && priv->rdBuffer[1] == 0x11 && priv->rdBuffer[2] == 0x11) ) {
      return 1;
    } else {
      return 0;
    }
  } else {
    return 1;
  }
}

/*------------------------------------------------------------*/
static int JVLHandler(Ascon *a)
{
	pJVL priv;
	char *data, token[20];
	int motNo, address,ret;
	union {
		unsigned char b[4];
		int iVal;
	}val;
	char chr;

	priv = (pJVL)a->private;
	switch(a->state){
	case AsconWriteStart:
		data = GetCharArray(a->wrBuffer);
		if(data[0] == 'r'){
			data = stptok(data,token,20," ");
			data = stptok(data,token,20," ");
			motNo = atoi(token);
			data = stptok(data,token,20," ");
			address = atoi(token);
			priv->wrBuffer[0] = 80; /* 0x50 */
			priv->wrBuffer[1] = 80;
			priv->wrBuffer[2] = 80;
			priv->wrBuffer[3] = motNo;
			priv->wrBuffer[4] = 255 - motNo;
			priv->wrBuffer[5] = address;
			priv->wrBuffer[6] = 255-address;
			priv->wrBuffer[7] = 170; /* 0xAA */
			priv->wrBuffer[8] = 170;
			priv->toWrite = 9;
			priv->toRead = 19;
			priv->rdPtr = priv->rdBuffer;
			priv->wrPtr = priv->wrBuffer;
			a->state = AsconWriting;
			priv->reading = 1;
			priv->start = time(NULL);
			return 1;
		} else if(data[0] == 'w'){
			data = stptok(data,token,20," ");
			data = stptok(data,token,20," ");
			motNo = atoi(token);
			data = stptok(data,token,20," ");
			address = atoi(token);
			data = stptok(data,token,20," ");
			val.iVal = atoi(token);
			priv->wrBuffer[0] = 82; /* 0x52 */
			priv->wrBuffer[1] = 82;
			priv->wrBuffer[2] = 82;
			priv->wrBuffer[3] = motNo;
			priv->wrBuffer[4] = 255 - motNo;
			priv->wrBuffer[5] = address;
			priv->wrBuffer[6] = 255-address;
			priv->wrBuffer[7] = 4; /* len */
			priv->wrBuffer[8] = 255 - 4;
			/*
			 * This code may be platform byte order dependent
			 */
			priv->wrBuffer[9] = val.b[0]; /* data */
			priv->wrBuffer[10] = 255 -val.b[0];
			priv->wrBuffer[11] = val.b[1];
			priv->wrBuffer[12] = 255 -val.b[1];
			priv->wrBuffer[13] = val.b[2];
			priv->wrBuffer[14] = 255 -val.b[2];
			priv->wrBuffer[15] = val.b[3];
			priv->wrBuffer[16] = 255 -val.b[3];
			priv->wrBuffer[17] = 170; /* 0xAA */
			priv->wrBuffer[18] = 170;
			priv->wrPtr = priv->wrBuffer;
			priv->toWrite = 19;
			priv->rdPtr = priv->rdBuffer;
			priv->toRead = 3;
			a->state = AsconWriting;
			priv->reading = 0;
			priv->start = time(NULL);
			return 1;
		} else {
			priv->reading = 0;
			AsconError(a,"Unknown JVL command", 0);
			return 1;
		}
		break;
	case AsconWriting:
	    AsconReadGarbage(a->fd);
	    ret = AsconWriteChars(a->fd, (char *)priv->wrPtr, priv->toWrite);
	    if(*priv->wrPtr != 80){
	      printf("Hugo\n");
            }
	    if (ret < 0) {
	      if (errno != EINTR && errno != EAGAIN) {
	        AsconError(a, "send failed:", errno);
	      }
	      /*
	       * Ooops: which state shall we go to after a write fail?
	       * This seems to retry.
	       */
	    } else {
	      priv->wrPtr += ret;
	      priv->toWrite -= ret;
	      if (priv->toWrite <= 0) {
	        a->state = AsconWriteDone;
	      }
	      return 1;
	    }
		break;
	  case AsconReadStart:
	    DynStringClear(a->rdBuffer);
	    a->state = AsconReading;
	    return 1;
	    break;
	case AsconReading:
	  /* if(!validResponse(priv)){ */
	  /*   a->state = AsconWriteStart; */
	  /*   return 1; */
	  /* } */
		ret = AsconReadChar(a->fd, &chr);
		if (ret < 0) {
			/* EINTR means we must retry */
			if (errno != EINTR && errno != EAGAIN) {
				AsconError(a, "AsconReadChar failed:", errno);
			}
		} else if (ret > 0) {
			*(priv->rdPtr) = (unsigned char)chr;
			priv->toRead--;
			priv->rdPtr++;
			if(!validResponse(priv)){
			  a->state=AsconWriteStart;
			  return 1;
			}
			if(priv->toRead <= 0){
				a->state = AsconReadDone;
				if(priv->reading){
					/* This code may be platform byte order dependent */
					val.b[0] = priv->rdBuffer[9];
					val.b[1] = priv->rdBuffer[11];
					val.b[2] = priv->rdBuffer[13];
					val.b[3] = priv->rdBuffer[15];
					snprintf(token,20,"%d", val.iVal);
					DynStringConcat(a->rdBuffer,token);
				} else {
					if(priv->rdBuffer[0] == 0x11){
						DynStringConcat(a->rdBuffer,"OK");
					} else {
						DynStringConcat(a->rdBuffer,"Unknown response code from JVL: ");
						snprintf(token,20,"%x", priv->rdBuffer[0]);
						DynStringConcat(a->rdBuffer, token);
					}
				}
			}
		} else {
		  if(time(NULL) > priv->start + a->timeout){
		    a->state = AsconReadDone;
		    DynStringConcat(a->rdBuffer,"timeout");
		    return 1;
		  }
		}
		return 1;
	    break;
	default:
		return AsconBaseHandler(a);
	}
	return AsconBaseHandler(a);
}
/*------------------------------------------------------------------------*/
static int JVLInit(Ascon * a, SConnection * con, int argc, char *argv[])
{
	  pJVL priv = NULL;

	  priv = calloc(sizeof(JVL), 1);
	  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 = 1.0;           /* sec */
	  }
	  a->private = priv;
	  a->killPrivate = free;
	  return 1;
}
/*------------------------------------------------------------------------*/
void AddJVLProtocoll()
{
  AsconProtocol *prot = NULL;

  prot = calloc(sizeof(AsconProtocol), 1);
  prot->name = strdup("jvl");
  prot->init = JVLInit;
  prot->handler = JVLHandler;
  AsconInsertProtocol(prot);
}