sics/site_ansto/hardsup/omron_asyncprotocol.c

#include "omron_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
 * structure:
 * STX
 * data
 * ETX
 * BCC
*/
#define PROTOCOL_NAME "OMRON_AP"
#define PROTOCOL_INIT OMRONInitProtocol
#define STX 2
#define ETX 3

/*
 * 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
 */

static int calc_bcc(const char* text, int len)
{
  int idx = 0;
  int bcc = 0;
  int c = 0;
  for (idx = 0; idx < len; ++idx) {
    c = text[idx] & 0xFF;
    bcc ^= c;
  }
  bcc ^= ETX;
  return bcc;
}

/*
 * Protocol Prepare Output
 */
static int Proto_Prepare(ProtoPrivate *priv, pDynString wrBuffer)
{
  int bcc;
  char stx[2] = {STX, '\0'};
  bcc = calc_bcc(GetCharArray(wrBuffer), GetDynStringLength(wrBuffer));
  DynStringInsert(wrBuffer, stx, 0);
  DynStringConcatChar(wrBuffer, ETX);
  DynStringConcatChar(wrBuffer, bcc);
  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_LAST=2, /* BCC */
    RX_STOP=99
  };
  int bcc;
  rxchar &= 0xFF;
  switch (priv->state) {
    case RX_START:
      if (rxchar != STX) {
        priv->state = RX_STOP;
        return PROTOCOL_ERROR;
      }
      priv->state = RX_TEXT;
      return PROTOCOL_CONTINUE;
    case RX_TEXT:
      if (rxchar == ETX) {
        priv->state = RX_LAST;
        return PROTOCOL_CONTINUE;
      }
      DynStringConcatChar(priv->rxBuffer, rxchar);
      return PROTOCOL_CONTINUE;
    case RX_LAST:
      bcc = calc_bcc(GetCharArray(priv->rxBuffer), GetDynStringLength(priv->rxBuffer));
      if (bcc == rxchar) {
        priv->state = RX_STOP;
        return PROTOCOL_COMPLETE;
      } else {
        SICSLogPrintf(eError,
            "Omron BCC mismatch, expected %02X but received %02X on message:",
            bcc, rxchar);
        SICSLogWriteHex(GetCharArray(priv->rxBuffer), GetDynStringLength(priv->rxBuffer), eError); 
        priv->state = RX_STOP;
        return PROTOCOL_ERROR;
      }

    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;
  }
}