#include "archinc.h"
#include "cli.h"
#include "i2cmaster.h"
#include "i2cswitch.h"
#include "sfp.h"
#include "sfpdiag.h"

#include <stdio.h>
#include <stdlib.h>
#include <math.h> // for log10f() function

/*
 * Note about reported transmit and receive optical power:
 * 
 * Mark wants the transmit and receive power in dBm. To convert from
 * mW to dBm:
 *       dBm = 10 * log10(mW)
 *      1 mW = 0 dBm
 *
 * conversely, dBm to W/mW:
 *       W = 10 ^ ((dBm - 30)/10)
 *      mW = 10 ^ (dBm/10)
 */

#if (defined(CONFIG_INCLUDE_LPC210X_I2C) && defined(CONFIG_INCLUDE_I2C_SFPDIAG))

// SFP diagnostic address is hardwired to 0xa2

/*
 * Control and status bits at offset 0x110
 *
 * 7 6 5 4 | 3 2 1 0          
 * x       |         - Tx Disable state (R/O)
 *   x     |         - Soft Tx disable (1=laser disabled)
 *     x   |         - reserved
 *       x |         - Rx rate select state (R/O)
 *         | x       - Soft rx rate select (1=full bandwidth)
 *         |   x     - Tx fault state (R/O)
 *         |     x   - LOS output state (R/O)
 *         |       x - Data ready state (R/O 0=data ready)
 *
 */

/* bit fields for address 0xa0, offset 92 decimal */

#define DIAG_MON_RESERVED2            (0x80)
#define DIAG_MON_IMPLEMENTED          (0x40)
#define DIAG_MON_INTERNAL_CALIB       (0x20)
#define DIAG_MON_EXTERNAL_CALIB       (0x10)
#define DIAG_MON_RX_POWER_DATA_TYPE   (0x08)
#define DIAG_MON_TYPE_ADDR_CHANGE_REQ (0x04)
#define DIAG_MON_TYPE_RESERVED1       (0x02)
#define DIAG_MON_TYPE_RESERVED0       (0x01)

static int sfpdiagHandler(int argc, char * argv[], int flags);

typedef struct {
  signed short temp;             // 16-bit signed
  unsigned short vcc;            // 16-bit unsigned
  unsigned short txBiasCurrent;  // 16-bit unsigned
  unsigned short txPower;        // 16-bit unsigned
  unsigned short rxPower;        // 16-bit unsigned
  char status;
  } sfpDiagDataRaw_t;

/* Calibration constants for external calibration (bytes 56-95)*/

typedef struct __attribute__ ((__packed__)) {
  float rxPower4;
  float rxPower3;
  float rxPower2;
  float rxPower1;
  float rxPower0;
  unsigned short txCurrentSlope;
  signed short txCurrentOffset;
  unsigned short txPowerSlope;
  signed short txPowerOffset;
  unsigned short tempSlope;
  signed short tempOffset;
  unsigned short voltSlope;
  signed short voltOffset;
  unsigned char reserved[3];
  unsigned char checksum;
  } sfpDiagCalConst_t;

static sfpDiagData_t sfpDiagData;
static sfpDiagDataRaw_t sfpDiagRawData;
static sfpDiagCalConst_t sfpDiagCalConst;

static boolean doCalib[4];

static unsigned short wavelength[4];

static char diagOptsRaw[2];

static unsigned char sfpControlShadow[4];

static int setupBus(int bus, unsigned char probeAddr)
{
  int retVal;

  // setup the bus mux
  retVal = i2cswitchSetBus(bus);

  if (retVal == -1)
    return retVal;

  retVal = i2cmasterProbe(probeAddr);

  if (retVal == -1)
    return -2;

  return retVal;
}

