/*---------------------------------------------------------------------------
modbus.c

Communication routines for Eurotherm ModBus instruments
  
Markus Zolliker, Aug 2005
----------------------------------------------------------------------------

there is no error return value, eab->errCode is used. On success, eab->errCode
is not changed, i.e. an existing errCode is not overwritten.
*/

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <fortify.h>
#include <math.h>
#include "sics.h"
#include "modbus.h"
#include "cnvrt.h"


/*----------------------------------------------------------------------------*/
static void uint2word(unsigned int adr, char word[2])
{
  unsigned char *uword;
  uword = (void *) word;
  uword[0] = adr / 256;
  uword[1] = adr % 256;
}

/*----------------------------------------------------------------------------*/
static unsigned int word2uint(char word[2])
{
  unsigned char *uword;
  uword = (void *) word;
  return uword[0] * 256 + uword[1];
}

/*----------------------------------------------------------------------------*/
static long quad2long(char word[4])
{
  unsigned char *uword;
  uword = (void *) word;
  return ((uword[0] * 256 + uword[1]) * 256 + uword[2]) * 256 + uword[3];
}

/*----------------------------------------------------------------------------*/
static void long2quad(long val, char word[4])
{
  unsigned char *uword;
  uword = (void *) word;
  uword[3] = val % 256;
  val = val / 256;
  uword[2] = val % 256;
  val = val / 256;
  uword[1] = val % 256;
  val = val / 256;
  uword[0] = val;
}

/*----------------------------------------------------------------------------*/
static void fadr2word(int adr, char word[2])
{
  unsigned char *uword;
  uword = (void *) word;
  uword[0] = (adr * 2) / 256 + 0x80;
  uword[1] = (adr * 2) % 256;
}

/*----------------------------------------------------------------------------*/
static int word2fadr(char word[2])
{
  unsigned char *uword;
  uword = (void *) word;
  return ((uword[0] & 0x7F) * 256 + uword[1]) / 2;
}

/*----------------------------------------------------------------------------*/
static int calc_crc(char *inp, int inpLen, int addCrc)
{

/* CRC runs cyclic Redundancy Check Algorithm on input inp */
/* Returns value of 16 bit CRC after completion and        */
/* adds 2 crc bytes if addCrc is not 0                     */
/* returns 0 if incoming message has correct CRC           */

  unsigned int crc = 0xffff;
  unsigned int next;
  int carry;
  int n;

  while (inpLen--) {
    next = *(unsigned char *) inp;
    crc ^= next;
    for (n = 0; n < 8; n++) {
      carry = crc & 1;
      crc >>= 1;
      if (carry) {
        crc ^= 0xA001;
      }
    }
    inp++;
  }
  if (addCrc) {
    inp[0] = crc % 256;
    inp[1] = crc / 256;
  }
  return crc;
}

/*----------------------------------------------------------------------------*/
void ModBusDump(EaseBase * eab, int iout, char *fmt, char *buffer,
                int length)
{
  char buf[128];
  int i;

  if (length > 40)
    length = 40;
  for (i = 0; i < length; i++) {
    snprintf(buf + i * 3, 4, " %2.2X", (unsigned char) buffer[i]);
  }
  buf[i * 3] = '\0';
  ParPrintf(eab, iout, fmt, buf);
}

/*----------------------------------------------------------------------------*/
void ModBusWrite(EaseBase * eab, int length)
{
  int iret;
  char trash[64];
  int l;

  assert(length < sizeof(eab->cmd));

  if (eab->errCode || eab->state == EASE_expect)
    return;
  while (availableRS232(eab->ser) == 1) {
    l = sizeof(trash);
    iret = readRS232TillTerm(eab->ser, trash, &l);
    if (iret < 0)
      break;
    ModBusDump(eab, -2, "trash %s\n", trash, l);
  }
  calc_crc(eab->cmd, length, 1);
  iret = writeRS232(eab->ser, eab->cmd, length + 2);
  if (iret < 0) {
    eab->errCode = iret;
    return;
  }
  ModBusDump(eab, -2, "cmd: %s", eab->cmd, length + 2);
  eab->state = EASE_expect;
  eab->cmdtime = time(NULL);
}

