/*
  SFBHWSerialBoard.cpp - Low-level serial support
  Copyright (C) 2008-2009 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA

  $Id$
*/

#include "clock.h"              /* For getCCLK() */
#include "irq.h"                /* For UART0_INT etc */
#include "SFBPins.h"            /* For reflexLedOn, reflexLedOff */

#include "SFBHWSerialBoard.h"
#include "SFBSerial.h"
#include "SFBAssert.h"
#include "SFBReflex.h"          /* For TEST_REFLEX_FLAGS */ 
#include "SFBPrintf.h"          /* For logf */ 
#include "SFBTicker.h"          /* For Ticker */

extern void leds_on();
extern void leds_off();
extern void printRegister(const char * msg, volatile long unsigned * address);
extern void printValue(const char * msg, u32 value) ;

/**
 * UART (hardware serial port) configuration information.  The
 * specific hardware register addresses, interrupt numbers, and so
 * forth, necessary to configure a serial port.
 */
struct UartDescriptor {

  uv32 * uartBaseAddress;    // The UART register base address
  u32 pconMask;              // The PCONP bit to power up the UART
  uv32 & pclkSel;            // The PCLK_SEL register to specify the CCLK : PCLK ratio
  uv32 & txPinsel;           // The PINSEL register to enable the TX function
  uv32 & rxPinsel;           // The PINSEL register to enable the RX

  u8 selPos;                 // The position in pclkSel to set the ratio
  u8 txPos;                  // The position in txPinsel to specify the TX function
  u8 rxPos;                  // The position in rxPinsel to specify the RX function
  u8 func;                   // The value to store in txPos/rxPos

  typedef void (*UartInterruptHandler)();

  UartInterruptHandler handler; // The interrupt handling function
  u8 interrupt;              // The interrupt number
  u8 priority;               // The interrupt priority
};
 
extern const UartDescriptor UARTDescriptions[FACE_COUNT];

u32 SFBHWSerial::getFD() {
  return (u32) UARTDescriptions[face].uartBaseAddress;
}

void Uart0Handler() {  Faces[0].getHWSerial().uartCommonHandler(); }
void Uart1Handler() {  Faces[1].getHWSerial().uartCommonHandler(); }
void Uart2Handler() {  Faces[2].getHWSerial().uartCommonHandler(); }
void Uart3Handler() {  Faces[3].getHWSerial().uartCommonHandler(); }

const UartDescriptor UARTDescriptions[FACE_COUNT] = {      /* const to get the uart descriptors into flash */

 /* NORTH */ { (uv32*) UART0_BASE_ADDR, PCUART0, PCLKSEL0, PINSEL0, PINSEL0,
                6, 4, 6, 1,
                Uart0Handler, UART0_INT, HIGH_PRIORITY },

 /* SOUTH */ { (uv32*) UART1_BASE_ADDR, PCUART1, PCLKSEL0, PINSEL0, PINSEL1,
                8,30, 0, 1,
                Uart1Handler, UART1_INT, HIGH_PRIORITY },

 /* EAST */  { (uv32*) UART2_BASE_ADDR, PCUART2, PCLKSEL1, PINSEL0, PINSEL0,
                16,20,22, 1,
                Uart2Handler, UART2_INT, HIGH_PRIORITY },

 /* WEST */  { (uv32*) UART3_BASE_ADDR, PCUART3, PCLKSEL1, PINSEL0, PINSEL0,
                18, 0, 2, 2,
                Uart3Handler, UART3_INT, HIGH_PRIORITY }
};

SFBSerial Faces[FACE_COUNT] IN_BODYRAM = {
  SFBSerial(NORTH),
  SFBSerial(SOUTH),
  SFBSerial(EAST),
  SFBSerial(WEST)
};

SFBSerial & SFBHWSerial::getFace(u32 face) {
  API_ASSERT_VALID_FACE(face);
  return Faces[face];
}

static u8 byteBuffers[FACE_COUNT][2][BYTE_BUFFER_BYTES] IN_TALKRAM_BSS;
static PacketBuffer packetBuffers[FACE_COUNT] IN_TALKRAM_BSS;

