#include "config.h"
#include "usb_otg_fs_impl.h"

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

#include <cstring>

#include "util/bits_counter.h"

const Uint8 USB_OTG_FS_Impl::DESC_STRING0[] = {
    0x00, // <= length, must be modified in the initialize stage.
    USB::DSC_TYPE_STRING,  
    0x09, 0x04
    };

const Uint8 USB_OTG_FS_Impl::DESC_STRING1[] = {
    0x00, // <= length, must be modified in the initialize stage.
    USB::DSC_TYPE_STRING,  
    'Y', 0,
    'O', 0,
    'U', 0,
    ' ', 0,
    'S', 0,
    'y', 0,
    's', 0,
    't', 0,
    'e', 0,
    'm', 0,
    };

const Uint8 USB_OTG_FS_Impl::DESC_STRING2[] = {
    0x00, // <= length, must be modified in the initialize stage.
    USB::DSC_TYPE_STRING,  
    'T', 0,
    'i', 0,
    'n', 0,
    'y', 0,
    ' ', 0,
    'F', 0,
    'e', 0,
    'a', 0,
    't', 0,
    'h', 0,
    'e', 0,
    'r', 0,
    };

const Uint8 USB_OTG_FS_Impl::DESC_STRING3[] = {
    0x00, // <= length, must be modified in the initialize stage.
    USB::DSC_TYPE_STRING,  
    'R', 0,
    'e', 0,
    'v', 0,
    '.', 0,
    'A', 0,
    };

#define Uint16L(x) (Uint8)(x & 0xFF)
#define Uint16H(x) (Uint8)((x >> 8) & 0xFF)

const USB::DeviceDescriptor USB_OTG_FS_Impl::DESC_DEVICE = {
    sizeof(USB::DeviceDescriptor),
    USB::DSC_TYPE_DEVICE,
    Uint16L(DESC_DEVICE_bcdUSB),
    Uint16H(DESC_DEVICE_bcdUSB),
    0x00, // bDeviceClass (TBD)
    0x00, // bDeviceSubClass
    0x00, // bDeviceProtocol
    USB_OTG_FS::EP0_PACKET_SIZE,
    Uint16L(DESC_DEVICE_idVendor),
    Uint16H(DESC_DEVICE_idVendor),
    0x00, // idProduct (TBD)
    0x00,
    0x00, // bcdDevice (TBD)
    0x00,
    DESC_DEVICE_iManufacturer,
    DESC_DEVICE_iProduct,
    DESC_DEVICE_iSerialNumber,
    0x01
    };

const USB::ConfigurationDescriptor USB_OTG_FS_Impl::DESC_CONFIG = {
    sizeof(USB::ConfigurationDescriptor),
    USB::DSC_TYPE_CONFIG,
    0x00, // Total Length (TBD)
    0x00,
    0x00, // Num of Interface (TBD)
    DESC_CONFIG_bConfigurationValue,    // bConfigurationValue
    DESC_CONFIG_iConfiguration,         // iConfiguration
    DESC_CONFIG_bmAttributes,           // bmAttributes
    DESC_CONFIG_MaxPower                // MaxPower
    };

