#include "archinc.h"
#include "eventq.h"
#include "medusa_rev2.h"

// #if 1
#ifdef CONFIG_BOARD_MEDUSA_REV2

/* MBARI Medusa rev 2 board */

#define ORANGE_LED_MASK (0x01000000)
#define GREEN_LED_MASK  (0x02000000)

static boolean greenLedState  = false;
static boolean orangeLedState = false;

static void handleLedOnEvent(void);
static void handleLedOffEvent(void);

int medusaInit(void)
{
  PINSEL0 = (unsigned long)0x00000055;
  PINSEL1 = (unsigned long)0x000002A9;

  IO0SET   = (unsigned long)0x70000000; // negate ZL50409 bus ctrl!
  IO0CLR   = (unsigned long)0x00200070; // clear ZL50409 A[2:0], SPI_INTR_TP 
  IO0DIR   = (unsigned long)0x70200070; // ZL50409 bus ctrl are outputs, 
                                        // SPI_INTR output, leave D[7:0] input

  IO1SET   = (unsigned long)0x00000000; // 
  IO1CLR   = (unsigned long)0x03F00000; // LEDs off to start, deassert SFP enables
  IO1DIR   = (unsigned long)0x03F00000; // LEDs, SFP enables are outputs

  eventqRegisterHandler(RUN_LED_ON,  handleLedOnEvent);
  eventqRegisterHandler(RUN_LED_OFF, handleLedOffEvent);

  return 0;
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
void medusaSetGreenLed(boolean onOff)
#else
void INLINE_KEYWORD medusaSetGreenLed(boolean onOff)
#endif
{
  if (onOff == false)
    IO1CLR = GREEN_LED_MASK;
  else
    IO1SET = GREEN_LED_MASK;

  greenLedState = onOff;
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
void medusa2ToggleGreenLed(void)
#else
void INLINE_KEYWORD medusaToggleGreenLed(void)
#endif
{
    medusaSetGreenLed(greenLedState == true ? false : true);
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
void medusaSetOrangeLed(boolean onOff)
#else
void INLINE_KEYWORD medusaSetOrangeLed(boolean onOff)
#endif
{
  if (onOff == false)
    IO1CLR = ORANGE_LED_MASK;
  else
    IO1SET = ORANGE_LED_MASK;

  orangeLedState = onOff;
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
void medusaToggleOrangeLed(void)
#else
void INLINE_KEYWORD medusaToggleOrangeLed(void)
#endif
{
    medusaSetOrangeLed(orangeLedState == true ? false : true);
}

static void handleLedOnEvent(void)
{
  medusaSetGreenLed(true);
}

static void handleLedOffEvent(void)
{
  medusaSetGreenLed(false);
}

boolean medusaSfpInsertStateGet(int channel)
{
  if ( ((IO1PIN) & (1 << (16 + channel))) != 0)
    return true;
  else
    return false;
}

void medusaSfpPowerStateSet(int channel, boolean powerState)
{
  unsigned long mask;
   
  if (channel == 0) {
    mask = (1 << (22 + channel));
    if (powerState == true) {
      IO0SET = mask;
      }
    else {
      IO0CLR = mask;
      }
    }
  else {
    mask = (1 << (20 + channel));
    if (powerState == true) {
      IO1SET = mask;
      }
    else {
      IO1CLR = mask;
      }
    }

  return;
}

#endif /* CONFIG_BOARD_MEDUSA_REV2 */
