#include "kernel.h"

#include <sem.h>
#include <tsk.h>

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

#include <cstdio>
#include <cctype>
#include <cstring>
#include <cstdlib>
#include <vector>
#include <algorithm>
#include <ctime>
#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/hmc58x3.h"
#include "peripheral/mag3110.h"
#include "peripheral/ms5611.h"
#include "peripheral/seeedOLED.h"
#include "peripheral/max2ufm.h"

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

#include "util/endian.h"
#include "param/constant.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_periodic_slow;
static SEM_Obj sem_1pps;

void Kernel::simulate_1pps(){
  SEM_postBinary(&sem_1pps);
}

#pragma DATA_ALIGN(0x100)
static FATFS fatfs;

static Uint8 kernel_state(0);
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++);
  }
}

static class KernelConfig {
  private:
    template <class T1, class T2>
    bool check_spec_then_convert(char *spec, const char *str, T1 &res, T2 &func){
      if(std::strstr(spec, str) != spec){return false;}
      spec += std::strlen(str);
      if((*spec > 0x20) && (*spec < 0x7F)){return false;} // next character must be neither printable nor space.
      res = func(spec);
      return true;
    }
    template <class T>
    bool check_spec_then_atoi(char *spec, const char *str, T &res){
      return check_spec_then_convert(spec, str, res, std::atoi);
    }
    template <class T>
    bool check_spec_then_atof(char *spec, const char *str, T &res){
      return check_spec_then_convert(spec, str, res, std::atof);
    }
  public:
    bool user_file_is_null;
    Uint32 uart1_baudrate, uart2_baudrate;
    Uint32 uart1_bit, uart2_bit;
    enum usb_bridge_mode_t {
        USB_BRIDGE_NOP = 0,
        USB_BRIDGE_GPS, USB_BRIDGE_GS_LINK,
        USB_BRIDGE_SHELL, USB_BRIDGE_LOOPBACK,
        USB_BRIDGE_NA} usb_bridge_mode;
    bool usb_bridge_log;
    enum servo_out_raito_t {
        SERVO_OUT_50Hz = 0, SERVO_OUT_100Hz = 1,
        SERVO_OUT_200Hz = 2, SERVO_OUT_400Hz = 3,
        SERVO_OUT_RATIO_NA = 4} servo_out_raito;
    bool servo_out_force_cic;
    KernelConfig()
        : user_file_is_null(false),
        uart1_baudrate(9600), uart2_baudrate(115200),
        uart1_bit(100008), uart2_bit(100008), // 1stopbit,noparity,8databit
        usb_bridge_mode(USB_BRIDGE_NOP), usb_bridge_log(false),
        servo_out_raito(SERVO_OUT_50Hz), servo_out_force_cic(false) {}
    ~KernelConfig() {}
    void read_config(FIL &f){
      char buf[512];
      int _usb_bridge_mode(usb_bridge_mode), _servo_out_raito(servo_out_raito);
      float_sylph_t v_f[3];
      while(f_gets(buf, sizeof(buf), &f)){
        if(std::sscanf("mag_hard_iron_offset %lf %lf %lf", buf,
            &v_f[0], &v_f[1], &v_f[2]) == 3){
          std::memcpy(navigation_config.mag_hard_iron_offset, v_f, sizeof(v_f));
          continue;
        }
        check_spec_then_atoi(buf, "user_file_is_null", user_file_is_null)
            || check_spec_then_atoi(buf, "uart1_baudrate", uart1_baudrate)
            || check_spec_then_atoi(buf, "uart2_baudrate", uart2_baudrate)
            || check_spec_then_atoi(buf, "uart1_bit", uart1_bit)
            || check_spec_then_atoi(buf, "uart2_bit", uart2_bit)
            || check_spec_then_atoi(buf, "usb_bridge_mode", _usb_bridge_mode)
            || check_spec_then_atoi(buf, "usb_bridge_log", usb_bridge_log)
            || check_spec_then_atoi(buf, "servo_out_raito", _servo_out_raito)
            || check_spec_then_atoi(buf, "servo_out_force_cic", servo_out_force_cic);
      }
#define check_and_substitute(target, range_min, range_over) \
if((_ ## target >= range_min) && (_ ## target < range_over)){ \
  target = (target ## _t)_ ## target; \
}
      check_and_substitute(usb_bridge_mode, USB_BRIDGE_NOP, USB_BRIDGE_NA);
      check_and_substitute(servo_out_raito, SERVO_OUT_50Hz, SERVO_OUT_RATIO_NA);
#undef check_and_substitute
    }
} kernel_config;

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());

struct Kernel::SystemIO : public MinimalIO {
  int ref_count;
  unsigned int receive(char *buf, const unsigned int &buf_size){return 0;}
  unsigned int transmit(const char *buf, const unsigned int &buf_size){return 0;}
  virtual bool ioctl(const Kernel::ioctl_keys &key, const void *param){return false;}
  SystemIO() : ref_count(0){}
  virtual ~SystemIO(){}
};

struct BufferedIO : public Kernel::SystemIO {
  SystemIO *delegatee;
  unsigned int tx_buf_size, rx_buf_size;
  char *tx_buf, *rx_buf;
  char *tx_buf_index, *rx_buf_index;
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    char * const _rx_buf(rx_buf);
    if(!_rx_buf){return delegatee->receive(buf, buf_size);}
    const unsigned int &_rx_buf_size(rx_buf_size);
    int receiving(_rx_buf_size - (rx_buf_index - _rx_buf));
    if(receiving > buf_size){
      std::memcpy(buf, rx_buf_index, buf_size);
      rx_buf_index += buf_size;
      return buf_size;
    }
    std::memcpy(buf, rx_buf_index, receiving);
    unsigned int remain(buf_size - receiving);
    if(remain > _rx_buf_size){
      receiving += delegatee->receive(buf + receiving, (remain / _rx_buf_size) * _rx_buf_size);
      remain = buf_size - receiving;
    }
    unsigned int rx_buf_filled(delegatee->receive(_rx_buf, _rx_buf_size));
    rx_buf_index = _rx_buf + _rx_buf_size - rx_buf_filled;
    if(rx_buf_index != _rx_buf){
      std::memmove(rx_buf_index, _rx_buf, rx_buf_filled);
    }
    if(remain > rx_buf_filled){remain = rx_buf_filled;}
    std::memcpy(buf + receiving, rx_buf_index, remain);
    rx_buf_index += remain;
    receiving += remain;
    return receiving;
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    char * const _tx_buf(tx_buf);
    if(!_tx_buf){return delegatee->transmit(buf, buf_size);}
    const unsigned int &_tx_buf_size(tx_buf_size);
    int transmitting(_tx_buf_size - (tx_buf_index - _tx_buf));
    if(buf_size < transmitting){
      std::memcpy(tx_buf_index, buf, buf_size);
      tx_buf_index += buf_size;
      return buf_size;
    }
    std::memcpy(tx_buf_index, buf, transmitting);
    unsigned int tx_buf_pushed(delegatee->transmit(_tx_buf, _tx_buf_size));
    if(tx_buf_pushed == _tx_buf_size){
      unsigned int remain(buf_size - transmitting);
      if(remain > _tx_buf_size){
        transmitting += delegatee->transmit(buf + transmitting, (remain / _tx_buf_size) * _tx_buf_size);
        remain = buf_size - transmitting;
        if(remain > _tx_buf_size){remain = _tx_buf_size;}
      }
      std::memcpy(_tx_buf, buf + transmitting, remain);
      tx_buf_index = _tx_buf + remain;
      transmitting += remain;
    }else{
      if(tx_buf_pushed > 0){
        std::memmove(_tx_buf, _tx_buf + tx_buf_pushed, _tx_buf_size - tx_buf_pushed);
      }
      tx_buf_index = _tx_buf + _tx_buf_size - tx_buf_pushed;
    }
    return transmitting;
  }
  void set_buffer(const unsigned int &buf_size_tx, const unsigned int &buf_size_rx){
    if((tx_buf_size = buf_size_tx) > 0){
      tx_buf = new char[tx_buf_size];
      tx_buf_index = tx_buf;
    }else{
      tx_buf = NULL;
    }
    if((rx_buf_size = buf_size_rx) > 0){
      rx_buf = new char[rx_buf_size];
      rx_buf_index = rx_buf + rx_buf_size;
    }else{
      rx_buf = NULL;
    }
  }
  bool ioctl(const Kernel::ioctl_keys &key, const void *param) {
    switch(key){
      case Kernel::IOCTL_BUFFER:
        delete [] tx_buf;
        delete [] rx_buf;
        const unsigned int *tx_rx_buffers(
            static_cast<const unsigned int *>(param));
        set_buffer(tx_rx_buffers[0], tx_rx_buffers[1]);
        return true;
      default:
        return delegatee->ioctl(key, param);
    }
  }
  BufferedIO(SystemIO *io,
      const unsigned int &buf_size_tx, const unsigned int &buf_size_rx)
      : Kernel::SystemIO(),
      delegatee(io) {
    set_buffer(buf_size_tx, buf_size_rx);
    (delegatee->ref_count)++;
  }
  ~BufferedIO() {
    delete [] tx_buf;
    delete [] rx_buf;
    if((--(delegatee->ref_count)) <= 0){delete delegatee;}
  }
};

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 Kernel::ioctl_keys &key, const void *param){
  switch(key){
    case Kernel::IOCTL_BUFFER:
      if(!(delegatee->ioctl(key, param))){
        const unsigned int *tx_rx_buffers(
            static_cast<const unsigned int *>(param));
        (delegatee->ref_count)--;
        delegatee = new BufferedIO(delegatee, tx_rx_buffers[0], tx_rx_buffers[1]);
        (delegatee->ref_count)++;
      }
      return true;
    default:
      return delegatee->ioctl(key, param);
  }
}
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;
  SEM_Obj sem_tx;
  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{
      SEM_pendBinary(&sem_tx, SYS_FOREVER);
      unsigned int res(uart1.transmit(buf, buf_size));
      SEM_postBinary(&sem_tx);
      return res;
    }
  }
  void after_setup(){
    SEM_new(&sem_tx, 1);
    blocking = false;
  }
  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 {
  protected:
  typedef std::vector<FAT_FILE_IO *> io_pool_t;
  static io_pool_t io_pool;

  FIL f;
  bool available;
  int autosync;
  char *fname;

  void open_seek(const char *fname) {
    available = (f_open(&f,
        (const char *)fname,
        (FA_OPEN_ALWAYS | FA_READ | FA_WRITE)) == FR_OK);
    if(available){
      // 追加書き込みにするため、シークする
      f_lseek(&f, f.fsize);
    }
  }
  FAT_FILE_IO(const void *params)
      : Kernel::SystemIO(),
      available(false),
      autosync(12), // (1 << 12) = 4KBごとに自動fsync
      fname(NULL) {
    const char *path((const char *)params);
    char fname_new[0x100];

    // Check special file extension ".***", which will be replaced to an unique number
    char *ext(std::strrchr(path, '.'));
    if(ext && (std::strcmp(ext, ".***") == 0)){
      DIR dir;
      FILINFO info;
      char dirname[0x100] = "/";
      const char *basename;
      if((basename = std::strrchr(path, '/'))
          || (basename = std::strchr(path, ':'))){
        basename += 1;
        int dirname_length(basename - path);
        std::memcpy(dirname, path, dirname_length);
        dirname[dirname_length] = '\0';
      }else{
        basename = path;
      }

      int max_suffix(0);
      if(f_opendir(&dir, dirname) != FR_OK){return;}
      while(true){
        if((f_readdir(&dir, &info) != FR_OK)
            || (info.fname[0] == 0)){break;} // read an entry, then check complete reading.
        if((info.fattrib & AM_DIR)
            || (info.fname[0] == '.')){continue;} // skip directory and dot entries

        const char *target_fname(
#if _USE_LFN
            (*info.lfname ? info.lfname : info.fname)
#else
            info.fname
#endif
            );
        if(std::strlen(target_fname) != (ext - basename + 4)){continue;}
        bool basename_match(true);
        for(int i(0); i < (ext - basename + 1); ++i){ // compare including dot without case sensitive
          if(std::toupper(target_fname[i]) != std::toupper(basename[i])){
            basename_match = false;
            break;
          }
        }
        if(!basename_match){continue;}
        int suffix;
        if((std::sscanf(&target_fname[ext - basename + 1], "%d", &suffix) >= 1)
            && (max_suffix <= suffix)){
          max_suffix = suffix + 1;
        }
      }
#if _FATFS > 6502
      f_closedir(&dir);
#endif

      std::memcpy(fname_new, path, ext - path);
      std::sprintf(&fname_new[ext - path], ".%03d", max_suffix % 1000);
      path = fname_new;
    }

    open_seek(path);
    io_pool.push_back(this); // regist to pool
    { // ファイル名のコピー
      int fname_buf_length(std::strlen(path) + 1);
      fname = new char [fname_buf_length];
      std::memcpy(fname, path, fname_buf_length);
    }
  }
  struct autolock_t {
    autolock_t(){SEM_pendBinary(&sem_emifa_mmcsd, SYS_FOREVER);}
    ~autolock_t(){SEM_postBinary(&sem_emifa_mmcsd);}
  };

  public:
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    if(!available){return 0;}
    UINT read;
    autolock_t autolock;
    if(f_read(&f, buf, buf_size, &read) == FR_OK){
      kernel_state |= kernel_state_fs_active;
    }
    return read;
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    if(!available){return 0;}
    UINT written;
    autolock_t autolock;
    DWORD previous_size(f.fsize);
    if(f_write(&f, buf, buf_size, &written) == FR_OK){
      kernel_state |= kernel_state_fs_active;
      if((autosync >= 0)
          && (previous_size >> autosync) < (f.fsize >> autosync)){
        f_sync(&f);
      }
    }
    return written;
  }
  bool ioctl(const Kernel::ioctl_keys &key, const void *param){
    switch(key){
      case Kernel::IOCTL_SYNC: {
        autolock_t autolock;
        if(f_sync(&f) != FR_OK){break;}
        return true;
      }
      case Kernel::IOCTL_SEEK: {
        autolock_t autolock;
        if(f_lseek(&f, *static_cast<const int *>(param)) != FR_OK){break;}
        return true;
      }
      case Kernel::IOCTL_RENAME: {
        const char *fname_new((const char *)param);
        if(std::strcmp(fname, fname_new) == 0){return true;}
        autolock_t autolock;
        if(f_close(&f) != FR_OK){break;}
        if(f_rename(fname, fname_new) != FR_OK){break;}
        open_seek(fname_new);
        int fname_buf_length(std::strlen(fname_new) + 1);
        delete [] fname;
        fname = new char [fname_buf_length];
        std::memcpy(fname, fname_new, fname_buf_length);
        return true;
      }
      case Kernel::IOCTL_AUTOSYNC:
        autosync = *static_cast<const int *>(param);
        return true;
    }
    return false;
  }
  ~FAT_FILE_IO(){
    autolock_t autolock;
    std::remove(io_pool.begin(), io_pool.end(), this); // remove from pool
    f_close(&f);
    delete [] fname;
  }
  static FAT_FILE_IO *get_io(const void *params){
    autolock_t autolock;
    // Multiple open to same file
    for(io_pool_t::const_iterator it(io_pool.begin()); it != io_pool.end(); it++){
      if(std::strcmp((const char *)params, (*it)->fname) == 0){
        return *it;
      }
    }
    return new FAT_FILE_IO(params);
  }
};
FAT_FILE_IO::io_pool_t FAT_FILE_IO::io_pool;

