sics/site_ansto/hardsup/nhq200util.c

/*--------------------------------------------------------------------------
  N H Q 2 0 0 U T I L

  A few utility functions for dealing with a NHQ200 voltage controller
  within the ANSTO setup: host === TCP/IP === MOXA === RS-232.

  Mark Koennecke, Juli 1997
  Mark Lesha, January 2006 (based on ITC4 code)
  Douglas Clowes, December 2006 (based on LAKESHORE340 code)

  Copyright:

  Labor fuer Neutronenstreuung
  Paul Scherrer Institut
  CH-5423 Villigen-PSI


  The authors hereby grant permission to use, copy, modify, distribute,
  and license this software and its documentation for any purpose, provided
  that existing copyright notices are retained in all copies and that this
  notice is included verbatim in any distributions. No written agreement,
  license, or royalty fee is required for any of the authorized uses.
  Modifications to this software may be copyrighted by their authors
  and need not follow the licensing terms described here, provided that
  the new terms are clearly indicated on the first page of each file where
  they apply.

  IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
  FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
  ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
  DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.

  THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE
  IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
  NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
  MODIFICATIONS.
----------------------------------------------------------------------------*/
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <netinet/tcp.h>
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <fortify.h>
#include <sics.h>
#include <modriv.h>
#include <nwatch.h>
#include <multichan.h>
#include "nhq200util.h"
/*-------------------------------------------------------------------*/

typedef struct __command Command, *pCommand;
typedef int (*CommandCallback)(void* ctx, const char* resp, int resp_len);

struct __command {
  pMultiChan unit;
  int cstate;
  int lstate;
  char* out_buf;
  int out_len;
  int out_idx;
  char* inp_buf;
  int inp_len;
  int inp_idx;
  CommandCallback func;
  void* cntx;
};

static int NHQ_Tx1(void* ctx)
{
  int iRet = 1;
  pCommand myCmd = (pCommand) ctx;

  assert(myCmd);
  iRet = MultiChanWrite(myCmd->unit, &myCmd->out_buf[myCmd->out_idx], 1);
  return iRet;
}

static int NHQ_Tx(void* ctx)
{
  pCommand myCmd = (pCommand) ctx;

  /*
   * Set/reset command states for send/resend of command
   */
  myCmd->cstate = 0;
  myCmd->lstate = 0;
  myCmd->out_idx = 0;
  myCmd->inp_idx = 0;
  return NHQ_Tx1(myCmd);
}

static int NHQ_Rx(void* ctx, int rxchar)
{
  int iRet = 1;
  pCommand myCmd = (pCommand) ctx;

  switch (myCmd->cstate) {
    case 0: /* send with echo */
      if (rxchar != myCmd->out_buf[myCmd->out_idx]) {
        /* TODO: bad echo */
      }
      else if (rxchar == 0x0A &&
          myCmd->out_idx > 0 &&
          myCmd->out_buf[myCmd->out_idx - 1] == 0x0D) {
        myCmd->inp_idx = 0;
        myCmd->cstate = 1;
        /* TODO: end of line */
      }
      else if (myCmd->out_idx < myCmd->out_len) {
        myCmd->out_idx++;
        iRet = NHQ_Tx1(myCmd);
      }
      else {
        /* TODO: out of data */
      }
      break;
    case 1: /* receiving reply */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == 0x0D)
        myCmd->cstate = 2;
      break;
    case 2: /* received CR and looking for LF */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == 0x0A) {
        /* end of line */
        myCmd->cstate = 3;
        myCmd->inp_idx -= 2;
        myCmd->inp_buf[myCmd->inp_idx] = '\0';
        if (myCmd->func)
          iRet = myCmd->func(myCmd->cntx, myCmd->inp_buf, myCmd->inp_idx);
        else
          iRet = 0;
      }
      else
        myCmd->cstate = 1;
      break;
  }
  if (iRet == 0) { /* end of command */
    free(myCmd->out_buf);
    free(myCmd->inp_buf);
    free(myCmd);
  }
  return iRet;
}

