#include "modbus_asyncprotocol.h"
#include "asyncprotocol.h"
#include "asyncqueue.h"
#include "ascon.i"
#include "dynstring.h"
#include <errno.h>
#include <stdlib.h>
#include <math.h>

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

/* Sample Messages
 * structure:
 * STX
 * data
 * ETX
 * BCC
 */
#define PROTOCOL_NAME "MODBUS_AP"
#define PROTOCOL_INIT MODBUSInitProtocol
#define ADUSIZE 17

/*
 * MODBUS DataTypes
 */
enum mbDtypes {U16, U32, F32};

/*
 * NumVal = number of values to read or write,
 * ie number of coils, input/outputs or registers.
 * Number of bytes = NumVal * (number of bytes in Dtype)
 */
struct cmdPar {
  unsigned int MBAddr;  /**< MODBUS Address */
  unsigned int Fcode;   /**< Function Code */
  unsigned int SAddr;
  unsigned int NumVal;  /**< Number of Values */
  unsigned int NumBytes;
  unsigned int NumRd;
  unsigned int NumWr;
  enum mbDtypes Dtype;
  double MBdata[2000];
};

enum MBFCODES {
  RdCoil=1,     /* Read Coil - single output bit */
  RdInp=2,      /* Read Input - single input bit */
  RdHldReg=3,   /* Read Holding Register */
  RdInpReg=4,   /* Read Input Register */
  WrCoil=5,     /* Write Coil - force output bit */
  WrReg=6,      /* Write Register */
  WrLoop=8,     /* Write Loopback */
  WrMCoils=15,  /* Write Multiple Coils */
  WrMRegs=16    /* Write Multiple Registers */
};

typedef struct modbus_private_t Private, *pPrivate;
struct modbus_private_t {
  struct cmdPar MBcmd;
  int BO[4];
  int aduLen;
  int RespLen;
  int DatLen;
  unsigned char ADU[ADUSIZE]; /* Allows upto 8 bytes if data */
  pDynString rdBuffer;
};

/*
 * 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 */
  Private mb_priv;
} 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
 */

/* IEEE code copied from psi/modbus.c */
static void double2ieee(double input, unsigned char ieee[4]) {

  /* convert double to IEEE 32 bit floating number
   * (denormalized numbers are considered as zero)
   */

  long mantissa;
  int exponent;

  if (input == 0) {
    ieee[0]= 0;
    ieee[1]= 0;
    ieee[2]= 0;
    ieee[3]= 0;
  } else {
    /* frexp -> 0.f * 2^23 */
    /* 2^24 * fract instead of 2^23
     * because we get 0.(f/2) not 1.f from frexp and exponent = e + 1
     */
    mantissa = 0x1000000 * (frexp(fabs(input), &exponent));
    exponent = exponent - 1 + 127;
    if (exponent < 0) {
      exponent = 0;
    } else if (exponent > 0xFE) {
      exponent = 0xFE;
    }
    if (input < 0) {
      ieee[0] = 0x80 | (exponent >> 1);
    } else {
      ieee[0] = exponent >> 1;
    }
    ieee[1] = (exponent & 1) << 7 | ((mantissa & 0x7F0000) >> 16);
    ieee[2] = (mantissa & 0xFF00) >> 8;
    ieee[3] = mantissa & 0xFF;
  }
  return;
}

/*-------------------------------------------------------------------------*/
static double ieee2double(unsigned char ieee[4]) {

  /* IEEE 32 bit floating number to double
   * (denormalized numbers are considered as zero)
   */

  long mantissa;
  double output, norm = 1;
  int exponent, bias = 127;

  mantissa = ((ieee[1] << 16) & 0x7F0000)
    | ((ieee[2] <<  8) & 0xFF00)
    | ((ieee[3]      ) & 0xFF);

  exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */
  if (exponent == 255) {
    if (mantissa == 0) {
      /*TODO INF */
    } else {
      /*TODO NAN */
    }
  }
  if (exponent == 0) {
    if (mantissa == 0) {
      return 0.0;
    } else {
      bias = 126;
      norm = 0;
    }
  }
  output = ldexp(mantissa, -23) + norm;
  if (ieee[0] & 0x80) {
    output = -output;
  }
  return output * ldexp(1, exponent - bias);
}
/*-------------------------------------------------------------------------*/

/** @brief Parse a command string of the form "MBAddr:Fcode:SAddr:Dtype:MBdata"
  Dtype U16, U32, F32 with optional byte order array eg U32[3,2,1,0]
  */