static struct USB_UART_IO : public Kernel::SystemIO {
  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) {
    return transmit_internal(buf, buf_size);
  }
} usb_uart_sys_io;
static Kernel::IO usb_uart_io(&usb_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: {
      if(kernel_config.user_file_is_null){
        return null_io;
      }else{
        return Kernel::IO(FAT_FILE_IO::get_io(params));
      }
    }
    case USB_UART: {
      return ((kernel_config.usb_bridge_mode == KernelConfig::USB_BRIDGE_NOP)
          ? usb_uart_io : null_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 MAG3110 mag3110(i2c1);
static HMC58X3 hmc58x3(i2c1);
static I2C_MAG_SENSOR *mag_sensor(NULL);
static MS5611 ms5611(i2c1);
static bool use_ms5611(false);
static SeeedOLED oled(i2c1);
static bool use_oled(false);

// 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 uart_reconfig(
    const Uint32 &baudrate, const Uint8 &stopbit, const Uint8 &parity, const Uint8 &databit){
  switch(kernel_config.usb_bridge_mode){
    case KernelConfig::USB_BRIDGE_GPS:
      uart1.baudrate(baudrate);
      uart1.bit_config((UART::stopbit_t)stopbit, (UART::parity_t)parity, (UART::databit_t)databit);
      break;
    case KernelConfig::USB_BRIDGE_GS_LINK:
      uart2.baudrate(baudrate);
      uart2.bit_config((UART::stopbit_t)stopbit, (UART::parity_t)parity, (UART::databit_t)databit);
      break;
  }
}

void usb_uart_status_changed(
    const Uint16 &status, const Uint16 &rts_mask, const Uint16 &dtr_mask){
  LOG_printf(&trace, "%s", (status & rts_mask) ? "RTS on" : "RTS off");
  LOG_printf(&trace, "%s", (status & dtr_mask) ? "DTR on" : "DTR off");
  switch(kernel_config.usb_bridge_mode){
    case KernelConfig::USB_BRIDGE_SHELL: // TODO
      const char *buf((status & dtr_mask) ? "\n" : "\x03");
      //shell.transmit(buf, std::strlen(buf));
      break;
  }
}

static struct BridgeLoopback_IO : public MinimalIO {
  SEM_Obj sem_ownership, sem_copied;
  unsigned int buf_remain;
  const char *buf_ready;
  BridgeLoopback_IO() : buf_remain(0), buf_ready(NULL) {
    SEM_new(&sem_ownership, 0);
    SEM_postBinary(&sem_ownership);
    SEM_new(&sem_copied, 0);
  }
  unsigned int receive(char *buf, const unsigned int &buf_size) {
    unsigned int res(buf_size);
    if(res > buf_remain){res = buf_remain;}
    if(res > 0){
      std::memcpy(buf, buf_ready, res);
      buf_ready += res;
      if((buf_remain -= res) == 0){
        SEM_postBinary(&sem_copied);
      }
    }
    return res;
  }
  unsigned int transmit(const char *buf, const unsigned int &buf_size) {
    if(buf_size > 0){
      SEM_pendBinary(&sem_ownership, SYS_FOREVER);
      buf_ready = buf;
      buf_remain = buf_size;
      SEM_pendBinary(&sem_copied, SYS_FOREVER);
      SEM_postBinary(&sem_ownership);
    }
    return buf_size;
  }
} bridge_loopback_io;

static MinimalIO *usb_bridge_target(NULL);

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_ADS] = MBX_create(sizeof(msg_ads_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] = 0; //1500; // free(0) or neutral(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_periodic_slow, 0);
  SEM_new(&sem_1pps, 0);
  
  led.init();
  LOG_printf(&trace, "LED ready.");
  
  // FAT
  f_mount(0, &fatfs);
  LOG_printf(&trace, "FAT on MMCSD ready.");
  
  spi0.init();
#ifdef GBL_
  Uint32 core_freq_KHz(GBL_getFrequency());
#else
  Uint32 core_freq_KHz(300000); // 300MHz
#endif
  Uint32 divider_minus_one;
  if(core_freq_KHz > 400000){
    spi0.set_prescale(1, 90); // For ADS1248 (max: CLK_period under 520us => 2.51MHz <= 456 / 2 / (90 + 1))
    spi0.set_prescale(2, 45); // For CPLD (max: 4.96MHz <= 456 / 2 / (45 + 1))
  }else if(core_freq_KHz > 300000){
    spi0.set_prescale(1, 74); // For ADS1248 (max: CLK_period under 520us => 2.5MHz <= 375 / 2 / (74 + 1))
    spi0.set_prescale(2, 37); // For CPLD (max: 4.93MHz <= 375 / 2 / (37 + 1))
  }else{
    spi0.set_prescale(1, 59); // For ADS1248 (max: CLK_period under 520us => 2.5MHz <= 300 / 2 / (59 + 1))
    spi0.set_prescale(2, 29); // For CPLD (max: 5MHz <= 300 / 2 / (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);
      
      wait(10000);
      cpld_general_reg[0] |= 0x80; // 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);
  }

  // Kernel config via file
  {
    FIL f;
    const char cfg_fname[] = "kernel.cfg";
    if(f_open(&f, cfg_fname, FA_OPEN_EXISTING | FA_READ) == FR_OK){
      kernel_config.read_config(f);
      f_close(&f);
    }
  }
  
  // Servo OUT setup
  {
    cpld_general_reg[2] = (cpld_general_reg[2] & 0xF8)
        | (kernel_config.servo_out_force_cic ? 0x04 : 0x00) 
        | kernel_config.servo_out_raito;
    //0x04 => force cic mode
    //0x00 => 50Hz; 0x01=> 100Hz; 0x02 => 200Hz; 0x03 => 400Hz
  }
  
  // Shell extension via file
  shell.initialize();
  do{
    FIL f_in, f_out;
    unsigned int new_read, new_write;
    char buf[128];
    const char f_in_fname[] = "extend.rb", f_out_fname[] = "extend.out";
    if(f_open(&f_in, f_in_fname, FA_OPEN_EXISTING | FA_READ) != FR_OK){break;}
    if(f_open(&f_out, f_out_fname, FA_CREATE_ALWAYS | FA_WRITE) != FR_OK){break;}
    do{
      if(f_read(&f_in, buf, sizeof(buf), &new_read) != FR_OK){
        break;
      }
      if(new_read == 0){
        shell.transmit("\n", 1);
      }else{
        shell.transmit(buf, new_read);
      }
      do{
        f_write(&f_out, buf, shell.receive(buf, sizeof(buf)), &new_write);
      }while(new_write > 0);
    }while(new_read > 0);
    f_close(&f_in);
    f_close(&f_out);
  }while(false);

  // 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);
  }
  
  // i2c1 and related devices (HMC5843/5883, ...)
  if(USE_I2C1){
    i2c1.init();
    LOG_printf(&trace, "I2C1 ready.");

    for(unsigned char i(0); i < 0x7F; ++i){
      if(!i2c1.has_device(i)){continue;}
      LOG_printf(&trace, "I2C1 device found, address => 0x%02x.", i);
    }

    if(i2c1.has_device(SeeedOLED::i2c_address)){
      oled.init(wait, 5000);
      oled.clearDisplay(); // Clear the screen and set start position to top left corner
      oled.setNormalDisplay(); // Set display to normal mode (i.e non-inverse mode)
      oled.setPageMode(); // Set addressing mode to Page Mode
      use_oled = true;
    }

    if(i2c1.has_device(MS5611::i2c_address)){ // MS5611
      Uint16 buf_native[8], buf_gpio[8];
      i2c1.via_gpio(false);
      ms5611.get_prom(buf_native);
      i2c1.via_gpio(true);
      ms5611.get_prom(buf_gpio);

      if(std::memcmp(buf_native, buf_gpio, sizeof(buf_native)) == 0){
        LOG_printf(&trace, "MS5611 found");
        for(int i(0); i < sizeof(buf_native) / sizeof(buf_native[0]); ++i){
          LOG_printf(&trace, "MS5611 PROM(%d) => %d.", i, buf_native[i]);
        }
        ms5611.init();
        use_ms5611 = true;
      }
    }

    if(i2c1.has_device(HMC58X3::i2c_address)){ // HMC58X3
      char buf_native[3], buf_gpio[3];
      i2c1.via_gpio(false);
      hmc58x3.get_id(buf_native);
      i2c1.via_gpio(true);
      hmc58x3.get_id(buf_gpio);

      if(std::memcmp(buf_native, buf_gpio, sizeof(buf_native)) == 0){
        char buf[4] = {0};
        std::memcpy(buf, buf_native, sizeof(buf_native));
        LOG_printf(&trace, "HMC58X3 found, device_id is %s.", buf);
        hmc58x3.init();
        mag_sensor = &hmc58x3;
      }
    }

    if(i2c1.has_device(MAG3110::i2c_address)){ // MAG3110
      char buf_native[1], buf_gpio[1];
      i2c1.via_gpio(false);
      mag3110.get_id(buf_native);
      i2c1.via_gpio(true);
      mag3110.get_id(buf_gpio);

      if(buf_native[0] == buf_gpio[0]){
        LOG_printf(&trace, "MAG3110 found, device_id is 0x%x.", (unsigned char)buf_native[0]);
        mag3110.init();
        mag_sensor = &mag3110;
      }
    }

    i2c1.via_gpio(false);
    unsigned int i2c1_clock(i2c1.set_clock(400000)); // set clock to 400KHz

    // Mag sensor test read
    if(mag_sensor){
      Int16 values[3];
      for(int i(0); i < 0x10; i++){
        mag_sensor->xyz(values);
        LOG_printf(&trace, "Magnetic sensor test read. %d, %d", values[0], values[1]); // Number of LOG_printf args must be less than 3.
        wait(1000);
      }
    }
  }

  // ublox setup
  do{
    uart1.baudrate(kernel_config.uart1_baudrate);
    uart1.init();
    uart1.bit_config(kernel_config.uart1_bit);
    LOG_printf(&trace, "UART1 ready.");

    if(kernel_config.usb_bridge_mode == KernelConfig::USB_BRIDGE_GPS){
      ublox_io = null_io;
      break;
    }
    
    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, 0x20, 10); // NAV-TIMEGPS // 16 + 8 = 24 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, 1);  // RXM-SFRB    // 42 + 8 = 50 bytes
    
    // additional configuration
    do{
      FIL f;
      unsigned int new_read;
      char buf[128];
      if(f_open(&f, "gps.cfg", FA_OPEN_EXISTING | FA_READ) != FR_OK){break;}
      do{
        if(f_read(&f, buf, sizeof(buf), &new_read) != FR_OK){break;}
        ublox_io.transmit(buf, new_read);
      }while(new_read > 0);
      f_close(&f);
    }while(false);

    ublox_sys_io.after_setup();
    
    LOG_printf(&trace, "GPS ready.");
  }while(false);
  
  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
  do{
    uart2.baudrate(kernel_config.uart2_baudrate);
    uart2.init();
    uart2.bit_config(kernel_config.uart2_bit);
    LOG_printf(&trace, "UART2 ready.");
    
    if(kernel_config.usb_bridge_mode == KernelConfig::USB_BRIDGE_GS_LINK){
      gs_link_io = null_io;
      break;
    }

    // additional initialize
    FIL f;
    const char cfg_fname[] = "gslink.cfg";
    if(f_open(&f, cfg_fname, FA_OPEN_EXISTING | FA_READ) != FR_OK){break;}
    Uint32 remain_bytes(f.fsize), read_byte;
    Uint8 buf[32];
    while(remain_bytes){
      f_read(&f, buf, sizeof(buf), &read_byte);
      if(read_byte == 0){break;}
      remain_bytes -= read_byte;
      for(int i(0); i < read_byte; i++){uart2.putc(buf[i]);} // transmit in blocking mode
    }
    f_close(&f);
    
    LOG_printf(&trace, "GS-Link ready.");
  }while(false);
  
  // ADS test
  {
    // SPI0 format3 for ADS
    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.");
  if(kernel_config.usb_bridge_mode != KernelConfig::USB_BRIDGE_NOP){
    if(USE_FTDI_MIMIC){
      usb0_otg_impl.ft232.config_changed_callback = uart_reconfig;
      usb0_otg_impl.ft232.line_status_changed_callback = usb_uart_status_changed;
    }else{
      usb0_otg_impl.cdc.config_changed_callback = uart_reconfig;
      usb0_otg_impl.cdc.line_status_changed_callback = usb_uart_status_changed;
    }
  }
  
  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();}

  // USB bridge setup
  switch(kernel_config.usb_bridge_mode){
    case KernelConfig::USB_BRIDGE_NOP:
      break;
    case KernelConfig::USB_BRIDGE_GPS:
      usb_bridge_target = &uart1;
      uart1.release_dma_rx();
      break;
    case KernelConfig::USB_BRIDGE_GS_LINK:
      usb_bridge_target = &uart2;
      uart2.release_dma_rx();
      break;
    case KernelConfig::USB_BRIDGE_SHELL:
      usb_bridge_target = &shell;
      break;
    case KernelConfig::USB_BRIDGE_LOOPBACK:
      usb_bridge_target = &bridge_loopback_io;
      break;
  }

  led.led(0);
  initialized = true;
}

