/* Raspberry Pi LPC1114 I/O Processor Expansion Board SPI Agent firmware  program */

// Copyright (C)2013-2018, Philip Munts, President, Munts AM Corp.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

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

#include <cpu.h>

#include <expansion_lpc1114.h>
#include <spi-agent.h>

// The following release number should track the Subversion revision number for this file

#define FIRMWARE_VERSION	17328

_BEGIN_STD_C

static volatile uint32_t timer_interrupt_mask[TIMER_ID_SENTINEL];
static volatile uint32_t timer_capture_value[TIMER_ID_SENTINEL];
static volatile uint32_t timer_capture_delta[TIMER_ID_SENTINEL];

_END_STD_C

/**************************************************************************/

// LPC1114 PIO1 interrupt service routine

_BEGIN_STD_C

void PIO1_IRQHandler(void)
{
  // Pulse INT high
  gpio_write(LPC1114_INT, 1);
  gpio_write(LPC1114_INT, 0);

  // Clear pending interrupts
  LPC_GPIO1->IC = 0xFFF;
}

_END_STD_C

/**************************************************************************/

// LPC1114 timer CT32B0 interrupt service routine

_BEGIN_STD_C

void CT32B0_IRQHandler(void)
{
  uint32_t IR = LPC_TMR32B0->IR;

  // Get capture delta value
  if (IR & 0x10)
  {
    timer_capture_delta[CT32B0] = LPC_TMR32B0->CR0 -
      timer_capture_value[CT32B0];
    timer_capture_value[CT32B0] = LPC_TMR32B0->CR0;
  }

  // Pulse INT high
  if (IR & timer_interrupt_mask[CT32B0])
  {
    gpio_write(LPC1114_INT, 1);
    gpio_write(LPC1114_INT, 0);
  }

  // Clear pending interrupts
  LPC_TMR32B0->IR = IR;
}

_END_STD_C

/**************************************************************************/

// LPC1114 timer CT32B1 interrupt service routine

_BEGIN_STD_C

void CT32B1_IRQHandler(void)
{
  uint32_t IR = LPC_TMR32B1->IR;

  // Get capture delta value
  if (IR & 0x10)
  {
    timer_capture_delta[CT32B1] = LPC_TMR32B1->CR0 -
      timer_capture_value[CT32B1];
    timer_capture_value[CT32B1] = LPC_TMR32B1->CR0;
  }

  // Pulse INT high
  if (IR & timer_interrupt_mask[CT32B1])
  {
    gpio_write(LPC1114_INT, 1);
    gpio_write(LPC1114_INT, 0);
  }

  // Clear pending interrupts
  LPC_TMR32B1->IR = IR;
}

_END_STD_C

/**************************************************************************/

// Support for the LEGO Power Functions Infrared Remote Control protocol

#define DELAY38KHZ	21
#define TURN_IRED_ON	gpio_write((unsigned) userdata, 1)
#define TURN_IRED_OFF	gpio_write((unsigned) userdata, 0)
#define TURN_LED_ON	gpio_write(LPC1114_LED, 1)
#define TURN_LED_OFF	gpio_write(LPC1114_LED, 0)

#include <legorc.h>

/**************************************************************************/

// Enter ISP mode

static void ReinvokeISP(void)
{
  typedef void (*IAP_ENTRY_PTR)(uint32_t *command, uint32_t *result);
  IAP_ENTRY_PTR IAP = (IAP_ENTRY_PTR) 0x1fff1ff1;

  uint32_t command = 57;

  IAP(&command, NULL);
}

/**************************************************************************/

// SPI Agent command handler functions

static void execute_NOP(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  resp->data = FIRMWARE_VERSION;
}

static void execute_LOOPBACK(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  resp->data = cmd->data;
}

static void execute_CONFIGURE_ANALOG_INPUT(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_ANALOG(cmd->pin))
    resp->error = ENODEV;
  else
  {
    adc_init(NULL, cmd->pin - LPC1114_AD1 + 1);
    resp->error = errno;
  }
}

