//#include "config.h"
#include "usb_otg_fs.h"
#include "usb_defines.h"

USB_OTG_FS::USB_OTG_FS(bool _is_device)
    : USB_OTG(),
    ep0_rw_buf(NULL), ep0_rw_size(0), ep0_callback(NULL),
    is_device(_is_device) {
  
  // Register setup @see USB_OTG_SelectCore()
  static const void *baseAddress(USB_OTG_FS_BASE_ADDR);
  usbRegs.GREGS = (USB_OTG_GREGS *)(baseAddress + USB_OTG_CORE_GLOBAL_REGS_OFFSET);
  
  { // device
    usbRegs.DREGS = (USB_OTG_DREGS *)(baseAddress + USB_OTG_DEV_GLOBAL_REG_OFFSET);
    usbRegs.INEP_REGS[0] = (USB_OTG_INEPREGS *)(baseAddress + USB_OTG_DEV_IN_EP_REG_OFFSET);
    usbRegs.OUTEP_REGS[0] = (USB_OTG_OUTEPREGS *)(baseAddress + USB_OTG_DEV_OUT_EP_REG_OFFSET);
    for(int i(1), j(0); i < ENDPOINTS; i++, j++){
      usbRegs.INEP_REGS[i] = (USB_OTG_INEPREGS *)((void *)usbRegs.INEP_REGS[j] + USB_OTG_EP_REG_OFFSET);
      usbRegs.OUTEP_REGS[i] = (USB_OTG_OUTEPREGS *)((void *)usbRegs.OUTEP_REGS[j] + USB_OTG_EP_REG_OFFSET);
    }
  }
  
  { // host
    usbRegs.HREGS = (USB_OTG_HC_REGS *)(baseAddress + USB_OTG_HOST_GLOBAL_REG_OFFSET);
    usbRegs.HPRT0 = (Uint32 *)(baseAddress + USB_OTG_HOST_PORT_REGS_OFFSET);
    usbRegs.HC_REGS[0] = (USB_OTG_HC_REGS *)(baseAddress + USB_OTG_HOST_CHAN_REGS_OFFSET);
    usbRegs.DFIFO[0] = (Uint32 *)(baseAddress + USB_OTG_DATA_FIFO_OFFSET);
    for(int i(1), j(0); i < HOST_CHANNELS; i++, j++){
      usbRegs.HC_REGS[i] = (USB_OTG_HC_REGS *)((void *)usbRegs.HC_REGS[j] + USB_OTG_HOST_CHAN_REGS_OFFSET);
      usbRegs.DFIFO[i] = (Uint32 *)((void *)usbRegs.DFIFO[j] + USB_OTG_DATA_FIFO_SIZE);
    }
  }
  
  usbRegs.PCGCCTL = (Uint32 *)(baseAddress + USB_OTG_PCGCCTL_OFFSET);
}

USB_OTG_FS::~USB_OTG_FS(){
  
}

void USB_OTG_FS::init() {
  
#if 0
  /* CONFIGURE THE DRVVBUS PIN HERE.*/
  /* See your device-specific System Reference Guide for more information on how to set up the pinmux. */
  // PINMUX9 (USB0_DRVVBUS/GP4[15] Control), required for C6745?
  BootCfg->PINMUX9 &= ~CSL_SYSCFG_PINMUX9_PINMUX9_7_4_MASK;
  BootCfg->PINMUX9 |= (
      CSL_SYSCFG_PINMUX9_PINMUX9_7_4_USB0_DRVVBUS 
          << CSL_SYSCFG_PINMUX9_PINMUX9_7_4_SHIFT); // Set [7-4] 0x01 for USB0_DRVVBUS
#endif
  init_core();
}