static Kernel::gps_time_t notified_latest_gps_time = {0, 0};
void Kernel::notify_gps_time(const gps_time_t &gps_time) {
  SWI_disable(); // disable interrupt from kernel_periodic();
  notified_latest_gps_time = gps_time;
  SWI_enable();
}
void Kernel::notify_gps_time(const Uint32 &wn, const Uint32 &itow_ms) {
  SWI_disable(); // disable interrupt from kernel_periodic();
  notified_latest_gps_time.wn = wn;
  notified_latest_gps_time.itow_ms = itow_ms;
  SWI_enable();
}

static Kernel::gps_time_t gps_time = {0, 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_ads_t msg_ads;
static Kernel::msg_mag_t msg_mag;

Kernel::gps_time_t Kernel::gps_time() const {
  return ::gps_time;
}
Uint32 Kernel::wn() const {
  return ::gps_time.wn;
}
Uint32 Kernel::itow_ms() const {
  return ::gps_time.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" {

// { // Time related code
extern _CODE_ACCESS std::time_t HOSTtime();
///< @see http://processors.wiki.ti.com/index.php/Time_and_clock_RTS_Functions
_CODE_ACCESS std::time_t std::time(std::time_t *timer){
   std::time_t res(0);
   //res = (std::time_t)HOSTtime();
   Kernel::gps_time_t notified(::notified_latest_gps_time), current(::gps_time);
   if(notified.wn > 0){ // valid
     res = (7u * 24 * 60 * 60) * current.wn
         + (current.itow_ms / 1000) 
         - latest_processed_info.gps.leap_seconds_gps_minus_utc
         + 2524932000u // Jan 01, 1900, 00:00:00 UTC-6(CST)
         + (60u * 60 * 6); // CST => UTC;
   }
   if(timer){*timer = res;}
   return(res);
}
///< FatFs time function
Uint32 get_fattime(){
  Uint32 res(0);
  std::time_t timer;
  std::time(&timer);
  
  if(timer > 0){
    std::tm *t(std::localtime(&timer));
    // bit31:25 => year - 1980
    res |= t->tm_year + 1900 - 1980;
    res <<= 4;
    // bit24:21 => month[1..12]
    res |= t->tm_mon + 1;
    res <<= 5;
    // bit20:16 => day[1..31]
    res |= t->tm_mday;
    res <<= 5;
    // bit15:11 => hour[0..23]
    res |= t->tm_hour;
    res <<= 6;
    // bit10:5 => minute[0..59]
    res |= t->tm_min;
    res <<= 5;
    // bit4:0 => second / 2
    res |= t->tm_sec / 2;
  }
  
  return res;
}
std::time_t timegm(struct std::tm *tm){
  return std::mktime(tm) - 60 * 60 * 6; // cancel UTC-6(CST)
}
// } // Time related code

void usb_bridge_d2h(){ // connect usb to other devices.
  if(!usb_bridge_target){return;}

  Kernel::IO file_d2h(null_io);

  if(kernel_config.usb_bridge_log){
    file_d2h = Kernel::get_instance().open(Kernel::FAT_FILE, "brdg_d2h.log");
    static const unsigned int file_tx_rx_buffers[] = {0x1000, 0};
    file_d2h.ioctl(Kernel::IOCTL_BUFFER, file_tx_rx_buffers);
  }

  char buf[0x40];
  int received, transmitted;
  unsigned int d2h_bytes(0);
  while(true){
    if((received = usb_bridge_target->receive(buf, sizeof(buf))) > 0){
      file_d2h.transmit(buf, received);
      transmitted = 0;
      do{
        transmitted += usb_uart_io.transmit(&buf[transmitted], received - transmitted);
        if(transmitted >= received){break;}
      }while(true);
      d2h_bytes += transmitted;
    }
    TSK_sleep(1);
  }
}

void usb_bridge_h2d(){ // connect usb to other devices.
  if(!usb_bridge_target){return;}
  
  Kernel::IO file_h2d(null_io);

  if(kernel_config.usb_bridge_log){
    file_h2d = Kernel::get_instance().open(Kernel::FAT_FILE, "brdg_h2d.log");
    static const unsigned int file_tx_rx_buffers[] = {0x1000, 0};
    file_h2d.ioctl(Kernel::IOCTL_BUFFER, file_tx_rx_buffers);
  }

  char buf[0x40];
  int received, transmitted;
  unsigned int h2d_bytes(0);
  while(true){
    if((received = usb_uart_io.receive(buf, sizeof(buf))) > 0){
      file_h2d.transmit(buf, received);
      transmitted = 0;
      do{
        transmitted += usb_bridge_target->transmit(&buf[transmitted], received - transmitted);
        if(transmitted >= received){break;}
      }while(true);
      h2d_bytes += transmitted;
    }
    TSK_sleep(1);
  }
}

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_ads(kernel.mbx(Kernel::MBX_ADS));
  Uint32 polling_invoked(0);
  
  while(true){
    SEM_pendBinary(&sem_periodic, SYS_FOREVER);
    
    // SPI0
    if(spi0.assign_dma()){

      // ADC(ADS1248)
      {
        const int adc_index(polling_invoked % 8);
        const int adc_index_next((polling_invoked + 1) % 8);

        ADS1248 &ads1248(ADC_IO::adc); // spi0 <=> ADS1248
        spi0.set_format(1);

        // Read result
        msg_adc.ch[adc_index] = ads1248.read_result();
        if(adc_index == 0){
          msg_adc.time_ms = gps_time.itow_ms;
        }else if(adc_index == 7){
          MBX_post(mbx_adc, &msg_adc, 0);
        }

        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();
      }

      // Servo obs / drv (CPLD)
      if((polling_invoked % 0x10) == 0){ // 1K / 16 = 62.5Hz
        spi0.set_format(2);

        // servo obs read
        {
          static const Uint8 prefix[] = {0x00};
          spi0.write((void *)prefix, sizeof(prefix), true);
          spi0.write_read((void *)msg_servo_read.ch_in, sizeof(msg_servo_read.ch_in));
          msg_servo_read.time_ms = gps_time.itow_ms;
        }

        // Servo drv write preparation
        MBX_pend(mbx_servo_write, &msg_servo_write, 0);

        // servo drv read / write
        {
          static const Uint8 prefix[] = {0x90};
          spi0.write((void *)prefix, sizeof(prefix), true);
          std::memcpy(msg_servo_read.ch_out, msg_servo_write.ch, sizeof(msg_servo_read.ch_out));
          spi0.write_read((void *)msg_servo_read.ch_out, sizeof(msg_servo_read.ch_out));
        }

        MBX_post(mbx_servo_read, &msg_servo_read, 0);
      }

      // ADS
      if((!use_ms5611) && ((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_ads.raw_buf, sizeof(msg_ads.raw_buf));
        msg_ads.time_ms = gps_time.itow_ms;
        MBX_post(mbx_ads, &msg_ads, 0);
      }

      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]));
        }
      }
    }
    
    // Slow task
    if((polling_invoked % 0x50) == 0){ // 12.5Hz
      SEM_postBinary(&sem_periodic_slow);
    }

    polling_invoked++;
    kernel_state |= kernel_state_polling_active;
  }
}