static int getCalType(int bus, boolean * calibReq)
{
  int retVal = -1;
  unsigned char u8;

  /* 
   * determine if we need to calibrate (external) or if values are already
   * calibrated (internal). This is determined by bit 5 being set if 
   * internal, or bit 4 being set if external at offset 92 (decimal) of 
   * address 0xa0. 
   */

  if ((retVal = setupBus(bus, 0xa0)) != 0) {
    return retVal;
    }
  
  // send the initial offset (92 decimal), and then read 1 byte
  if (i2cmasterSendOne(0xa0, 92, false) != -1) {
    // TODO: check size
    retVal = i2cmasterReceive(0xa0, 1, true);

    if (retVal != -1) {
      i2cmasterGetData(&u8);
      if ((u8 & DIAG_MON_INTERNAL_CALIB) != 0) {
        // internal calibration -- absolute measurements returned
        *calibReq = false;
        }
      else if ((u8 & DIAG_MON_EXTERNAL_CALIB) != 0) {
        // external calibration -- values are ADC counts and need cal
        *calibReq = true;
        }
      else {
        // shouldn't get here, but what can you do? 
        *calibReq = false;
        }
      }
    }

  i2cswitchDisable();

  return 0;
}

int sfpdiagInit(void)
{
  int retVal;
  boolean modulePresent;
  int i;

  for(i=0;i<4;i++) {
    sfpModulePresent(i, &modulePresent);
#if 1
    if (modulePresent == true) {
      getCalType(i, &(doCalib[i]));
      }
#else
    doCalib[i] = false;
#endif
    }

  cliRegisterCommand("sfpdiag", sfpdiagHandler, "get SFP diagnostic info on [bus number 0-3]");

  return retVal;
}

static unsigned char sfpDiagBuffer[64];

static void decodeDiagDataRaw(char * pDiagData)
{
  union {
    unsigned short int uint16;
    signed short int sint16;
    unsigned char uint8[2];
    } u;

  int i;

  if (pDiagData == NULL)
    return;

  for(i=0;i<8;i++) {

    u.uint8[0] = pDiagData[((2*i)+1)];
    u.uint8[1] = pDiagData[(2*i)];

    switch(i) {
      case 0:
        sfpDiagRawData.temp = u.sint16;
        break;
      case 1:
        sfpDiagRawData.vcc = u.uint16;
        break;
      case 2:
        sfpDiagRawData.txBiasCurrent = u.uint16;
        break;
      case 3:
        sfpDiagRawData.txPower = u.uint16;
        break;
      case 4:
        sfpDiagRawData.rxPower = u.uint16;
        break;
      case 5:
      case 6:
        // reserved
        break;
      case 7:
        // need to specify u8[1] because we've swapped byte 110 into the MSB
        sfpDiagRawData.status = u.uint8[1];
        break;
      }
    }

  return;
}

/*
 * assumes internal calibration
 */
static void decodeDiagDataInternal(void)
{
  // 16-bit signed, 2's complement, LSB=1/256 deg C
  sfpDiagData.temp = ( ((float)sfpDiagRawData.temp) * ((float)(1.0/256.0)));

  // 16-bit unsigned, LSB=100mV
  sfpDiagData.vcc = ( ((float)sfpDiagRawData.vcc) * ((float)(1.0/10000.0)));

  // 16-bit unsigned, LSB=2uA, expressed as mA
  sfpDiagData.txBiasCurrent = ( ((float)sfpDiagRawData.txBiasCurrent) * ((float)(1.0/2000.0)) );

  // 16-bit unsigned, LSB=0.1 uWatt, expressed in mW
  sfpDiagData.txPower = ( ((float)sfpDiagRawData.txPower) * ((float)(1.0/10000.0)) );

  // convert txPower from mW to dBm
  if (sfpDiagData.txPower > ((float)0.0)) {
    sfpDiagData.txPower = ((float)10.0) * log10f(sfpDiagData.txPower);
    }
  else {
    sfpDiagData.txPower = ((float)-99.9999);
    }

  // 16-bit unsigned, LSB=0.1 uWatt, expressed in mW
  sfpDiagData.rxPower = ( ((float)sfpDiagRawData.rxPower) * ((float)(1.0/10000.0)) );

  // convert rxPower to from mW to dBm
  if (sfpDiagData.rxPower > ((float)0.0)) {
    sfpDiagData.rxPower = ((float)10.0) * log10f(sfpDiagData.rxPower);
    }
  else {
    sfpDiagData.rxPower = ((float)-99.9999);
    }

  sfpDiagData.status = sfpDiagRawData.status;

  return;
}