static void execute_CONFIGURE_GPIO_INPUT(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_GPIO(cmd->pin))
    resp->error = ENODEV;
  else
    switch (cmd->data)
    {
      case false :
        gpio_configure(cmd->pin, GPIO_MODE_INPUT_PULLDOWN);
        resp->error = errno;
        break;

      case true :
        gpio_configure(cmd->pin, GPIO_MODE_INPUT_PULLUP);
        resp->error = errno;
        break;

      default :
        resp->error = EINVAL;
        break;
    }
}

static void execute_CONFIGURE_GPIO_OUTPUT(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (cmd->data > true)
    resp->error = EINVAL;
  else if (IS_GPIO(cmd->pin))
  {
    gpio_configure(cmd->pin, GPIO_MODE_OUTPUT);
    gpio_write(cmd->pin, cmd->data);
    resp->error = errno;
  }
  else if (IS_LED(cmd->pin) || IS_INT(cmd->pin)) // These pins are already configured as outputs
  {
    gpio_write(cmd->pin, cmd->data);
    resp->error = errno;
  }
  else
    resp->error = ENODEV;
}

static void execute_CONFIGURE_PWM_OUTPUT(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_PWM(cmd->pin))
    resp->error = ENODEV;
  else if ((cmd->data < LPC1114_PWM_FREQ_MIN) || (cmd->data > LPC1114_PWM_FREQ_MAX))
    resp->error = EINVAL;
  else if (cmd->pin == LPC1114_PWM4)
  {
    pwm_init(12, cmd->data);
    resp->error = errno;
  }
  else
  {
    pwm_init(cmd->pin - LPC1114_PWM1 + 4, cmd->data);
    resp->error = errno;
  }
}

static void execute_GET_ANALOG(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_ANALOG(cmd->pin))
    resp->error = ENODEV;
  else
  {
    resp->data = adc_read(NULL, cmd->pin - LPC1114_AD1 + 1);
    resp->error = errno;
  }
}

static void execute_GET_GPIO(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_GPIO(cmd->pin) && !IS_LED(cmd->pin) && !IS_INT(cmd->pin))
    resp->error = ENODEV;
  else
  {
    resp->data = gpio_read(cmd->pin);
    resp->error = errno;
  }
}

static void execute_PUT_GPIO(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_GPIO(cmd->pin) && !IS_LED(cmd->pin) && !IS_INT(cmd->pin))
    resp->error = ENODEV;
  else if (cmd->data > true)
    resp->error = EINVAL;
  else
  {
    gpio_write(cmd->pin, cmd->data);
    resp->error = errno;
  }
}

static void execute_PUT_PWM(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_PWM(cmd->pin))
    resp->error = ENODEV;
  else if (cmd->data > 65535)
    resp->error = EINVAL;
  else if (cmd->pin == LPC1114_PWM4)
  {
    pwm_set(12, cmd->data);
    resp->error = errno;
  }
  else
  {
    pwm_set(cmd->pin - LPC1114_PWM1 + 4, cmd->data);
    resp->error = errno;
  }
}

static void execute_CONFIGURE_GPIO_INTERRUPT(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_GPIO(cmd->pin))
    resp->error = ENODEV;
  else if (cmd->data >= LPC1114_GPIO_INTERRUPT_SENTINEL)
    resp->error = EINVAL;
  else
  {
    gpio_configure_interrupt(cmd->pin, cmd->data);
    resp->error = errno;
  }
}

static void execute_CONFIGURE_GPIO(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_GPIO(cmd->pin) && !IS_LED(cmd->pin) && !IS_INT(cmd->pin))
    resp->error = ENODEV;
  else if ((cmd->data >= LPC1114_GPIO_MODE_SENTINEL) || ((IS_LED(cmd->pin) || IS_INT(cmd->pin)) && (cmd->data != LPC1114_GPIO_MODE_OUTPUT)))
    resp->error = EINVAL;
  else
  {
    gpio_configure(cmd->pin, cmd->data);
    resp->error = errno;
  }
}