static void oled_refresh(const int &line_num){
  char buf[32];
  int length(0);
  switch(line_num){
    case 0: {
      length = std::snprintf(buf, sizeof(buf), "Tiny Feather %c%02d",
          (latest_processed_info.gps.fix_type < latest_processed_info.gps.FIX_3D ? ' ' : '*'),
          latest_processed_info.gps.using_satellites);
      break;
    }
    case 1: {
      length = std::snprintf(buf, sizeof(buf), " T %4d:%8.1f", gps_time.wn, (float_sylph_t)gps_time.itow_ms / 1000);
      break;
    }
    case 2: {
      float_sylph_t lat(rad2deg(latest_processed_info.navigation.latitude_rad));
      if(lat >= 0){
        length = std::snprintf(buf, sizeof(buf), " N  %012.9f", lat);
      }else{
        length = std::snprintf(buf, sizeof(buf), " S  %012.9f", -lat);
      }
      break;
    }
    case 3: {
      float_sylph_t lng(rad2deg(latest_processed_info.navigation.longitude_rad));
      if(lng >= 0){
        length = std::snprintf(buf, sizeof(buf), " E %013.9f", lng);
      }else{
        length = std::snprintf(buf, sizeof(buf), " W %013.9f", -lng);
      }
      break;
    }
    case 4: {
      length = std::snprintf(buf, sizeof(buf), " A %+08.1f", latest_processed_info.navigation.height_meter);
      break;
    }
    case 5: {
      length = std::snprintf(buf, sizeof(buf), " V %+04.0f%+04.0f%+05.1f",
          latest_processed_info.navigation.v_north_ms, latest_processed_info.navigation.v_east_ms, latest_processed_info.navigation.v_down_ms);
      break;
    }
    case 6: {
      float_sylph_t
          heading(rad2deg(latest_processed_info.navigation.heading_rad)),
          pitch(rad2deg(latest_processed_info.navigation.pitch_rad)),
          roll(rad2deg(latest_processed_info.navigation.roll_rad));
      if(heading < 0){heading += 360;}
      length = std::snprintf(buf, sizeof(buf), " H%03.0f P%+03.0f R%+04.0f", heading, pitch, roll);
      break;
    }
    //case 7: break;
  }
  oled.setTextXY(line_num, 0); // Set the cursor to Xth Page, Yth Column
  oled.putString(buf, length); // Print the String
}

