#include "usb_drv.h"

#include "flatUSB_config.h"

#include "stm32f4xx_hal.h"

#include "usb_common.h"
#include "usb_common_defs.h"

// ------------------------

#ifdef USB_HIGH_SPEED
__weak void usbdrv_ulpi_init() {
    return;
}
#endif

// ------------------------

// initialize USB peripheral
void usbdrv_periph_init() {
    USB_CLOCK_ENABLE();

    // HAL_Delay(1000);

    //__HAL_RCC_USB_OTG_FS_ULPI_CLK_ENABLE();
    //__HAL_RCC_USB_OTG_FS_FORCE_RESET();
    //__HAL_RCC_USB_OTG_FS_RELEASE_RESET();

#if defined(USB_STM32H7)
    SET_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_PHYSEL); // select the internal FS PHY

    WAIT_FOR_nBIT_DELAY(USBG->GRSTCTL, USB_OTG_GRSTCTL_AHBIDL, 1);

    SET_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_CSRST); // reset the USB core
    HAL_Delay(1);
    WAIT_FOR_BIT_DELAY(USBG->GRSTCTL, USB_OTG_GRSTCTL_CSRST, 1);
#else

#if defined(USB_HIGH_SPEED)
    CLEAR_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_PHYSEL); // select the external HS PHY
    CLEAR_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_TSDPS);
    CLEAR_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_ULPIFSLS);

    CLEAR_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_ULPIEVBUSD);
    CLEAR_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_ULPIEVBUSI);
    // SET_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_PHYSEL);

    SET_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_CSRST); // reset USB core
    HAL_Delay(1);
    WAIT_FOR_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_CSRST);

    usbdrv_ulpi_init(); // initialize PHY
#endif

#endif
    CLEAR_BIT(USBG->GCCFG, USB_OTG_GCCFG_PWRDWN); // power the internal transceiver peripheral

    CLEAR_BIT(USBG->GAHBCFG, USB_OTG_GAHBCFG_GINT);                            // mask all interrupts for now
    CLEAR_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_HNPCAP | USB_OTG_GUSBCFG_SRPCAP); // disable HNP and SRP
    WRITE_FIELD(USBG->GUSBCFG, USB_OTG_GUSBCFG_TRDT, TRDT_VALUE);              // set TRDT according to the RM
    // WRITE_FIELD(USBG->GUSBCFG, USB_OTG_GUSBCFG_TOCAL, TOCAL_VALUE);            // set TOCAL
    CLEAR_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_FHMOD); // clear Host mode forcing
    SET_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_FDMOD);   // force Device mode

    HAL_Delay(25); // wait for Device mode forcing propagation
    WAIT_FOR_BIT(USBG->GINTSTS, 0b1);

    // SET_BIT(USBD->DCTL, USB_OTG_DCTL_SDIS); // soft disconnect peripheral (should be upper, but since it's controlled by a Device register, cannot be set before switching to device mode)

#if defined(USB_STM32H7)
    CLEAR_BIT(USBG->GCCFG, USB_OTG_GCCFG_VBDEN);                                // turn off VBUSSENSE
    SET_BIT(USBG->GOTGCTL, USB_OTG_GOTGCTL_BVALOEN | USB_OTG_GOTGCTL_BVALOVAL); // force B-session
#elif defined(USB_STM32F4)
    SET_BIT(USBG->GCCFG, USB_OTG_GCCFG_NOVBUSSENS); // turn off VBUSSENSE
    CLEAR_BIT(USBG->GCCFG, USB_OTG_GCCFG_VBUSBSEN | USB_OTG_GCCFG_VBUSASEN);
#endif

    // HAL_Delay(50); // it takes time to forcing Device mode takes effect

#ifdef USB_HIGH_SPEED
    // WRITE_FIELD(USBD->DCFG, USB_OTG_DCFG_DSPD, USB_LINESPEED_FULL_SPEED);
    // WRITE_FIELD(USBD->DCFG, USB_OTG_DCFG_DSPD, USB_LINESPEED_HIGH_SPEED_ULPI);
    WRITE_FIELD(USBD->DCFG, USB_OTG_DCFG_DSPD, USB_LINESPEED_FULL_SPEED_ULPI);
#else
    WRITE_FIELD(USBD->DCFG, USB_OTG_DCFG_DSPD, USB_LINESPEED_FULL_SPEED); // there's no other possible option
#endif

    // allow specific interrupts
    uint32_t intmask =             /*USB_OTG_GINTMSK_WUIM |     // Wake up */
        USB_OTG_GINTMSK_OEPINT |   // OUT EP events
        USB_OTG_GINTMSK_IEPINT |   // IN EP events
        USB_OTG_GINTMSK_ENUMDNEM | // (Linespeed) Enumeration (negotiation) done
        USB_OTG_GINTMSK_USBRST |   // USB Reset
        USB_OTG_GINTMSK_USBSUSPM | // USB Suspend
        USB_OTG_GINTMSK_RXFLVLM;   // RX FIFO level (signal if non-empty)
    USBG->GINTMSK = intmask;

    // set global NAK on all Endpoints
    // usbdrv_set_global_NAK(USB_IN, true);
    // usbdrv_set_global_NAK(USB_OUT, true);

    // flush Tx and Rx FIFOs
    usbdrv_flush_rx_fifo();
    usbdrv_flush_tx_fifo(USB_FLUSH_TX_FIFO_ALL);

    // make Tx FIFO empty interrupt fire when Tx FIFO is emtpy
    // SET_BIT(USBG->GAHBCFG, USB_OTG_GAHBCFG_TXFELVL);

    // set masks for endpoint interrupts
    SET_BIT(USBD->DIEPMSK, USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_TOM | USB_OTG_DIEPMSK_ITTXFEMSK); // transfer complete, timeout and Tx FIFO empty while receiving IN for IN EPs
    SET_BIT(USBD->DOEPMSK, USB_OTG_DOEPMSK_XFRCM | USB_OTG_DOEPMSK_STUPM);                           // transfer complete and SETUP complete for OUT EPs

    // mask all endpoint interrupts in both directions and also clear flags
    USBD->DAINTMSK = 0;
    USBD->DAINT = 0;

    // enbale global interrupts
    SET_BIT(USBG->GAHBCFG, USB_OTG_GAHBCFG_GINT);
}

// initialize USB subsystem
void usbdrv_init() {
    HAL_NVIC_DisableIRQ(USB_IRQn);

    //usbdrv_init_global_state();
    usbdrv_gpio_init();
    usbdrv_periph_init();
    usbdrv_initial_ep0_setup();
    usbdrv_power_and_connect(true);

    HAL_NVIC_SetPriority(USB_IRQn, 8, 0);
    HAL_NVIC_EnableIRQ(USB_IRQn);
}