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

#include <stdio.h>
#include <stdlib.h>

#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)
 *
 */

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

typedef struct {
  unsigned short temp;           // 16-bit signed, 2's complement, LSB=1/256 deg C
  unsigned short vcc;            // 16-bit unsigned, LSB=100mV
  unsigned short txBiasCurrent;  // 16-bit unsigned, LSB=2uA
  unsigned short txPower;        // 16-bit unsigned, LSB=0.1 uWatt
  unsigned short rxPower;        // 16-bit unsigned, LSB=0.1 uWatt
  char status;
  } sfpDiagInfoRaw_t;

typedef struct {
  float temp;           // 16-bit signed, 2's complement, LSB=1/256 deg C
  float vcc;            // 16-bit unsigned, LSB=100mV
  float txBiasCurrent;  // 16-bit unsigned, LSB=2uA
  float txPower;        // 16-bit unsigned, LSB=0.1 uWatt
  float rxPower;        // 16-bit unsigned, LSB=0.1 uWatt
  char status;
  } sfpDiagInfo_t;

static sfpDiagInfo_t sfpDiagInfo[4];
static sfpDiagInfoRaw_t sfpDiagInfoRaw[4];

static unsigned short wavelength[4];

static char diagOptsRaw[2];

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

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

  return retVal;
}

static unsigned char sfpDiagRawData[2*12];

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

  int i;

  if ((pDiagData == NULL) || (pSfpDiagInfo == NULL))
    return;

  for(i=0;i<8;i++) {
    // NOTE: swap the bytes: even byte is MSB, odd byte is LSB
    u.uint8[1] = pDiagData[(2*i)];
    u.uint8[0] = pDiagData[((2*i)+1)];
    switch(i) {
      case 0:
        // 16-bit signed, 2's complement, LSB=1/256 deg C
        pSfpDiagInfo->temp = (float)(u.sint16 * ((float)(1.0/256.0)));
        break;
      case 1:
        // 16-bit unsigned, LSB=100mV
        pSfpDiagInfo->vcc = (float)(u.uint16 * ((float)(1.0/10000.0)));
        break;
      case 2:
        // 16-bit unsigned, LSB=2uA, expressed as mA
        pSfpDiagInfo->txBiasCurrent = (float)(u.uint16 * ((float)(1.0/2000.0)));
        break;
      case 3:
        // 16-bit unsigned, LSB=0.1 uWatt, expressed in mW
        pSfpDiagInfo->txPower = (float)(u.uint16 * ((float)(1.0/10000.0)));
        break;
      case 4:
        // 16-bit unsigned, LSB=0.1 uWatt, expressed in mW
        pSfpDiagInfo->rxPower = (float)(u.uint16 * ((float)(1.0/10000.0)));
        break;
      case 5:
      case 6:
        // reserved
        break;
      case 7:
        pSfpDiagInfo->status = u.uint8[1];
        break;
      }
    }

  return;
}

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

  int i;

  if ((pDiagData == NULL) || (pSfpDiagInfoRaw == NULL))
    return;

  for(i=0;i<8;i++) {
        u.uint8[0] = pDiagData[(2*i)];
        u.uint8[1] = pDiagData[((2*i)+1)];
    switch(i) {
      case 0:
        // 16-bit signed, 2's complement, LSB=1/256 deg C
        pSfpDiagInfoRaw->temp = u.sint16;
        break;
      case 1:
        // 16-bit unsigned, LSB=100mV
        pSfpDiagInfoRaw->vcc = u.uint16;
        break;
      case 2:
        // 16-bit unsigned, LSB=2uA
        pSfpDiagInfoRaw->txBiasCurrent = u.uint16;
        break;
      case 3:
        // 16-bit unsigned, LSB=0.1 uWatt
        pSfpDiagInfoRaw->txPower = u.uint16;
        break;
      case 4:
        // 16-bit unsigned, LSB=0.1 uWatt
        pSfpDiagInfoRaw->rxPower = u.uint16;
        break;
      case 5:
      case 6:
        // reserved
        break;
      case 7:
        pSfpDiagInfoRaw->status = u.uint8[0];
        break;
      }
    }

  return;
}

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;
}

// 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(&(sfpDiagRawData[i]));
      }
    }

  i2cswitchDisable();

  decodeDiagData(sfpDiagRawData, &(sfpDiagInfo[bus]));
  decodeDiagDataRaw(sfpDiagRawData, &(sfpDiagInfoRaw[bus]));

  return 0;
}


