#include "kernel.h"

#include <sem.h>

#include <hwi.h>
#include <c64.h>
#include <c6x.h>

#include <cstring>
#include <ti/pspiom/cslr/soc_C6747.h>
#include <ti/pspiom/cslr/cslr_syscfg_C6747.h>
#include <ti/pspiom/cslr/cslr_psc_C6747.h>
#include <ti/pspiom/cslr/cslr_emifa2.h>
#include <ti/pspiom/cslr/cslr_gpio.h>
#include <ti/pspiom/cslr/cslr_edma3cc.h>
#include <ti/pspiom/cslr/cslr_edma3tc.h>

#include "config.h"

#include "peripheral/usb0_impl.h"
#include "peripheral/led.h"
#include "peripheral/uart.h"
#include "peripheral/spi.h"
#include "peripheral/i2c.h"
#include "peripheral/ads1248.h"
#include "peripheral/spirom.h"
#include "peripheral/ublox.h"
#include "peripheral/mmcsd.h"
#include "peripheral/hmc5843.h"
#include "peripheral/max2ufm.h"

#include "fatfs/ff.h"
#include "navigation.h"
#include "calibration.h"
#include "guidance_control.h"

volatile Uint16 * const cpld_servo_obs((volatile Uint16 *)0x62000000u);
volatile Uint16 * const cpld_servo_drv((volatile Uint16 *)0x62000010u);
volatile Uint8 * const cpld_general_reg((volatile Uint8 *)0x62000020u);

static SEM_Obj sem_emifa_mmcsd;
static SEM_Obj sem_spi0;
static SEM_Obj sem_periodic;
static SEM_Obj sem_1pps;

#pragma DATA_ALIGN(0x100)
static FATFS fatfs;

static Uint8 kernel_state;
static const Uint8 kernel_state_polling_active(0x01);
static const Uint8 kernel_state_fs_active(0x02);

static void wait(const unsigned int &us){
  for(volatile int i(0); i < us; i++){
    for(volatile int j(0); j < 300; j++);
  }
}

Kernel::Kernel() : initialized(false) {}
Kernel::Kernel(const Kernel &orig) 
    : initialized(orig.initialized) {}
Kernel::~Kernel() {}

Kernel &Kernel::get_instance(){
  static Kernel only_one;
  return only_one;
}
static Kernel &kernel(Kernel::get_instance());

unsigned int Kernel::SystemIO::receive(char *buf, const unsigned int &buf_size){
  return 0;
}
unsigned int Kernel::SystemIO::transmit(const char *buf, const unsigned int &buf_size){
  return 0;
}
bool Kernel::SystemIO::ioctl(const void *params){
  return false;
}
Kernel::SystemIO::SystemIO() : ref_count(0) {}
Kernel::SystemIO::~SystemIO() {}

unsigned int Kernel::IO::receive(char *buf, const unsigned int &buf_size){
  return delegatee->receive(buf, buf_size);
}
unsigned int Kernel::IO::transmit(const char *buf, const unsigned int &buf_size){
  return delegatee->transmit(buf, buf_size);
}
bool Kernel::IO::ioctl(const void *params){
  return delegatee->ioctl(params);
}
Kernel::IO::IO(SystemIO *io) : delegatee(io) {(delegatee->ref_count)++;}
Kernel::IO::IO(const IO &orig) : delegatee(orig.delegatee) {(delegatee->ref_count)++;}
Kernel::IO &Kernel::IO::operator=(const IO &rhs){
  if(delegatee != rhs.delegatee){
    if((--(delegatee->ref_count)) <= 0){delete delegatee;}
    delegatee = rhs.delegatee;
    (delegatee->ref_count)++;
  }
  return *this;
}
Kernel::IO::~IO(){
  if((--(delegatee->ref_count)) <= 0){delete delegatee;}
}

struct SYS_MAPPED_IO : public Kernel::SystemIO {
  const Uint8 *addr_base;
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    int count(0);
    volatile Uint8 *addr(const_cast<Uint8 *>(addr_base));
    SEM_pendBinary(&sem_emifa_mmcsd, SYS_FOREVER);
    while(count < buf_size){
      buf[count++] = *(addr++);
    }
    SEM_postBinary(&sem_emifa_mmcsd);
    return count;
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    int count(0);
    volatile Uint8 *addr(const_cast<Uint8 *>(addr_base));
    SEM_pendBinary(&sem_emifa_mmcsd, SYS_FOREVER);
    while(count < buf_size){
      *(addr++) = buf[count++];
    }
    SEM_postBinary(&sem_emifa_mmcsd);
    return count;
  }
  SYS_MAPPED_IO(const void *params) 
      : Kernel::SystemIO(), addr_base((const Uint8 *)params) {}
};

static struct UBLOX_IO : public Kernel::SystemIO {
  bool blocking;
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    if(blocking){
      int count(0);
      while(count < buf_size){buf[count++] = uart1.getc();}
      return count;
    }else{
      return uart1.receive(buf, buf_size);
    }
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    if(blocking){
      int count(0);
      while(count < buf_size){uart1.putc(buf[count++]);}
      return count;
    }else{
      return uart1.transmit(buf, buf_size);
    }
  }
  UBLOX_IO() : Kernel::SystemIO(), blocking(true) {}
} ublox_sys_io;
static Kernel::IO ublox_io(&ublox_sys_io);
static Ublox ubx(ublox_io);

static struct GS_LINK_IO : public Kernel::SystemIO {
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    return uart2.receive(buf, buf_size);
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    return uart2.transmit(buf, buf_size);
  }
  GS_LINK_IO() : Kernel::SystemIO() {}
} gs_link_sys_io;
static Kernel::IO gs_link_io(&gs_link_sys_io);