SFBHWSerial::SFBHWSerial(int theFace) :
  rxBuf(packetBuffers[theFace]),
  rxBytes(byteBuffers[theFace][0]),
  txBytes(byteBuffers[theFace][1]),
  theFrame(rxBytes,txBytes,rxBuf,theFace),
  lastInputActivity(millis()),
  flags(0),
  face(theFace),
  preferredBaudCode(SFBAUD115200),
  maxBaudCode(SFBAUD_CODE_COUNT)
{ }

void SFBHWSerial::setPreferredBaudCode(u8 code) {
  API_ASSERT_MAX(code,SFBAUD_CODE_COUNT);
  preferredBaudCode = code;
}

/* NOTE: This findDivisorAndPrescale is code by
 *
 *  "Jan Vanek" <j3vanek@googlemail.com>
 *
 * who generously posted it to the LPC2000 mailing list on Thu, 12 Mar 2009.
 */

u32 findDivisorAndPrescale(u32 frequency, u32 baudRate, bool allowSmallerBaudRate, u32& mulVal, u32& divAddVal) {
  // The FDR is set to prescale the UART_CLK with factor M/(M+D),
  // where both M and D are in <0, 15>. This is to achieve smaller
  // error when calculating the divisor (the UnDL).
  //                UART_CLK      M
  // UARTbaudrate = --------- x -----
  //                16 x UnDL   (M+D)

  u32 divisor = frequency / (16 * baudRate);
  u32 baudRateError = (frequency / (16 * divisor)) - baudRate;

  if (allowSmallerBaudRate) {
    u32 otherDivisor = divisor + 1;
    u32 otherBaudRateError = baudRate - (frequency / (16 * otherDivisor));
    if (baudRateError > otherBaudRateError) {
      divisor = otherDivisor;
      baudRateError = otherBaudRateError;
    }
  }
  mulVal = 1;
  divAddVal = 0;
  for (u32 mv = 0; ++mv < 16;) {
    for (u32 dav = 0; ++dav < mv;)  {  // ACKLEY: SHOULDN'T THIS BE: '++dav < mv' (according to manual 2.12 pg 426???)
      u32 prescaledClock = (frequency * mv) / (16 * (mv + dav));
      u32 newDivisor = prescaledClock / baudRate;
      if (newDivisor != 0) {
        u32 newBaudRateError = (prescaledClock / newDivisor) - baudRate;
        if (baudRateError > newBaudRateError) {
          divisor = newDivisor;
          mulVal = mv;
          divAddVal = dav;
          baudRateError = newBaudRateError;
        }
      }
      if (allowSmallerBaudRate) {
        ++newDivisor;
        u32 newBaudRateError = baudRate - (prescaledClock / newDivisor);
        if (baudRateError > newBaudRateError) {
          divisor = newDivisor;
          mulVal = mv;
          divAddVal = dav;
          baudRateError = newBaudRateError;
        }
      }
    }
  }
  // u32 calculatedBaudRate = (frequency * mulVal) / ((16 * divisor) * (mulVal + divAddVal));
  return divisor;
}

void SFBHWSerial::checkTx() {

  if (flags&FLAG_TX_ACTIVE) return; /* Interrupt still in charge of output */

  uv32 * uartBaseAddr = UARTDescriptions[face].uartBaseAddress;

  if (!CAN_WRITE(uartBaseAddr)) return;  /* return unless THRE is set */

  u32 len;
  u32 status;
  suspendInterrupts(status);  /* START CRITICAL SECTION */
  for (len = 0; len < 16; ++len) {
    int val = txBytes.fetchByteIL(); // Watch out! Calling interrupt level routine!
    if (val < 0)
      break;
    WRITE_BYTE(uartBaseAddr,(u8) val); /* Send byte off to the UART */ 
  }
  if (len)
    flags |= FLAG_TX_ACTIVE;  /* Pump is primed; interrupt now owns output */
  restoreInterrupts(status);  /* END CRITICAL SECTION */

  if (len && IS_EARLIER(millis(),lastInputActivity+10000))  /* If had input in last ten secs.. */
    reflexLedOn(pinInFace(FACE_LED_PIN,face));            /* ..show we're doing output. */ 
}