// 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;
}

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

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

    // setup value to write
  writeVal = ((doDisable == true) ? 0x40 : 0x00);

  i2cmasterPut(0xa2, 110);
  i2cmasterPut(0xa2, writeVal);
  return i2cmasterSend(0xa2, true);
}


sfpDiagInfo_t * sfpdiagRetrieveInfo(int bus)
{
  return &(sfpDiagInfo[bus]);
}

sfpDiagInfoRaw_t * sfpdiagRetrieveInfoRaw(int bus)
{
  return &(sfpDiagInfoRaw[bus]);
}

static char vendorName[16];

static int sfpdiagGetVendor(int bus)
{
  int i, retVal;

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

  if (i2cmasterSendOne(0xa0, 20, true) != -1) {
    // TODO: check size
    retVal = i2cmasterReceive(0xa0, 16, true);
    for(i=0;((i<16) && (retVal != -1));i++) {
      retVal = i2cmasterGetData(&(vendorName[i]));
      }
    }

  return retVal;
}

static void printSfpDiags(int diagChannel)
{
  int bufLen;
  char * pOutBuf;
  sfpDiagInfo_t * pSfpInfo;
  sfpDiagInfoRaw_t * pSfpInfoRaw;

  pOutBuf = clicharGetOutputBuffer(&bufLen);
    
  sfpdiagGetDiags(diagChannel);
  pSfpInfo = sfpdiagRetrieveInfo(diagChannel);
  pSfpInfoRaw = sfpdiagRetrieveInfoRaw(diagChannel);

  sprintf(pOutBuf, "--- SFP diagnostics for channel [%d] ---\r\n", diagChannel);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "temperature   : raw[%04x] %f deg C\r\n", pSfpInfoRaw->temp, pSfpInfo->temp);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "Vcc           : raw[%04x] %f V\r\n", pSfpInfoRaw->vcc, pSfpInfo->vcc);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "txBiasCurrent : raw[%04x] %f mA\r\n", pSfpInfoRaw->txBiasCurrent, pSfpInfo->txBiasCurrent);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "txPower       : raw[%04x] %f mW\r\n", pSfpInfoRaw->txPower, pSfpInfo->txPower);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "rxPower       : raw[%04x] %f mW\r\n", pSfpInfoRaw->rxPower, pSfpInfo->rxPower);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sprintf(pOutBuf, "status        : raw[%02x]\r\n", pSfpInfo->status);
  clicharSendOutputBuffer(pOutBuf, bufLen);    
  sfpdiagGetWavelength(diagChannel);
  sprintf(pOutBuf, "wavelength    : %u nm\r\n", wavelength[diagChannel]);
  clicharSendOutputBuffer(pOutBuf, bufLen);

  return;
}

static void printMinSfpDiags(int diagChannel)
{
  int bufLen;
  char * pOutBuf;
  sfpDiagInfo_t * pSfpInfo;
  sfpDiagInfoRaw_t * pSfpInfoRaw;

  pOutBuf = clicharGetOutputBuffer(&bufLen);
    
  sfpdiagGetDiags(diagChannel);
  pSfpInfo = sfpdiagRetrieveInfo(diagChannel);
  pSfpInfoRaw = sfpdiagRetrieveInfoRaw(diagChannel);
  sfpdiagGetWavelength(diagChannel);

  sprintf(pOutBuf, "sfpdiag: channel=%d, temp=%f, vcc=%f, txBiasI=%f, txPower=%f, rxPower=%f, lambda=%u, status=%02x\r\n",
    diagChannel, pSfpInfo->temp, pSfpInfo->vcc, pSfpInfo->txBiasCurrent, pSfpInfo->txPower, 
    pSfpInfo->rxPower, wavelength[diagChannel], pSfpInfo->status);
  clicharSendOutputBuffer(pOutBuf, bufLen);

  return;

}

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

  pOutBuf = clicharGetOutputBuffer(&bufLen);

  if (argc == 2) {
    probeBus = (int)strtol(argv[1], (char **)NULL, 10);
    if ((probeBus < 0) || (probeBus > 3)) {
      return retVal;
      }
    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++) {
      if (sfpProbe(i) == 0) {
        printMinSfpDiags(i);
        }
      }
    retVal = 0;
    }

  return retVal;
}

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