int NHQ_SendCmd(pMultiChan unit,
    char* command, int cmd_len,
    CommandCallback callback, void* context, int rsp_len)
{
  pCommand myCmd = NULL;

  assert(unit);
  myCmd = (pCommand) malloc(sizeof(Command));
  assert(myCmd);
  memset(myCmd, 0, sizeof(Command));
  myCmd->out_buf = (char*) malloc(cmd_len + 5);
  memcpy(myCmd->out_buf, command, cmd_len);
  myCmd->out_len = cmd_len;
  if (myCmd->out_len < 2 ||
      myCmd->out_buf[myCmd->out_len - 1] != 0x0A ||
      myCmd->out_buf[myCmd->out_len - 2] != 0x0D) {
    myCmd->out_buf[myCmd->out_len++] = 0x0D;
    myCmd->out_buf[myCmd->out_len++] = 0x0A;
  }
  myCmd->out_buf[myCmd->out_len] = '\0';
  myCmd->func = callback;
  myCmd->cntx = context;
  if (rsp_len == 0)
    myCmd->inp_buf = NULL;
  else {
    myCmd->inp_buf = malloc(rsp_len + 1);
    memset(myCmd->inp_buf, 0, rsp_len + 1);
  }
  myCmd->inp_len = rsp_len;
  myCmd->unit = unit;
  return MultiChanEnque(unit, myCmd, NHQ_Tx, NHQ_Rx);
}

static void NHQ_Notify(void* context, int event)
{
  pNHQ200 self = (pNHQ200) context;

  switch (event) {
    case MCC_RECONNECT:
      do {
        mkChannel* sock = MultiChanGetSocket(self->mcc);
        int flag = 1;
        setsockopt(sock->sockid, /* socket affected */
            IPPROTO_TCP,     /* set option at TCP level */
            TCP_NODELAY,     /* name of option */
            (char *) &flag,  /* the cast is historical cruft */
            sizeof(int));    /* length of option value */
        return;
      } while (0);
  }
  return;
}

static void parse_hash(pNHQ200 self, const char* resp, int resp_len)
{
  int iSrc;
  int iDst;
  iSrc = 0;
  iDst = 0;
  while (iSrc < resp_len && resp[iSrc]) {
    if (resp[iSrc] == ';') {
      ++iSrc;
      break;
    }
    self->serial_number[iDst++] = resp[iSrc++];
  }
  self->serial_number[iDst] = '\0';
  iDst = 0;
  while (iSrc < resp_len && resp[iSrc]) {
    if (resp[iSrc] == ';') {
      ++iSrc;
      break;
    }
    self->software_version[iDst++] = resp[iSrc++];
  }
  self->software_version[iDst] = '\0';
  iDst = 0;
  while (iSrc < resp_len && resp[iSrc]) {
    if (resp[iSrc] == ';') {
      ++iSrc;
      break;
    }
    self->voltage_max[iDst++] = resp[iSrc++];
  }
  self->voltage_max[iDst] = '\0';
  iDst = 0;
  while (iSrc < resp_len && resp[iSrc]) {
    if (resp[iSrc] == ';') {
      ++iSrc;
      break;
    }
    self->current_max[iDst++] = resp[iSrc++];
  }
  self->current_max[iDst] = '\0';
  /* TODO: convert voltage and current */
}

static void parse_Sx(pNHQ200 self, const char* resp, int resp_len)
{
  memcpy(self->status_s, resp, resp_len);
  if (resp_len > 0 && self->status_s[resp_len - 1] == ' ')
    --resp_len;
  self->status_s[resp_len] = '\0';
}

static void parse_Tx(pNHQ200 self, const char* resp, int resp_len)
{
  memcpy(self->status_t, resp, resp_len);
  self->status_t[resp_len] = '\0';
  self->module_status = strtol(self->status_t, NULL, 10);
}

static void parse_Mx(pNHQ200 self, const char* resp, int resp_len)
{
  self->vmax_percent = strtol(resp, NULL, 10);
}

static void parse_Nx(pNHQ200 self, const char* resp, int resp_len)
{
  self->imax_percent = strtol(resp, NULL, 10);
}

static void parse_Vx(pNHQ200 self, const char* resp, int resp_len)
{
  self->ramp_input = strtol(resp, NULL, 10);
}