/*----------------------------------------------------------------------------*/
int ModBusHandler(void *object)
{
  int iret, l;
  EaseBase *eab = EaseBaseCast(object);

  if (eab->state < EASE_idle)
    goto quit;
  if (availableNetRS232(eab->ser) || availableRS232(eab->ser)) {
    eab->msg[0] = '\0';
    l = 5;
    iret = readRS232TillTerm(eab->ser, eab->ans, &l);
    if (eab->state != EASE_expect && eab->state != EASE_lost) {
      if (iret == 1) {
        ModBusDump(eab, eError, "unexpected answer %s", eab->ans, l);
      }
      goto quit;
    }
    if (iret == 1) {
      if (eab->cmd[0] != eab->ans[0]
          || eab->cmd[1] != (eab->ans[1] & 0x7F)) {
        iret = EASE_FAULT;
        ModBusDump(eab, eError, "bad answer: %s", eab->ans, l);
      } else if (eab->ans[1] & 0x80) {
        iret = EASE_FAULT;
        if (eab->ans[2] == 2) {
          ParPrintf(eab, eError, "ERROR: illegal address");
        } else if (eab->ans[2] == 3) {
          ParPrintf(eab, eError, "ERROR: illegal data");
        } else {
          ModBusDump(eab, eError, "bad answer: %s", eab->ans, l);
        };
      } else if (eab->ans[1] == 16) {   /* answer from write n words */
        l = 3;                  /* finish response */
        iret = readRS232TillTerm(eab->ser, eab->ans + 5, &l);
        l += 5;
      } else if (eab->ans[1] == 3) {    /* answer for read n words */
        l = eab->ans[2];        /* bytes to read */
        if (l < 0 || l >= sizeof(eab->ans) - 5) {
          l = 64;
        }
        iret = readRS232TillTerm(eab->ser, eab->ans + 5, &l);
        l += 5;
      } else if (eab->ans[1] == 8) {    /* loopback info */
        l = 1;
        iret = readRS232TillTerm(eab->ser, eab->ans + 5, &l);
        l += 5;
        if (eab->state == EASE_lost) {
          if (eab->ans[4] == 44 && eab->ans[5] == 55) {
            eab->state = EASE_idle;
          }
          goto quit;
        }
      } else {
        iret = EASE_ILL_ANS;
      }
      if (eab->state == EASE_lost) {
        goto quit;
      }
      ModBusDump(eab, -2, "ans: %s", eab->ans, l);
      if (iret == 1) {
        if (calc_crc(eab->ans, l, 0)) {
          iret = MODBUS_BAD_CRC;
        }
      }
    }
    if (iret != 1) {
      eab->errCode = iret;
      eab->state = EASE_idle;
      goto error;
    }
    eab->state = EASE_read;
  } else if (eab->state == EASE_expect) {
    if (time(NULL) > eab->cmdtime + 20) {
      eab->state = EASE_lost;
    }
  } else if (eab->state == EASE_lost) {
    if (time(NULL) > eab->cmdtime) {
      eab->cmd[1] = 8;
      eab->cmd[2] = 0;
      eab->cmd[3] = 0;
      eab->cmd[4] = 44;
      eab->cmd[5] = 55;
      ParPrintf(eab, -2, "loopback");
      ModBusWrite(eab, 6);
      eab->state = EASE_lost;
    }
  }
  goto quit;
error:
  /* EaseWriteError(eab); */
quit:
  return EaseHandler(eab);
}

/*----------------------------------------------------------------------------*/
void ModBusPutFloats(EaseBase * eab, int adr, int npar, float val[])
{
  int l, n;

  assert(npar <= 5);

  eab->cmd[0] = 1;              /* device address */
  eab->cmd[1] = 16;             /* write n words */
  fadr2word(adr, eab->cmd + 2);
  uint2word(npar * 2, eab->cmd + 4);
  eab->cmd[6] = npar * 4;       /* number of bytes */
  l = 7;
  for (n = 0; n < npar; n++) {
    double2ieee(val[n], eab->cmd + l);
    l += 4;
  }
  if (npar == 1) {
    ParPrintf(eab, -1, "write #%d %.5g", adr, val[0]);
  } else {
    ParPrintf(eab, -1, "write #%d %.5g %.5g ... (%d)", adr, val[0], val[1], npar);
  }
  ModBusWrite(eab, l);
}