struct FLASHROM_IO : public Kernel::SystemIO {
  static SPIROM spirom;
  static bool is_spi0_connecting_flash;
  Uint32 offset;
  static void spi0_2_flash(){
    if(is_spi0_connecting_flash){return;}
    SEM_pendBinary(&sem_emifa_mmcsd, SYS_FOREVER);
    cpld_general_reg[0] &= ~0x04; // spi0 <=>  SST25VF040B
    spi0.set_format(0);
    SEM_postBinary(&sem_emifa_mmcsd);
    is_spi0_connecting_flash = true;
  }
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    SEM_pendBinary(&sem_spi0, SYS_FOREVER);
    spi0_2_flash();
    spirom.read(offset, buf, buf_size);
    SEM_postBinary(&sem_spi0);
    return buf_size;
  }
  FLASHROM_IO(const void *params) 
      : Kernel::SystemIO(), offset(*(Uint32 *)params) {}
};
SPIROM FLASHROM_IO::spirom(spi0);
bool FLASHROM_IO::is_spi0_connecting_flash;

struct ADC_IO : public Kernel::SystemIO {
  static ADS1248 adc;
  Uint8 address;
  static void spi0_2_adc(){
    if(!FLASHROM_IO::is_spi0_connecting_flash){return;}
    SEM_pendBinary(&sem_emifa_mmcsd, SYS_FOREVER);
    cpld_general_reg[0] |= 0x04; // spi0 <=> ADS1248
    spi0.set_format(1);
    SEM_postBinary(&sem_emifa_mmcsd);
    FLASHROM_IO::is_spi0_connecting_flash = false;
  }
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    SEM_pendBinary(&sem_spi0, SYS_FOREVER);
    spi0_2_adc();
    adc.read_register(address, (Uint8 *)buf, buf_size);
    SEM_postBinary(&sem_spi0);
    return buf_size;
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    SEM_pendBinary(&sem_spi0, SYS_FOREVER);
    spi0_2_adc();
    adc.write_register(address, (const Uint8 *)buf, buf_size);
    SEM_postBinary(&sem_spi0);
    return buf_size;
  }
  ADC_IO(const void *params) 
      : Kernel::SystemIO(), address(*(Uint8 *)params) {}
};
ADS1248 ADC_IO::adc(spi0);

struct FAT_FILE_IO : public Kernel::SystemIO {
  FIL f;
  bool available;
  Uint8 autosync;
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    if(!available){return 0;}
    UINT read;
    SEM_pendBinary(&sem_emifa_mmcsd, SYS_FOREVER);
    if(f_read(&f, buf, buf_size, &read) == FR_OK){
      kernel_state |= kernel_state_fs_active;
    }
    SEM_postBinary(&sem_emifa_mmcsd);
    return read;
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    if(!available){return 0;}
    UINT written;
    SEM_pendBinary(&sem_emifa_mmcsd, SYS_FOREVER);
    DWORD previous_size(f.fsize);
    if(f_write(&f, buf, buf_size, &written) == FR_OK){
      kernel_state |= kernel_state_fs_active;
      if((previous_size >> autosync) < (f.fsize >> autosync)){
        f_sync(&f);
      }
    }
    SEM_postBinary(&sem_emifa_mmcsd);
    return written;
  }
  FAT_FILE_IO(const void *params) 
      : Kernel::SystemIO(),
      autosync(12) { // (1 << 12) = 4KBごとに自動fsync
    SEM_pendBinary(&sem_emifa_mmcsd, SYS_FOREVER);
    available = (f_open(&f, 
          (const char *)params, 
          (FA_OPEN_ALWAYS | FA_READ | FA_WRITE)) == FR_OK);
    if(available){
      // 追加書き込みにするため、シークする
      f_lseek(&f, f.fsize);
    }
    SEM_postBinary(&sem_emifa_mmcsd);
  }
  bool ioctl(const void *params){
    return false; // TODO
  }
  ~FAT_FILE_IO(){f_close(&f);}
};

static struct USB_UART_IO : public Kernel::SystemIO {
  char tx_scratch[0x40 - (USE_FTDI_MIMIC ? 2 : 0)];
  int tx_scratch_index;
  USB_UART_IO() : tx_scratch_index(0) {}
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    if(USE_FTDI_MIMIC){
      return usb0_otg_impl.ft232.receive(buf, buf_size);
    }else{
      return usb0_otg_impl.cdc.receive(buf, buf_size);
    }
  }
  unsigned int transmit_internal(const char *buf, const unsigned int &buf_size){
    if(USE_FTDI_MIMIC){
      return usb0_otg_impl.ft232.transmit(buf, buf_size);
    }else{
      return usb0_otg_impl.cdc.transmit(buf, buf_size);
    }
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
#if 0
    unsigned int _buf_size(buf_size);
    unsigned int total_transmitted(0);
    while(_buf_size > 0){
      int margin(sizeof(tx_scratch) - tx_scratch_index);
      if(margin > _buf_size){
        std::memcpy(&tx_scratch[tx_scratch_index], buf, _buf_size);
        tx_scratch_index += _buf_size;
        return total_transmitted + _buf_size;
      }else{
        if(tx_scratch_index == 0){
          unsigned int transmitted(transmit_internal(buf, sizeof(tx_scratch)));
          total_transmitted += transmitted;
          if(transmitted < sizeof(tx_scratch)){
            return total_transmitted;
          }
          buf += sizeof(tx_scratch);
          _buf_size -= sizeof(tx_scratch);
        }else{
          std::memcpy(&tx_scratch[tx_scratch_index], buf, margin);
          unsigned int transmitted(transmit_internal(tx_scratch, sizeof(tx_scratch)));
          total_transmitted += (margin - (sizeof(tx_scratch) - transmitted));
          if(transmitted < sizeof(tx_scratch)){
            return total_transmitted;
          }
          tx_scratch_index = 0;
          buf += margin;
          _buf_size -= margin;
        }
      }
    } // TODO: 同じパケットが送出されることがあるので詰め詰めで使うこの部分は無効にした、USBをDMA化すること
#endif
    
    // 0バイト書き込み(特殊)
    return transmit_internal(buf, buf_size);
  }
} uart_sys_io;
static Kernel::IO uart_io(&uart_sys_io);