void USB_OTG_FS::init_core() {
  USB::dev_state = DEV_DEFAULT;
  
  ep0_state = EP_IDLE;
  
  // The following procedure is determined by reference to DCD_Init() / HCD_Init()

  disable_global_interrupt();

  { // @see USB_OTG_CoreInit()
    USB_OTG_GUSBCFG_TypeDef usbcfg;

    usbcfg.d32 = USB_OTG_READ_REG32(&usbRegs.GREGS->GUSBCFG);
    usbcfg.b.physel = 1; /* FS Interface */
    USB_OTG_WRITE_REG32(&usbRegs.GREGS->GUSBCFG, usbcfg.d32);

    /* Reset after a PHY select and set Host mode */
    reset_core();
    
    /* Enable the I2C interface and deactivate the power down*/
    USB_OTG_GCCFG_TypeDef gccfg;
    gccfg.d32 = 0;
    gccfg.b.pwdn = 1;

    gccfg.b.vbussensingA = 1;
    gccfg.b.vbussensingB = 1;
#ifndef VBUS_SENSING_ENABLED
    gccfg.b.disablevbussensing = 1;
#endif    
    
    //if(pdev->cfg.Sof_output){gccfg.b.sofouten = 1;}

    USB_OTG_WRITE_REG32(&usbRegs.GREGS->GCCFG, gccfg.d32);
    //USB_OTG_BSP_mDelay(20);
    
    /* initialize OTG features */
#ifdef  USE_OTG_MODE
    usbcfg.d32 = USB_OTG_READ_REG32(&pdev->regs.GREGS->GUSBCFG);
    usbcfg.b.hnpcap = 1;
    usbcfg.b.srpcap = 1;
    USB_OTG_WRITE_REG32(&usbRegs.GREGS->GUSBCFG, usbcfg.d32);
    USB_OTG_EnableCommonInt(pdev);
#endif
  }

  set_current_mode(is_device);

  if(is_device){ // @see USB_OTG_CoreInitDev()

    USB_OTG_WRITE_REG32(&usbRegs.PCGCCTL, 0); // Restart the Phy Clock

    USB_OTG_DCFG_TypeDef dcfg;
    dcfg.d32 = USB_OTG_READ_REG32(&usbRegs.DREGS->DCFG); // Device configuration register
    dcfg.b.perfrint = DCFG_FRAME_INTERVAL_80;
    USB_OTG_WRITE_REG32(&usbRegs.DREGS->DCFG, dcfg.d32);

    set_device_speed(USB_OTG_SPEED_PARAM_FULL);

#if 0 // FIFO size/address setup
    USB_OTG_FSIZ_TypeDef nptxfifosize;
    USB_OTG_FSIZ_TypeDef txfifosize;
    nptxfifosize.d32 = 0;
    txfifosize.d32 = 0;

    USB_OTG_WRITE_REG32(&usbRegs.GREGS->GRXFSIZ, RX_FIFO_FS_SIZE); // Rx FIFO

    // EP0 TX
    nptxfifosize.b.depth = TX0_FIFO_FS_SIZE;
    nptxfifosize.b.startaddr = RX_FIFO_FS_SIZE;
    USB_OTG_WRITE_REG32(&usbRegs.GREGS->DIEPTXF0_HNPTXFSIZ, nptxfifosize.d32 );

    // EP1 TX
    txfifosize.b.startaddr = nptxfifosize.b.startaddr + nptxfifosize.b.depth;
    txfifosize.b.depth = TX1_FIFO_FS_SIZE;
    USB_OTG_WRITE_REG32(&usbRegs.GREGS->DIEPTXF[0], txfifosize.d32 );

    // EP2 TX
    txfifosize.b.startaddr += txfifosize.b.depth;
    txfifosize.b.depth = TX2_FIFO_FS_SIZE;
    USB_OTG_WRITE_REG32(&usbRegs.GREGS->DIEPTXF[1], txfifosize.d32 );

    // EP3 TX
    txfifosize.b.startaddr += txfifosize.b.depth;
    txfifosize.b.depth = TX3_FIFO_FS_SIZE;
    USB_OTG_WRITE_REG32(&usbRegs.GREGS->DIEPTXF[2], txfifosize.d32 );
#endif

    __IO USB_OTG_GRSTCTL_TypeDef  greset;
    greset.d32 = 0;
    greset.b.txfflsh = 1;
    greset.b.txfnum = 0x10; // Flush the all TX FIFOs
    greset.b.rxfflsh = 1;
    flush_fifo(greset);

    // Clear all pending Device Interrupts
    USB_OTG_WRITE_REG32(&usbRegs.DREGS->DIEPMSK, 0);
    USB_OTG_WRITE_REG32(&usbRegs.DREGS->DOEPMSK, 0);
    USB_OTG_WRITE_REG32(&usbRegs.DREGS->DAINT, 0xFFFFFFFF);
    USB_OTG_WRITE_REG32(&usbRegs.DREGS->DAINTMSK, 0);

    for(int i(0); i < ENDPOINTS; i++){
      USB_OTG_DEPCTL_TypeDef depctl;
      __IO Uint32 *depctl_addr[2] = {
          &(usbRegs.INEP_REGS[i]->DIEPCTL), &(usbRegs.OUTEP_REGS[i]->DOEPCTL)};
      for(int j(0); j < sizeof(depctl_addr) / sizeof(depctl_addr[0]); j++){
        depctl.d32 = USB_OTG_READ_REG32(depctl_addr[j]);
        if(depctl.b.epena){
          depctl.d32 = 0;
          depctl.b.epdis = 1;
          depctl.b.snak = 1;
        }else{
          depctl.d32 = 0;
        }
        USB_OTG_WRITE_REG32(depctl_addr[j], depctl.d32);
      }
      USB_OTG_WRITE_REG32(&usbRegs.INEP_REGS[i]->DIEPTSIZ, 0);
      USB_OTG_WRITE_REG32(&usbRegs.OUTEP_REGS[i]->DOEPTSIZ, 0);
      USB_OTG_WRITE_REG32(&usbRegs.INEP_REGS[i]->DIEPINT, 0xFF);
      USB_OTG_WRITE_REG32(&usbRegs.OUTEP_REGS[i]->DOEPINT, 0xFF);
    }

    USB_OTG_DIEPMSK_TypeDef msk;
    msk.d32 = 0;
    msk.d32 = 0;
    msk.b.txfifoundrn = 1;
    USB_OTG_MODIFY_REG32(&usbRegs.DREGS->DIEPMSK, msk.d32, msk.d32);

    /*if(pdev->cfg.dma_enable == 1){
      USB_OTG_DTHRCTL_TypeDef dthrctl;
      dthrctl.d32 = 0;
      dthrctl.b.non_iso_thr_en = 1;
      dthrctl.b.iso_thr_en = 1;
      dthrctl.b.tx_thr_len = 64;
      dthrctl.b.rx_thr_en = 1;
      dthrctl.b.rx_thr_len = 64;
      USB_OTG_WRITE_REG32(&usbRegs.DREGS->DTHRCTL, dthrctl.d32);
    }*/

    enable_device_interrupt();
  }else{ // TODO: @see USB_OTG_CoreInitHost()

  }

  enable_global_interrupt();
}

void USB_OTG_FS::suspend(){
  // TODO: 実装
}

void USB_OTG_FS::resume(){
  // TODO: 実装
}

void USB_OTG_FS::reset(){
  USB::dev_state = DEV_DEFAULT;
  
  ep0_state = EP_IDLE;
  for(int i(0); i < ENDPOINTS; i++){
    set_ep_state((SetupBuffer::DRD_IN) | i, EP_HALT);
    set_ep_state((SetupBuffer::DRD_OUT) | i, EP_HALT);
  }
}

