sics/site_ansto/hardsup/knauer_asyncprotocol.c

#include "knauer_asyncprotocol.h"
#include "asyncprotocol.h"
#include "asyncqueue.h"
#include "ascon.i"
#include "dynstring.h"
#include <errno.h>

#define PROTOCOL_CONTINUE 0
#define PROTOCOL_COMPLETE 1
#define PROTOCOL_ERROR (-1)

/* Sample Messages
 55 aa 00 08 52 45 4d 4f 54 45 3a 30
 55 aa 00 16 45 52 52 4f 52 3a 34 38 2c 4e 6f 74 20 73 75 70 70 6f 72 74 65 64
 55 aa 00 18 45 52 52 4f 52 3a 31 36 2c 49 6e 76 61 6c 69 64 20 63 6f 6d 6d 61 6e 64
structure:
 55 aa    - sync bytes
 00 xx    - length of following bytes
 yy .. zz - count(00 xx) message bytes with no terminator
*/
#define PROTOCOL_NAME "KNAUER_AP"
#define PROTOCOL_INIT KNAUERInitProtocol
#define SYN1 0x55
#define SYN2 0xAA
#define CR '\r'
#define LF '\n'

/*
 * Protocol Private data block
 */
typedef struct proto_private_t {
  int state;    /**< protocol state machine */
  int len;      /**< length from the protocol */
  pDynString wrBuffer;  /**< transmitted message */
  pDynString rxBuffer;  /**< received message */
} ProtoPrivate;

static ProtoPrivate *makeProtoPrivate()
{
  ProtoPrivate *priv = calloc(sizeof(ProtoPrivate), 1);
  priv->wrBuffer = CreateDynString(100, 100);
  priv->rxBuffer = CreateDynString(100, 100);
  return priv;
}

static ProtoPrivate *Proto_KillPrivate(ProtoPrivate *priv)
{
  if (priv) {
    if (priv->rxBuffer) {
      DeleteDynString(priv->rxBuffer);
      priv->rxBuffer = NULL;
    }
    free(priv);
  }
  return NULL;
}

/*
 * Protocol Specific Support Functions
 */
/*
 * Protocol Prepare Output
 */
static int Proto_Prepare(ProtoPrivate *priv, pDynString wrBuffer)
{
  DynStringConcatChar(wrBuffer, CR);
  return PROTOCOL_COMPLETE;
}

/*
 * Protocol receive character - characater by character
 */
static int Proto_RxChar(ProtoPrivate *priv, int rxchar)
{
  enum RX_STATE {
    RX_START=0,
    RX_TEXT=1,
    RX_SYN2=2,
    RX_LEN1=3,
    RX_LEN2=4,
    RX_STOP=99
  };
  rxchar &= 0xFF;
  switch (priv->state) {
    case RX_START:
      if (rxchar != SYN1) {
        priv->state = RX_STOP;
        return PROTOCOL_ERROR;
      }
      priv->state = RX_SYN2;
      return PROTOCOL_CONTINUE;
    case RX_SYN2:
      if (rxchar != SYN2) {
        priv->state = RX_STOP;
        return PROTOCOL_ERROR;
      }
      priv->state = RX_LEN1;
      return PROTOCOL_CONTINUE;
    case RX_LEN1:
      priv->len = 256 * rxchar;
      priv->state = RX_LEN2;
      return PROTOCOL_CONTINUE;
    case RX_LEN2:
      priv->len += rxchar;
      priv->state = RX_TEXT;
      return PROTOCOL_CONTINUE;
    case RX_TEXT:
      DynStringConcatChar(priv->rxBuffer, rxchar);
      if (GetDynStringLength(priv->rxBuffer) >= priv->len) {
        priv->state = RX_STOP;
        return PROTOCOL_COMPLETE;
      } else {
        return PROTOCOL_CONTINUE;
      }

    default:
        return PROTOCOL_ERROR;
  }
  return PROTOCOL_ERROR;
}

/*
 * AsyncProtocol handling
 * ======================
 */

static void Async_KillPrivate(pAsyncTxn pTxn)
{
  Proto_KillPrivate((ProtoPrivate *) pTxn->proto_private);
  pTxn->proto_private = NULL;
}

/*
 * AsyncProtocol Receive Character
 */
static int Async_Rx(pAsyncProtocol p, pAsyncTxn pTxn, int rxchar) {
  int iRet, str_len;
  ProtoPrivate *priv = (ProtoPrivate *) pTxn->proto_private;

  iRet = Proto_RxChar(priv, rxchar);
  /*
   * Keep inp_buf and inp_idx up-to-date after each character
   */
  str_len = GetDynStringLength(priv->rxBuffer);
  if (str_len > pTxn->inp_idx && pTxn->inp_idx < pTxn->inp_len) {
    int xfr_len;
    char *tgt = &pTxn->inp_buf[pTxn->inp_idx];
    char *loc = &GetCharArray(priv->rxBuffer)[pTxn->inp_idx];
    if (str_len > pTxn->inp_len)
      xfr_len = pTxn->inp_len - pTxn->inp_idx;
    else
      xfr_len = str_len - pTxn->inp_idx;
    memcpy(tgt, loc, xfr_len);
    pTxn->inp_idx += xfr_len;
  }

  if (iRet == PROTOCOL_CONTINUE) {
    return 1; /* Keep Going */
  }

  if (iRet == PROTOCOL_ERROR) {
    return -1; /* Error condition */
  }

  /* Message Complete */
  return AQU_POP_CMD;
}

/*
 * AsyncProtocol Event callback
 */