const USB_OTG_FS_Impl::CDC_Related_Descriptor USB_OTG_FS_Impl::DESC_CDC = {
    {
        sizeof(USB::InterfaceDescriptor),
        USB::DSC_TYPE_INTERFACE,
        0x00, // Interface Number (TBD)
        0x00,
        0x01,
        0x02, // bInterfaceClass (Communication Class)
        0x02, // bInterfaceSubClass (Abstract Control Model)
        0x01, // bInterfaceProcotol (V.25ter, Common AT commands)
        0x00  // iInterface (TBD)
        },
    {
        sizeof(USB_CDC::HeaderFunctionDescriptor),
        USB_CDC::DSC_TYPE_CS_INTERFACE,
        USB_CDC::DSC_SUBTYPE_CS_HEADER_FUNC,
        Uint16L(0x0110), // bcdCDC (CDC spec release number, 1.1)
        Uint16H(0x0110)
        },
    {
        sizeof(USB_CDC::AbstractControlManagementFunctionalDescriptor),
        USB_CDC::DSC_TYPE_CS_INTERFACE,
        USB_CDC::DSC_SUBTYPE_CS_ABST_CNTRL,
        0x06 // bmCapabilities (Supports Send_Break, Set_Line_Coding, Set_Control_Line_State, Get_Line_Coding, and the notification Serial_State)
        },
    {
        sizeof(USB_CDC::UnionFunctionalDescriptor),
        USB_CDC::DSC_TYPE_CS_INTERFACE,
        USB_CDC::DSC_SUBTYPE_CS_UNION_FUNC,
        0x00, // bMasterInterface (Interface number master interface in the union) (TBD)
        0x01  // bSlaveInterface0 (Interface number slave interface in the union) (TBD)
        },
    {
        sizeof(USB_CDC::CallManagementFunctionalDescriptor),
        USB_CDC::DSC_TYPE_CS_INTERFACE,
        USB_CDC::DSC_SUBTYPE_CS_CALL_MAN,
        0x01, // bmCapabilities (only over Communication Class IF / handles itself)
        0x01  // bDataInterface (Interface number of Data Class interface) (TBD)
        },
    {
        sizeof(USB::EndpointDescriptor),
        USB::DSC_TYPE_ENDPOINT,
        USB::SetupBuffer::DRD_IN, // bEndpointAddress (TBD)
        USB::DSC_EP_INTERRUPT,
        Uint16L(0x40),
        Uint16H(0x40),
        0x10
        },
    {
        sizeof(USB::InterfaceDescriptor),
        USB::DSC_TYPE_INTERFACE,
        0x01, // Interface Number (TBD)
        0x00,
        0x02,
        0x0A, // bInterfaceClass (Data Interface Class)
        0x00, // bInterfaceSubClass
        0x00, // bInterfaceProcotol
        0x00  // iInterface (TBD)
        },
    {
        sizeof(USB::EndpointDescriptor),
        USB::DSC_TYPE_ENDPOINT,
        USB::SetupBuffer::DRD_IN, // bEndpointAddress (TBD)
        USB::DSC_EP_BULK, //USB::DSC_EP_INTERRUPT,
        Uint16L(0x40),
        Uint16H(0x40),
        0 //1
        },
    {
        sizeof(USB::EndpointDescriptor),
        USB::DSC_TYPE_ENDPOINT,
        USB::SetupBuffer::DRD_OUT, // bEndpointAddress (TBD)
        USB::DSC_EP_BULK,
        Uint16L(0x40),
        Uint16H(0x40),
        0
        }
    };

const USB_OTG_FS_Impl::FT232_Related_Descriptor USB_OTG_FS_Impl::DESC_FT232 = {
    {
        sizeof(USB::InterfaceDescriptor),
        USB::DSC_TYPE_INTERFACE,
        0x00, // Interface Number (TBD)
        0x00,
        0x02,
        0xFF,
        0xFF,
        0xFF,
        0x00  // iInterface (TBD)
        },
    {
        sizeof(USB::EndpointDescriptor),
        USB::DSC_TYPE_ENDPOINT,
        USB::SetupBuffer::DRD_IN, // bEndpointAddress (TBD)
        USB::DSC_EP_BULK,
        Uint16L(0x40),
        Uint16H(0x40),
        0
        },
    {
        sizeof(USB::EndpointDescriptor),
        USB::DSC_TYPE_ENDPOINT,
        USB::SetupBuffer::DRD_OUT, // bEndpointAddress (TBD)
        USB::DSC_EP_BULK,
        Uint16L(0x40),
        Uint16H(0x40),
        0
        }
    };