#define STATE_HASH  1
#define STATE_SX    2
#define STATE_TX    3
#define STATE_MX    4
#define STATE_NX    5
#define STATE_VX    6
#define STATE_END   9
static int InitCallback(void* ctx, const char* resp, int resp_len)
{
  char cmd[20];
  int cmd_len;
  /* TODO: FIXME finish initialisation */
  pNHQ200 self = (pNHQ200) ctx;
  switch (self->iState) {
    case 0: /* Initial */
      cmd_len = snprintf(cmd, sizeof(cmd), "#");
      NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
      self->iState = STATE_HASH;
      break;
    case STATE_HASH: /* # */
      parse_hash(self, resp, resp_len);
      cmd_len = snprintf(cmd, sizeof(cmd), "S%d", self->iControl);
      NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
      self->iState = STATE_SX;
      break;
    case STATE_SX: /* Sx */
      parse_Sx(self, resp, resp_len);
      cmd_len = snprintf(cmd, sizeof(cmd), "T%d", self->iControl);
      NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
      self->iState = STATE_TX;
      break;
    case STATE_TX: /* Tx */
      parse_Tx(self, resp, resp_len);
      cmd_len = snprintf(cmd, sizeof(cmd), "M%d", self->iControl);
      NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
      self->iState = STATE_MX;
      break;
    case STATE_MX: /* Mx */
      parse_Mx(self, resp, resp_len);
      cmd_len = snprintf(cmd, sizeof(cmd), "N%d", self->iControl);
      NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
      self->iState = STATE_NX;
      break;
    case STATE_NX: /* Nx */
      parse_Nx(self, resp, resp_len);
      cmd_len = snprintf(cmd, sizeof(cmd), "V%d", self->iControl);
      NHQ_SendCmd(self->mcc, cmd, cmd_len, InitCallback, self, 80);
      self->iState = STATE_VX;
      break;
    case STATE_VX: /* Vx */
      parse_Vx(self, resp, resp_len);
      self->iState = STATE_END;
      break;
    case STATE_END:
      break;
  }
  return 0;
}

static void NHQ_Init(pNHQ200 self)
{
  self->iState = 0;
  NHQ_SendCmd(self->mcc, "", 0, InitCallback, self, 80);
}

/*
 * \brief GetCallback is the callback for the get position/value command.
 */
static int GetCallback(void* ctx, const char* resp, int resp_len)
{
  int iRet;
  float fRead;
  pNHQ200 self = (pNHQ200) ctx;

  iRet = sscanf(resp,"%g",&fRead);
  if(iRet != 1) { // Not a number, probably an error response
    self->iError = NHQ200__BADREAD;
  }
  else {
    if (fRead < 0)
      fRead = -fRead;
    self->fValue = fRead;
  }
  self->iGetOut = 0;
  return 0;
}

/*
 * \brief TransCallback is the callback for the general command transaction.
 */
static int TransCallback(void* ctx, const char* resp, int resp_len)
{
  pNHQ200 self = (pNHQ200) ctx;

  memcpy(self->transReply, resp, resp_len);
  self->transReply[resp_len] = '\0';
  self->transWait = 0;
  return 0;
}

/*------------------------------------------------------------------------*/
int transactNHQ200(pNHQ200 self, void *send, int sendLen,
    void *reply, int replyLen)
{
  assert(self);
  self->transReply = reply;
  self->transWait = 1;
  NHQ_SendCmd(self->mcc,
      send, sendLen,
      TransCallback, self, replyLen);
  while (self->transWait)
    TaskYield(pServ->pTasker);
  return 1;
}

/*------------------------------------------------------------------------*/
int NHQ200_Check_Status(pNHQ200 self)
  /* Can be called to check for correct operation of the NHQ200 */
{
  int iRet, iRetry, busy, notbusy;
  char pCommand[20];
  char pReply[132];
  /* Check the busy status.
  * While busy, wait but not too long - set an upper limit of about 100
  * queries, should translate to about 1 or 2 seconds which is enough time for
  * any commands to complete. Since we don't issue any time-consuming commands,
  * this shouldn't be necessary anyway.
  * Register a comms failure if a not-busy response isn't able to be received.
   */
  iRetry=0;
  printf("Checking status...");
  do
  {
    sprintf(pCommand,"S%d", self->iControl);
    if ((iRet=transactNHQ200(self,pCommand,strlen(pCommand),pReply,79))<=0)
    {
      printf("Comms error!\n");
      return iRet; // Comms problem
    }
    sprintf(pCommand,"S%d=ON", self->iControl);
    busy=(strncmp(pReply,pCommand,5) != 0);
    notbusy=!busy;
    if (notbusy)
    {
      printf("Status OK.\n");
      return 1; // Lakeshore 340 is ready to accept command
    }
  } while((++iRetry<100)&&busy);
  /* If we fell out of the loop, the Lakeshore 340 is either still busy */
  /* or some bad response was received, log the response. */
  sprintf(self->pAns,"BUSY response=%s",pReply);
  printf("Busy or bad response received!\n");
  return NHQ200__BADREAD;
}