USB::ep_state_t USB_OTG_FS::get_ep_state(const Uint8 &ep_index) const {
  Uint8 index(ep_index & SetupBuffer::DRR_MASK);
  if(index > ENDPOINTS){
    return EP_VOID;
  }
  if(index == 0){
    return ep0_state;
  }
  index--;
  __IO uint32_t *depctl_addr;
  if((ep_index & SetupBuffer::DRD_MASK) == SetupBuffer::DRD_IN){
    depctl_addr = &(usbRegs.INEP_REGS[index]->DIEPCTL);
  }else{
    depctl_addr = &(usbRegs.OUTEP_REGS[index]->DOEPCTL);
  }
  
  USB_OTG_DEPCTL_TypeDef depctl;
  depctl.d32 = USB_OTG_READ_REG32(&depctl_addr);
  if(depctl.b.stall == 1){
    return EP_HALT;
  }else if (depctl.b.naksts == 1){
    return EP_NACK;
  }else{
    return EP_IDLE;
  }
}
USB::ep_state_t USB_OTG_FS::set_ep_state(const Uint8 &ep_index, const ep_state_t &state){
  Uint8 index(ep_index & SetupBuffer::DRR_MASK);
  if(index > ENDPOINTS){
    return EP_VOID;
  }
  
  if(index == 0){
    return ep0_state; // EP0だけは外から弄れないようにする
  }
  index--;
  
  __IO Uint32 *depctl_addr;
  if((ep_index & SetupBuffer::DRD_MASK) == SetupBuffer::DRD_IN){
    depctl_addr = &(usbRegs.INEP_REGS[index]->DIEPCTL);
  }else{
    depctl_addr = &(usbRegs.OUTEP_REGS[index]->DOEPCTL);
  }
  
  USB_OTG_DEPCTL_TypeDef depctl;
  depctl.d32 = USB_OTG_READ_REG32(depctl_addr);
  switch(state){
    case EP_HALT:
      if(((ep_index & SetupBuffer::DRD_MASK) == SetupBuffer::DRD_IN)
          && depctl.b.epena){
        depctl.b.epdis = 1;
      }
      depctl.b.stall = 1;
      break;
    case EP_IDLE:
      if(depctl.b.stall == 1){ // clear stall
        depctl.b.stall = 0;
        depctl.b.setd0pid = 1; // assume interrupt or bulk EP.
      }
      depctl.b.cnak = 1;
      depctl.b.usbactep = 1; 
      depctl.b.epena = 1;
      break;
    case EP_NACK:
      depctl.b.snak = 1;
      break;
    case EP_VOID:
      depctl.b.usbactep = 0;
      break;
    default:
      return get_ep_state(ep_index);
  }
  USB_OTG_WRITE_REG32(depctl_addr, depctl.d32);
  return get_ep_state(ep_index);
}

void USB_OTG_FS::ep0_tx(const void *buf, const Uint16 &size){
  ep0_state = EP_TX;
  ep0_rw_buf = (Uint8 *)buf;
  ep0_rw_size = size;
}
void USB_OTG_FS::ep0_tx(const void *buf, const Uint16 &size, CallbackFunctor &callback){
  ep0_tx(buf, size);
  ep0_callback = &callback;
}
void USB_OTG_FS::ep0_rx(void *buf, const Uint16 &size){
  ep0_state = EP_RX;
  ep0_rw_buf = (Uint8 *)buf;
  ep0_rw_size = size;
}
void USB_OTG_FS::ep0_rx(void *buf, const Uint16 &size, CallbackFunctor &callback){
  ep0_rx(buf, size);
  ep0_callback = &callback;
}

// TODO
#if 0
USB_OTG_FS::Uint16 USB_OTG_FS::ep_rx_size_check(const Uint8 &out_index) const {
  if(out_index > ENDPOINTS){return 0;}
  
  // FIFO上のバイト数を確認
  return (out_index == 0) 
      ? usbRegs->EPCSR[0].COUNT.COUNT0 
      : usbRegs->EPCSR[out_index].COUNT.RXCOUNT;
}

USB_OTG_FS::Uint16 USB_OTG_FS::ep_rx_size_check(const Uint8 &out_index, const Uint16 &max_size) const {
  Uint16 available(ep_rx_size_check(out_index));
  return (max_size > available) ? available : max_size;
}

USB_OTG_FS::Uint16 USB_OTG_FS::read(const Uint8 &out_index, void *buf, const Uint16 &size){
  if(size == 0){return 0;}
  Uint8 *_buf((Uint8 *)buf);
  Uint16 available(ep_rx_size_check(out_index)), _size(size);
  bool clear_flag(false);
  if(available <= size){
    _size = available;
    clear_flag = true;
  }
  
  volatile Uint32 *fifo((&(usbRegs->FIFO0)) + out_index);
  for(int i(0); i < _size; i++){
    *_buf = *((volatile Uint8 *)fifo);
    _buf++;
  }
  
  // Load new data, but auto-clear feature is convenient 
  // usbRegs->EPCSR[out_index].RXCSR.PERI_RXCSR |= CSL_USB_OTG_PERI_RXCSR_AUTOCLEAR_MASK;
  if((out_index > 0) && clear_flag){
    usbRegs->EPCSR[out_index].RXCSR.PERI_RXCSR &= ~CSL_USB_OTG_PERI_RXCSR_RXPKTRDY_MASK;
  }
  
  return _size;
}

USB_OTG_FS::Uint16 USB_OTG_FS::read_skip(const Uint8 &out_index, const Uint16 &size){
  if(size == 0){return 0;}
  Uint16 available(ep_rx_size_check(out_index)), _size(size);
  bool clear_flag(false);
  if(available <= size){
    _size = available;
    clear_flag = true;
  }
  
  volatile Uint32 *fifo((&(usbRegs->FIFO0)) + out_index);
  for(int i(0); i < _size; i++){
    Uint8 v = *((volatile Uint8 *)fifo);
  }
  
  // Load new data, but auto-clear feature is convenient 
  // usbRegs->EPCSR[out_index].RXCSR.PERI_RXCSR |= CSL_USB_OTG_PERI__RXCSRAUTOCLEAR_MASK;
  if((out_index > 0) && clear_flag){
    usbRegs->EPCSR[out_index].RXCSR.PERI_RXCSR &= ~CSL_USB_OTG_PERI_RXCSR_RXPKTRDY_MASK;
  }
  
  return _size;
}
#endif

USB_OTG_FS::Uint16 USB_OTG_FS::ep_tx_size_check(const Uint8 &in_index) const {
  if(in_index > ENDPOINTS){return 0;}
  
#if 0
  // パケットの最大バイト数を確認
  if(in_index == 0){
    USB_OTG_FSIZ_TypeDef nptxfifosize;
    nptxfifosize.d32 = USB_OTG_READ_REG32(&usbRegs.GREGS->DIEPTXF0_HNPTXFSIZ);
    return nptxfifosize.b.depth;
  }else{
    USB_OTG_FSIZ_TypeDef txfifosize;
    txfifosize.d32 = USB_OTG_READ_REG32(&usbRegs.GREGS->DIEPTXF[in_index - 1]);
    return txfifosize.b.depth;
  }
#endif
  
  USB_OTG_DTXFSTSn_TypeDef txstatus;
  txstatus.d32 = USB_OTG_READ_REG32(&usbRegs.INEP_REGS[in_index]->DTXFSTS);
  return txstatus.b.txfspcavail * sizeof(Uint32);
}

