#include "archinc.h"
#include "cli.h"
#include "eventq.h"
#include "timer0.h"
#include "zarlink.h"
#include "zl_bus.h"
#include "zl_event.h"
#include "zlregs.h"
#include "nvstore.h"
#include "portmgr.h"
#include "zl_trunk.h"

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

#ifdef CONFIG_INCLUDE_ZARLINK

#define TRUNK_STATUS_UNUSED    (0)  // empty trunk entry
#define TRUNK_STATUS_INACTIVE  (1)  // trunk registered, not activated
#define TRUNK_STATUS_ACTIVE    (2)  // trunk has been setup and is active 
#define TRUNK_STATUS_STANDBY   (3)  // failover to standby has occurred

#define TRUNK_ATTRIB_VALID_MASK  (0x01)
#define TRUNK_ATTRIB_VALID       (0x01)
#define TRUNK_ATTRIB_INVALID     (0x00)

#define TRUNK_ATTRIB_ENABLE_MASK (0x02)
#define TRUNK_ATTRIB_ENABLE      (0x02)
#define TRUNK_ATTRIB_DISABLE     (0x00)

#define TRUNK_ATTRIB_FAILCW_MASK (0x04)
#define TRUNK_ATTRIB_FAILCOLD    (0x00)
#define TRUNK_ATTRIB_FAILWARM    (0x04)

#define TRUNK_ATTRIB_LHB_MASK    (0x08)
#define TRUNK_ATTRIB_LHB_ENABLE  (0x00)
#define TRUNK_ATTRIB_LHB_DISABLE (0x08)

#define NUM_TRUNKS (2)

typedef struct __attribute__ ((__packed__)) {
  unsigned char activeChannel;         // active channel in trunk
  unsigned char standbyChannel;        // stby channel in trunk
  unsigned char attributes;
  } trunkCfg_t;

static trunkCfg_t config[NUM_TRUNKS];
static int status[NUM_TRUNKS];

static const char * const statusTags[] = {" unused  ", 
                                          "inactive ", 
                                          " active  ", 
                                          " standby "};

static int trunkSetupHandler(int argc, char * argv[], int flags);
static int trunkEnableHandler(int argc, char * argv[], int flags);
static int trunkDeleteHandler(int argc, char * argv[], int flags);

// find the trunk that a channel (either active or standby) is in
static int trunkGetId(int channel)
{
  int i;

  for(i=0;i<NUM_TRUNKS;i++) {
    if ((config[i].activeChannel == ((unsigned char)channel)) ||
        (config[i].standbyChannel == ((unsigned char)channel))) {
      break;
      }
    }

  return ((i == NUM_TRUNKS) ? -1 : i);
}

#ifdef CONFIG_INCLUDE_NVSTORE

static char nvItemName[8];

static int checkTrunkConfig(int trunkId)
{
  if ((trunkId < 0) || (trunkId >= NUM_TRUNKS)) {
    return -1;
    }

  // lookup data structure in nvram
  sprintf(nvItemName, "trunk%d\x0", trunkId);
  return nvstoreLookup(nvItemName);
}

static int saveTrunkConfig(int trunkId)
{
  if ((trunkId < 0) || (trunkId >= NUM_TRUNKS)) {
    return -1;
    }

  // write data structure to nvram
  sprintf(nvItemName, "trunk%d\x0", trunkId);
  return nvstoreWrite(nvItemName, (unsigned char *)&(config[trunkId]), sizeof(trunkCfg_t));
}

static int loadTrunkConfig(int trunkId)
{
  int retVal;
  unsigned char bytesRead;

  if ((trunkId < 0) || (trunkId >= NUM_TRUNKS)) {
    return -1;
    }

  sprintf(nvItemName, "trunk%d\x0", trunkId);
  retVal = nvstoreRead(nvItemName, (unsigned char *)&(config[trunkId]), &bytesRead);

  if (retVal == -1) {
    return -1;
    }
  else if (((size_t)bytesRead) != sizeof(trunkCfg_t)) {
    return -1;
    }
  else {
    return 0;
    }
}

#endif /* CONFIG_INCLUDE_NVSTORE */

static int trunkBuild(unsigned char active, unsigned char standby, unsigned char attrib)
{
  int i;
  int retVal;

  // check port numbers
  if ((active >= NUM_ALL_PORTS) || (standby >= NUM_ALL_PORTS)) {
    return -1;
    }

  // can't specify active and standby as same port
  if (active == standby) {
    return -1;
    }

  for(i=0;i<NUM_TRUNKS;i++) {
    if (status[i] == TRUNK_STATUS_UNUSED) {
      break;
      }
    }

  if (i == NUM_TRUNKS) {
    return -1;
    }

  config[i].activeChannel = active;
  config[i].standbyChannel = standby;
  config[i].attributes = attrib;

  status[i] = TRUNK_STATUS_INACTIVE;

  return i;
}