static int Async_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
  if (event == AQU_TIMEOUT) {
    /* handle command timeout */
    pTxn->txn_status = ATX_TIMEOUT;
    return AQU_POP_CMD;
  }
  return AQU_POP_CMD;
}

/*
 * AsyncProtocol Prepare Transaction
 */
static int Async_PrepareTxn(pAsyncProtocol p, pAsyncTxn pTxn, const char* cmd, int cmd_len, int rsp_len) {
  ProtoPrivate *priv;
  priv = makeProtoPrivate();
  if (priv == NULL) {
    SICSLogWrite("ERROR: Out of memory in Async_PrepareTxn", eError);
    return 0;
  }
  priv->state = 0;
  priv->len = 0;
  DynStringConcatBytes(priv->wrBuffer, cmd, cmd_len);
  Proto_Prepare(priv, priv->wrBuffer);
  pTxn->out_len = GetDynStringLength(priv->wrBuffer);
  pTxn->out_buf = (char*) malloc(pTxn->out_len);
  if (pTxn->out_buf == NULL) {
    SICSLogPrintf(eError, "ERROR: Out of memory in %s:%s", __FILE__, __FUNCTION__);
    Proto_KillPrivate(priv);
    return 0;
  }
  pTxn->proto_private = priv;
  pTxn->kill_private = Async_KillPrivate;
  memcpy(pTxn->out_buf, GetCharArray(priv->wrBuffer), pTxn->out_len);

  return 1;
}

/*
 * Ascon Protocol handling
 * =======================
 */

static void Ascon_KillPrivate(void *priv)
{
  Proto_KillPrivate((ProtoPrivate *) priv);
}

/*
 * Ascon Protocol WriteStart
 */
static int Ascon_Prepare(Ascon *a) {
  ProtoPrivate *priv = (ProtoPrivate *) a->private;
  Proto_Prepare(priv, a->wrBuffer);
  a->wrPos = 0;
  a->state = AsconWriting;
  return 1;
}

/*
 * Ascon Protocol Read Poll
 */
static int Ascon_Rx(Ascon *a) {
  int ret, status;
  char chr = '\0';
  ProtoPrivate *priv = (ProtoPrivate *) a->private;

  ret =  AsconReadChar(a->fd, &chr);
  while (ret > 0) {
    a->start = DoubleTime();
    status = Proto_RxChar(priv, chr);
    if (GetDynStringLength(priv->rxBuffer) > GetDynStringLength(a->rdBuffer)) {
      int len_rd = GetDynStringLength(a->rdBuffer);
      int len_rx = GetDynStringLength(priv->rxBuffer);
      char *loc = &GetCharArray(priv->rxBuffer)[len_rd];
      DynStringConcatBytes(a->rdBuffer, loc, len_rx - len_rd);
    }
    if (status > 0) { /* Complete */
      a->state = AsconReadDone;
      return 1;
    }
    else if (status < 0) { /* Error */
      AsconError(a, "Protocol Input Error:", status);
      /*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */
      a->lastReconnect = DoubleTime();
      return 1;
    }
    ret = AsconReadChar(a->fd, &chr);
  }
  if (ret < 0) {
    AsconError(a, "AsconReadChar failed:", errno);
    return 1;
  }
  if (a->state != AsconReadDone) {
    if (a->timeout > 0) {
      if (DoubleTime() - a->start > a->timeout) {
        AsconError(a, "read timeout", 0);
        a->state = AsconTimeout;
      }
    }
  }
  return 1;
}

/*
 * Ascon Protocol Poll Loop
 */
static int AsconProtHandler(Ascon *a) {
  ProtoPrivate *priv = (ProtoPrivate *) a->private;
  int ret;

  switch(a->state){
    case AsconWriteStart:
      ret = Ascon_Prepare(a);
      return ret;
    case AsconReadStart:
      DynStringClear(priv->rxBuffer);
      a->start = DoubleTime();
      priv->state = 0;
      priv->len = 0;
      ret = AsconStdHandler(a);
      return ret;
    case AsconReading:
      ret = Ascon_Rx(a);
      return ret;
    default:
      ret = AsconStdHandler(a);
      return ret;
  }
  return 1;
}

/*
 * Ascon Protocol Connection Init
 */
static int AsconInit(Ascon *a, SConnection *con, int argc, char *argv[])
{
  int iRet;
  ProtoPrivate *priv;

  iRet = AsconStdInit(a, con, argc, argv);
  priv = makeProtoPrivate();
  a->private = priv;
  a->killPrivate = Ascon_KillPrivate;
  return iRet;
}

static AsyncProtocol *My_Async_Protocol = NULL;
static AsconProtocol *My_Ascon_Protocol = NULL;

/*
 * Protocol Initialisation
 */
void PROTOCOL_INIT(SicsInterp *pSics) {
  if (My_Async_Protocol == NULL) {
    AsyncProtocol *prot;
    prot = AsyncProtocolCreate(pSics, PROTOCOL_NAME, NULL, NULL);
    prot->sendCommand = NULL;
    prot->handleInput = Async_Rx;
    prot->handleEvent = Async_Ev;
    prot->prepareTxn = Async_PrepareTxn;
    My_Async_Protocol = prot;
  }

  if (My_Ascon_Protocol == NULL) {
    AsconProtocol *prot;
    prot = calloc(sizeof(AsconProtocol), 1);
    prot->name = strdup(PROTOCOL_NAME);
    prot->init = AsconInit;
    prot->handler = AsconProtHandler;
    AsconInsertProtocol(prot);
    My_Ascon_Protocol = prot;
  }
}