const USB_OTG_FS_Impl::MSC_Related_Descriptor USB_OTG_FS_Impl::DESC_MSC = {
    {
        sizeof(USB::InterfaceDescriptor),
        USB::DSC_TYPE_INTERFACE,
        0x00, // Interface Number (TBD)
        0x00,
        0x02,
        USB_MSC::DSC_INTERFACE_CLASS,
        USB_MSC::DSC_INTERFACE_SUBCLASS,
        USB_MSC::DSC_PROTOCOL,
        0x00  // iInterface (TBD)
        },
    {
        sizeof(USB::EndpointDescriptor),
        USB::DSC_TYPE_ENDPOINT,
        USB::SetupBuffer::DRD_IN, // bEndpointAddress (TBD)
        USB::DSC_EP_BULK,
        Uint16L(0x40),
        Uint16H(0x40),
        0
        },
    {
        sizeof(USB::EndpointDescriptor),
        USB::DSC_TYPE_ENDPOINT,
        USB::SetupBuffer::DRD_OUT, // bEndpointAddress (TBD)
        USB::DSC_EP_BULK,
        Uint16L(0x40),
        Uint16H(0x40),
        0
        }
    };

#undef Uint16L
#undef Uint16H

const Uint8 USB_OTG_FS_Impl::EP_INDEX_COMCTL = 1;
const Uint8 USB_OTG_FS_Impl::EP_INDEX_COMDATA = 2;
const Uint8 USB_OTG_FS_Impl::EP_INDEX_MSC = 3;

USB_OTG_FS_Impl::USB_OTG_FS_Impl()
    : USB_OTG_FS(false, true), descriptor_buf_cached(false), dma_ready(false),
    ft232(*this, EP_INDEX_COMDATA, EP_INDEX_COMDATA), 
    cdc(*this, EP_INDEX_COMCTL, EP_INDEX_COMDATA, EP_INDEX_COMDATA) {
  
}

USB_OTG_FS_Impl::~USB_OTG_FS_Impl(){}

int USB_OTG_FS_Impl::interfaces() const{
  if(USE_FTDI_MIMIC){
    return 1;
  }else{
    return 2;
  }
}