USB_OTG_FS::Uint16 USB_OTG_FS::ep_tx_size_check(const Uint8 &in_index, const Uint16 &max_size) const {
  Uint16 available(ep_tx_size_check(in_index));
  return (max_size > available) ? available : max_size;
}

USB_OTG_FS::Uint16 USB_OTG_FS::write(const Uint8 &in_index, const void *buf, const Uint16 &size){
  __packed Uint32 *_buf((__packed Uint32 *)buf);
  Uint16 _size(ep_tx_size_check(in_index, size));
  
  if(_size == 0){return 0;}
  
  for(int i(0); i < (_size + sizeof(Uint32) - 1) / sizeof(Uint32); i++){
    USB_OTG_WRITE_REG32(&usbRegs.DFIFO[in_index], *buf);
    _buf++;
  }
  
  return _size;
}

USB_OTG_FS::Uint16 USB_OTG_FS::write_fill(const Uint8 &in_index, const Uint8 c, const Uint16 &size){
  Uint16 _size(ep_tx_size_check(in_index, size));
  
  if(_size == 0){return 0;}
  
  Uint32 buf(c);
  for(int i(1); i < sizeof(Uint32) / sizeof(Uint8); i++){
    buf <<= 8;
    buf |= c;
  }
  
  for(int i(0); i < (_size + sizeof(Uint32) - 1) / sizeof(Uint32); i++){
    USB_OTG_WRITE_REG32(&usbRegs.DFIFO[in_index], buf);
  }
  
  return _size;
}

void USB_OTG_FS::handle_ep0(){
  
  // Temporary storage for EP control status register (PERI_CSR0)
  Uint16 control_reg(usbRegs->EPCSR[0].TXCSR.PERI_CSR0);
  
  // @see http://e2e.ti.com/support/microcontrollers/stellaris_arm_cortex-m3_microcontroller/f/471/p/44369/158771.aspx
  // @see http://e2e.ti.com/support/dsp/omap_applications_processors/f/42/p/29276/101760.aspx#101760
  if(address_changed){
    address_changed = false;
    //LOG_printf(&trace, "USB_SET_ADDRESS: 0x%x", get_address());
  }
  
  // If last packet was a sent stall, reset STSTL
  // bit and return EP0 to idle state
  if (control_reg & CSL_USB_OTG_PERI_CSR0_SENTSTALL_MASK){
    control_reg &= ~CSL_USB_OTG_PERI_CSR0_SENTSTALL_MASK;
    usbRegs->EPCSR[0].TXCSR.PERI_CSR0 = control_reg;
    ep0_state = EP_IDLE;
  }

  // If last setup transaction was ended prematurely then set
  if(control_reg & CSL_USB_OTG_PERI_CSR0_SETUPEND_MASK){
    // Serviced Setup End bit and return EP0 to idle state
    control_reg |= CSL_USB_OTG_PERI_CSR0_SERV_SETUPEND_MASK;
    usbRegs->EPCSR[0].TXCSR.PERI_CSR0 = control_reg;
    ep0_state = EP_IDLE;
  }
  
  control_reg = usbRegs->EPCSR[0].TXCSR.PERI_CSR0;
  
  // If Endpoint 0 is in idle mode
  if(ep0_state == EP_IDLE){
    
    // Make sure that EP 0 has an Out Packet ready from host
    // although if EP0 is idle, this should always be the case
    if(!(control_reg & CSL_USB_OTG_PERI_CSR0_RXPKTRDY_MASK)){
      return;
    }
      
    SetupBuffer buf;
    
    bool request_completed(false);
    
    // 受信サイズ確認
    //LOG_printf(&trace, "USB_IDLE_START: %d, 0x%x", usbRegs->EPCSR[0].COUNT.COUNT0, usbRegs->EPCSR[0].CONFIGDATA);
    if(read(0, &buf, sizeof(buf)) == sizeof(buf)){
    
      ep0_rw_buf = NULL;
      ep0_rw_size = 0;
      ep0_callback = NULL;
      
      // Call correct subroutine to handle each kind of standard request
      switch(buf.bmRequestType & SetupBuffer::DRT_MASK){
        case SetupBuffer::DRT_STD :
          LOG_printf(&trace, "USB_STD_REQ: 0x%x, 0x%x", buf.bRequest, buf.wValue);
          request_completed = standard_request_handler.handle(buf);
          LOG_printf(&trace, "USB_STD_RES: 0x%x, 0x%x", ep0_rw_buf, ep0_rw_size);
          break;
        case SetupBuffer::DRT_CLASS :
          request_completed = class_request_handle(buf);
          break;
        case SetupBuffer::DRT_VENDOR :
          request_completed = vendor_request_handle(buf);
          break;
      }
    }
    
    control_reg |= CSL_USB_OTG_PERI_CSR0_SERV_RXPKTRDY_MASK;
    
    if(!request_completed){
      
      ep0_state = EP_HALT;
      control_reg |= CSL_USB_OTG_PERI_CSR0_SENDSTALL_MASK;
      LOG_printf(&trace, "USB_STALL: 0x%x, 0x%x", buf.bRequest, buf.wValue);
      // SENDSTALL bit will be cleared automatically.
    }else if(ep0_state == EP_IDLE){
      
      // Indicate setup packet has been serviced
      control_reg |= CSL_USB_OTG_PERI_CSR0_DATAEND_MASK;
    }
    usbRegs->EPCSR[0].TXCSR.PERI_CSR0 = control_reg;
    LOG_printf(&trace, "USB_IDLE_END: 0x%x", usbRegs->EPCSR[0].TXCSR.PERI_CSR0);
  }else if(ep0_state == EP_RX){
    // Verify packet was received
    if(!(control_reg & CSL_USB_OTG_PERI_CSR0_RXPKTRDY_MASK)){return;}
      
    // get data from the FIFO
    Uint16 received(read(0, ep0_rw_buf, ep0_rw_size));
    
    control_reg |= CSL_USB_OTG_PERI_CSR0_SERV_RXPKTRDY_MASK;
    
    if(ep0_rw_size > received){
      // Update the scheduled number to be received
      ep0_rw_size -= received;
      
      // Advance the pointer
      ep0_rw_buf += received;
    }else{
      // Meet the end of the data stage (ep0_rw_size == 0)
      
      if(ep0_callback){(*ep0_callback)();}
      
      // Signal end of data stage
      control_reg |= CSL_USB_OTG_PERI_CSR0_DATAEND_MASK;
      ep0_state = EP_IDLE;
    }
    usbRegs->EPCSR[0].TXCSR.PERI_CSR0 = control_reg;
    LOG_printf(&trace, "USB_RX: 0x%x, 0x%x", received, usbRegs->EPCSR[0].TXCSR.PERI_CSR0);
  }

  // See if the endpoint has data to transmit to host
  if(ep0_state == EP_TX){
    control_reg = usbRegs->EPCSR[0].TXCSR.PERI_CSR0;
    // Make sure you don't overwrite last packet
    if(control_reg & CSL_USB_OTG_PERI_CSR0_TXPKTRDY_MASK){return;}
      
    // put data to the FIFO
    Uint16 sent(write(0, ep0_rw_buf, ep0_rw_size));
    
    // Add In Packet ready flag to CSR0 bitmask
    control_reg |= CSL_USB_OTG_PERI_CSR0_TXPKTRDY_MASK;
    
    if(ep0_rw_size -= sent){
      
      // Advance data pointer
      ep0_rw_buf += sent;
    }else{
      
      if(ep0_callback){(*ep0_callback)();}
      
      // Add Data End bit to bitmask also @see http://e2e.ti.com/support/dsp/omap_applications_processors/f/42/p/57718/206162.aspx#206162
      control_reg |= CSL_USB_OTG_PERI_CSR0_DATAEND_MASK;
    
      // Return EP 0 to idle state
      ep0_state = EP_IDLE;
    }
    usbRegs->EPCSR[0].TXCSR.PERI_CSR0 = control_reg;
    LOG_printf(&trace, "USB_TX: 0x%x, 0x%x", sent, usbRegs->EPCSR[0].TXCSR.PERI_CSR0);
  }
}