static void execute_PUT_LEGORC(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  // Validate the pin number

  if (!IS_GPIO(cmd->pin))
  {
    resp->error = ENODEV;
    return;
  }

  // Parse cmd->data to break out channel, motor, direction, and speed

  uint8_t channel = (cmd->data >> 24) & 0xFF;
  uint8_t motor = (cmd->data >> 16) & 0xFF;
  uint8_t direction = (cmd->data >> 8) & 0xFF;
  uint8_t speed = cmd->data & 0xFF;

  // Validate parameters

  if ((channel < 1) || (channel > LEGO_RC_CHANNELS))
  {
    resp->error = EINVAL;
    return;
  }

  switch (motor)
  {
    case LEGORC_ALLSTOP :			// Panic stop command
      if (direction != 0)
      {
        resp->error = EINVAL;
        return;
      }

      if (speed != 0)
      {
        resp->error = EINVAL;
        return;
      }
      break;

    case LEGORC_MOTORA :	// Single output PWM mode command
    case LEGORC_MOTORB :
      if (direction >= LEGORC_DIRECTION_SENTINEL)
      {
        resp->error = EINVAL;
        return;
      }

      if (speed > 7)
      {
        resp->error = EINVAL;
        return;
      }

      if ((direction == LEGORC_REVERSE) && (speed != 0))
        speed = 16 - speed;
      break;

    case LEGORC_COMBODIRECT :	// Combo direct mode command
      if (direction != 0)
      {
        resp->error = EINVAL;
        return;
      }

      if (speed > 15)
      {
        resp->error = EINVAL;
        return;
      }
      break;

    case LEGORC_COMBOPWM :	// Combo PWM mode command
      if (direction != 0)
      {
        resp->error = EINVAL;
        return;
      }
      break;

    default :			// Invalid motor ID
      resp->error = EINVAL;
      return;
  }

  // Dispatch the command

  SendCommand(channel, motor, speed, false, (void *) cmd->pin);
  resp->error = errno;
}

static void execute_GET_SFR(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_SFR(cmd->pin))
  {
    resp->error = ENODEV;
    return;
  }

  resp->data = *((uint32_t *) cmd->pin);
}

static void execute_PUT_SFR(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (!IS_SFR(cmd->pin))
  {
    resp->error = ENODEV;
    return;
  }

  *((uint32_t *) cmd->pin) = cmd->data;
}

static void execute_CONFIGURE_TIMER_MODE(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (cmd->pin >= TIMER_ID_SENTINEL)
  {
    resp->error = ENODEV;
    return;
  }

  if (cmd->data > TIMER_MODE_CAP0_BOTH)
  {
    resp->error = EINVAL;
    return;
  }

  if (timer_configure_mode(cmd->pin, cmd->data))
  {
    resp->error = errno;
    return;
  }
}

static void execute_CONFIGURE_TIMER_PRESCALER(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (cmd->pin >= TIMER_ID_SENTINEL)
  {
    resp->error = ENODEV;
    return;
  }

  if (timer_configure_prescaler(cmd->pin, cmd->data))
  {
    resp->error = errno;
    return;
  }
}

#define CAPTURE_EDGE		(cmd->data & 0x0F)
#define CAPTURE_INTERRUPT	(cmd->data & 0x10)

static void execute_CONFIGURE_TIMER_CAPTURE(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (cmd->pin >= TIMER_ID_SENTINEL)
  {
    resp->error = ENODEV;
    return;
  }

  if (CAPTURE_EDGE >= TIMER_CAPTURE_EDGE_SENTINEL)
  {
    resp->error = EINVAL;
    return;
  }

  if (timer_configure_capture(cmd->pin, CAPTURE_EDGE, true))
  {
    resp->error = errno;
    return;
  }

  timer_capture_value[cmd->pin] = 0;
  timer_capture_delta[cmd->pin] = 0;

  if (CAPTURE_INTERRUPT)
    timer_interrupt_mask[cmd->pin] |= 0x00000010;
  else
    timer_interrupt_mask[cmd->pin] &= 0xFFFFFFEF;
}