static void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) {
  int len;
  unsigned int i;
  char *nextPar, dataType[4];
  double dVal;

  MBC->MBAddr = strtoul(cmdLine, &nextPar, 0);
  cmdLine = nextPar+1;
  MBC->Fcode = strtoul(cmdLine, &nextPar, 0);
  cmdLine = nextPar+1;
  MBC->SAddr = strtoul(cmdLine, &nextPar, 0);
  cmdLine = nextPar+1;
  if (MBC->Fcode != WrCoil) {
    MBC->NumVal = strtod(cmdLine, &nextPar);
    cmdLine = nextPar + 1;
    strncpy(dataType,cmdLine,3);
    dataType[3] = '\0';
    cmdLine += 3;
    nextPar = cmdLine;
    if (strcmp(dataType, "U16") == 0) {
      MBC->Dtype = U16;
    } else if (strcmp(dataType, "U32") == 0) {
      MBC->Dtype = U32;
    } else if (strcmp(dataType, "F32") == 0) {
      MBC->Dtype = F32;
    }
    if (*cmdLine == '[') {
      cmdLine++;
      BO[0] = strtol(cmdLine, &nextPar, 10);
      cmdLine = nextPar + 1;
      BO[1] = strtol(cmdLine, &nextPar, 10);
      cmdLine = nextPar + 1;
      BO[2] = strtol(cmdLine, &nextPar, 10);
      cmdLine = nextPar + 1;
      BO[3] = strtol(cmdLine, &nextPar, 10);
    }
    cmdLine = nextPar + 1;
  }
  switch (MBC->Fcode) {
    case RdCoil:
      /* Coils are numbered from 1 but addressed from 0 */
      MBC->SAddr--;
      MBC->NumBytes = MBC->NumVal;
      MBC->NumRd = MBC->NumVal;
      break;
    case WrCoil:
      MBC->SAddr--;
      dVal = strtod(cmdLine, &nextPar);
      if (dVal == 1) {
        MBC->MBdata[0] = 255;
      } else if (dVal == 0) {
        MBC->MBdata[0] = 0;
      } else {
        /* TODO ERROR */
      }
      break;
    case RdHldReg:
      if (MBC->Dtype == F32 || MBC->Dtype == U32) {
        MBC->NumRd = MBC->NumVal * 2;
      } else if (MBC->Dtype == U16) {
        MBC->NumRd = MBC->NumVal;
      } else {
        /* TODO ERROR */
      }
      break;
    case WrMRegs:
      for (i=0; i < MBC->NumVal; i++) {
        MBC->MBdata[i] = strtod(cmdLine, &nextPar);
        cmdLine = nextPar + 1;
      }
      if (MBC->Dtype == F32 || MBC->Dtype == U32) {
        MBC->NumWr = MBC->NumVal * 2;
        MBC->NumBytes = MBC->NumVal * 4;
      } else if (MBC->Dtype == U16) {
        MBC->NumWr = MBC->NumVal;
        MBC->NumBytes = MBC->NumVal * 2;
      }
      break;
  }
}