void USB_OTG_FS::handle_isr(){
  
  // INTSRCR[24] => USB_DRVVBUS, INTSRCR[23:16] => INTRUSB @see http://e2e.ti.com/support/dsp/davinci_digital_media_processors/f/100/p/7691/30551.aspx#30551
  Uint32 intsrcr(usbRegs->INTSRCR);
  
  if(intsrcr & CSL_USB_OTG_INTSRCR_RESUME_MASK){
    // Resume
    LOG_printf(&trace, "USB0_RESUME"); 
    resume();
  }
  
  // Endpointの処理などを書く
  if(intsrcr & CSL_USB_OTG_INTSRCR_EP0_MASK){
    // EP0
    //LOG_printf(&trace, "USB_IRQ_EP0: 0x%x, 0x%x", intsrcr, usbRegs->EPCSR[0].TXCSR.PERI_CSR0);
    usbRegs->INDEX = 0;
    handle_ep0();
  }
  
  for(int i(0); i < ENDPOINTS; i++){
    usbRegs->INDEX = (i + 1);
    if(intsrcr & (CSL_USB_OTG_INTSRCR_EP1TX_MASK << i)){
      // EPn TX
      
      // Clear sent stall and flush FIFO if last packet returned a stall
      usbRegs->TXCSR.PERI_TXCSR &= ~CSL_USB_OTG_PERI_TXCSR_SENTSTALL_MASK;
      // TODO: invoke some func.
    }
    
    if(intsrcr & (CSL_USB_OTG_INTSRCR_EP1RX_MASK << i)){
      // EPn RX
      
      // Clear sent stall bit if last packet was a stall
      usbRegs->RXCSR.PERI_RXCSR &= ~CSL_USB_OTG_PERI_RXCSR_SENTSTALL_MASK;
      // TODO: invoke some func.
    }
  }
  
  if(intsrcr & CSL_USB_OTG_INTSRCR_SOF_MASK){
    // SOF
    //LOG_printf(&trace, "USB_SOF");
    resume();
  }
  
  if(intsrcr & CSL_USB_OTG_INTSRCR_DISCONNECT_MASK){
    // Disconnect
    LOG_printf(&trace, "USB_DISCONNECT");
  }
  
  if(intsrcr & CSL_USB_OTG_INTSRCR_SUSPEND_MASK){
    // Suspend
    LOG_printf(&trace, "USB0_SUSPEND");
    suspend();
  }
  
  if(intsrcr & CSL_USB_OTG_INTSRCR_RESET_BABBLE_MASK){
    // Reset
    reset();
  }
  
  handle_isr_dma();
  
  // @see http://e2e.ti.com/support/dsp/davinci_digital_media_processors/f/100/p/7695/30576.aspx#30576
  usbRegs->INTCLRR = intsrcr; // Clear all flag
  usbRegs->EOIR = 0; // Interrupt end
}


void USB_OTG_FS::handle_isr_dma(){
#if 0
  void *descriptor;
  static const Uint16 isr_queues[] 
      = {QUEUE_TX_COMPLETE, QUEUE_TX_COMPLETE + 1, QUEUE_RX_COMPLETE, QUEUE_RX_COMPLETE + 1};
  for(int i(0); i < sizeof(isr_queues) / sizeof(isr_queues[0]); i++){
    while(descriptor = dma_pop_queue(isr_queues[i])){
      dma_push_queue(QUEUE_UNASSIGNED, descriptor);
    }
  }
#endif
}

#if 0

void USB_OTG_FS::set_dma_protocol(
    const Uint8 &ep_index, const dma_protocol_t &protocol,
    const Uint32 &generic_RNDIS_packet_size){
  Uint8 index(ep_index & SetupBuffer::DRR_MASK);
  if((index == 0) || (index > ENDPOINTS)){
    return;
  }
  Uint8 target_bits_shift(index * 4);
  if((ep_index & SetupBuffer::DRD_MASK) == SetupBuffer::DRD_OUT){
    target_bits_shift += 16;
  }
  usbRegs->MODE &= ~((Uint32)0xF << target_bits_shift);
  usbRegs->MODE |= (protocol << target_bits_shift); 
  
  if(protocol == GENERIC_RNDIS){
    usbRegs->GENRNDISSZ[index - 1].SIZE = generic_RNDIS_packet_size & CSL_USB_OTG_SIZE_SIZE_MASK;
  }
}