bool USB_OTG_FS_Impl::descriptor(
    const Uint16 wValue, const Uint16 wIndex, 
    unsigned char **buf, int *size){
  if(!descriptor_buf_cached){
    // Descriptorを組み立てる
    
    Uint8 *cache(descriptor_buf);
    
    if(USE_FTDI_MIMIC){
      // FT232もどきを組み立ててみる
      std::memcpy(cache, &DESC_DEVICE, sizeof(DESC_DEVICE));
      ((DeviceDescriptor *)cache)->idProduct_L = 0x96;
      ((DeviceDescriptor *)cache)->idProduct_H = 0x01;
      ((DeviceDescriptor *)cache)->bcdDevice_L = 0x00;
      ((DeviceDescriptor *)cache)->bcdDevice_H = 0x06;
      cache += sizeof(DESC_DEVICE);
      
      std::memcpy(cache, &DESC_CONFIG, sizeof(DESC_CONFIG));
      Uint16 length(
          sizeof(ConfigurationDescriptor)
          + sizeof(FT232_Related_Descriptor));
      ((ConfigurationDescriptor *)cache)->wTotalLength_L = (length & 0xFF);
      ((ConfigurationDescriptor *)cache)->wTotalLength_H = (length >> 8) & 0xFF;
      ((ConfigurationDescriptor *)cache)->bNumInterfaces = 1;
      cache += sizeof(DESC_CONFIG);
      
      std::memcpy(cache, &DESC_FT232, sizeof(DESC_FT232));
      // default => ((FT232_Related_Descriptor *)cache)->i0.bInterfaceNumber = 0;
      // default => ((FT232_Related_Descriptor *)cache)->i0.iInterface = 0; 
      ((FT232_Related_Descriptor *)cache)->e0.bEndpointAddress += EP_INDEX_COMDATA;
      ((FT232_Related_Descriptor *)cache)->e1.bEndpointAddress += EP_INDEX_COMDATA;
      cache += sizeof(DESC_FT232);
    }else{
      // CDC
      std::memcpy(cache, &DESC_DEVICE, sizeof(DESC_DEVICE));
      ((DeviceDescriptor *)cache)->idProduct_L = 0x98;
      ((DeviceDescriptor *)cache)->idProduct_H = 0x01;
      ((DeviceDescriptor *)cache)->bcdDevice_L = 0x00;
      ((DeviceDescriptor *)cache)->bcdDevice_H = 0x00;
      ((DeviceDescriptor *)cache)->bDeviceClass = 0x02; // bDeviceClass (Communication Class)
      cache += sizeof(DESC_DEVICE);
      
      std::memcpy(cache, &DESC_CONFIG, sizeof(DESC_CONFIG));
      Uint16 length(
          sizeof(ConfigurationDescriptor)
          + sizeof(CDC_Related_Descriptor));
      ((ConfigurationDescriptor *)cache)->wTotalLength_L = (length & 0xFF);
      ((ConfigurationDescriptor *)cache)->wTotalLength_H = (length >> 8) & 0xFF;
      ((ConfigurationDescriptor *)cache)->bNumInterfaces = 2;
      cache += sizeof(DESC_CONFIG);
      
      std::memcpy(cache, &DESC_CDC, sizeof(DESC_CDC));
      // default => ((CDC_Related_Descriptor *)cache)->i0.bInterfaceNumber = 0;
      // default => ((CDC_Related_Descriptor *)cache)->i0.iInterface = 0;
      ((CDC_Related_Descriptor *)cache)->e0.bEndpointAddress += EP_INDEX_COMCTL;
      // default => ((CDC_Related_Descriptor *)cache)->i1.bInterfaceNumber = 1;
      // default => ((CDC_Related_Descriptor *)cache)->i1.iInterface = 0;
      ((CDC_Related_Descriptor *)cache)->e1.bEndpointAddress += EP_INDEX_COMDATA;
      ((CDC_Related_Descriptor *)cache)->e2.bEndpointAddress += EP_INDEX_COMDATA;
      cache += sizeof(DESC_CDC);
    }
    
    std::memcpy(&descriptor_buf[0x200], DESC_STRING0, sizeof(DESC_STRING0));
    descriptor_buf[0x200] = sizeof(DESC_STRING0);
    std::memcpy(&descriptor_buf[0x280], DESC_STRING1, sizeof(DESC_STRING1));
    descriptor_buf[0x280] = sizeof(DESC_STRING1);
    std::memcpy(&descriptor_buf[0x300], DESC_STRING2, sizeof(DESC_STRING2));
    descriptor_buf[0x300] = sizeof(DESC_STRING2);
    std::memcpy(&descriptor_buf[0x380], DESC_STRING3, sizeof(DESC_STRING3));
    descriptor_buf[0x380] = sizeof(DESC_STRING3);
    
    BCACHE_wb(descriptor_buf, sizeof(descriptor_buf), true);
    descriptor_buf_cached = true;
  }
  
  switch(wValue >> 8){
    case DSC_TYPE_DEVICE:
      if((wValue & 0xFF) > 0){return false;}
      *buf = descriptor_buf;
      *size = ((DeviceDescriptor *)(*buf))->bLength;
      break;
    case DSC_TYPE_CONFIG:
      if((wValue & 0xFF) > 0){return false;}
      *buf = descriptor_buf + ((DeviceDescriptor *)descriptor_buf)->bLength;
      *size = ((ConfigurationDescriptor *)(*buf))->wTotalLength_L
          | ((((ConfigurationDescriptor *)(*buf))->wTotalLength_H) << 8);
      break;
    case DSC_TYPE_STRING:
      if((wValue & 0xFF) >= 4){return false;}
      *buf = &descriptor_buf[0x200 + ((wValue & 0xFF) * 0x80)];
      *size = **buf;
      break;
    default:
      return false;
  }
  
  return true;
}

