#include "archinc.h"
#include "cli.h"
#include "clichar.h"
#include "timer0.h"
#include "i2cmaster.h"
#include "i2cport.h"
#include "i2cswitch.h"
#include "sfp.h"
#include "sfpdiag.h"
#include "portmgr.h"

#ifdef CONFIG_BOARD_MEDUSA_REV2
#include "medusa_rev2.h"
#endif

#if (defined(CONFIG_INCLUDE_LPC210X_I2C) && defined(CONFIG_INCLUDE_I2C_SFP))

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

boolean modulePresent[4];

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

int sfpInit(void)
{
  int i;

  for(i=0;i<NUM_OPTICAL_PORTS;i++) {
    medusaSfpPowerStateSet(i, true);
    timer0Delay(5); // give the module time to power up

    if (sfpProbe(i) == 0) {
      modulePresent[i] = true;
      portmgrOpticalStatusRequest(i, OPTICAL_STATUS_NORMAL);
      }
    else {
      modulePresent[i] = false;
      portmgrOpticalStatusRequest(i, OPTICAL_STATUS_NO_SFP);
      medusaSfpPowerStateSet(i, false);
      }
    }

  return cliRegisterCommand("sfp", sfpHandler, "sfp [probe | (-)enable | (-)txDisable | (-)softTxDisable] [bus number 0-3]");
}

int sfpModulePresent(int bus, boolean *pModulePresent)
{
  if ((bus < 0) || (bus > 3))
    return -1;

  *pModulePresent = modulePresent[bus];
  return 0;
}

int sfpProbe(int bus)
{
  int retVal;
  
  // setup the bus mux
  retVal = i2cswitchSetBus(bus);

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

  retVal = i2cmasterProbe(0xa2);

  i2cswitchDisable();

  return retVal;
}

#if defined(CONFIG_BOARD_MEDUSA) && defined(CONFIG_INCLUDE_I2C_PORT)

int sfpTransmitDisable(int bus, boolean txDisable)
{
  unsigned short txEnaMask;
  int retVal;

  // TODO: do some sort of checking on channel num
  txEnaMask = (unsigned short)(1 << (bus+4));

  if (txDisable == true) 
    retVal = i2cportSet(1, txEnaMask);
  else
    retVal = i2cportClear(1, txEnaMask);

  return retVal;
}

int sfpEnable(int bus, boolean sfpEnable)
{
  unsigned short sfpEnaMask;
  int retVal;

  // TODO: do some sort of checking on channel num
  sfpEnaMask = (unsigned short)(1 << bus);

  if (sfpEnable == true) 
    retVal = i2cportSet(1, sfpEnaMask);
  else
    retVal = i2cportClear(1, sfpEnaMask);

  return retVal;
}

#else

int sfpTransmitDisable(int bus, boolean txDisable)
{
  return sfpdiagSoftTxDisable(bus, txDisable);
}

int sfpEnable(int bus, boolean sfpEnable)
{
#ifdef CONFIG_BOARD_MEDUSA_REV2
  medusaSfpPowerStateSet(bus, sfpEnable);
  return 0;
#else
  return -1;
#endif /* CONFIG_BOARD_MEDUSA_REV2 */
}

#endif /* CONFIG_INCLUDE_I2C_PORT */

static int sfpHandler(int myArgc, char * myArgv[], int flags)
{
  int retVal = -1;
  int probeBus;
  int bufLen;
  char * pOutBuf;
  boolean doDisable;
  char * pSprintfFormat;
  char * pCh;

  pOutBuf = clicharGetOutputBuffer(&bufLen);

  if (myArgc < 3)
    return -1;

  probeBus = atoi(myArgv[2]);

  if ((probeBus < 0) || (probeBus > 3))
    return -1;

  doDisable = true;
  pCh = myArgv[1];
  if (*pCh == '-') {
    doDisable = false;
    pCh++;
    }

  switch(*pCh) {
    case 'e':
    case 'E':
      if ((retVal = sfpEnable(probeBus, doDisable)) == 0) {
        if (doDisable == true) {
          pSprintfFormat = "enable: channel [%d] enabled\r\n";
          }
        else {
          pSprintfFormat = "enable: channel [%d] disabled\r\n";
          }
        sprintf(pOutBuf, pSprintfFormat, probeBus);
        }
      else {
        sprintf(pOutBuf, "enable: channel [%d] not found\r\n", probeBus);
        }
      clicharSendOutputBuffer(pOutBuf, bufLen);
      break;
    case 'p':
    case 'P': 
      if ((retVal = sfpProbe(probeBus)) == 0) {
        sprintf(pOutBuf, "probe: found SFP on bus [%d]\r\n", probeBus);
        }
      else {
        sprintf(pOutBuf, "probe: error SFP not found on bus [%d]\r\n", probeBus);
        }
      clicharSendOutputBuffer(pOutBuf, bufLen);
      break;

    case 's':
    case 'S':
      if ((retVal = sfpdiagSoftTxDisable(probeBus, doDisable)) == 0) {
        if (doDisable == true) {
          pSprintfFormat = "softTx: channel [%d] soft TX disabled\r\n";
          }
        else {
          pSprintfFormat = "softTx: channel [%d] soft TX enabled\r\n";
          }
        sprintf(pOutBuf, pSprintfFormat, probeBus);
        }
      else {
        sprintf(pOutBuf, "softTx: channel [%d] not found\r\n", probeBus);
        }
      clicharSendOutputBuffer(pOutBuf, bufLen);
      break;
    case 't':
    case 'T':
      if ((retVal = sfpTransmitDisable(probeBus, doDisable)) == 0) {
        if (doDisable == true) {
          pSprintfFormat = "txdisable: channel [%d] tx disabled\r\n";
          }
        else {
          pSprintfFormat = "txdisable: channel [%d] tx enabled\r\n";
          }
        sprintf(pOutBuf, pSprintfFormat, probeBus);
        }
      else {
        sprintf(pOutBuf, "txenable: channel [%d] not found\r\n", probeBus);
        }
      clicharSendOutputBuffer(pOutBuf, bufLen);
    default:
      break;
    }

  return retVal;
}

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