#pragma DATA_SECTION(".sysdata") // IRAM
#pragma DATA_ALIGN(0x100)
static Uint8 usb0_link_ram0[0x400];
#pragma DATA_SECTION(".sysdata") // IRAM
#pragma DATA_ALIGN(0x100)
static Uint8 usb0_link_ram1[sizeof(usb0_link_ram0)];

#pragma DATA_SECTION(".sysdata") // IRAM
#pragma DATA_ALIGN(0x100)
static Uint8 usb0_descriptor_buf0[32][32];

void USB_OTG_FS::init_dma(
    const Uint16 rx_submit_queues[ENDPOINTS],
    const Uint16 rx_complete_queues[ENDPOINTS]){
  //usbRegs->CTRLR |= CSL_USB_OTG_CTRLR_RNDIS_MASK; // Enable RNDIS from Global Level
  usbRegs->CTRLR &= ~CSL_USB_OTG_CTRLR_RNDIS_MASK; // Disable RNDIS from Global Level
  
#ifndef _USB_PERIPHERAL_ // If Controller is assuming Host Role
#ifdef TRANSPARENT
  usbRegs->AUTOREQ=00; // No Auto Req
#else
  usbRegs->AUTOREQ=01; // Auto Req on all but EOP
#endif
#endif

  // A Single Queue Manager (00b) exist and 16 Regions; no particular assignment exists.
  
  /*
   * Program Link Ram0 and Link Ram1, Base & Size.
   * No Link Ram1 Size Register exists. Most likely is using the same Size Register used
   * Link Ram1 Base
   */
  usbRegs->QMGR.LRAM0BASE = (Uint32)usb0_link_ram0;
  usbRegs->QMGR.LRAM0SIZE = (sizeof(usb0_link_ram0) + sizeof(usb0_link_ram1)) / 4;
  usbRegs->QMGR.LRAM1BASE = (Uint32)usb0_link_ram1;

  /*
   * Allocate Resource to Region 0 (can use any of the 16 available) 
   * (memory location for Host Packet Descriptors)
   * DESC_SIZE value should be [1-8]. Values of 9 - 15 are reserved.
   * Since a minimum of 32 Bytes is required, only program values above 32.
   * Host Packet Descriptor sizes: Min/Max = 32/(104 + Opt S/W Data)
   * REG_SIZE is the total # of Descriptors the Region can accommodate. 
   * At the minimum should be capable of handling 32 Descriptors.
   */
  usbRegs->QMEMREGION[0].QMEMRBASE = (Uint32)usb0_descriptor_buf0;
  usbRegs->QMEMREGION[0].QMEMRCTRL 
      = (0 << CSL_USB_OTG_QMEMRCTRL_START_INDEX_SHIFT) // REG0START_INDEX 
          | (0 << CSL_USB_OTG_QMEMRCTRL_DESC_SIZE_SHIFT) // DESC_SIZE 32 => 0 
          | (0 << CSL_USB_OTG_QMEMRCTRL_REG_SIZE_SHIFT); // REG_SIZE 32 => 0
  
  /*
   * Configure the Scheduler
   * 
   * IMPORTANT NOTICE : CHANNEL is strongly associated with PORT in the below description!!
   * 
   * Configure the Tx/Rx Word[x=0,1] and Scheduler Configuration Register
   * Priorities are handled by programming more of the channel number wanting to see serviced
   * 64 Words exist for a total of 256 entries.
   * Credit can be given to both Tx and Rx Channel within the same Register.
   * Here Rx Credit is given first for the single Rx Channel defined by chan_num.
   * Here we are using the first 8 credits and is assigned to the Channel/Endpoint
   * being serviced.
   */
  usbRegs->DMA_SCHED.ENTRY[0] // Corresponds to WORD0 Offset 0x2800
      = (((Uint32)3 | 0x80) << CSL_USB_OTG_ENTRY_ENTRY3_CHANNEL_SHIFT) // RX EP4
          | (((Uint32)2 | 0x80) << CSL_USB_OTG_ENTRY_ENTRY2_CHANNEL_SHIFT)  // RX EP3
          | (((Uint32)1 | 0x80) << CSL_USB_OTG_ENTRY_ENTRY1_CHANNEL_SHIFT)  // RX EP2
          | (((Uint32)0 | 0x80) << CSL_USB_OTG_ENTRY_ENTRY0_CHANNEL_SHIFT); // RX EP1
  
  usbRegs->DMA_SCHED.ENTRY[1] // Corresponds to WORD1 Offset 0x2804, etc
      = ((Uint32)3 << CSL_USB_OTG_ENTRY_ENTRY3_CHANNEL_SHIFT) // TX EP4
          | ((Uint32)2 << CSL_USB_OTG_ENTRY_ENTRY2_CHANNEL_SHIFT)  // TX EP3
          | ((Uint32)1 << CSL_USB_OTG_ENTRY_ENTRY1_CHANNEL_SHIFT)  // TX EP2
          | ((Uint32)0 << CSL_USB_OTG_ENTRY_ENTRY0_CHANNEL_SHIFT); // TX EP1
  
  // Scheduler is Enabled and number of Credits entered
  usbRegs->DMA_SCHED.DMA_SCHED_CTRL // Scheduler Control Register Offset 0x2000
      = CSL_USB_OTG_DMA_SCHED_CTRL_ENABLE_MASK | (8 - 1);
  
  // TODO: CDMA Teardown Free Descriptor Queue Control Register
  //usbRegs->DMA_GLOBAL.TDFDQ;
  
  /*
   * Configure Tx and Rx DMA State Registers
   * 
   * The Rx Channel Global Configuration Registers are used to initialize the global
   * (non descriptor type specific) behavior of each of the Rx DMA channels. If the
   * enable bit is being set, the Rx/Tx Channel Global Configuration Register SHOULD ONLY
   * BE WRITTEN AFTER ALL OF THE OTHER Rx/Tx CONFIGURATION REGISTERS HAVE BEEN INITIALIZED.
   * Only a Single Queue Manager exists & its value is 0.
   * RxHPCRA/B requires for Queue Manager (have only one and is 00b) and Receive Submit Queues for the
   * first 4 Host Buffer Descriptors to be programmed. Since Queues 0 to 15 are alloted for Receive
   * operations and there exist no dedicated queue assignments for each channel, a user can
   * associate any Queue with any Channel and this association is not fixed for the Receive Operations.
   * Queue[15:0]<==>Any Rx Channel
   * However, for Tx Operations, Dedicated Submit Queues have been assigned for Each Channel, Endpoints.
   * Queue[17:16]<==>TxCh[0], Queue[19:18]<==>TxCh[1], Queue[21:20]<==>TxCh[2], Queue[23:22]<==>TxCh[3]
   * CDMA Rx Chanel x Host Packet Configuration Registers A & B
   * For a Single Descriptor setup, all you will be needing is RXHPCRA[13,12 & 11:00]
   * Assumed that all Descriptors are going to be using the same Queue, but this is configurable.
   */
  for(int i(0); i < ENDPOINTS; i++){
    usbRegs->DMA_CTRL[i].RXHPCRA // Rx Channel i Host Pkt Cfg Reg A
        = (((Uint32)rx_submit_queues[i] << CSL_USB_OTG_RXHPCRA_RX_HOST_FDQ0_QNUM_SHIFT)
            | ((Uint32)rx_submit_queues[i] << CSL_USB_OTG_RXHPCRA_RX_HOST_FDQ1_QNUM_SHIFT));
    usbRegs->DMA_CTRL[i].RXHPCRB // Rx Channel i Host Pkt Cfg Reg B
        = (((Uint32)rx_submit_queues[i] << CSL_USB_OTG_RXHPCRB_RX_HOST_FDQ2_QNUM_SHIFT)
            | ((Uint32)rx_submit_queues[i] << CSL_USB_OTG_RXHPCRB_RX_HOST_FDQ3_QNUM_SHIFT));
    
    /*
     * Rx Submit Queues are assigned by H/W but they are not port specific. 
     * Queues 0-15 are available for any channel.
     */
    usbRegs->DMA_CTRL[i].RXGCR // Rx Channel i Host Pkt Cfg Register
        = (CSL_USB_OTG_RXGCR_RX_ENABLE_MASK
            | CSL_USB_OTG_RXGCR_RX_ERROR_HANDLING_MASK
            |  (1u << CSL_USB_OTG_RXGCR_RX_DEFAULT_DESC_TYPE_SHIFT) // Host Descriptor
            | (Uint32)rx_complete_queues[i]); // Use Queue 26 for Completion/Return Queue
    
    /*
     * Tx Submit Queues are assigned by H/W and are Channel Specific.
     * 2 Submit Queues for each port. Queues 16, 17 for port 0, etc
     */
    usbRegs->DMA_CTRL[i].TXGCR // Tx Channel i Host Pkt Cfg Register
        = (CSL_USB_OTG_TXGCR_TX_ENABLE_MASK
            | QUEUE_TX_COMPLETE);
  }
  
  // prepare each descriptor with data buffer, and register it to the RX_ready or Free queue. 
  for(int i(0); i < sizeof(usb0_descriptor_buf0) / sizeof(usb0_descriptor_buf0[0]); i++){
    dma_push_queue(QUEUE_UNASSIGNED, usb0_descriptor_buf0[i]);
  }
}