enum {
  QUEUE_RX1_SUBMIT = 8,
  QUEUE_RX2_SUBMIT = 10,
  QUEUE_RX3_SUBMIT = 12,
  QUEUE_RX4_SUBMIT = 14,
  QUEUE_COMDATA_TX_FREE = USB_OTG_FS::QUEUE_UNASSIGNED + 1,
  QUEUE_COMDATA_RX_COMPLETE = USB_OTG_FS::QUEUE_UNASSIGNED + 2,
};

static const Uint16 dma_queues_rx[] = {
    0,
    QUEUE_RX1_SUBMIT,
    QUEUE_RX2_SUBMIT,
    QUEUE_RX3_SUBMIT,
    QUEUE_RX4_SUBMIT,};

static const Uint16 dma_queues_tx[] = {
    0,
    USB_OTG_FS::QUEUE_TX1_SUBMIT,
    USB_OTG_FS::QUEUE_TX2_SUBMIT,
    USB_OTG_FS::QUEUE_TX3_SUBMIT,
    USB_OTG_FS::QUEUE_TX4_SUBMIT,};

static USB_OTG_FS::DMA_HostDescriptor *rx_completed_descriptor_head[USB_OTG_FS::ENDPOINTS + 1];

static const int dma_tx_descriptors = 16;
static const int dma_rx_descriptors = 16;

#pragma DATA_ALIGN(0x100)
static Uint8 dma_buf[dma_tx_descriptors + dma_rx_descriptors][0x40];

void USB_OTG_FS_Impl::init(){
  USB_OTG_FS::init();
  
  Uint16 rx_submit_queues[] 
      = {QUEUE_RX1_SUBMIT, QUEUE_RX2_SUBMIT, QUEUE_RX3_SUBMIT, QUEUE_RX4_SUBMIT};
  Uint16 rx_complete_queues[] 
      = {QUEUE_RX_COMPLETE, QUEUE_RX_COMPLETE, QUEUE_RX_COMPLETE, QUEUE_RX_COMPLETE};
  rx_complete_queues[EP_INDEX_COMDATA - 1] = QUEUE_COMDATA_RX_COMPLETE;
  init_dma(rx_submit_queues, rx_complete_queues);
}