static int trunkActiveSet(int trunkId)
{
  int retVal = -1;
  unsigned char hashValue;
  unsigned char groupBitmap;

  if ((trunkId < 0) || (trunkId >= NUM_TRUNKS)) {
    return -1;
    }

  hashValue = (((unsigned char)config[trunkId].activeChannel) & 0x0f);
  hashValue |= (hashValue << 4);

  zarlinkIndexedWrite(TRUNK_HASH10(trunkId), hashValue);
  zarlinkIndexedWrite(TRUNK_HASH32(trunkId), hashValue);
  zarlinkIndexedWrite(TRUNK_HASH54(trunkId), hashValue);
  zarlinkIndexedWrite(TRUNK_HASH76(trunkId), hashValue);

  // multicast hash setup is by bitmapped channel
  groupBitmap = (unsigned char)(0x1 << (config[trunkId].activeChannel));
  zarlinkIndexedWrite(MCAST_HASH_L(trunkId), groupBitmap);

  return (int)(config[trunkId].activeChannel);
}

static int trunkStandbySet(int trunkId)
{
  int retVal = -1;
  unsigned char hashValue;
  unsigned char groupBitmap;

  if ((trunkId < 0) || (trunkId >= NUM_TRUNKS)) {
    return -1;
    }

  hashValue = (((unsigned char)config[trunkId].standbyChannel) & 0x0f);
  hashValue |= (hashValue << 4);

  zarlinkIndexedWrite(TRUNK_HASH10(trunkId), hashValue);
  zarlinkIndexedWrite(TRUNK_HASH32(trunkId), hashValue);
  zarlinkIndexedWrite(TRUNK_HASH54(trunkId), hashValue);
  zarlinkIndexedWrite(TRUNK_HASH76(trunkId), hashValue);

  // multicast hash setup is by bitmapped channel
  groupBitmap = (unsigned char)(0x1 << (config[trunkId].standbyChannel));
  zarlinkIndexedWrite(MCAST_HASH_L(trunkId), groupBitmap);

  return (int)(config[trunkId].standbyChannel);
}

static int trunkActivate(int trunkId)
{
  int retStat;
  unsigned char groupBitmap;
  int activePortStatus;
  unsigned int timeout;
  volatile unsigned long dummy;

  if ((trunkId < 0) || (trunkId >= NUM_TRUNKS))
    return -1;

  // ensure the active port is powered up
  portmgrPortStatusRequest((int)(config[trunkId].activeChannel), PORT_STATUS_UP);

  timeout = 100;
  while (timeout > 0) {
    dummy = CTIME0;
    timeout--;
    }

  eventqDispatch();

  /*
   * Wait for the active port to become 'up' before turning on LHB or else it
   * will immediately fail
   */

  timeout = 1000000;
  while (timeout > 0) {
    portmgrPortStatusGet(config[trunkId].activeChannel, &activePortStatus);
    if (activePortStatus == PORT_STATUS_UP) {
      break;
      }
    dummy = CTIME0;

    timeout--;
    }

  if (activePortStatus != PORT_STATUS_UP)
    return -1;


  // assign trunk group membership
  groupBitmap = (unsigned char)(0x1 << config[trunkId].activeChannel); 
  groupBitmap |= (unsigned char)(0x1 << config[trunkId].standbyChannel);
  zarlinkIndexedWrite(TRUNK_GROUP(trunkId), groupBitmap);

  // route all traffic over the active channel
  retStat = trunkActiveSet(trunkId);

  // if cold standby attribute set, then power down the standby port
  if ((config[trunkId].attributes & TRUNK_ATTRIB_FAILCW_MASK) == TRUNK_ATTRIB_FAILCOLD) {
    portmgrPortStatusRequest((int)(config[trunkId].standbyChannel), PORT_STATUS_DOWN);
    }

  // if LHB attribute set, activate LHB
  if ((config[trunkId].attributes & TRUNK_ATTRIB_LHB_MASK) == TRUNK_ATTRIB_LHB_ENABLE) {
    portmgrLhbStatusRequest((int)(config[trunkId].activeChannel), LHB_STATUS_ENABLED);
    }

  eventqDispatch();
  
  status[trunkId] = TRUNK_STATUS_ACTIVE;
  
  return retStat;
}