#define MATCH_ACTION	(cmd->data & 0x0F)
#define MATCH_INTERRUPT	((cmd->data >> 4) & 0x01)
#define MATCH_RESET	((cmd->data >> 5) & 0x01)
#define MATCH_STOP	((cmd->data >> 6) & 0x01)

static void execute_CONFIGURE_TIMER_MATCH(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (cmd->pin >= TIMER_ID_SENTINEL)
  {
    resp->error = ENODEV;
    return;
  }

  unsigned match = cmd->command - SPIAGENT_CMD_CONFIGURE_TIMER_MATCH0;

  if (match >= TIMER_MATCH_OUTPUTS)
  {
    resp->error = EINVAL;
    return;
  }

  // CT32B0 match outputs MAT0-3 are not available, so we can't configure match output actions

  if ((cmd->pin == LPC1114_CT32B0) && (MATCH_ACTION > LPC1114_TIMER_MATCH_OUTPUT_DISABLED))
  {
    resp->error = EINVAL;
    return;
  }

  if (timer_configure_match(cmd->pin, match, MATCH_ACTION, MATCH_INTERRUPT,
        MATCH_RESET, MATCH_STOP))
  {
    resp->error = errno;
    return;
  }

  if (MATCH_INTERRUPT)
    timer_interrupt_mask[cmd->pin] |= (1 << match);
  else
    timer_interrupt_mask[cmd->pin] &= ~(1 << match);
}

static void execute_CONFIGURE_TIMER_MATCH_VALUE(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (cmd->pin >= TIMER_ID_SENTINEL)
  {
    resp->error = ENODEV;
    return;
  }

  unsigned match = cmd->command - SPIAGENT_CMD_CONFIGURE_TIMER_MATCH0_VALUE;

  if (match >- TIMER_MATCH_OUTPUTS)
  {
    resp->error = EINVAL;
    return;
  }

  if (timer_configure_match_value(cmd->pin, match, cmd->data))
  {
    resp->error = errno;
    return;
  }
}

static void execute_GET_TIMER_VALUE(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  switch (cmd->pin)
  {
    case CT32B0:
      resp->data = LPC_TMR32B0->TC;
      break;

    case CT32B1:
      resp->data = LPC_TMR32B1->TC;
      break;

    default:
      resp->error = ENODEV;
      break;
  }
}

static void execute_GET_TIMER_CAPTURE(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  switch (cmd->pin)
  {
    case CT32B0:
      resp->data = LPC_TMR32B0->CR0;
      break;

    case CT32B1:
      resp->data = LPC_TMR32B1->CR0;
      break;

    default:
      resp->error = ENODEV;
      break;
  }
}

static void execute_GET_TIMER_CAPTURE_DELTA(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (cmd->pin >= TIMER_ID_SENTINEL)
  {
    resp->error = ENODEV;
    return;
  }

  resp->data = timer_capture_delta[cmd->pin];
  timer_capture_delta[cmd->pin] = 0;
}

static void execute_INIT_TIMER(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp)
{
  if (cmd->pin >= TIMER_ID_SENTINEL)
  {
    resp->error = ENODEV;
    return;
  }

  if (timer_init(cmd->pin))
  {
    resp->error = errno;
    return;
  }
}

/**************************************************************************/

// SPI Agent command handler table

typedef void (*cmdhandler_t)(SPIAGENT_COMMAND_MSG_t *cmd, SPIAGENT_RESPONSE_MSG_t *resp);

