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


/*-------------------------------------------------------------------------*/
static void double2ieee(double input, 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 {
    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(char ieee[4]) {

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

  long mantissa;
  double output;
  int exponent;
  
  mantissa = ((ieee[1] << 16) & 0x7FFFFF) 
           | ((ieee[2] <<  8) & 0xFF00)
           | ((ieee[3]      ) & 0xFF);
  
  exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */
  if (exponent == 0 && mantissa == 0) {
    return 0.0;
  }
  output = ldexp(mantissa, -23) + 1.0;
  if (ieee[0] & 0x80) {
    output = -output;
  }
  return output * ldexp(1, exponent - 127);
}
/*----------------------------------------------------------------------------*/
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        */
/* always adds 2 crc bytes to message                      */
/* 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;
      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;
  }
  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);
  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;
  
  if (eab->state != EASE_read) {
    return 0.0;
  }
  startAdr = word2fadr(eab->cmd + 2);
  i = adr - startAdr;
  if (i < 0 || i >= word2uint(eab->cmd + 4)) {
    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);
  }
  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);
    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;
  }
  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);
  }  
}