static int trunkTeardown(int trunkId)
{
  if ((trunkId < 0) || (trunkId >= NUM_TRUNKS)) {
    return -1;
    }

  /*
   * If we're still on the active channel and LHB is enabled, then
   * disable it before we continue tearing down the trunk
   */

  if ( (status[trunkId] == TRUNK_STATUS_ACTIVE) &&
       (config[trunkId].attributes & TRUNK_ATTRIB_LHB_MASK) == TRUNK_ATTRIB_LHB_ENABLE) {
    portmgrLhbStatusRequest((int)(config[trunkId].activeChannel), LHB_STATUS_DISABLED);
    }

  // deactivate trunk in the switch
  zarlinkIndexedWrite(TRUNK_GROUP(trunkId), 0);
  zarlinkIndexedWrite(TRUNK_HASH10(trunkId), 0);
  zarlinkIndexedWrite(TRUNK_HASH32(trunkId), 0);
  zarlinkIndexedWrite(TRUNK_HASH54(trunkId), 0);
  zarlinkIndexedWrite(TRUNK_HASH76(trunkId), 0);

  return 0;
}

int zarlinkTrunkGetStatus(int trunkId)
{
  if ((trunkId < 0) || (trunkId >= NUM_TRUNKS))
    return -1;

  // return failover status
  return (status[trunkId]);
}

// return the port number that is now active if any
int zarlinkTrunkFailover(int portNum)
{
  int trunkId;

  // first see if a trunk exists for this port
  trunkId = trunkGetId(portNum);
  if (trunkId == -1) {
    return -1;
    }

  // if a trunk exists, then check status and see if a failover occurred already
  if (status[trunkId] != TRUNK_STATUS_ACTIVE)
    return -1;

  // check to see that this is the _active_ channel of the trunk
  if (portNum != (int)(config[trunkId].activeChannel))
    return -1;

  /*
   * At this point, we've established that a trunk is configured and that
   * the active channel is the one that has failed, so commence failover.
   */

  // power up the standby port -- it won't hurt if we're doing warm failover
  portmgrPortStatusRequest((int)(config[trunkId].standbyChannel), PORT_STATUS_UP);

  // failover to the standby channel
  trunkStandbySet(trunkId);

  // power down the now failed active port to save power -- traffic is now on standby
  portmgrPortStatusRequest((int)(config[trunkId].activeChannel), PORT_STATUS_DOWN);

  // indicate that we're now running on the standby channel
  status[trunkId] = TRUNK_STATUS_STANDBY;

  return 0;
}

int zarlinkTrunkInit(void)
{
  int i;

  // initialize trunk status
  for(i=0;i<NUM_TRUNKS;i++) {
    status[i] = TRUNK_STATUS_UNUSED;
    }

  // initialize trunk config
  for(i=0;i<NUM_TRUNKS;i++) {
    config[i].activeChannel = 0;
    config[i].standbyChannel = 0;
    config[i].attributes = 0;
    }

#ifdef CONFIG_INCLUDE_NVSTORE

  // load trunk config from nvstore
  for(i=0;i<NUM_TRUNKS;i++) {
    if (checkTrunkConfig(i) == 0) {
      if (loadTrunkConfig(i) == 0) {
        if ((config[i].attributes & TRUNK_ATTRIB_VALID_MASK) == TRUNK_ATTRIB_VALID) {
          status[i] = TRUNK_STATUS_INACTIVE;
	  }
        if ((config[i].attributes & TRUNK_ATTRIB_ENABLE_MASK) == TRUNK_ATTRIB_ENABLE) {
          trunkActivate(i);
	  }
        }
      }
    }

#endif /* CONFIG_INCLUDE_NVSTORE */

  cliRegisterCommand("trunk", trunkSetupHandler, "trunk [activeCh] [standbyCh] {attrib} - sets up trunk");
  cliRegisterCommand("trdel", trunkDeleteHandler, "trdel [trunk] - removes a trunk configuration");
  cliRegisterCommand("trenable", trunkEnableHandler, "trenable [disable | enable] [trunk] - enable configured trunk");

  return 0;
}  