// converts funky 16-bit fixed point slope to float format
static float convertSlopeToFloat(unsigned short fixPointSlope)
{
  float retSlope;

  union {
    unsigned char u8[2];
    unsigned short u16;
    } u;

  u.u16 = fixPointSlope;

  retSlope = (float)(((float)u.u8[0]) / ((float)256.0));
  retSlope += ((float)u.u8[1]);

  return retSlope;
}

/*
 * assumes external calibration
 */
static void decodeDiagDataExternal(void)
{
  int i;
  float slope;

  // 16-bit signed
  slope = convertSlopeToFloat(sfpDiagCalConst.tempSlope);
  sfpDiagData.temp = ( ( ((float)sfpDiagRawData.temp) * slope ) + ((float)sfpDiagCalConst.tempOffset) );
  sfpDiagData.temp = ( (sfpDiagData.temp) * ((float)(1.0/256.0)));

  // 16-bit unsigned
  slope = convertSlopeToFloat(sfpDiagCalConst.voltSlope);
  sfpDiagData.vcc = ( ( ((float)sfpDiagRawData.vcc) * slope ) + ((float)sfpDiagCalConst.voltOffset) );
  sfpDiagData.vcc = ( (sfpDiagData.vcc) * ((float)(1.0/10000.0)));

  // 16-bit unsigned
  slope = convertSlopeToFloat(sfpDiagCalConst.txCurrentSlope);
  sfpDiagData.txBiasCurrent = ( ( ((float)sfpDiagRawData.txBiasCurrent) * slope ) + ((float)sfpDiagCalConst.txCurrentOffset) );
  sfpDiagData.txBiasCurrent = ( (sfpDiagData.txBiasCurrent) * ((float)(1.0/2000.0)) );

  // 16-bit unsigned
  slope = convertSlopeToFloat(sfpDiagCalConst.txPowerSlope);
  sfpDiagData.txPower = ( ( ((float)sfpDiagRawData.txPower) * slope ) + ((float)sfpDiagCalConst.txPowerOffset) );
  sfpDiagData.txPower = ( (sfpDiagData.txPower) * ((float)(1.0/10000.0)) );

  // convert txPower from mW to dBm
  if (sfpDiagData.txPower > ((float)0.0)) {
    sfpDiagData.txPower = ((float)10.0) * log10f(sfpDiagData.txPower);
    }
  else {
    sfpDiagData.txPower = ((float)-99.9999);
    }

  // 16-bit unsigned
  sfpDiagData.rxPower = ( ( ((float)sfpDiagRawData.rxPower) * sfpDiagCalConst.rxPower4 ) +
                          ( ((float)sfpDiagRawData.rxPower) * sfpDiagCalConst.rxPower3 ) +
                          ( ((float)sfpDiagRawData.rxPower) * sfpDiagCalConst.rxPower2 ) +
                          ( ((float)sfpDiagRawData.rxPower) * sfpDiagCalConst.rxPower1 ) +
                          sfpDiagCalConst.rxPower0 );
  sfpDiagData.rxPower = ( (sfpDiagData.rxPower) * ((float)(1.0/10000.0)) );

  // convert rxPower from mW to dBm
  if (sfpDiagData.rxPower > ((float)0.0)) {
    sfpDiagData.rxPower = ((float)10.0) * log10f(sfpDiagData.rxPower);
    }
  else {
    sfpDiagData.rxPower = ((float)-99.9999);
    }

  sfpDiagData.status = sfpDiagRawData.status;

  return;
}

static void swapFour(unsigned char *lsb)
{
  unsigned char temp;

  // swap the outer two bytes
  temp = lsb[0];
  lsb[0] = lsb[3];
  lsb[3] = temp;

  // swap the inner two bytes
  temp = lsb[1];
  lsb[1] = lsb[2];
  lsb[2] = temp;
}

static void swapTwo(unsigned char *lsb)
{
  unsigned char temp;

  temp = lsb[0];
  lsb[0] = lsb[1];
  lsb[1] = temp;
}