static const cmdhandler_t CommandHandlers[] =
{
  execute_NOP,
  execute_LOOPBACK,
  execute_CONFIGURE_ANALOG_INPUT,
  execute_CONFIGURE_GPIO_INPUT,
  execute_CONFIGURE_GPIO_OUTPUT,
  execute_CONFIGURE_PWM_OUTPUT,
  execute_GET_ANALOG,
  execute_GET_GPIO,
  execute_PUT_GPIO,
  execute_PUT_PWM,
  execute_CONFIGURE_GPIO_INTERRUPT,
  execute_CONFIGURE_GPIO,
  execute_PUT_LEGORC,
  execute_GET_SFR,
  execute_PUT_SFR,
  execute_CONFIGURE_TIMER_MODE,
  execute_CONFIGURE_TIMER_PRESCALER,
  execute_CONFIGURE_TIMER_CAPTURE,
  execute_CONFIGURE_TIMER_MATCH,
  execute_CONFIGURE_TIMER_MATCH,
  execute_CONFIGURE_TIMER_MATCH,
  execute_CONFIGURE_TIMER_MATCH,
  execute_CONFIGURE_TIMER_MATCH_VALUE,
  execute_CONFIGURE_TIMER_MATCH_VALUE,
  execute_CONFIGURE_TIMER_MATCH_VALUE,
  execute_CONFIGURE_TIMER_MATCH_VALUE,
  execute_GET_TIMER_VALUE,
  execute_GET_TIMER_CAPTURE,
  execute_GET_TIMER_CAPTURE_DELTA,
  execute_INIT_TIMER,
};

/**************************************************************************/

// Process command messages from the SPI master

static void SPIMessageHandler(void)
{
  SPIAGENT_COMMAND_MSG_t command;
  SPIAGENT_RESPONSE_MSG_t response;
  bool ISPFlag = false;
  uint8_t *p;
  int i;

  puts("Listening for commands on SPI...");

  // Initialize the slave SPI port

  if (spi_slave_init(SPI_PORT1, 8, 3, SPI_MSBFIRST))
  {
    printf("ERROR: spi_slave_init() failed, %s\n", strerror(errno));
    exit(1);
  }

  // Turn on the LED to indicate end of initialization

  LEDS_set(LED1);

  // The SPI message loop follows

  for (;;)
  {

    // Copy incoming command message to RAM

    p = (uint8_t *) &command;

    for (i = 0; i < sizeof(command); i++)
    {
      while (!(LPC_SSP0->SR & 0x04));
      *p++ = LPC_SSP0->DR;
    }

    // Deassert READY

    gpio_write(LPC1114_READY, 0);

    // Prepare the outgoing response message

    response.command = command.command;
    response.pin = command.pin;
    response.data = 0;
    response.error = 0;

    // Dispatch to the appropriate command handler

    if ((command.command == 0x12345678) && (command.pin == 0x23456789) && (command.data == 0x3456789A))
      ISPFlag = true;
    else if (command.command < SPIAGENT_CMD_SENTINEL)
      CommandHandlers[command.command](&command, &response);
    else
      response.error = EINVAL;

    // Assert READY

    gpio_write(LPC1114_READY, 1);

    // Transmit response message to the SPI master

    p = (uint8_t *) &response;

    for (i = 0; i < sizeof(response); i++)
    {
      while (!(LPC_SSP0->SR & 0x02));
      LPC_SSP0->DR = *p++;
    }

    // Wait for SSP0 not busy

    while (LPC_SSP0->SR & 0x10);

    // Drain the receive FIFO

    while (LPC_SSP0->SR & 0x04)
      (void) LPC_SSP0->DR;

    // Enter ISP mode if requested

    if (ISPFlag) ReinvokeISP();
  }
}

/**************************************************************************/

// Receive command message from the I2C master