void kernel_polling_slow(){
  
  unsigned int loop_count(0);

  Uint8 ubx_sv_eph_selector(0);

  MBX_Handle mbx_mag(kernel.mbx(Kernel::MBX_MAG));
  MBX_Handle mbx_ads(kernel.mbx(Kernel::MBX_ADS));

  if(use_oled){
    oled.setBrightness(0xFF);
  }

  while(true){
    SEM_pendBinary(&sem_periodic_slow, SYS_FOREVER);

    // Ublox ephemeris
    if((loop_count % 0x10) == 0){ // (12.5 / 16) Hz
      ubx.poll_rxm_eph(++ubx_sv_eph_selector);
      if(ubx_sv_eph_selector == 32){
        ubx.poll_aid_hui();
        ubx_sv_eph_selector = 0;
      }
    }

    // Magnetic sensor via I2C1
    if(mag_sensor){
      msg_mag.time_ms = gps_time.itow_ms;
      mag_sensor->raw(msg_mag.raw_buf);
      MBX_post(mbx_mag, &msg_mag, 0);
    }
    
    // Pressure & Temp via I2C1
    if(use_ms5611){
      msg_ads.time_ms = gps_time.itow_ms;
      msg_ads.raw_buf[0] = msg_ads.raw_buf[4] = 0;
      { // read ADC1 (pressure)
        ms5611.send_command(0x42); // OSR=512
        TSK_sleep(2);
        ms5611.read_adc((Uint8 (&)[3])(msg_ads.raw_buf[1]));
      }
      { // read ADC2 (temperature)
        ms5611.send_command(0x52); // OSR=512
        TSK_sleep(2);
        ms5611.read_adc((Uint8 (&)[3])(msg_ads.raw_buf[5]));
      }
      { // raw data => value
        Int32 pressure, temperature;
        ms5611.convert(
            (Uint8 (&)[3])(msg_ads.raw_buf[1]), (Uint8 (&)[3])(msg_ads.raw_buf[5]), pressure, temperature);
        std::memcpy(&msg_ads.raw_buf[0], &(pressure = be_num_2_num(pressure)), sizeof(Int32));
        std::memcpy(&msg_ads.raw_buf[4], &(temperature = be_num_2_num(temperature)), sizeof(Int32));
      }
      MBX_post(mbx_ads, &msg_ads, 0);
    }
    
    if(use_oled){
      oled_refresh(loop_count % 8);
    }

    loop_count++;
  }
}