void USB_OTG_FS::init_dma(){
  const Uint16 rx_submit_queues[ENDPOINTS] 
      = {QUEUE_RX_READY_OR_FREE, QUEUE_RX_READY_OR_FREE, QUEUE_RX_READY_OR_FREE, QUEUE_RX_READY_OR_FREE};
  const Uint16 rx_complete_queues[ENDPOINTS] 
      = {QUEUE_RX_COMPLETE, QUEUE_RX_COMPLETE, QUEUE_RX_COMPLETE, QUEUE_RX_COMPLETE};
  init_dma(rx_submit_queues, rx_complete_queues);
}

#define access_atr(ctype, fname, shift, length) \
Uint32 USB_OTG_FS::DMA_HostDescriptor::ctype::fname() const { \
  return (word & mask<shift, length>()) >> shift; \
} \
void USB_OTG_FS::DMA_HostDescriptor::ctype::fname(const Uint32 &v){ \
  word &= ~mask<shift, length>(); \
  word |= (v << shift); \
}

access_atr(word0_t, descriptor_type, 27, 5);
access_atr(word0_t, protocol_specific_valid_word_count, 22, 5);
access_atr(word0_t, packet_length, 0, 22);

access_atr(word1_t, src_port, 27, 5);
access_atr(word1_t, src_channel, 21, 6);
access_atr(word1_t, src_subchannel, 16, 5);
access_atr(word1_t, dst, 0, 16);

access_atr(word2_t, error, 31, 1);
access_atr(word2_t, type, 26, 5);
access_atr(word2_t, is_zero_length, 19, 1);
access_atr(word2_t, protocol_specific, 16, 3);
access_atr(word2_t, return_policy, 15, 1);
access_atr(word2_t, is_on_chip, 14, 1);
access_atr(word2_t, return_queue_mgr, 12, 2);
access_atr(word2_t, return_queue, 0, 12);
#undef access_atr
  
void USB_OTG_FS::DMA_HostDescriptor::init_common(
    const Uint16 &return_queue, Uint32 *buf, const Uint32 &buf_length){
  word0.protocol_specific_valid_word_count(0);

  // This is actual Packet Length. It will be populated by the Rx Port of the CPPI DMA
  word1.src_subchannel(0); //Always programmed to ZERO.
  word1.dst(0); //Always programmed to ZERO.
  
  word2.return_queue(return_queue); //24 and 25 for Tx - 26 and 27 for Rx Completion
  word2.return_queue_mgr(0);
  word2.is_on_chip(1); // Descriptor is located On-Chip(1) (or Off-chip(0))
  word2.return_policy(0);
  word2.protocol_specific(0);
  word2.error(0);

  word3_buf_length = buf_length;
  word4_buf_address = (Uint32)buf;
  word5_next_ptr = 0; //Current Descriptor is the Last Descriptor: Null Value is used as the Next Buffer Descriptor Address
  word6_org_buf_length = word3_buf_length;
  word7_org_buf_address = word4_buf_address;
}

