#include "usb_otg.h"
#include "usb_defines.h"

USB_OTG::USB_OTG() : USB(), usbRegs(), address_changed(false){

}

USB_OTG::~USB_OTG(){}

void USB_OTG::disable_global_interrupt(){
  // @see USB_OTG_DisableGlobalInt()
  USB_OTG_GAHBCFG_TypeDef ahbcfg;
  ahbcfg.d32 = 0;
  ahbcfg.b.glblintrmsk = 1;
  USB_OTG_MODIFY_REG32(&usbRegs.GREGS->GAHBCFG, ahbcfg.d32, 0);
}

void USB_OTG::enable_global_interrupt(){
  // @see USB_OTG_EnableGlobalInt()
  USB_OTG_GAHBCFG_TypeDef ahbcfg;
  ahbcfg.d32 = 0;
  ahbcfg.b.glblintrmsk = 1;
  USB_OTG_MODIFY_REG32(&usbRegs.GREGS->GAHBCFG, 0, ahbcfg.d32);
}

void USB_OTG::enable_common_interrupt(){
  // @see USB_OTG_EnableCommonInt(), Enable the common interrupts
#ifndef USE_OTG_MODE
  // Clear any pending USB_OTG Interrupts
  USB_OTG_WRITE_REG32(&usbRegs.GREGS->GOTGINT, 0xFFFFFFFF);
#endif

  // Clear any pending interrupts
  USB_OTG_WRITE_REG32(&usbRegs.GREGS->GINTSTS, 0xFFFFFFFF);

  // Enable the interrupts in the INTMSK
  USB_OTG_GINTMSK_TypeDef  int_mask;
  int_mask.d32 = 0;
  int_mask.b.wkupintr = 1;
  int_mask.b.usbsuspend = 1;
#ifdef USE_OTG_MODE
  int_mask.b.otgintr = 1;
  int_mask.b.sessreqintr = 1;
  int_mask.b.conidstschng = 1;
#endif
  USB_OTG_WRITE_REG32(&usbRegs.GREGS->GINTMSK, int_mask.d32);
}

void USB_OTG::enable_device_interrupt(){
  // @see USB_OTG_EnableDevInt

  USB_OTG_WRITE_REG32(&usbRegs.GREGS->GINTMSK, 0); // Disable all interrupts
  USB_OTG_WRITE_REG32(&usbRegs.GREGS->GINTSTS, 0xFFFFFFFF); // Clear any pending interrupts

  enable_common_interrupt();

  //if(pdev->cfg.dma_enable == 0){intmsk.b.rxstsqlvl = 1;}

  // Enable interrupts matching to the Device mode ONLY
  USB_OTG_GINTMSK_TypeDef  intmsk;
  intmsk.d32 = 0;
  intmsk.b.usbsuspend = 1;
  intmsk.b.usbreset = 1;
  intmsk.b.enumdone = 1;
  intmsk.b.inepintr = 1;
  intmsk.b.outepintr = 1;
  intmsk.b.sofintr = 1;
  intmsk.b.incomplisoin = 1;
  intmsk.b.incomplisoout = 1;
#ifdef VBUS_SENSING_ENABLE
  intmsk.b.sessreqintr = 1;
  intmsk.b.otgintr = 1;
#endif
  USB_OTG_MODIFY_REG32(&usbRegs.GREGS->GINTMSK, intmsk.d32, intmsk.d32);
}

void USB_OTG::set_current_mode(const bool &is_device){
  // @see USB_OTG_SetCurrentMode()
  USB_OTG_GUSBCFG_TypeDef usbcfg;
  usbcfg.d32 = USB_OTG_READ_REG32(&usbRegs.GREGS->GUSBCFG);
  if(is_device){
    usbcfg.b.force_host = 0;
    usbcfg.b.force_dev = 1;
  }else{
    usbcfg.b.force_host = 1;
    usbcfg.b.force_dev = 0;
  }
  USB_OTG_WRITE_REG32(&usbRegs.GREGS->GUSBCFG, usbcfg.d32);
  // USB_OTG_BSP_mDelay(50);
}

void USB_OTG::set_device_speed(const Uint8 &devspd){
  // @see USB_OTG_InitDevSpeed()
  USB_OTG_DCFG_TypeDef dcfg;
  dcfg.d32 = USB_OTG_READ_REG32(&usbRegs.DREGS->DCFG);
  dcfg.b.devspd = devspd; // Full speed
  USB_OTG_WRITE_REG32(&usbRegs.DREGS->DCFG, dcfg.d32);
}

void USB_OTG::flush_fifo(__IO USB_OTG_GRSTCTL_TypeDef &greset){
  USB_OTG_WRITE_REG32(&usbRegs.GREGS->GRSTCTL, greset.d32);
  for(Uint32 count(0); count < 200000; count++){
    greset.d32 = USB_OTG_READ_REG32(&usbRegs.GREGS->GRSTCTL);
    if((greset.b.txfflsh != 1) && (greset.b.rxfflsh != 1)){break;}
  }
  //USB_OTG_BSP_uDelay(3); // Wait for 3 PHY Clocks
}
void USB_OTG::flush_tx_fifo(const Uint8 &txfnum){
  // @see USB_OTG_FlushTxFifo()
  __IO USB_OTG_GRSTCTL_TypeDef greset;
  greset.d32 = 0;
  greset.b.txfflsh = 1;
  greset.b.txfnum = txfnum;
  flush_fifo(greset);
}
void USB_OTG::flush_rx_fifo(){
  // @see USB_OTG_FlushRxFifo()
  __IO USB_OTG_GRSTCTL_TypeDef greset;
  greset.d32 = 0;
  greset.b.rxfflsh = 1;
  flush_fifo(greset);
}

void USB_OTG::reset_core(){
  __IO USB_OTG_GRSTCTL_TypeDef greset;

  /* Wait for AHB master IDLE state. */
  greset.d32 = USB_OTG_READ_REG32(&usbRegs.GREGS->GRSTCTL);
  for(Uint32 count(0); greset.b.ahbidle != 0; count++){
    //USB_OTG_BSP_uDelay(3);
    greset.d32 = USB_OTG_READ_REG32(&usbRegs.GREGS->GRSTCTL);
    if(count > 200000){return;}
  }

  /* Core Soft Reset */
  greset.b.csftrst = 1;
  USB_OTG_WRITE_REG32(&usbRegs.GREGS->GRSTCTL, greset.d32);
  for(Uint32 count(0); count < 200000; count++){
    greset.d32 = USB_OTG_READ_REG32(&usbRegs.GREGS->GRSTCTL);
    if(greset.b.csftrst != 1){break;}
  }

  //USB_OTG_BSP_uDelay(3); // Wait for 3 PHY Clocks
  address_changed = false;
}

USB_OTG::Uint8 USB_OTG::get_address() const {
  USB_OTG_DCFG_TypeDef  dcfg;
  dcfg.d32 = USB_OTG_READ_REG32(&usbRegs.DREGS->DCFG);
  return (Uint8)dcfg.b.devaddr;
}
USB_OTG::Uint8 USB_OTG::set_address(const Uint8 &new_address){
  USB_OTG_DCFG_TypeDef  dcfg;
  dcfg.d32 = USB_OTG_READ_REG32(&usbRegs.DREGS->DCFG);
  dcfg.b.devaddr = new_address;
  USB_OTG_WRITE_REG32(&usbRegs.DREGS->DCFG, dcfg.d32);
  address_changed = true;
  return new_address;
}