static struct LED_IO : public Kernel::SystemIO {
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    if(buf_size < 1){return 0;}
    buf[0] = led.led();
    return 1;
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    if(buf_size < 1){return 0;}
    led.led(buf[0]);
    return 1;
  }
} led_sys_io;
static Kernel::IO led_io(&led_sys_io);

static Kernel::SystemIO null_sys_io;
static Kernel::IO null_io(&null_sys_io);
Kernel::IO::IO() : delegatee(&null_sys_io) {(delegatee->ref_count)++;}

Kernel::IO Kernel::open(const special_devices &id, const void *params){
  switch(id){
    case SYS_MAPPED: return IO(new SYS_MAPPED_IO(params));
    case UBLOX: return ublox_io;
    case GS_LINK: return gs_link_io;
    case FLASHROM: return IO(new FLASHROM_IO(params));
    case ADC: return IO(new ADC_IO(params));
    case FAT_FILE: return IO(new FAT_FILE_IO(params));
    case USB_UART: return uart_io;
    case OBCLED: return led_io;
  }
  return null_io;
}
MBX_Handle Kernel::mbx(const mbx_devices &id) const {
  return mbx_handles[id];
}

struct AISFeeder {
  FIL &_f;
  unsigned int count;
  Uint8 word[sizeof(Uint32)];
  AISFeeder(FIL &f) : _f(f), count(0) {}
  unsigned int operator()(Uint8 *buf, const unsigned int &size){
    unsigned int res(0), word_index(count % sizeof(word));
    while(res < size){
      if(word_index == 0){
        unsigned int new_read;
        f_read(&_f, word, sizeof(word), &new_read);
        if(new_read < sizeof(word)){break;}
        // Little Endian => Big Endian
        /*Uint8 tmp;
        tmp = word[0]; word[0] = word[3]; word[3] = tmp;
        tmp = word[1]; word[1] = word[2]; word[2] = tmp;*/
      }
      buf[res++] = word[word_index++];
      count++;
      if(word_index == sizeof(word)){
        word_index = 0;
      }
    }
    led.led(count >> 12);
    return res;
  }
};

/* 
 * ina111 configuration (AD7689)
 * 
 * 0xB1, 0x44 = 1 011 000 1 010 00 1 (PAD:00), temperature
 * 1   overwrite
 * 011 Temp
 * 000 IN0
 * 1   full B/W
 * 010 ext. ref. temp enable
 * 00  disable seq
 * 1   do not read back
 */
static const Uint8 ina111_spi_prefix[] = {0xFC, 0xFD};
static const Uint8 ina111_spi_sequence[][2] = {
    {0xF5, 0x44}, {0xF7, 0x44}, {0xF9, 0x44},   // ch.2, ch.3, ch.4, 
    {0xFB, 0x44}, {0xFD, 0x44}, {0xFF, 0x44},   // ch.5, ch.6, ch.7,
    {0xB1, 0x44}, {0xF1, 0x44}, {0xF3, 0x44},}; // temp, ch.0, ch.1,

static HMC5843 hmc5843(i2c1);

// MUX0: 0b00 nnn 000 (current.src.off, nnn, mux.sn=ch0)
static const Uint8 adc_mux0_table[] = {
    0x02 << 3, 0x07 << 3, 0x06 << 3, 0x04 << 3, 0x05 << 3, 0x03 << 3, 0x01 << 3};