void kernel_periodic(){ // called from PRD(SWI)
  
  if(SEM_pendBinary(&sem_1pps, 0)){
    gps_time.wn = notified_latest_gps_time.wn;
    gps_time.itow_ms = ((notified_latest_gps_time.itow_ms / 1000) + 1) * 1000;
  }else{
    ++gps_time.itow_ms;
  }
  
  if(gps_time.itow_ms >= (60u * 60 * 24 * 7 * 1000)){ // 1 week roll over
    ++gps_time.wn;
    gps_time.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 = latest_processed_info.gps.using_satellites;
      count = 0;
    }
    switch(count % 250){
      case 100:
        if(satellites){
          satellites--;
          led.led3(true);
        }
        break;
      case 150:
        led.led3(false);
        break;
    }
  }
  
  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(){
  static const char rom_rev001[] = "d800k001"; // Silicon Revision 1.0(-), 1.1(A)
  static const char rom_rev003[] = "d800k003"; // Silicon Revision 2.0(B)
  static const char rom_rev005[] = "d800k005"; // Silicon Revision 2.1(C), 3.0(D)

  int silicon_revision(0);
  if(std::memcmp(rom_rev001, (const void *)0x11700008, sizeof(rom_rev001) - 1) == 0){
    silicon_revision = 10;
  }else if(std::memcmp(rom_rev003, (const void *)0x11700008, sizeof(rom_rev003) - 1) == 0){
    silicon_revision = 20;
  }else if(std::memcmp(rom_rev005, (const void *)0x11700008, sizeof(rom_rev005) - 1) == 0){
    silicon_revision = 21;
  }

  if(silicon_revision > 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 @see Chap.8 and Fig.3.1of spruh91a
    // every module is enable(3)
    Uint32 psc0_num_state[][2] = {
        {0, 3}, {1, 3}, {2, 3}, {3, 3}, {4, 3}, {5, 3}, {9, 3}, {11, 3}, {12, 3}, {13, 3}};
    for(int i(0); i < sizeof(psc0_num_state) / sizeof(psc0_num_state[0]); i++){
      CSL_PscRegsOvly reg((CSL_PscRegsOvly)CSL_PSC_0_REGS);
      while((reg->PTSTAT & 0x1) != 0);
      Uint32 index(psc0_num_state[i][0]), desired_state(psc0_num_state[i][1] & 0x3);
      if(((reg->MDSTAT)[index] & 0x3F) == desired_state){continue;}
      (reg->MDCTL)[index] = ((reg->MDCTL)[index] & 0xFFFFFF00u) | desired_state;
      reg->PTCMD = 1;
      while((reg->PTSTAT & 0x1) != 0); // Wait for power state transition to finish
      while(((reg->MDSTAT)[index] & 0x3F) != desired_state);
    }
    // PSC1@see Chap.8 and Fig.3.1of spruh91a
    // 24(SCR8(BR15)) is enable(3) because of 8.2.2.1, and 5(EMAC), 7-9(McASP[0-2]), 17(eHRPWM), 20(eCAP), 21(eQEP) are disable(2).
    Uint32 psc1_num_state[][2] = {
        {1, 3}, {3, 3}, {5, 2}, {6, 3}, {7, 2}, {8, 2}, {9, 2},
        {10, 3}, {11, 3}, {12, 3}, {13, 3}, {17, 2}, {20, 2}, {21, 2}, {24, 3}, {25, 3}, {26, 3}};
    for(int i(0); i < sizeof(psc1_num_state) / sizeof(psc1_num_state[0]); i++){
      CSL_PscRegsOvly reg((CSL_PscRegsOvly)CSL_PSC_1_REGS);
      while((reg->PTSTAT & 0x1) != 0);
      Uint32 index(psc1_num_state[i][0]), desired_state(psc1_num_state[i][1] & 0x3);
      if(((reg->MDSTAT)[index] & 0x3F) == desired_state){continue;}
      (reg->MDCTL)[index] = ((reg->MDCTL)[index] & 0xFFFFFF00u) | desired_state;
      reg->PTCMD = 1;
      while((reg->PTSTAT & 0x1) != 0); // Wait for power state transition to finish
      while(((reg->MDSTAT)[index] & 0x3F) != desired_state);
    }
    // EMIFA
    {
      CSL_EmifaRegsOvly emifaRegs((CSL_EmifaRegsOvly)CSL_EMIFA_0_REGS);
      emifaRegs->AWCC = 0x300400ff;
      emifaRegs->CE3CFG = 0x4C5462B8;
      emifaRegs->NANDFCR &= ~2;
    }
  }
}

}