void SFBHWSerial::uartCommonHandler() {

  Ticker.pushTimeISR(TICKER_SERIAL_ISR);

  uv32 * uartBaseAddr = UARTDescriptions[face].uartBaseAddress;

  while (1) {
    u32 iir = (REGISTER_OFFSET(uartBaseAddr,UxIIR)>>1)&0x7;

    if (iir == 0x02 || iir == 0x06) { /* If RDA or CTI interrupt */

      u32 ch = REGISTER_OFFSET(uartBaseAddr,UxRBR);   /* Read a char */

      rxBytes.storeByteIL(ch);  /* Store it (if possible) */

      lastInputActivity = millis();  /* Note input activity */ 

    } else if (iir == 0x01) {        /* THRE interrupt: We can now send up to 16 bytes */

      if (flags&FLAG_TX_ACTIVE) {    /* If we think there might be anything.. */

        int i;
        for (i = 0; i < 16; ++i) {
          int val = txBytes.fetchByteIL();
          if (val < 0)
            break;
          WRITE_BYTE(uartBaseAddr,(u8) val);
        }
        if (!i) {
          flags &= ~FLAG_TX_ACTIVE; /* If nothing to write, up to sketch to prime pump */
          reflexLedOff(pinInFace(FACE_LED_PIN,face));
        }
      }

    } else if (iir == 0x03) {        /* RLS interrupt - need to read status */

      u32 status = REGISTER_OFFSET(uartBaseAddr,UxLSR);   /* Read the line status register */

      status &= 0x9e; /* Report: Overrun (0x2), parity (0x4), framing (0x8), break (0x10), rxfifo (0x80) 
                         (Drop: RDR, THRE, TEMT) */

      if (status) {
        rxBytes.storeErrorStatusIL((u8) status); 
      }

    } else
      break;
  }

  Ticker.popTimeISR();
}

/* Our 'official' legal range of baud rates is 50..2250000
 *
 * Note: It seems the standard FTDI USB-Serial cable is officially
 * only rated for 300..1000000 baud!  (Ad hoc testing showed the cable
 * also working at 200 and 2000000, but failing at 110.)
 */