/*----------------------------------------------------------------------------*/
void ModBusRequestValues(EaseBase * eab, int adr, int npar)
{

  assert(npar <= 14);

  eab->cmd[0] = 1;              /* device address */
  eab->cmd[1] = 3;              /* read n words */
  fadr2word(adr, eab->cmd + 2);
  uint2word(npar * 2, eab->cmd + 4);
  ParPrintf(eab, -2, "read #%d (%d values)", adr, npar);
  ModBusWrite(eab, 6);
}

/*----------------------------------------------------------------------------*/
static double ModBus2double(char ieee[4])
{
  long t;
  /* convert 32 bit values by guessing the type.
     will not work when a floating point value is a very low positive number
     (below 2^-125) or when a time is divisible by 65536 ms */
  if (ieee[0]) {                /* guess its a float */
    return ieee2double(ieee);
  }
  if (ieee[1] != 0 && ieee[2] == 0 && ieee[3] == 0) {
    /* guess its an int, a float 0 will also be returned correctly */
    return word2uint(ieee);
  }
  /* guess its a time */
  t = word2uint(ieee);
  return (t * 65536 + word2uint(ieee + 2)) * 0.001;
}

/*----------------------------------------------------------------------------*/
float ModBusGet(EaseBase * eab, int adr, int type)
{
  int startAdr;
  int i, n;

  if (eab->state != EASE_read) {
    return 0.0;
  }
  n = word2uint(eab->cmd + 4);
  if (n <= 2) return ModBusGetValue(eab, type);
  startAdr = word2fadr(eab->cmd + 2);
  i = adr - startAdr;
  if (i < 0 || i >= n) {
    return 0.0;
  }
  if (type == modBusTime) {
    return quad2long(eab->ans + 3 + i * 4) * 0.001;
  }
  return ieee2double(eab->ans + 3 + i * 4);
}

/*----------------------------------------------------------------------------*/
void ModBusRequestValue(EaseBase * eab, int adr, int type)
{

  eab->cmd[0] = 1;              /* device address */
  eab->cmd[1] = 3;              /* read n words */
  if (type == modBusInt) {
    uint2word(adr, eab->cmd + 2);
    uint2word(1, eab->cmd + 4);
  } else {
    fadr2word(adr, eab->cmd + 2);
    uint2word(2, eab->cmd + 4);
  }
  ParPrintf(eab, -2, "read #%d", adr);
  ModBusWrite(eab, 6);
}

/*----------------------------------------------------------------------------*/
void ModBusPutValue(EaseBase * eab, int adr, int type, float val)
{
  int l;

  eab->cmd[0] = 1;              /* device address */
  eab->cmd[1] = 16;             /* write n words */
  if (type == modBusInt) {
    uint2word(adr, eab->cmd + 2);
    uint2word(1, eab->cmd + 4);
    eab->cmd[6] = 2;
    uint2word((int) (val), eab->cmd + 7);
    ParPrintf(eab, -1, "write #%d %.5g", adr, val);
    ModBusWrite(eab, 9);
    return;
  }
  fadr2word(adr, eab->cmd + 2);
  uint2word(2, eab->cmd + 4);
  eab->cmd[6] = 4;              /* number of bytes */
  if (type == modBusFloat) {
    double2ieee(val, eab->cmd + 7);
  } else if (type == modBusTime) {
    long2quad((long) (val * 1000. + 0.5), eab->cmd + 7);
  } else {
    uint2word((int) (val + 0.5), eab->cmd + 7);
    eab->cmd[9] = 0;
    eab->cmd[10] = 0;
  }
  ParPrintf(eab, -1, "write #%d %.5g", adr, val);
  ModBusWrite(eab, 11);
}

/*----------------------------------------------------------------------------*/
float ModBusGetValue(EaseBase * eab, int type)
{

  if (eab->state != EASE_read) {
    return 0.0;
  }
  if (type == modBusFloat) {
    return ieee2double(eab->ans + 3);
  } else if (type == modBusTime) {
    return quad2long(eab->ans + 3) * 0.001;
  } else {
    return word2uint(eab->ans + 3);
  }
}