bool USB_OTG_FS_Impl::setup(const int &configuration){
  
  unsigned int fifo_occupied_units(8); // FIFO address. Leave 64-bytes for endpoint 0.
  
  if(USE_FTDI_MIMIC){
    // FT232のセットアップ
    usbRegs->INDEX = EP_INDEX_COMDATA;
  
    usbRegs->EPCSR[EP_INDEX_COMDATA].RXMAXP = DESC_FT232.e1.wMaxPacketSize_L
        | (DESC_FT232.e1.wMaxPacketSize_H << 8);
    usbRegs->EPCSR[EP_INDEX_COMDATA].TXMAXP = DESC_FT232.e0.wMaxPacketSize_L
        | (DESC_FT232.e0.wMaxPacketSize_H << 8);
    usbRegs->EPCSR[EP_INDEX_COMDATA].TXCSR.PERI_TXCSR |= CSL_USB_OTG_PERI_TXCSR_MODE_MASK;
    usbRegs->EPCSR[EP_INDEX_COMDATA].RXCSR.PERI_RXCSR |= CSL_USB_OTG_PERI_RXCSR_AUTOCLEAR_MASK;
    
    // set FIFO size & enable double buffering
    usbRegs->RXFIFOSZ = BitsCounter::ntz(usbRegs->EPCSR[EP_INDEX_COMDATA].RXMAXP >> 3) | CSL_USB_OTG_RXFIFOSZ_DPB_MASK;
    usbRegs->RXFIFOADDR = fifo_occupied_units;
    fifo_occupied_units += (usbRegs->EPCSR[EP_INDEX_COMDATA].RXMAXP >> (3 - 1)); // double buffer
    usbRegs->TXFIFOSZ = BitsCounter::ntz(usbRegs->EPCSR[EP_INDEX_COMDATA].TXMAXP >> 3) | CSL_USB_OTG_TXFIFOSZ_DPB_MASK;
    usbRegs->TXFIFOADDR = fifo_occupied_units;
    
    set_ep_state(DESC_FT232.e0.bEndpointAddress + EP_INDEX_COMDATA, EP_IDLE);
    set_ep_state(DESC_FT232.e1.bEndpointAddress + EP_INDEX_COMDATA, EP_IDLE);
  }else{
    // CDCのセットアップ
    usbRegs->INDEX = EP_INDEX_COMCTL;
    
    usbRegs->EPCSR[EP_INDEX_COMCTL].TXMAXP = DESC_CDC.e0.wMaxPacketSize_L
        | (DESC_CDC.e0.wMaxPacketSize_H << 8);
    usbRegs->EPCSR[EP_INDEX_COMCTL].TXCSR.PERI_TXCSR |= CSL_USB_OTG_PERI_TXCSR_MODE_MASK;
    
    usbRegs->TXFIFOSZ = BitsCounter::ntz(usbRegs->EPCSR[EP_INDEX_COMCTL].TXMAXP >> 3) | CSL_USB_OTG_TXFIFOSZ_DPB_MASK;
    usbRegs->TXFIFOADDR = fifo_occupied_units;
    fifo_occupied_units += (usbRegs->EPCSR[EP_INDEX_COMCTL].TXMAXP >> (3 - 1)); // double buffer
    
    usbRegs->INDEX = EP_INDEX_COMDATA;
  
    usbRegs->EPCSR[EP_INDEX_COMDATA].RXMAXP = DESC_CDC.e2.wMaxPacketSize_L
        | (DESC_CDC.e2.wMaxPacketSize_H << 8);
    usbRegs->EPCSR[EP_INDEX_COMDATA].TXMAXP = DESC_CDC.e1.wMaxPacketSize_L
        | (DESC_CDC.e1.wMaxPacketSize_H << 8);
    usbRegs->EPCSR[EP_INDEX_COMDATA].TXCSR.PERI_TXCSR |= CSL_USB_OTG_PERI_TXCSR_MODE_MASK;
    usbRegs->EPCSR[EP_INDEX_COMDATA].RXCSR.PERI_RXCSR |= CSL_USB_OTG_PERI_RXCSR_AUTOCLEAR_MASK;
    
    // set FIFO size & enable double buffering
    usbRegs->RXFIFOSZ = BitsCounter::ntz(usbRegs->EPCSR[EP_INDEX_COMDATA].RXMAXP >> 3) | CSL_USB_OTG_RXFIFOSZ_DPB_MASK;
    usbRegs->RXFIFOADDR = fifo_occupied_units;
    fifo_occupied_units += (usbRegs->EPCSR[EP_INDEX_COMDATA].RXMAXP >> (3 - 1));  // double buffer; 
    usbRegs->TXFIFOSZ = BitsCounter::ntz(usbRegs->EPCSR[EP_INDEX_COMDATA].TXMAXP >> 3) | CSL_USB_OTG_TXFIFOSZ_DPB_MASK;
    usbRegs->TXFIFOADDR = fifo_occupied_units;
    
    set_ep_state(DESC_CDC.e0.bEndpointAddress + EP_INDEX_COMCTL, EP_IDLE);
    set_ep_state(DESC_CDC.e1.bEndpointAddress + EP_INDEX_COMDATA, EP_IDLE);
    set_ep_state(DESC_CDC.e2.bEndpointAddress + EP_INDEX_COMDATA, EP_IDLE);
  }
  
  // DMA setup
  {
    for(int i(0); i < sizeof(rx_completed_descriptor_head) / sizeof(rx_completed_descriptor_head[0]); i++){
      rx_completed_descriptor_head[i] = 0;
    }
    
    USB_OTG_FS::DMA_HostDescriptor *descriptor;

    int buf_index(0);

    // for COMDATA_IN
    for(int i(0); i < dma_tx_descriptors; i++, buf_index++){
      if(!(descriptor = static_cast<USB_OTG_FS::DMA_HostDescriptor *>(dma_pop_queue(QUEUE_UNASSIGNED)))){
        break;
      }
      descriptor->word6_org_buf_length = sizeof(dma_buf[0]) / sizeof(dma_buf[0][0]);
      descriptor->word7_org_buf_address = (Uint32)dma_buf[buf_index];
      dma_push_queue(QUEUE_COMDATA_TX_FREE, descriptor);
    }
    set_dma_protocol(SetupBuffer::DRD_IN | EP_INDEX_COMDATA, USB_OTG_FS::TRANSPARENT);
    dma_enable(SetupBuffer::DRD_IN | EP_INDEX_COMDATA);

    // for COMDATA_OUT
    for(int i(0); i < dma_rx_descriptors; i++, buf_index++){ // for RX
      if(!(descriptor = static_cast<USB_OTG_FS::DMA_HostDescriptor *>(dma_pop_queue(QUEUE_UNASSIGNED)))){
        break;
      }
      descriptor->init_rx_buffer(
          QUEUE_COMDATA_RX_COMPLETE, 
          (Uint32 *)dma_buf[buf_index], sizeof(dma_buf[0]) / sizeof(dma_buf[0][0]));
      dma_push_queue(dma_queues_rx[EP_INDEX_COMDATA], descriptor);
    }
    set_dma_protocol(SetupBuffer::DRD_OUT | EP_INDEX_COMDATA, USB_OTG_FS::TRANSPARENT);
    dma_enable(SetupBuffer::DRD_OUT | EP_INDEX_COMDATA);

    dma_ready = true;
  }

  return true;
}