void SFBHWSerial::setBaudRate(u32 baud, bool isOddParity) {
  u32 bits;             /* Scratch for bit ops */
  u32 curCCLK = getCCLK();

  if (isOddParity)
    flags |= FLAG_ODD_PARITY;
  else
    flags &= ~FLAG_ODD_PARITY;
  currentBaudRate = baud;

  ////
  // Determine slowest sufficient PCLK for this baud rate

  u32 pclk;
  u8 pclkCode;
  if (curCCLK/4/16/(4+1) >= baud) {
    pclk = curCCLK/4;
    pclkCode = 0;
  } else if (curCCLK/2/16/(4+1) >= baud) {
    pclk = curCCLK/2;
    pclkCode = 2;
  } else {
    pclk = curCCLK;    // Yah.  Right.  This'll work.
    pclkCode = 1;
  }

  API_ASSERT(baud >= 50 && pclk/16 >= baud, E_API_BAD_BAUD);

  ////
  // Access per-uart details
  const struct UartDescriptor & udesc = UARTDescriptions[face];
  uv32 * uartBaseAddr = udesc.uartBaseAddress;

  ////
  // Power up the UART

  PCONP |= udesc.pconMask;

  ////
  // Set the peripheral clock divider

  bits = udesc.pclkSel;
  BIT_PAIR_AT_POSITION(bits,pclkCode,udesc.selPos); // Select CCLK, CCLK/2, or CCLK/4..
  udesc.pclkSel = bits;

  ////
  // Select frequency divisor for the baud rate,

  u32 mulVal, divAddVal, Fdiv;

  Fdiv = findDivisorAndPrescale(pclk, baud, true, mulVal, divAddVal);

  ////
  // Set framing info, access divisor latches, set up baud rate.

  const u32 enableLatches = isOddParity?0x8B:0x83; /* 8O1 or 8N1, enable access to divisor latches */
  const u32 disableLatches = isOddParity?0x0B:0x03; /* 8O1 or 8N1, disable access to divisor latches */

  ////
  // Disable interrupts globally (just for a few!), so there's no
  // chance of taking a UART interrupt while changing the UART
  // registers.
  u32 status;
  suspendInterrupts(status);

  REGISTER_OFFSET(uartBaseAddr,UxLCR) = enableLatches;
  REGISTER_OFFSET(uartBaseAddr,UxFDR) = (divAddVal<<0)|(mulVal <<4); /* Set the fractional divisor */
  REGISTER_OFFSET(uartBaseAddr,UxDLM) = Fdiv>>8;   /* Set high baud rate divisor */
  REGISTER_OFFSET(uartBaseAddr,UxDLL) = Fdiv&0xff; /* Set low baud rate divisor */

  ////
  // Enable and reset FIFOs and select trigger level for rx interrupt

  // Sun Apr 5 05:26:53 2009 Ackley: The RX trigger level is a drag.
  // We'd like do all of:
  // (1) amortize interrupt overhead (prefer max trigger), and
  // (2) get short packets delivered promptly (prefer min trigger) --
  //     e.g., the distributed blinky sketch relies on packet arrival
  //     times -- and
  // (3) have some spare hardware buffer so we'll be okay if we're a
  //     little late handling an interrupt even at high baud rates (prefer
  //     large/medium trigger).
  //
  // Perhaps some future API could allow changing the value.  Just
  // saying 4 for now.
  //
  // Sun Apr 19 07:26:17 2009 Ackley: Well, based on a little experience
  // playing with this thing, trying upping the trigger level to 8.  Let's
  // see if that let's us get to 1MB on interrupt..

  REGISTER_OFFSET(uartBaseAddr,UxFCR) =
    (1<<0)                      /* Enable FIFO */
    | (1<<1)                    /* Reset the RX FIFO */
    | (1<<2)                    /* Reset the TX FIFO */
    //    | (1<<6)                    /* Set RX trigger level to 4 characters */
    | (2<<6)                    /* Set RX trigger level to 8 characters */
    ;

  ////
  // Specify pin modes

  bits = udesc.txPinsel;        /* Read pinsel register appropriate for tx */
  BIT_PAIR_AT_POSITION(bits,udesc.func,udesc.txPos); /* Select appropriate function at position */
  udesc.txPinsel = bits;        /* Write pinsel register */

  bits = udesc.rxPinsel;        /* Read pinsel register appropriate for rx */
  BIT_PAIR_AT_POSITION(bits,udesc.func,udesc.rxPos); /* Select appropriate function at position */
  udesc.rxPinsel = bits;        /* Write pinsel register */

  // We're not explicitly disabling pulldowns here -- should we?
  //pinMode(SOUTH_RX_PIN,REFLEX_MODE); // XXX HACK DEBUG
  //reflexSetPinResistor(SOUTH_RX_PIN,NEITHER_PULLUP_NOR_PULLDOWN); // XXX HACK DEBUG

  ////
  // Disable access to divisor latches, enable interrupts
  REGISTER_OFFSET(uartBaseAddr,UxLCR) = disableLatches;      /* 8O1 or 8N1, disable access to divisor latches */

  ////
  // Reenable interrupts globally
  restoreInterrupts(status);

}

