#include "archinc.h"
#include "cli.h"
#include "clichar.h"
#include "i2cmaster.h"
#include "i2cport.h"
#include "i2cswitch.h"
#include "sfp.h"
#include "sfpdiag.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];


// SFP diagnostic address is hardwired to 0xa2

// unsigned char sfpSlaveAddr = 0xa2;

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

int sfpInit(void)
{
  int retVal = -1;
  int i;

  //
  // for Medusa rev2, turn power on to the module before probe. If there is
  // no module there, then turn power off
  //

  for(i=0;i<3;i++) {
#ifdef CONFIG_BOARD_MEDUSA_REV2
    medusaSfpPowerStateSet(i, true);
#endif
    if (sfpProbe(i) == 0) {
      modulePresent[i] = true;
      }
    else {
      modulePresent[i] = false;
#ifdef CONFIG_BOARD_MEDUSA_REV2
      medusaSfpPowerStateSet(i, false);
#endif
      }
    }

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

  return retVal;
}

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(probeBus, doDisable)
}

int sfpEnable(int bus, boolean sfpEnable)
{
#ifdef CONFIG_BOARD_MEDUSA_REV2
  return medusaSfpPowerStateSet(bus, sfpEnable);
#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)) */