static void I2CMessageHandler(void)
{
  int len;
  SPIAGENT_COMMAND_MSG_t command;
  SPIAGENT_RESPONSE_MSG_t response;
  bool ISPFlag = false;

  puts("Listening for commands on I2C...");

  // Initialize the port

  if (i2c_slave_init(0, LPC1114_I2C_ADDRESS))
  {
    printf("ERROR: i2c_slave_init() failed, %s\n", strerror(errno));
    exit(1);
  }

  // Turn on the LED to indicate end of initialization

  LEDS_set(LED1);

  // The I2C message loop follows

  for (;;)
  {
    memset(&command, 0, sizeof(command));

    // Get incoming command message

    len = i2c_slave_read(0, (uint8_t *) &command, sizeof(command));
    if (len < 0)
    {
      printf("ERROR: i2c_slave_read() failed, %s\n", strerror(errno));
      continue;
    }

    if (len != sizeof(command)) continue;

    // Deassert READY

    gpio_write(LPC1114_READY, 0);

    // Prepare the outgoing response message

    response.command = command.command;
    response.pin = command.pin;
    response.data = 0;
    response.error = 0;

    // Dispatch to the appropriate command handler

    if ((command.command == 0x12345678) && (command.pin == 0x23456789) && (command.data == 0x3456789A))
      ISPFlag = true;
    else if (command.command < SPIAGENT_CMD_SENTINEL)
      CommandHandlers[command.command](&command, &response);
    else
      response.error = EINVAL;

    // Assert READY

    gpio_write(LPC1114_READY, 1);

    // Send response message

    len = i2c_slave_write(0, (uint8_t *) &response, sizeof(response));
    if (len < 0)
    {
      printf("ERROR: i2c_slave_write() failed, %s\n", strerror(errno));
      continue;
    }

    // Enter ISP mode if requested

    if (ISPFlag) ReinvokeISP();
  }
}

/**************************************************************************/

// See if SPI slave select is pulled low, indicating we should expect
// commands via I2C instead of SPI.

static bool UseI2C(void)
{
#ifdef FORCEI2C
  return true;
#else
  int i;
  int counter = 0;

  gpio_configure(PIO0_2, GPIO_MODE_INPUT_PULLDOWN);

  for (i = 0; i < 10000; i++)
    if (gpio_read(PIO0_2) == 0)
      counter++;

  gpio_configure(PIO0_2, GPIO_MODE_INPUT);

  return (counter > 9900);
#endif
}

/**************************************************************************/

int main(void)
{
  cpu_init(DEFAULT_CPU_FREQ);
  serial_stdio(CONSOLE_PORT);

  // Display startup banner

  printf("\033[H\033[2J%s SPI Agent v%d (" __DATE__ " " __TIME__ ")\n\n",
    MCUFAMILYNAME, FIRMWARE_VERSION);
  printf("CPU Freq:%u Hz  Compiler:%s %s %s\n\n", (unsigned int) SystemCoreClock,
    __COMPILER__, __VERSION__, __ABI__);

  // Initialize the GPIO terminals as inputs with pull-downs

  gpio_configure(LPC1114_GPIO0, GPIO_MODE_INPUT_PULLDOWN);
  gpio_configure(LPC1114_GPIO1, GPIO_MODE_INPUT_PULLDOWN);
  gpio_configure(LPC1114_GPIO2, GPIO_MODE_INPUT_PULLDOWN);
  gpio_configure(LPC1114_GPIO3, GPIO_MODE_INPUT_PULLDOWN);
  gpio_configure(LPC1114_GPIO4, GPIO_MODE_INPUT_PULLDOWN);
  gpio_configure(LPC1114_GPIO5, GPIO_MODE_INPUT_PULLDOWN);
  gpio_configure(LPC1114_GPIO6, GPIO_MODE_INPUT_PULLDOWN);
  gpio_configure(LPC1114_GPIO7, GPIO_MODE_INPUT_PULLDOWN);

  // Initialize the interrupt outputs

  gpio_configure(LPC1114_INT, GPIO_MODE_OUTPUT);
  gpio_configure(LPC1114_READY, GPIO_MODE_OUTPUT);

  gpio_write(LPC1114_INT, 0);
  gpio_write(LPC1114_READY, 1);

  // Initialze the LED output

  LEDS_initialize();

  // Enable some LPC1114 peripheral interrupts

  NVIC_EnableIRQ(EINT1_IRQn);
  NVIC_EnableIRQ(TIMER_32_0_IRQn);
  NVIC_EnableIRQ(TIMER_32_1_IRQn);

  // Run appropriate message handler

  if (UseI2C())
    I2CMessageHandler();
  else
    SPIMessageHandler();

  exit(0);
}