void SFBHWSerial::start(u32 baud,bool oddParity) {

  stop();        // Stop, to get a clean start (e.g, interrupts _off_)

  ////
  // Initialize state

  flags = FLAG_STARTED;     // Set FLAG_STARTED (and clear rest, e.g. FLAG_TX_ACTIVE)
  rxBytes.resetBG();
  txBytes.resetBG();

  ////
  // Set up the hardware including the baud rate
  setBaudRate(baud,oddParity);

  ////
  // Access uart descriptor for interrupt details
  const struct UartDescriptor & udesc = UARTDescriptions[face];

  ////
  // Install the interrupt vector!
  install_irq(udesc.interrupt, udesc.handler, udesc.priority);

  ////
  // Enable the interrupts!
  REGISTER_OFFSET(udesc.uartBaseAddress,UxIER) = (1<<0)|(1<<1)|(1<<2);  /* Enable the RDA/CTI, the THRE, and the RLS interrupts */
}

void SFBHWSerial::reinit() {
  if (flags&FLAG_STARTED) 
    setBaudRate(currentBaudRate, flags&FLAG_ODD_PARITY);
}

static const u8 maxBaudCodesByCCLK[CSC_MAX] = {
  0,                            // CSC_UNSET
  SFBAUD115200,                 // 12MHz
  SFBAUD230400,                 // 24MHz
  SFBAUD500000,                 // 36MHz
  SFBAUD500000,                 // 48MHz
  SFBAUD500000,                 // 60MHz
  SFBAUD1000000                 // 72MHz
};
u8 SFBHWSerial::getHighBaudCode() {
  u32 high = maxBaudCodesByCCLK[getCCLKCode()];
  if (high == 0)
    high = SFBAUD115200;
  if (maxBaudCode != SFBAUD_CODE_COUNT && (maxBaudCode < high || high == 0))
    high = maxBaudCode;
  return high;
}

u8 SFBHWSerial::getPreferredBaudCode() {
  u8 pref = preferredBaudCode;
  u8 high = getHighBaudCode();
  if (high < pref)
    pref = high;
  return pref;
}

void SFBHWSerial::reinitAll() {
  for (u32 f = NORTH; f <= WEST; ++f) 
    Faces[f].getHWSerial().reinit();
}

void SFBHWSerial::stop()
{
  u32 bits;

  const struct UartDescriptor & udesc = UARTDescriptions[face];

  ////
  // Disable the interrupt

  uninstall_irq(udesc.interrupt);

  ////
  // Disable interrupts for this UART
  REGISTER_OFFSET(udesc.uartBaseAddress,UxIER) = 0;

  ////
  // Clear fractional divider in case other users are unaware of it.

  REGISTER_OFFSET(udesc.uartBaseAddress,UxLCR) = 0x8B/*XXX8O1 0x83*/;      /* 8N1, enable access to divisor latches */
  REGISTER_OFFSET(udesc.uartBaseAddress,UxFDR) = 0x10;      /* Reset fractional divisor */
  REGISTER_OFFSET(udesc.uartBaseAddress,UxLCR) = 0x0B/*XXX8O1 0x03*/;      /* 8N1, disable access to divisor latches */

  /////
  // Turn the pins back to GPIOs

  bits = udesc.txPinsel;        /* Read pinsel register appropriate for tx */
  BIT_PAIR_AT_POSITION(bits,0,udesc.txPos); /* Select function 0 at position */
  udesc.txPinsel = bits;        /* Write pinsel register */

  bits = udesc.rxPinsel;        /* Read pinsel register appropriate for rx */
  BIT_PAIR_AT_POSITION(bits,0,udesc.rxPos); /* Select function 0 at position */
  udesc.rxPinsel = bits;        /* Write pinsel register */

  ////
  // Power down the UART

  PCONP &= ~udesc.pconMask;

  ////
  // Clear flags (including FLAG_STARTED)
  flags = 0;
}

bool SFBHWSerial::moreFaces(u32 face) { return face <= WEST; }
u32 SFBHWSerial::nextFace(u32 face) { return face+1; }

void SFBHWSerial::putcBlocking(const u8 byte) {
#ifndef HOST_MODE
  ledOn(BODY_RGB_BLUE_PIN);
#endif
  uv32 * uartBaseAddr = UARTDescriptions[face].uartBaseAddress;
  while (!CAN_WRITE(uartBaseAddr)) { /* block */ }
  WRITE_BYTE(uartBaseAddr,byte);
}