void USB_OTG_FS::DMA_HostDescriptor::init_tx_packet(
    const Uint32 &packet_length, const Uint8 &endpoint,
    const Uint16 &return_queue, Uint32 *buf, const Uint32 &buf_length){

  word0.descriptor_type(16); // This value is always fixed. For Packet Type Decriptors = 16
  // Packet Length
  // For Transparent Mode this is <= Tx/RxMaxP Value.
  // For RNDIS or like it is = Tx/RxMaxP Value. This is the size of the Packet noted at descriptor level to be Transmitted.
  word0.packet_length(packet_length);
  // This is different from the USB Max Packet Size. This is the total data length.
  word1.src_port(endpoint); // Ports[1,2,3,4] is associated with Endpoints[1,2,3,4] respectively.
  word1.src_channel(endpoint - 1);
  word2.type(5); // USB Packet ID is 5
  word2.is_zero_length((packet_length > 0) ? 0 : 1);

  init_common(return_queue, buf, buf_length);
}

void USB_OTG_FS::DMA_HostDescriptor::init_tx_packet(
    const Uint32 &packet_length, const Uint8 &endpoint,
    const Uint16 &return_queue){
  init_tx_packet(packet_length, endpoint, return_queue, (Uint32 *)word7_org_buf_address, word6_org_buf_length);
}

void USB_OTG_FS::DMA_HostDescriptor::init_tx_buffer(
    DMA_HostDescriptor &previous,
    const Uint16 &return_queue, Uint32 *buf, const Uint32 &buf_length){

  // Word0, Word1, and Half of Word2 are Reserved for Buffer Descriptors.
  // This and other Packet related info will be updated by the PORT for Receive and can be any value.
  word0.word = 0;
  word1.word = 0;
  word2.word = 0;

  init_common(return_queue, buf, buf_length);

  previous.word5_next_ptr = (Uint32)this; //Modify Previous Link Address
}

void USB_OTG_FS::DMA_HostDescriptor::init_tx_buffer(
    DMA_HostDescriptor &previous,
    const Uint16 &return_queue){
  init_tx_buffer(previous, return_queue, (Uint32 *)word7_org_buf_address, word6_org_buf_length);
}

void USB_OTG_FS::DMA_HostDescriptor::init_rx_buffer(
    const Uint16 &return_queue, Uint32 *buf, const Uint32 &buf_length){

  // Word0, Word1, and Half of Word2 are Reserved for Buffer Descriptors.
  // This and other Packet related info will be updated by the PORT for Receive and can be any value.
  word0.word = 0;
  word1.word = 0;
  word2.word = 0;

  init_common(return_queue, buf, buf_length);
}

void USB_OTG_FS::DMA_HostDescriptor::init_rx_buffer(
    const Uint16 &return_queue){
  init_rx_buffer(return_queue, (Uint32 *)word7_org_buf_address, word6_org_buf_length);
}

void USB_OTG_FS::dma_push_queue(const Uint16 &queueNum, const void *descriptor) {
  usbRegs->QCTRL[queueNum].CTRLD
      = ((Uint32)descriptor & CSL_USB_OTG_CTRLD_DESC_PTR_MASK)
          | 0x2; // bits[4:0] = dec_size = [0-31] = [24,28,32,...,148]
}

void *USB_OTG_FS::dma_pop_queue(const Uint16 &queueNum) {
  return (void *)((usbRegs->QCTRL[queueNum].CTRLD) & CSL_USB_OTG_CTRLD_DESC_PTR_MASK);
}

void USB_OTG_FS::dma_enable(const Uint8 &ep_index) {
  Uint8 index(ep_index & SetupBuffer::DRR_MASK);
  if((index > ENDPOINTS) || (index == 0)){
    return;
  }
  if((ep_index & SetupBuffer::DRD_MASK) == SetupBuffer::DRD_IN){
    usbRegs->EPCSR[index].TXCSR.PERI_TXCSR
        &= ~CSL_USB_OTG_PERI_TXCSR_AUTOSET_MASK; // Clear AUTOSET
    usbRegs->EPCSR[index].TXCSR.PERI_TXCSR
        |= (CSL_USB_OTG_PERI_TXCSR_DMAEN_MASK | CSL_USB_OTG_PERI_TXCSR_DMAMODE_MASK); // Set DMAReqEnab & DMAReqMode
  }else{
    usbRegs->EPCSR[index].RXCSR.PERI_RXCSR
        &= ~(CSL_USB_OTG_PERI_RXCSR_AUTOCLEAR_MASK | CSL_USB_OTG_PERI_RXCSR_DMAMODE_MASK); // Clear AUTOCLEAR and DMAReqMode
    usbRegs->EPCSR[index].RXCSR.PERI_RXCSR
        |= CSL_USB_OTG_PERI_RXCSR_DMAEN_MASK; // Set DMAReqEnab
  }
}

void USB_OTG_FS::dma_disable(const Uint8 &ep_index) {
  Uint8 index(ep_index & SetupBuffer::DRR_MASK);
  if((index > ENDPOINTS) || (index == 0)){
    return;
  }
  if((ep_index & SetupBuffer::DRD_MASK) == SetupBuffer::DRD_IN){
    usbRegs->EPCSR[index].TXCSR.PERI_TXCSR
        &= ~CSL_USB_OTG_PERI_TXCSR_AUTOSET_MASK; // Clear AUTOSET
    usbRegs->EPCSR[index].TXCSR.PERI_TXCSR
        &= ~(CSL_USB_OTG_PERI_TXCSR_DMAEN_MASK | CSL_USB_OTG_PERI_TXCSR_DMAMODE_MASK); // Clear DMAReqEnab & DMAReqMode
  }else{
    usbRegs->EPCSR[index].RXCSR.PERI_RXCSR
        &= ~CSL_USB_OTG_PERI_RXCSR_DMAEN_MASK; // Clear DMAReqEnab
  }
}

#endif