static int trunkSetupHandler(int argc, char * argv[], int flags)
{
  int retVal;
  int i;
  char * pOutBuf;
  int bufLen;
  int trunkId;
  unsigned char active, standby, attr;

  pOutBuf = clicharGetOutputBuffer(&bufLen);

  if (argc == 1) {
    // show defined trunks
    for(i=0;i<NUM_TRUNKS;i++) {
      sprintf(pOutBuf, "trunk[%d]: active[%u] standby[%u] attrib[%02x] status[%s]\r\n", 
              i, config[i].activeChannel, config[i].standbyChannel, 
	      config[i].attributes, statusTags[status[i]]);
      clicharSendOutputBuffer(pOutBuf, bufLen);
      }
    retVal = 0;
    }
  else if (argc == 4) {
    // define a trunk
    active  = (unsigned char)strtol(argv[1], NULL, 10);
    standby = (unsigned char)strtol(argv[2], NULL, 10);
    attr = (unsigned char)strtol(argv[3], NULL, 16);

    trunkId = trunkBuild(active, standby, attr);

    sprintf(pOutBuf, "%s: trunk[%d] built with active[%u] standby[%u] attributes[%02x] \r\n", 
            argv[0], trunkId, active, standby, attr);
    clicharSendOutputBuffer(pOutBuf, bufLen);

#ifdef CONFIG_INCLUDE_NVSTORE
    if (trunkId != -1) {
      retVal = saveTrunkConfig(trunkId);
      }
#endif
    } // if (argc == 4)
  else if (argc == 3) {
    // define a trunk with default attributes (LHB, cold failover)
    active  = (unsigned char)strtol(argv[1], NULL, 10);
    standby = (unsigned char)strtol(argv[2], NULL, 10);
    attr = TRUNK_ATTRIB_VALID;

    trunkId = trunkBuild(active, standby, attr);

    sprintf(pOutBuf, "%s: trunk[%d] built with active[%u] standby[%u] default attrib[%02x] \r\n", 
            argv[0], trunkId, active, standby, attr);
    clicharSendOutputBuffer(pOutBuf, bufLen);

#ifdef CONFIG_INCLUDE_NVSTORE
    if (trunkId != -1) {
      retVal = saveTrunkConfig(trunkId);
      }
#endif
    } // if (argc == 3)
  else {
    retVal = -1;
    }

  return retVal;
}

static int trunkDeleteHandler(int argc, char * argv[], int flags)
{
  char * pOutBuf;
  int bufLen;
  int trunkId;

  pOutBuf = clicharGetOutputBuffer(&bufLen);

  if (argc != 2) {
    return -1;
    }

  // delete a trunk
  trunkId = strtol(argv[1], NULL, 10);

  if ((trunkId < 0)  || (trunkId >= NUM_TRUNKS)) {
    return -1;
    }

  sprintf(pOutBuf, "%s: removing trunk [%d]\r\n", argv[0], trunkId);
  clicharSendOutputBuffer(pOutBuf, bufLen);

  if (status[trunkId] == TRUNK_STATUS_ACTIVE) {
    trunkTeardown(trunkId);
    }

  // update failover status
  status[trunkId] = TRUNK_STATUS_UNUSED;

  config[trunkId].activeChannel = 0;
  config[trunkId].standbyChannel = 0;
  config[trunkId].attributes = 0;

  return saveTrunkConfig((int)trunkId);
}

static int trunkEnableHandler(int argc, char * argv[], int flags)
{
  char * pOutBuf;
  int bufLen;
  int trunkId;
  char * pCh;

  pOutBuf = clicharGetOutputBuffer(&bufLen);

  if (argc != 3) {
    return -1;
    }

  // enable or disable a trunk
  trunkId = strtol(argv[2], NULL, 10);

  if ((trunkId >= NUM_TRUNKS)  || (trunkId < 0)) {
    return -1;
    }

  switch(*argv[1]) {
    case 'e':
    case 'E':
      // enable trunk
      if ((config[trunkId].attributes & TRUNK_ATTRIB_ENABLE_MASK) == TRUNK_ATTRIB_DISABLE) {
        sprintf(pOutBuf, "%s: enabling trunk [%d]\r\n", argv[0], trunkId); 
        config[trunkId].attributes |= TRUNK_ATTRIB_ENABLE;
	trunkActivate(trunkId);
        }
      break;
    case 'd':
    case 'D':
      // disable trunk
      if ((config[trunkId].attributes & TRUNK_ATTRIB_ENABLE_MASK) == TRUNK_ATTRIB_ENABLE) {
        sprintf(pOutBuf, "%s: disabling trunk [%d]\r\n", argv[0], trunkId);
        config[trunkId].attributes &= ~TRUNK_ATTRIB_ENABLE;
        trunkTeardown(trunkId);
        }
      break;

    }

  clicharSendOutputBuffer(pOutBuf, bufLen);

  return saveTrunkConfig((int)trunkId);
}

#endif /* CONFIG_INCLUDE_ZARLINK */