static void swapCalData(unsigned char * pCalData)
{
  int i;

  // 4 byte swap first 5 values
  for (i=0;i<5;i++) {
    swapFour(pCalData);
    pCalData += 4;
    }

  for(i=0;i<8;i++) {
    swapTwo(pCalData);
    pCalData += 2;
    }
}

int sfpdiagGetCalData(int bus)
{
  int retVal = -1;
  int i;
  unsigned char * pCalData = (unsigned char *)(&sfpDiagCalConst);

  if ((retVal = setupBus(bus, 0xa2)) != 0)
    return retVal;
  
  // send the initial offset (56 decimal), and then read 40 bytes
  if (i2cmasterSendOne(0xa2, 56, false) != -1) {
    // TODO: check size
    retVal = i2cmasterReceive(0xa2, 40, true);
    for(i=0;((i<40) && (retVal != -1)); i++, pCalData++) {
      retVal = i2cmasterGetData(pCalData);
      }
    }

  i2cswitchDisable();

  // perform needed byte swapping
  swapCalData((unsigned char *)(&sfpDiagCalConst));

  return retVal;
}

// retrieves the diagnostic info from a particular bus
int sfpdiagGetWavelength(int bus)
{
  int retVal = -1;
  int i;

  union {
    unsigned short uint16;
    char uint8[2];
  } u;

  if ((retVal = setupBus(bus, 0xa0)) != 0)
    return retVal;
  
  // send the initial offset (60 decimal!!), and then read 2 bytes
  if (i2cmasterSendOne(0xa0, 60, false) != -1) {
    // TODO: check size
    retVal = i2cmasterReceive(0xa0, 2, true);

    if (retVal != -1) {
      i2cmasterGetData(&(u.uint8[1]));
      i2cmasterGetData(&(u.uint8[0]));
      wavelength[bus] = u.uint16;
      }
    }

  i2cswitchDisable();

  return 0;
}


// retrieves the diagnostic info from a particular bus
int sfpdiagGetDiags(int bus)
{
  int retVal = -1;
  int i;

  if ((retVal = setupBus(bus, 0xa2)) != 0)
    return retVal;
  
  // send the initial offset (96 decimal!!), and then read 16 bytes
  if (i2cmasterSendOne(0xa2, 96, false) != -1) {
    // TODO: check size
    retVal = i2cmasterReceive(0xa2, 24, true);
    for(i=0;((i<24) && (retVal != -1));i++) {
      retVal = i2cmasterGetData(&(sfpDiagBuffer[i]));
      }
    }

  i2cswitchDisable();

  decodeDiagDataRaw(sfpDiagBuffer);

  if (doCalib[bus] == true) {
    // external 
    if (sfpdiagGetCalData(bus) == 0) {
      decodeDiagDataExternal();
      retVal = 0;
      }
    }
  else {
    // internal 
    decodeDiagDataInternal();
    retVal = 0;
    }

  return retVal;
}


int sfpdiagSoftTxDisable(int bus, boolean doDisable)
{
  int retVal = -1;
  int i;

  if ((retVal = setupBus(bus, 0xa2)) != 0)
    return retVal;

    // setup value to write
  if (doDisable == true)
    sfpControlShadow[bus] |= 0x40;
  else
    sfpControlShadow[bus] &= ~0x40;

  i2cmasterPut(0xa2, 110);
  i2cmasterPut(0xa2, sfpControlShadow[bus]);
  retVal = i2cmasterSend(0xa2, true);

  i2cswitchDisable();
  return retVal;
}

int sfpdiagSoftRxRateSelect(int bus, boolean fullBandwidth)
{
  int retVal = -1;
  int i;

  if ((retVal = setupBus(bus, 0xa2)) != 0)
    return retVal;

    // setup value to write
  if (fullBandwidth == true)
    sfpControlShadow[bus] |= 0x08;
  else
    sfpControlShadow[bus] &= ~0x08;

  i2cmasterPut(0xa2, 110);
  i2cmasterPut(0xa2, sfpControlShadow[bus]);
  retVal = i2cmasterSend(0xa2, true);

  i2cswitchDisable();
  return retVal;
}