bool USB_OTG_FS_Impl::class_request_handle(const SetupBuffer &buf){
  switch(buf.wIndex){
    case 0:
      if(!USE_FTDI_MIMIC){
        return cdc.class_request_handler.handle(buf);
      }
  }
  return false;
}
bool USB_OTG_FS_Impl::vendor_request_handle(const SetupBuffer &buf){
  if(USE_FTDI_MIMIC){
    return ft232.vendor_request_handler.handle(buf);
  }
  return false;
}

Uint16 USB_OTG_FS_Impl::read(const Uint8 &out_index, void *buf, const Uint16 &size){
  if(dma_ready && (out_index == EP_INDEX_COMDATA)){
    Uint16 remain(size);
    Uint8 *_buf((Uint8 *)buf);
    while(remain){
      if(!rx_completed_descriptor_head[out_index]){
        rx_completed_descriptor_head[out_index]
            = static_cast<USB_OTG_FS::DMA_HostDescriptor *>(dma_pop_queue(QUEUE_COMDATA_RX_COMPLETE));
        if(!rx_completed_descriptor_head[out_index]){
          break;
        }
        BCACHE_inv(
            (void *)(rx_completed_descriptor_head[out_index]->word7_org_buf_address), 
            rx_completed_descriptor_head[out_index]->word3_buf_length, 
            true);
      }
      if(rx_completed_descriptor_head[out_index]->word3_buf_length > remain){
        std::memcpy(_buf, 
            (void *)(rx_completed_descriptor_head[out_index]->word4_buf_address),
            remain);
        rx_completed_descriptor_head[out_index]->word3_buf_length -= remain;
        rx_completed_descriptor_head[out_index]->word4_buf_address += remain;
        remain = 0;
        break;
      }else{
        std::memcpy(_buf, 
            (void *)(rx_completed_descriptor_head[out_index]->word4_buf_address),
            rx_completed_descriptor_head[out_index]->word3_buf_length);
        _buf += rx_completed_descriptor_head[out_index]->word3_buf_length;
        remain -= rx_completed_descriptor_head[out_index]->word3_buf_length;
        rx_completed_descriptor_head[out_index]->init_rx_buffer(QUEUE_COMDATA_RX_COMPLETE);
        dma_push_queue(
            dma_queues_rx[out_index], rx_completed_descriptor_head[out_index]);
        rx_completed_descriptor_head[out_index] = 0;
      }
    }
    return size - remain;
  }
  return USB_OTG_FS::read(out_index, buf, size);
}
Uint16 USB_OTG_FS_Impl::read_skip(const Uint8 &out_index, const Uint16 &size){
  if(dma_ready && (out_index == EP_INDEX_COMDATA)){
    Uint16 remain(size);
    while(remain){
      if(!rx_completed_descriptor_head[out_index]){
        rx_completed_descriptor_head[out_index]
            = static_cast<USB_OTG_FS::DMA_HostDescriptor *>(dma_pop_queue(QUEUE_COMDATA_RX_COMPLETE));
        if(!rx_completed_descriptor_head[out_index]){
          break;
        }
      }
      if(rx_completed_descriptor_head[out_index]->word3_buf_length > remain){
        rx_completed_descriptor_head[out_index]->word3_buf_length -= remain;
        rx_completed_descriptor_head[out_index]->word4_buf_address += remain;
        remain = 0;
        break;
      }else{
        remain -= rx_completed_descriptor_head[out_index]->word3_buf_length;
        rx_completed_descriptor_head[out_index]->init_rx_buffer(QUEUE_COMDATA_RX_COMPLETE);
        dma_push_queue(
            dma_queues_rx[out_index], rx_completed_descriptor_head[out_index]);
        rx_completed_descriptor_head[out_index] = 0;
      }
    }
    return size - remain;
  }
  return USB_OTG_FS::read_skip(out_index, size);
}