/* Operations common to both Open and Config functions */
static int NHQ200_Setup(pNHQ200 self, int iControl)
{
  int iRet;
  char pCommand[20];
  char pReply[132];

  if (!self)
    return NHQ200__BADCOM;


  return 1; /* Success */
}

int NHQ200_Open(pNHQ200 *pData, char *pName, int iSensor, int iCTRL, int iMode)
{
  int iRet = 1;
  mkChannel* sock = NULL;
  pNHQ200 self = NULL;

  self = (pNHQ200)malloc(sizeof(NHQ200));
  if(self == NULL)
  {
    return NHQ200__BADMALLOC;
  }
  memset(self, 0, sizeof(NHQ200));
  *pData = self;
  self->iControl = iCTRL;
  self->iRead    = iSensor;
  self->iReadOnly = iMode;

  /* The NHQ200 doesn't require divisors or multipliers
     and they are always forced to 1.0 */
  self->fDiv = 1.0;
  self->fMult = 1.0;

  if (MultiChanCreate(pName, &self->mcc) == 0) {
    return NHQ200__NONHQ200;
  }
  MultiChanSetNotify(self->mcc, self, NHQ_Notify);

  sock = MultiChanGetSocket(self->mcc);
  if (sock) {
    int flag = 1;
    iRet = setsockopt(sock->sockid, /* socket affected */
        IPPROTO_TCP,     /* set option at TCP level */
        TCP_NODELAY,     /* name of option */
        (char *) &flag,  /* the cast is historical cruft */
        sizeof(int));    /* length of option value */
    if (iRet < 0)
      return NHQ200__BADCOM;
  }
  else
    return NHQ200__BADCOM;

  NHQ_Init(self);
  return iRet;
}
/*--------------------------------------------------------------------------*/
void NHQ200_Close(pNHQ200 *pData)
{
  pNHQ200 self;

  self = *pData;
  if (!self)
    return; // Just in case

  return;
}
/*--------------------------------------------------------------------------*/
int NHQ200_Config(pNHQ200 *pData, int iTmo, int iRead, int iControl,
    float fDiv,float fMult)
{
  pNHQ200 self;

  self = *pData;

  return NHQ200_Setup(self, iControl);
}
/*--------------------------------------------------------------------------*/
int NHQ200_Send(pNHQ200 *pData, char *pCommand, char *pReply, int iLen)
{
  int iRet;
  int commandlen;
  pNHQ200 self;

  self = *pData;

  /* Send command direct to the NHQ200 */
  commandlen=strlen(pCommand);

  iRet=transactNHQ200(self,pCommand,commandlen,pReply,iLen);

  return iRet;
}
/*--------------------------------------------------------------------------*/
int NHQ200_Read(pNHQ200 *pData, float *fVal)
{
  char pCommand[20];
  int iRet;
  pNHQ200 self;

  self = *pData;


  /* for the NHQ200 there are two units available */
  sprintf(pCommand,"U%d", self->iControl);

  if (self->iGetOut == 0) {
    struct timeval tv_this;
    gettimeofday(&tv_this, NULL);
    if ((tv_this.tv_sec - self->tv_last.tv_sec) > 0) {
      NHQ_SendCmd(self->mcc,
          pCommand, 2,
          GetCallback, self, 132);
      self->iGetOut = 1;
      self->tv_last = tv_this;
    }
  }
  *fVal = self->fValue;
  iRet = 1;

  return iRet;
}
/*-------------------------------------------------------------------------*/
int NHQ200_Set(pNHQ200 *pData, float fVal)
{
  char pCommand[20], pCommandRead[20], pReply[132], pCommandGo[20];
  int iRet, i;
  const float fPrecision = 0.1;
  float fDelta, fRead;
  pNHQ200 self;

  self = *pData;

  if(self->iReadOnly)
  {
    return NHQ200__READONLY;
  }

  /* command to set the voltage */
  sprintf(pCommand,"D%d=%d", self->iControl, (int) (fVal + 0.5));
  /* command to read back and check the set value */
  sprintf(pCommandRead,"D%d", self->iControl);

  /* send Dn=nnn command, we get a blank line response */
  iRet = transactNHQ200(self,pCommand,strlen(pCommand),pReply,131);
  if (iRet <= 0)
    return iRet;
  /* read the set value again using the Dn command */
  iRet = transactNHQ200(self,pCommandRead,strlen(pCommandRead),pReply,131);
  if (iRet <= 0)
    return iRet;
  printf("D%d:  Response %d chars: '%s'\n",self->iControl, iRet, pReply);
  /* Convert the value read back. */
  if(sscanf(pReply,"%g",&fRead)!=1)
    return NHQ200__BADREAD;
  printf(" Parsed response OK, value=%g\n",fRead);
  /* check the value read back */
  printf(" Setpoint=%g actual=%g\n",fVal,fRead);
  fDelta = fRead - fVal;
  if(fDelta < 0)
    fDelta = -fDelta;
  printf(" delta=%g precision=%g\n",fDelta,fPrecision);
  if(fDelta < fPrecision)
  {
    sprintf(pCommandGo, "G%d", self->iControl);
    iRet = transactNHQ200(self,pCommandGo,strlen(pCommandGo),pReply,131);
    if (iRet <= 0)
      return iRet;
    printf("G%d:  Response %d chars: '%s'\n",self->iControl, iRet, pReply);
    printf("SET OK, checking status and returning.\n");
    return 1;
  }
  printf("SET failed!\n");
  return NHQ200__BADSET;
}
/*-------------------------------------------------------------------------*/
void NHQ200_ErrorTxt(pNHQ200 *pData,int iCode, char *pError, int iLen)
{
  char pBueffel[512];
  pNHQ200 self;

  self = *pData;

  switch(iCode)
  {
    case NHQ200__BADCOM:
      sprintf(pBueffel,"NHQ200: Invalid command or offline, got %s",
          self->pAns);
      strncpy(pError,pBueffel,iLen);
      break;
    case NHQ200__BADPAR:
      strncpy(pError,"NHQ200: Invalid parameter specified",iLen);
      break;
    case NHQ200__BADMALLOC:
      strncpy(pError,"NHQ200: Error allocating memory in NHQ200",iLen);
      break;
    case NHQ200__BADREAD:
      strncpy(pError,"NHQ200: Badly formatted answer",iLen);
      break;
    case NHQ200__BADSET:
      strncpy(pError,"NHQ200: Failed to write new set value to NHQ200",iLen);
      break;
    case NHQ200__FAULT: // Covers various NHQ200 self-diagnosed fault conditions
      sprintf(pBueffel,"NHQ200: Internal fault: %s",self->pAns);
      strncpy(pError,pBueffel,iLen);
      break;
    case NHQ200__NONHQ200:
      sprintf(pBueffel,"NHQ200: Unit not found: %s",self->pAns);
      strncpy(pError,pBueffel,iLen);
      break;
    default:
      snprintf(pError, iLen, "NHQ200: Unknown Error: %d", iCode);
      break;
  }
}