sfpDiagData_t * sfpdiagRetrieveData(int bus)
{
  return (&sfpDiagData);
}

sfpDiagDataRaw_t * sfpdiagRetrieveDataRaw(int bus)
{
  return (&sfpDiagRawData);
}

static void printSfpDiags(int diagChannel)
{
  int bufLen;
  char * pOutBuf;

  pOutBuf = clicharGetOutputBuffer(&bufLen);
    
  sfpdiagGetDiags(diagChannel);

  sprintf(pOutBuf, "--- SFP diagnostics for channel [%d] ---\r\n", diagChannel);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "temperature   : raw[%#04x] %4.2f deg C\r\n", sfpDiagRawData.temp, sfpDiagData.temp);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "Vcc           : raw[%#04x] %2.4f V\r\n", sfpDiagRawData.vcc, sfpDiagData.vcc);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "txBiasCurrent : raw[%#04x] %2.4f mA\r\n", sfpDiagRawData.txBiasCurrent, sfpDiagData.txBiasCurrent);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "txPower       : raw[%#04x] %2.4f dBm\r\n", sfpDiagRawData.txPower, sfpDiagData.txPower);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "rxPower       : raw[%#04x] %2.4f dBm\r\n", sfpDiagRawData.rxPower, sfpDiagData.rxPower);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "status        : raw[%#02x]\r\n", sfpDiagData.status);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sfpdiagGetWavelength(diagChannel);
  sprintf(pOutBuf, "wavelength    : %4u nm\r\n", wavelength[diagChannel]);
  clicharSendOutputBuffer(pOutBuf, bufLen);
  sprintf(pOutBuf, "calibration   : %s\r\n", ((doCalib[diagChannel] == false) ? "internal" : "external") );
  clicharSendOutputBuffer(pOutBuf, bufLen);

  return;
}

static void printMinSfpDiags(int diagChannel)
{
  int bufLen;
  char * pOutBuf;

  pOutBuf = clicharGetOutputBuffer(&bufLen);
    
  sfpdiagGetDiags(diagChannel);

  sprintf(pOutBuf, "channel=%d, temp=%4.2f, vcc=%2.4f, txBiasI=%2.4f, txPower=%2.4f, rxPower=%2.4f, status=%#02x\r\n",
    diagChannel, sfpDiagData.temp, sfpDiagData.vcc, sfpDiagData.txBiasCurrent, sfpDiagData.txPower, 
    sfpDiagData.rxPower, sfpDiagData.status);
  clicharSendOutputBuffer(pOutBuf, bufLen);

  return;

}

int sfpdiagGetData(int channel, sfpDiagData_t **pSfpData)
{
  if (pSfpData == NULL)
    return -1;

  sfpdiagGetDiags(channel);
  *pSfpData = sfpdiagRetrieveData(channel);

  return 0;
}

static int sfpdiagHandler(int argc, char * argv[], int flags)
{
  int retVal = -1;
  int bufLen;
  char * pOutBuf;
  int i;
  char blah;
  int probeBus;
  boolean moduleThere;

  pOutBuf = clicharGetOutputBuffer(&bufLen);

  if (argc == 2) {
    probeBus = (int)strtol(argv[1], (char **)NULL, 10);
    if ((probeBus < 0) || (probeBus > 3)) {
      return retVal;
      }
    
    sfpModulePresent(probeBus, &moduleThere);
    if (sfpProbe(probeBus) != 0) {
      sprintf(pOutBuf, "%s: error SFP not found on bus [%d]\r\n", argv[0], probeBus);
      clicharSendOutputBuffer(pOutBuf, bufLen);
      }
    else {
      printSfpDiags(probeBus);
      }
    retVal = 0;
    }
  else if (argc == 1) {
    for(i=0;i<4;i++) {
      sfpModulePresent(i, &moduleThere);
      if (moduleThere == true) {
        printMinSfpDiags(i);
        }
      }
    retVal = 0;
    }

  return retVal;
}

#endif /* (defined(CONFIG_INCLUDE_LPC210X_I2C) && defined(CONFIG_INCLUDE_I2C_SFPDIAG)) */