void Kernel::initialize() {
  if(initialized){return;}
  
  // Mail box
  {
    mbx_handles[MBX_ADC] = MBX_create(sizeof(msg_adc_t), 8, NULL);
    mbx_handles[MBX_SERVO_WRITE] = MBX_create(sizeof(msg_servo_write_t), 8, NULL);
    mbx_handles[MBX_SERVO_READ] = MBX_create(sizeof(msg_servo_read_t), 8, NULL);
    mbx_handles[MBX_PITOT] = MBX_create(sizeof(msg_pitot_t), 4, NULL);
    mbx_handles[MBX_MAG] = MBX_create(sizeof(msg_mag_t), 4, NULL);
    mbx_handles[MBX_TU_INFO] = MBX_create(sizeof(TimeUpdateInfo), 16, NULL);
    mbx_handles[MBX_MU_INFO] = MBX_create(sizeof(MeasurementUpdateInfo), 4, NULL);
    mbx_handles[MBX_MAG_INFO] = MBX_create(sizeof(MagInfo), 8, NULL);
    mbx_handles[MBX_NAV_INFO] = MBX_create(sizeof(NAVInfo), 8, NULL);
    mbx_handles[MBX_GC_INFO] = MBX_create(sizeof(GCInfo), 4, NULL);
    
    // initial drv values
    msg_servo_write_t servo_write;
    servo_write.time_ms = 0;
    for(int i(0); i < sizeof(servo_write.ch) / sizeof(servo_write.ch[0]); i++){
      servo_write.ch[i] = 1500;
    }
    MBX_post(mbx_handles[MBX_SERVO_WRITE], &servo_write, 0);
  }
  
  SEM_new(&sem_emifa_mmcsd, 1);
  SEM_new(&sem_spi0, 1);
  SEM_new(&sem_periodic, 0);
  SEM_new(&sem_1pps, 0);
  
  led.init();
  LOG_printf(&trace, "LED ready.");
  
  uart1.baudrate(9600);
  uart1.init();
  LOG_printf(&trace, "UART1 ready.");
  
  // i2c1 and related devices (HMC5843, ...)
  if(USE_I2C1){
    i2c1.init();
    hmc5843.init();
    
    // HMC5843 test read
    Int16 values[3];
    for(int i(0); i < 0x10; i++){
      hmc5843.xyz(values);
      wait(1);
    }
  }
  
  // Servo OUT
  //cpld_general_reg[2] |= 0x04; // force cic mode
  //cpld_general_reg[2] |= 0x01; // 100Hz
  //cpld_general_reg[2] |= 0x02; // 200Hz
  //cpld_general_reg[2] |= 0x03; // 400Hz
  
  // FAT
  f_mount(0, &fatfs);
  LOG_printf(&trace, "FAT on MMCSD ready.");
  
  spi0.init();
  spi0.set_prescale(1, 59); // For ADS1248 (max: CLK_period under 520us => 2.5MHz <= 150 / (59 + 1))
  spi0.set_prescale(2, 29); // For CPLD (max: 5MHz <= 150 / (29 + 1))
  LOG_printf(&trace, "SPI0 ready.");
  
  // SST25VF040B setup
  {
    SPIROM &spirom(FLASHROM_IO::spirom);
    cpld_general_reg[0] &= ~0x04; // spi0 <=>  SST25VF040B
    FLASHROM_IO::is_spi0_connecting_flash = true;
    
    // JEDEC Read-ID
    Uint32 jedec_id(spirom.jedec_id());
    wait(1);
    
    // test read
    for(int i(0); i < 0x100; i++){
      Uint8 buf[0x10];
      spirom.read(i * sizeof(buf), buf, sizeof(buf));
      wait(1);
    }
  
    // boot ROM renewal
    while(true){
      FIL f;
      const char rom_new_fname[] = "rom_new.bin";
      const char rom_last_fname[] = "rom_last.bin";
      if(f_open(&f, rom_new_fname, FA_OPEN_EXISTING | FA_READ) != FR_OK){break;}
      AISFeeder feeder(f);
      spirom.chip_flush(feeder);
      f_close(&f);
      f_unlink(rom_last_fname);
      f_rename(rom_new_fname, rom_last_fname);
      
      // TODO: reboot
    }
  }
  
  // MAXII UFM setup
  {
    MAX2UFM max2ufm(spi0);
    
    spi0.format_register(3) = (
        (0x3F << CSL_SPI_SPIFMT_WDELAY_SHIFT) // WDELAY
        | (0xFF << CSL_SPI_SPIFMT_PRESCALE_SHIFT)
        | (8 << CSL_SPI_SPIFMT_CHARLEN_SHIFT) // charelen = 8bits
        //| CSL_SPI_SPIFMT_POLARITY_MASK // polarity inactive = high
        | CSL_SPI_SPIFMT_PHASE_MASK // phase
        );
    spi0.set_format(3);
    
    cpld_general_reg[0] |= 0x02; // spi0 <=>  MAX2UFM
    
    static const Uint32 magic(0x5AA59669u);
    Uint8 buf[MAX2UFM::max_bytes];
    
    // UFM flush test
    if(false){
      Uint8 *dst(buf);
      std::memcpy(dst, &magic, sizeof(magic)); // 1 * sizeof(Uint32)
      dst += sizeof(magic);
      std::memcpy(dst, &calibrator.accel, sizeof(calibrator.accel)); // 18 * sizeof(double)
      dst += sizeof(calibrator.accel);
      std::memcpy(dst, &calibrator.omega, sizeof(calibrator.omega)); // 18 * sizeof(double)
      max2ufm.chip_flush(buf, sizeof(buf));
      //max2ufm.chip_erace();
    }
    
    // UFM renewal
    while(true){
      FIL f;
      const char cfg_new_fname[] = "cfg_new.bin";
      const char cfg_last_fname[] = "cfg_last.bin";
      if(f_open(&f, cfg_new_fname, FA_OPEN_EXISTING | FA_READ) != FR_OK){break;}
      if(f.fsize){
        std::memcpy(buf, &magic, sizeof(magic)); // 1 * sizeof(Uint32)
        unsigned int cfg_size;
        f_read(&f, buf + sizeof(magic), sizeof(buf) - sizeof(magic), &cfg_size);
        max2ufm.chip_flush(buf, cfg_size + sizeof(magic));
      }else{
        max2ufm.chip_erace();
      }
      f_close(&f);
      f_unlink(cfg_last_fname);
      f_rename(cfg_new_fname, cfg_last_fname);
    }

    // config read
    max2ufm.read(0, buf, sizeof(buf));
    
    Uint8 *src(buf); 
    Uint32 magic_buf;
    std::memcpy(&magic_buf, src, sizeof(magic_buf)); // 1 * sizeof(Uint32)
    if(magic == magic_buf){
      src += sizeof(magic_buf);
      std::memcpy(&calibrator.accel, src, sizeof(calibrator.accel)); // 18 * sizeof(double)
      src += sizeof(calibrator.accel);
      std::memcpy(&calibrator.omega, src, sizeof(calibrator.omega)); // 18 * sizeof(double)
    }

    cpld_general_reg[0] &= ~0x02; // spi0 <=>  SST25VF040B
    spi0.set_format(0);
  }

  // ADS1248 setup
  {
    ADS1248 &ads1248(ADC_IO::adc);
    spi0.set_format(1);
    Uint8 buf[8];
    cpld_general_reg[0] |= 0x04; // spi0 <=> CPLD, ADS1248
    FLASHROM_IO::is_spi0_connecting_flash = false;
    
    cpld_general_reg[0] = (cpld_general_reg[0] & ~0x18) | 0x10; // ADS1248 start to high
    wait(20000); // TODO: wait for more than 16ms?
    
    // reset
    ads1248.reset();
    ads1248.stopload();
    wait(1000);
    
    // read reg(IDAC0:0x0A)
    ads1248.read_register(0x0A, buf, 1);
    
    // MUX1: 0b0 01 10 000 (int.osc, int.ref, onbrd.ref, nor.op)
    buf[0] = 0x30;
    ads1248.write_register(0x02, buf, 1);
    ads1248.read_register(0x02, buf, 1);
    
    // SYS0: 0b0 000 1001 (PGA=1, 2000SPS)
    buf[0] = 0x09;
    ads1248.write_register(0x03, buf, 1);
    ads1248.read_register(0x03, buf, 1);
    
    ads1248.calibrate();
    wait(1000);
    
    // raw read reg test
    {
      Uint8 read_regs[] = {0xFF, 0x20, 0x0E, 
          0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
          0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
      spi0.write_read(read_regs, sizeof(read_regs));
      wait(1);
    }
    
    // Test Read
    msg_adc_t msg_adc;
    for(int i((sizeof(adc_mux0_table) / sizeof(adc_mux0_table[0])) - 1);
        i >= 0; i--){
      ads1248.write_register(0x00, &(adc_mux0_table[i]), 1);
      wait(1000);
      msg_adc.ch[i] = ads1248.read_result();
    }
    wait(1);
  }

  // CPLD test
  {
    spi0.set_format(2);
    Uint16 buf[8];
    
    // servo obs read
    static const Uint8 servo_read_prefix[] = {0x00};
    spi0.write((void *)servo_read_prefix, sizeof(servo_read_prefix), true);
    spi0.write_read((void *)buf, sizeof(buf));
    
    // servo drv write
    static const Uint8 servo_write_prefix[] = {0x90};
    spi0.write((void *)servo_write_prefix, sizeof(servo_write_prefix), true);
    spi0.write_read((void *)buf, sizeof(buf));
    
    wait(1);
  }
  
  // ublox setup
  {
    ubx.set_ubx_cfg_prt(115200); // baudrate change
    uart1.baudrate(115200);
    
    wait(10000);
    
    ubx.set_ubx_cfg_rate(200); // 1s / 200ms = 5Hz
    ubx.set_ubx_cfg_tp();
    ubx.set_ubx_cfg_sbas(3);
    ubx.set_ubx_cfg_nav5(); // dynamic_mode => portable
    ubx.set_ubx_cfg_rxm(); // max performance mode
  
    ubx.set_ubx_cfg_msg(0x01, 0x02, 1);  // NAV-POSLLH  // 28 + 8 = 36 bytes
    ubx.set_ubx_cfg_msg(0x01, 0x03, 5);  // NAV-STATUS  // 16 + 8 = 24 bytes
    ubx.set_ubx_cfg_msg(0x01, 0x04, 5);  // NAV-DOP     // 18 + 8 = 26 bytes
    ubx.set_ubx_cfg_msg(0x01, 0x06, 1);  // NAV-SOL     // 52 + 8 = 60 bytes
    ubx.set_ubx_cfg_msg(0x01, 0x12, 1);  // NAV-VELNED  // 36 + 8 = 44 bytes
    ubx.set_ubx_cfg_msg(0x01, 0x30, 10); // NAV-SVINFO  // (8 + 12 * x) + 8 = 112 bytes (@8)
    ubx.set_ubx_cfg_msg(0x02, 0x10, 1);  // RXM-RAW     // (8 + 24 * x) + 8 = 208 bytes (@8)
    ubx.set_ubx_cfg_msg(0x02, 0x11, 10); // RXM-SFRB    // 42 + 8 = 50 bytes
    
    ublox_sys_io.blocking = false;
    
    LOG_printf(&trace, "GPS ready.");
  }
  
  wait(1000);
  
  // GPIO5_10,5_11
  {
    CSL_GpioRegsOvly reg((CSL_GpioRegsOvly)CSL_GPIO_0_REGS);
    // set dir to input; @see 2.7.1 sprufl8b (GPIO)
    reg->BANK[GP5].DIR |= (GP5P10 | GP5P11);
    // enable interrupt of bank 5 @see 2.10.2 sprufl8b (GPIO)
    reg->BINTEN |= CSL_GPIO_BINTEN_EN5_MASK;
    // interrupt setting for detection of falling edge: @see 2.10.3 sprufl8b (GPIO)
    reg->BANK[GP5].CLR_RIS_TRIG |= GP5P10;
    reg->BANK[GP5].SET_FAL_TRIG |= GP5P10;
  }
  
  // GS-Link (XBee etc.) setup
  {
    uart2.baudrate(9600); // TODO: variable configurations, such as baudrate
    uart2.init();
    LOG_printf(&trace, "UART2 ready.");
  }
  
  // Pitot test
  {
    // SPI0 format3 for pitot
    spi0.format_register(3) = (
        CSL_SPI_SPIFMT_POLARITY_MASK // polarity inactive = high
        | (0x3F << CSL_SPI_SPIFMT_WDELAY_SHIFT) // WDELAY
        | (149 << CSL_SPI_SPIFMT_PRESCALE_SHIFT) // prescale 150MHz / (149+1) = 1MHz
        | (8 << CSL_SPI_SPIFMT_CHARLEN_SHIFT) // charelen = 8bits
        );
    
    spi0.set_format(3);
    Uint8 buf[12];
    
    // read
    for(int i(0); i < 0x10; i++){
      buf[0] = 0xFE;
      buf[1] = 0xA5;
      spi0.write_read((void *)buf, sizeof(buf));
      wait(1);
    }
  }
  
#if 0
  { // CPLD spi slave test
    spi0.set_format(2);
    Uint8 buf[4];
    for(int i(0); i < 0x100; i++){
      buf[0] = 0xFC;
      buf[1] = i;
      buf[2] = buf[3] = 0;
      spi0.write_read((void *)buf, sizeof(buf));
      if((buf[0] != 0xAA) || (buf[1] != 0xFF) || (buf[2] != 0xFF) || (buf[3] != 0xFF)){
        wait(1);
      }
    }
  }
#endif
  
  if(USE_INA111){ // ina111 test
    // SPI0 format2 for ina111
    spi0.set_format(2);
    Uint8 buf[3];
    
    // Read
    for(int i(0); i < sizeof(ina111_spi_prefix) / sizeof(ina111_spi_prefix[0]); i++){
      for(int j(0); j < sizeof(ina111_spi_sequence) / sizeof(ina111_spi_sequence[0]); j++){
        buf[0] = ina111_spi_prefix[i];
        std::memcpy(&buf[1], ina111_spi_sequence[j], sizeof(buf) - 1);
        spi0.write_read((void *)buf, sizeof(buf));
        wait(1);
      }
    }
  }

  usb0_otg_impl.init();
  LOG_printf(&trace, "USB ready.");
  
  mmcsd.lock_pins(); // after this, EMIFA can not work completely.
  
  // enable ECM interrupt
  C64_enableIER(C64_EINT7);
  C64_enableIER(C64_EINT8);
  C64_enableIER(C64_EINT9);
  C64_enableIER(C64_EINT10);
  
  // DMA
  CSL_Edma3tcRegsOvly dmaTC0Regs((CSL_Edma3tcRegsOvly)CSL_EDMA3TC_0_REGS);
  CSL_Edma3tcRegsOvly dmaTC1Regs((CSL_Edma3tcRegsOvly)CSL_EDMA3TC_1_REGS);
  dmaTC0Regs->ERREN |= 0xF;
  dmaTC1Regs->ERREN |= 0xF;
  
  uart1.assign_dma_rx();
  uart1.assign_dma_tx();
  uart2.assign_dma_rx();
  uart2.assign_dma_tx();
  mmcsd.assign_dma();
  if(USE_I2C1){i2c1.assign_dma();}
  
  led.led(0);
  initialized = true;
}

static Uint32 notified_latest_itow_ms(0);
void Kernel::notify_itow(const Uint32 &latest_itow_ms) {
  notified_latest_itow_ms = latest_itow_ms;
}

static Uint8 notified_latest_satellites(0);
void Kernel::notify_satellites(const Uint8 &latest_satellites) {
  notified_latest_satellites = latest_satellites;
}

static Uint32 polling_invoked(0);
static Uint32 itow_ms(0xFFFFFFFFu);
static Kernel::msg_adc_t msg_adc;
static Kernel::msg_servo_write_t msg_servo_write;
static Kernel::msg_servo_read_t msg_servo_read;
static Kernel::msg_pitot_t msg_pitot;
static Kernel::msg_mag_t msg_mag;
static Uint32 msg_requested(0);

Uint32 Kernel::itow_ms() const {
  return ::itow_ms;
}

static const int servo_obs_channels(sizeof(msg_servo_read.ch_in) / sizeof(msg_servo_read.ch_in[0]));
static const int servo_drv_channels(sizeof(msg_servo_read.ch_out) / sizeof(msg_servo_read.ch_out[0]));

extern "C" {

void kernel_polling(){
  
  MBX_Handle mbx_adc(kernel.mbx(Kernel::MBX_ADC));
  MBX_Handle mbx_servo_read(kernel.mbx(Kernel::MBX_SERVO_READ));
  MBX_Handle mbx_servo_write(kernel.mbx(Kernel::MBX_SERVO_WRITE));
  MBX_Handle mbx_pitot(kernel.mbx(Kernel::MBX_PITOT));
  MBX_Handle mbx_mag(kernel.mbx(Kernel::MBX_MAG));
  
  Uint8 ubx_sv_eph_selector(0);
  
  while(true){
    SEM_pendBinary(&sem_periodic, SYS_FOREVER);
    
    // SPI0
    {
      Uint8 spi0_buf[0x100];
      Uint32 requested(msg_requested);
      unsigned int rx_size(spi0.flush_dma(spi0_buf, sizeof(spi0_buf))), rx_offset(0);
      
      // ADC
      static const unsigned int adc_rx_size(3);
      if(requested & (1 << Kernel::MBX_ADC)){
        const int adc_index(polling_invoked % 8);
        msg_adc.ch[adc_index] = ADS1248::convert_result(spi0_buf);
        if(adc_index == 0){
          msg_adc.time_ms = itow_ms;
        }else if(adc_index == 7){
          MBX_post(mbx_adc, &msg_adc, 0);
        }
        rx_offset += adc_rx_size;
        rx_size -= adc_rx_size;
      }
      
      // Servo obs / drv
      static const unsigned int servo_read_rx_size(
          sizeof(msg_servo_read.ch_in) + sizeof(msg_servo_read.ch_out));
      if((requested & (1 << Kernel::MBX_SERVO_READ))
          && (rx_size >= servo_read_rx_size)){
        std::memcpy(msg_servo_read.ch_in, &spi0_buf[rx_offset], sizeof(msg_servo_read.ch_in));
        rx_offset += sizeof(msg_servo_read.ch_in);
        std::memcpy(msg_servo_read.ch_out, &spi0_buf[rx_offset], sizeof(msg_servo_read.ch_out));
        rx_offset += sizeof(msg_servo_read.ch_out);
        rx_size -= servo_read_rx_size;
        MBX_post(mbx_servo_read, &msg_servo_read, 0);
      }
      
      // Servo drv write prepairation
      MBX_pend(mbx_servo_write, &msg_servo_write, 0);
      
      // Pitot
      static const unsigned int pitot_rx_size(sizeof(msg_pitot.raw_buf));
      if((requested & (1 << Kernel::MBX_PITOT))
          && (rx_size >= pitot_rx_size)){
        std::memcpy(msg_pitot.raw_buf, &spi0_buf[rx_offset], pitot_rx_size);
        MBX_post(mbx_pitot, &msg_pitot, 0);
        rx_offset += pitot_rx_size;
        rx_size -= pitot_rx_size;
      }
    }
    
    // Ublox ephemeris
    if((polling_invoked % 0x400) == 0){
      ubx.poll_rxm_eph(++ubx_sv_eph_selector);
      if(ubx_sv_eph_selector == 32){
        ubx.poll_aid_hui();
        ubx_sv_eph_selector = 0;
      }
    }
    
    // HMC5843 via I2C1
    if(USE_I2C1 && ((polling_invoked % 0x50) == 0)){ // 12.5Hz
      msg_mag.time_ms = itow_ms;
      hmc5843.raw(msg_mag.raw_buf);
      MBX_post(mbx_mag, &msg_mag, 0);
    }
    
    // CDC
    if((!USE_FTDI_MIMIC) && ((polling_invoked % 0x10) == 0)){ // 62.5Hz
      usb0_otg_impl.cdc.handle_ctl();
    }
    
    polling_invoked++;
    kernel_state |= kernel_state_polling_active;
  }
}

void kernel_periodic(){
  
  if(SEM_pendBinary(&sem_1pps, 0)){
    itow_ms = ((notified_latest_itow_ms / 1000) + 1) * 1000;
  }else{
    if((++itow_ms) >= (60u * 60 * 24 * 7 * 1000)){ // 1 week roll over
      itow_ms = 0;
    }
  }
  
  // LED (state indicate)
  {
    static const Uint16 count_max(2000);
    static Uint16 count(count_max - 1);
    static Uint8 state(0);
    if(++count == count_max){
      led.led4(true);
      state = kernel_state;
      kernel_state = 0;
      count = 0;
    }else{
      switch(count % 250){
        case 0:
          if(state & 0x1){led.led4(true);}
          state >>= 1;
          break;
        case 50:
          led.led4(false);
          break;
      }
    }
  }
  
  // LED (GPS satellites)
  {
    static const Uint16 count_max(10000);
    static Uint16 count(count_max - 1);
    static Uint8 satellites(0);
    if(++count == count_max){
      satellites = notified_latest_satellites;
      notified_latest_satellites = 0;
      count = 0;
    }
    switch(count % 250){
      case 100:
        if(satellites){
          satellites--;
          led.led3(true);
        }
        break;
      case 150:
        led.led3(false);
        break;
    }
  }
  
  // SPI0
  if(spi0.assign_dma()){
    msg_requested = 0;
  
    // ADS1248 polling
    {
      const int adc_index_next((polling_invoked + 1) % 8);
      
      ADS1248 &ads1248(ADC_IO::adc); // spi0 <=> ADS1248
      spi0.set_format(1);
      
      // Read result
      ads1248.read_result();
      
      if(adc_index_next == 7){
        // MUX1: 0b0 01 10 011 (int.osc, int.ref, onbrd.ref, temp.diode)
        static const Uint8 buf[] = {0x33};
        ads1248.write_register(0x02, buf, sizeof(buf));
      }else if(adc_index_next == 0){
        // MUX1: 0b0 01 10 000 (int.osc, int.ref, onbrd.ref, normal.op)
        static const Uint8 buf[] = {adc_mux0_table[0], 0x00, 0x30};
        ads1248.write_register(0x00, buf, sizeof(buf));
      }else{
        ads1248.write_register(0x00, &(adc_mux0_table[adc_index_next]), 1);
      }
      
      //ads1248.sync();
      msg_requested |= (1 << Kernel::MBX_ADC);
    }
    
    // CPLD update
    if((polling_invoked % 0x10) == 0){ // 1K / 16 = 62.5Hz
      spi0.set_format(2);
      
      // servo obs read
      {
        static const Uint8 prefix[] = {0x00};
        Uint8 buf[sizeof(Uint16) * servo_obs_channels];
        spi0.write((void *)prefix, sizeof(prefix), true);
        spi0.write_read((void *)buf, sizeof(buf));
        msg_servo_read.time_ms = itow_ms;
      }
      
      // servo drv read / write
      {
        static const Uint8 prefix[] = {0x90};
        Uint8 buf[sizeof(Uint16) * servo_drv_channels];
        std::memcpy(buf, msg_servo_write.ch, sizeof(buf));
        spi0.write((void *)prefix, sizeof(prefix), true);
        spi0.write_read((void *)buf, sizeof(buf));
      }
      
      msg_requested |= (1 << Kernel::MBX_SERVO_READ);
    }
    
    // Pitot
    if((polling_invoked % 0x10) == 8){ // 1K / 16 = 62.5Hz
      spi0.set_format(3);
      
      static const Uint8 prefix[] = {0xFE, 0xA5};
      spi0.write((void *)prefix, sizeof(prefix), true);
      spi0.write_read((void *)msg_pitot.raw_buf, sizeof(msg_pitot.raw_buf));
      msg_pitot.time_ms = itow_ms;
      
      msg_requested |= (1 << Kernel::MBX_PITOT);
    }
    
    if(USE_INA111){ // ina111
      spi0.set_format(2);
      int i(polling_invoked % (sizeof(ina111_spi_prefix) / sizeof(ina111_spi_prefix[0])));
      for(int j(0); j < sizeof(ina111_spi_sequence) / sizeof(ina111_spi_sequence[0]); j++){
        spi0.write((void *)(&ina111_spi_prefix[i]), sizeof(ina111_spi_prefix[0]), true);
        spi0.write_read((void *)ina111_spi_sequence[j], sizeof(ina111_spi_sequence[0]));
      }
    }
  }
  
  SEM_postBinary(&sem_periodic);
}

void edma_isr(){
  CSL_Edma3ccRegsOvly dmaRegs((CSL_Edma3ccRegsOvly)CSL_EDMA3CC_0_REGS);
  
  if(dmaRegs->EMR){
    dmaRegs->EMCR = 0xFFFFFFFF;
  }
  
  if(dmaRegs->CCERR){
    dmaRegs->CCERRCLR = 0xFFFFFFFF;
  }
  
  Uint32 ipr(dmaRegs->IPR);
  
  do{
    uart1.dma_callback_rx(ipr);
    uart1.dma_callback_tx(ipr);
    uart2.dma_callback_rx(ipr);
    uart2.dma_callback_tx(ipr);
    mmcsd.dma_callback(ipr);
    spi0.dma_callback(ipr);
    i2c1.dma_callback(ipr);
  }while(ipr = (dmaRegs->IPR));
  
  // @see sprufl1c 2.9.2
  // @see http://e2e.ti.com/support/dsp/tms320c6000_high_performance_dsps/f/112/t/85034.aspx
  // Example.2
}

void edma_cc0_err_isr(){
  return;
}

void edma_tc0_err_isr(){
  return;
}

void edma_tc1_err_isr(){
  return;
}

void gpio5_isr(){
  CSL_GpioRegsOvly reg((CSL_GpioRegsOvly)CSL_GPIO_0_REGS);
  Uint32 flag(reg->BANK[GP5].INTSTAT & 0xFFFF0000);
  if(flag & GP5P10){ // GPS 1PPS
    led.toggle1();
    SEM_postBinary(&sem_1pps);
  }
  reg->BANK[GP5].INTSTAT |= flag; // clear flag
}

/* Initialize the board APIs */
void TinyFeather_init(){
  const char rom_rev001[] = "d800k001";
  const char rom_rev003[] = "d800k003";
  if((std::memcmp(rom_rev001, (const void *)0x11700008, sizeof(rom_rev001) - 1) == 0)
      || (std::memcmp(rom_rev003, (const void *)0x11700008, sizeof(rom_rev003) - 1) == 0)){
    CSL_SyscfgRegsOvly bootCfg((CSL_SyscfgRegsOvly)(CSL_SYSCFG_0_REGS));
    bootCfg->KICK0R = KICK0KEY; // Write Access Key 0
    bootCfg->KICK1R = KICK1KEY; // Write Access Key 1
    // PINMUX
    bootCfg->PINMUX0  = 0x11112100;  // EMIFB
    bootCfg->PINMUX1  = 0x11111111;  // EMIFB
    bootCfg->PINMUX2  = 0x01111111;  // EMIFB
    bootCfg->PINMUX3  = 0x00000000;
    bootCfg->PINMUX4  = 0x00000000;
    bootCfg->PINMUX5  = 0x11111110;  // EMIFB
    bootCfg->PINMUX6  = 0x11111111;  // EMIFB
    bootCfg->PINMUX7  = 0x11111111;  // EMIFB, SPI0
    bootCfg->PINMUX8  = 0x28822022;  // UART2, GPIO5[11-10], I2C0, I2C1
    bootCfg->PINMUX9  = 0x00000002;  // UART2
    bootCfg->PINMUX10 = 0x00000000;
    bootCfg->PINMUX11 = 0x11101100;  // McASP1, UART1
    bootCfg->PINMUX12 = 0x11111110;  // McASP1
    bootCfg->PINMUX13 = 0x11011111;  // EMIFA / McASP1 / (SD)
    bootCfg->PINMUX14 = 0x00111111;  // EMIFA / (SD)
    bootCfg->PINMUX15 = 0x11000000;  // EMIFA / (SD)
    bootCfg->PINMUX16 = 0x11111111;  // EMIFA / (SD)
    bootCfg->PINMUX17 = 0x00011111;  // EMIFA
    bootCfg->PINMUX18 = 0x00111010;  // EMIFA
    bootCfg->PINMUX19 = 0x00000001;  // EMIFA
    // CHIP CONFIG 2
    bootCfg->CFGCHIP2 = 0x0000EB42; // USB0REF_FREQ=2 24MHz, USB0PHY_PLLON=1, USB0PHYPWDN=0, EB42 = 1110 1011 0100 0010
    // PSC0
    Uint32 psc0_num[] = {0,1,2,3,4,5,6,8,9,10,11,12,13};
    for(int i(0); i < sizeof(psc0_num) / sizeof(psc0_num[0]); i++){
      CSL_PscRegsOvly reg((CSL_PscRegsOvly)CSL_PSC_0_REGS);
      while((reg->PTSTAT & 0x1) != 0);
      if(((reg->MDSTAT)[psc0_num[i]] & 0x1F) == 0x3){continue;}
      (reg->MDCTL)[psc0_num[i]] = ((reg->MDCTL)[psc0_num[i]] & 0xFFFFFFE0u) | 0x0003;
      reg->PTCMD = 1;
      while((reg->PTSTAT & 0x1) != 0); // Wait for power state transition to finish
      while(((reg->MDSTAT)[psc0_num[i]] & 0x1F) != 0x3);
    }
    // PSC1
    Uint32 psc1_num[] = {1,2,3,4,5,6,7,8,9,10,11,12,13,16,17,20,21,24,25,26,31};
    for(int i(0); i < sizeof(psc1_num) / sizeof(psc1_num[0]); i++){
      CSL_PscRegsOvly reg((CSL_PscRegsOvly)CSL_PSC_1_REGS);
      while((reg->PTSTAT & 0x1) != 0);
      if(((reg->MDSTAT)[psc1_num[i]] & 0x1F) == 0x3){continue;}
      (reg->MDCTL)[psc1_num[i]] = ((reg->MDCTL)[psc1_num[i]] & 0xFFFFFFE0u) | 0x0003;
      reg->PTCMD = 1;
      while((reg->PTSTAT & 0x1) != 0); // Wait for power state transition to finish
      while(((reg->MDSTAT)[psc1_num[i]] & 0x1F) != 0x3);
    }
    // EMIFA
    {
      CSL_EmifaRegsOvly emifaRegs((CSL_EmifaRegsOvly)CSL_EMIFA_0_REGS);
      emifaRegs->AWCC = 0x300400ff;
      emifaRegs->CE3CFG = 0x4C5462B8;
      emifaRegs->NANDFCR &= ~2;
    }
  }
}

}