static int GALIL_Tx(void* ctx)
{
  int iRet = 1;
  pCommand myCmd = (pCommand) ctx;

  if (myCmd) {
    iRet = MultiChanWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
    /* TODO handle errors */
    if (iRet < 0) { /* TODO: EOF */
      iRet = MultiChanReconnect(myCmd->unit);
      if (iRet == 0)
        return 0;
    }
  }
  return 1;
}

static int GALIL_Rx(void* ctx, int rxchar)
{
  int iRet = 1;
  pCommand myCmd = (pCommand) ctx;

  switch (myCmd->cstate) {
    case 0: /* first character */
      if (rxchar == ':') {
        /* normal prompt */
        myCmd->cstate = 99;
      }
      else if (rxchar == '?') {
        /* TODO: error prompt, send TC1 */
        myCmd->cstate = 99;
      }
      else {
        /* normal data */
        myCmd->cstate = 1;
      }
      /* note fallthrough */
    case 1: /* receiving reply */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == 0x0D)
        myCmd->cstate = 2;
      break;
    case 2: /* received CR and looking for LF */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == 0x0A) {
        myCmd->cstate = 99;
        /* end of line */
      }
      else
        myCmd->cstate = 1;
      break;
  }
  if (myCmd->cstate == 99) {
    myCmd->inp_buf[myCmd->inp_idx] = '\0';
    if (myCmd->func)
      iRet = myCmd->func(myCmd->cntx, myCmd->inp_buf, myCmd->inp_idx);
    else
      iRet = 0;
    myCmd->cstate = 0;
    myCmd->inp_idx = 0;
  }
  if (iRet == 0) { /* end of command */
    free(myCmd->out_buf);
    free(myCmd->inp_buf);
    free(myCmd);
  }
  return iRet;
}