static int ModbusOutput(pPrivate myPriv, pDynString wrBuffer, pDynString send_buffer) {
  int ADUlen, PDUlenPlusUID;
  unsigned int i, j;
  char *cmdLine;
  unsigned char ADU[32], ieee[4];

  cmdLine = GetCharArray(wrBuffer);
  for (i=0; i < 4; i++)
    myPriv->BO[i] = i;
  parseCmd(cmdLine, &myPriv->MBcmd, myPriv->BO);

  ADU[0] = 0;
  ADU[1] = 0;
  ADU[2] = 0;
  ADU[3] = 0;
  ADU[6] = myPriv->MBcmd.MBAddr;
  ADU[7] = myPriv->MBcmd.Fcode;
  switch (myPriv->MBcmd.Fcode) {
    case RdCoil:
    case RdHldReg:
      /* TODO Max NumRd = 125 */
      ADU[8]  = (myPriv->MBcmd.SAddr & 0xFF00) >> 8;
      ADU[9]  =  myPriv->MBcmd.SAddr & 0xFF;
      ADU[10] = (myPriv->MBcmd.NumRd & 0xFF00) >> 8;
      ADU[11] =  myPriv->MBcmd.NumRd & 0xFF;
      ADUlen  = 12;
      break;
    case WrCoil:
      ADU[8]  = (myPriv->MBcmd.SAddr & 0xFF00) >> 8;
      ADU[9]  =  myPriv->MBcmd.SAddr & 0xFF;
      ADU[10] = myPriv->MBcmd.MBdata[0];
      ADU[11] = 0;
      ADUlen  = 12;
      break;
    case WrMRegs:
      ADU[8]  = (myPriv->MBcmd.SAddr & 0xFF00) >> 8;
      ADU[9]  =  myPriv->MBcmd.SAddr & 0xFF;
      ADU[10] = (myPriv->MBcmd.NumWr & 0xFF00) >> 8;
      ADU[11] =  myPriv->MBcmd.NumWr & 0xFF;
      ADU[12] =  myPriv->MBcmd.NumBytes;
      switch (myPriv->MBcmd.Dtype) {
        case U16:
          for (i=0, j=13; i < myPriv->MBcmd.NumVal; i++, j+=4) {
            ADU[j + myPriv->BO[0]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF00)     >>  8;
            ADU[j + myPriv->BO[1]] =  (unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF;
          }
          break;
        case U32:
          for (i=0, j=13; i < myPriv->MBcmd.NumVal; i++, j+=4) {
            ADU[j + myPriv->BO[0]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF000000) >> 24;
            ADU[j + myPriv->BO[1]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF0000)   >> 16;
            ADU[j + myPriv->BO[2]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF00)     >>  8;
            ADU[j + myPriv->BO[3]] =  (unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF;
          }
          break;
        case F32:
          for (i=0, j=13; i < myPriv->MBcmd.NumVal; i++, j+=4) {
            double2ieee(myPriv->MBcmd.MBdata[i], ieee);
            ADU[j + myPriv->BO[0]] = ieee[0];
            ADU[j + myPriv->BO[1]] = ieee[1];
            ADU[j + myPriv->BO[2]] = ieee[2];
            ADU[j + myPriv->BO[3]] = ieee[3];
          }
          break;
      }
      ADUlen  = 13 + myPriv->MBcmd.NumBytes;
      break;
    default:
      // Not Implemented
      ADUlen = 7;
      break;
  }
  /* Length field in MBAP includes byte 7 of the MBAP, so Length = ADU length - 6. */
  PDUlenPlusUID = ADUlen - 6;
  ADU[4]  = (PDUlenPlusUID & 0xFF00) >> 8;
  ADU[5]  =  PDUlenPlusUID & 0xFF;
  DynStringClear(send_buffer);
  DynStringConcatBytes(send_buffer, (char *) ADU, ADUlen);
  return 1;
}


/** @brief decode modbus response
*/
/* ADU[4] = MSB of DatLen, ADU[5] = LSB of DatLen */

/* NOTE: Min aduLen is 9. The header (7 bytes) + an Error code and Exception code */
static const int TIDidx=0;
static const int PIDidx=2;
static const int LENidx=4;
static const int UIDidx=6;
static const int CODEidx=7;
static const int DATLENidx=8;
static const int DATAidx=9;

static void StartIn(pPrivate myPriv) {
  myPriv->aduLen = 0;
  myPriv->RespLen = 0;
  myPriv->DatLen = 0;
}

static int ModbusInput(pPrivate myPriv, unsigned char chr, pDynString rdBuffer) {
  int i, byteCnt;
  unsigned char ieee[4];
  double dVal;
  long int liVal;

  if (myPriv->aduLen == 0)
    DynStringClear(rdBuffer);

  myPriv->ADU[myPriv->aduLen++] = chr;
  if (myPriv->RespLen == 0) {
    if (myPriv->aduLen >= UIDidx) {
      myPriv->RespLen = (myPriv->ADU[LENidx] << 8) + myPriv->ADU[LENidx+1];
      if (myPriv->RespLen <= 0) {
        DynStringReplace(rdBuffer, "ASCERR: Length field exception", 0);
        return PROTOCOL_ERROR;
      }
    } else {
      return PROTOCOL_CONTINUE;
    }
  }
  if (myPriv->aduLen < (6 + myPriv->RespLen)) {
    return PROTOCOL_CONTINUE;
  }

  if (myPriv->ADU[CODEidx] > 0x80) {
    /* snprintf(errMsg, ERRLEN, "TCPMODBUS: Function code %d exception %d:", ADU[CODEidx] & 0x0F, ADU[8] &0x0F); */
    DynStringReplace(rdBuffer, "ASCERR: Function code exception", 0);
    return PROTOCOL_ERROR;
  } else if (myPriv->ADU[CODEidx] != myPriv->MBcmd.Fcode) {
    /* snprintf(errMsg, ERRLEN, "TCPMODBUS: Requested function %d but got response for %d:", MBcmd.Fcode, ADU[CODEidx]); */
    DynStringReplace(rdBuffer, "ASCERR: Function code response exception", 0);
    return PROTOCOL_ERROR;
  } else {
    char temp[65];
    DynStringClear(rdBuffer);
    switch (myPriv->MBcmd.Fcode) {
      case RdCoil:
        byteCnt = myPriv->ADU[8];
        for (i=0; i < byteCnt; i++) {
          snprintf(temp, 64, "%d", myPriv->ADU[9+i]);
          DynStringConcat(rdBuffer, temp);
          DynStringConcatChar(rdBuffer, ' ');
        }
        break;
      case RdHldReg:
        byteCnt = myPriv->ADU[8];
        switch (myPriv->MBcmd.Dtype) {
          case U16:
            for (i=0; i < byteCnt/2; i++) {
              ieee[ myPriv->BO[0] ] = myPriv->ADU[9+i*2];
              ieee[ myPriv->BO[1] ] = myPriv->ADU[10+i*2];
              liVal = (ieee[0] << 8) | ieee[1];
              snprintf(temp, 64, "%ld", liVal);
              DynStringConcat(rdBuffer, temp);
              DynStringConcatChar(rdBuffer, ' ');
            }
            break;
          case U32:
          case F32:
            for (i=0; i < byteCnt/4; i++) {
              ieee[ myPriv->BO[0] ] = myPriv->ADU[9+i*4];
              ieee[ myPriv->BO[1] ] = myPriv->ADU[10+i*4];
              ieee[ myPriv->BO[2] ] = myPriv->ADU[11+i*4];
              ieee[ myPriv->BO[3] ] = myPriv->ADU[12+i*4];
              if (myPriv->MBcmd.Dtype == F32) {
                dVal = ieee2double(ieee);
                snprintf(temp, 64, "%g", dVal);
              } else if (myPriv->MBcmd.Dtype == U32) {
                liVal = (ieee[0] << 24) | (ieee[1] << 16) | (ieee[2] << 8) | ieee[3];
                snprintf(temp, 64, "%ld", liVal);
              }
              DynStringConcat(rdBuffer, temp);
              DynStringConcatChar(rdBuffer, ' ');
            }
            break;
          default:
            break;
        }
        break;
      case WrCoil:
        if (myPriv->ADU[7] == myPriv->MBcmd.Fcode && (myPriv->ADU[8] * 256 | myPriv->ADU[9]) == myPriv->MBcmd.SAddr &&
            myPriv->ADU[10] == myPriv->MBcmd.MBdata[0]) {
          DynStringReplace(rdBuffer, "OK", 0);
        } else {
          DynStringReplace(rdBuffer, "ASCERR: Response doesn't match set request", 0);
          return PROTOCOL_ERROR;
        }
        break;
      case WrMRegs:
        if (myPriv->ADU[7] == myPriv->MBcmd.Fcode && (myPriv->ADU[8] * 256 | myPriv->ADU[9]) == myPriv->MBcmd.SAddr &&
            (myPriv->ADU[10] * 256 | myPriv->ADU[11]) == myPriv->MBcmd.NumWr) {
          DynStringReplace(rdBuffer, "OK", 0);
        } else {
          DynStringReplace(rdBuffer, "ASCERR: Response doesn't match set request", 0);
          return PROTOCOL_ERROR;
        }
        break;
      default:
        break;
    }
  }
  myPriv->aduLen = 0, myPriv->DatLen = 0, myPriv->RespLen = 0;
  memset(myPriv->ADU, 0, ADUSIZE);
  return PROTOCOL_COMPLETE;
}

/*
 * Protocol Prepare Output
 */
static int Proto_Prepare(ProtoPrivate *priv, pDynString wrBuffer)
{
  ModbusOutput(&priv->mb_priv, wrBuffer, wrBuffer);
  StartIn(&priv->mb_priv);
  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 iRet;
  rxchar &= 0xFF;
  iRet = ModbusInput(&priv->mb_priv, rxchar, priv->rxBuffer);
  if (iRet == PROTOCOL_COMPLETE)
    return PROTOCOL_COMPLETE;
  if (iRet == PROTOCOL_CONTINUE)
    return PROTOCOL_CONTINUE;
  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, (char *) 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;
  }
}