Uint16 USB_OTG_FS_Impl::write(const Uint8 &in_index, const void *buf, const Uint16 &size){
  if(dma_ready && (in_index == EP_INDEX_COMDATA)){
    USB_OTG_FS::DMA_HostDescriptor *descriptor(
        static_cast<USB_OTG_FS::DMA_HostDescriptor *>(dma_pop_queue(QUEUE_COMDATA_TX_FREE)));
    if(!descriptor){return 0;}
    Uint16 res(size);
    if(res > descriptor->word6_org_buf_length){
      res = descriptor->word6_org_buf_length;
    }
    std::memcpy((void *)(descriptor->word7_org_buf_address), buf, res);
    BCACHE_wb((void *)(descriptor->word7_org_buf_address), res, true);
    descriptor->init_tx_packet(res, in_index, QUEUE_COMDATA_TX_FREE);
    dma_push_queue(dma_queues_tx[in_index], descriptor);
    return res;
  }
  return USB_OTG_FS::write(in_index, buf, size);
}
Uint16 USB_OTG_FS_Impl::write_fill(const Uint8 &in_index, const Uint8 c, const Uint16 &size){
  if(dma_ready && (in_index == EP_INDEX_COMDATA)){
    USB_OTG_FS::DMA_HostDescriptor *descriptor(
        static_cast<USB_OTG_FS::DMA_HostDescriptor *>(dma_pop_queue(QUEUE_COMDATA_TX_FREE)));
    if(!descriptor){return 0;}
    Uint16 res(size);
    if(res > descriptor->word6_org_buf_length){
      res = descriptor->word6_org_buf_length;
    }
    std::memset((void *)(descriptor->word7_org_buf_address), c, res);
    BCACHE_wb((void *)(descriptor->word7_org_buf_address), res, true);
    descriptor->init_tx_packet(res, in_index, QUEUE_COMDATA_TX_FREE);
    dma_push_queue(dma_queues_tx[in_index], descriptor);
    return res;
  }
  return USB_OTG_FS::write_fill(in_index, c, size);
}

USB_OTG_FS_Impl usb_otg_fs_impl;

#ifdef __cplusplus
extern "C" {
#endif
void usb0_isr(){
  usb0_otg_impl.handle_isr();
}
#ifdef __cplusplus
}
#endif