#include "usb_drv.h"

#include <stdint.h>
#include <string.h>

#include "flatUSB_config.h"
#include "usb_driver_common.h"

#include FLATUSB_DESCRIPTOR_HEADER

#include "common_defs.h"
#include "usb_common.h"

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

#define USB_RX_BUF_SIZE (512) ///< Receive buffer size

static USBDRV_GlobalState gs;                       ///< Blobal USB state
static uint8_t rx_buf[USB_RX_BUF_SIZE] DWORD_ALIGN; ///< Receive buffer
static UsbDrv_IN_cb cbs[USB_NUM_OF_ENDPOINTS];      ///< Callbacks for IN completion

/** \cond false */
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
/** \endcond */

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

#ifdef USBDBGMSG
static const char *FIFO_STATUS_STR[6] = {
    "GLOBAL OUT NAK",
    "OUT DATA RECV",
    "OUT TRANSFER CPLT",
    "OUT SETUP CPLT",
    "",
    "OUT SETUP RECV"};
#endif

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

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

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

void usbdrv_register_IN_complete_cb(uint8_t ep, UsbDrv_IN_cb cb) {
    cbs[ep] = cb;
}

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

static UsbDrv_DrvIntf drvIntf;

void usbdrv_init_intf() {
    drvIntf.init = usbdrv_init;
    drvIntf.reset = usbdrv_reset;
    drvIntf.stall = usbdrv_stall_endpoint;
    drvIntf.arm_IN = usbdrv_arm_IN_endpoint;
    drvIntf.arm_OUT = usbdrv_arm_OUT_endpoint;
    drvIntf.set_address = usbdrv_set_address;
    drvIntf.set_config = usbdrv_fetch_endpoint_configuration;
    drvIntf.autoarm = usbdrv_autoarm_OUT_endpoint;
    drvIntf.en_ep_irq = usbdrv_enable_endpoint_interrupt;
    drvIntf.reg_IN_cb = usbdrv_register_IN_complete_cb;
}

UsbDrv_DrvIntf *usbdrv_get_intf() {
    return &drvIntf;
}

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

#if USB_INTERNAL && USB_UPLI
#error "Select just one from USB_INTERNAL and USB_ULPI!"
#endif

// issue a hardware reset
static void usbdrv_hw_reset() {
    WAIT_FOR_nBIT_DELAY(USBG->GRSTCTL, USB_OTG_GRSTCTL_AHBIDL, 1); // wait for AHB transactions to cease
    SET_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_CSRST);                 // reset the USB core
    WAIT_FOR_BIT_DELAY(USBG->GRSTCTL, USB_OTG_GRSTCTL_CSRST, 1);
    // SET_BIT(USBG->GINTSTS, USB_OTG_GINTSTS_USBRST);
}

// initialize USB peripheral
void usbdrv_periph_init(bool reset) {
    if (!reset) {
        // enable USB clock
        USB_CLOCK_ENABLE();

        // select either the internal or the external ULPI transceiver
#if USB_INTERNAL
        SET_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_PHYSEL); // select the internal FS PHY
#endif

#if USB_ULPI
        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);
#endif

        usbdrv_hw_reset(); // commence a hardware reset

#if USB_ULPI
        usbdrv_ulpi_init(); // initialize PHY
#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

    USB_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)
#if !USB_VBUSSENSE
    CLEAR_BIT(USBG->GCCFG, USB_OTG_GCCFG_VBDEN); // turn off VBUSSENSE
#endif
    SET_BIT(USBG->GOTGCTL, USB_OTG_GOTGCTL_BVALOEN | USB_OTG_GOTGCTL_BVALOVAL); // force B-session
#elif defined(USB_STM32F4)
#if !USB_VBUSSENSE
    SET_BIT(USBG->GCCFG, USB_OTG_GCCFG_NOVBUSSENS); // turn off VBUSSENSE
#endif
    CLEAR_BIT(USBG->GCCFG, USB_OTG_GCCFG_VBUSBSEN | USB_OTG_GCCFG_VBUSASEN);
#endif

    USB_DELAY(25); // it takes time to forcing Device mode takes effect

    if (!reset) {
#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;

    if (!reset) {
        // flush Tx and Rx FIFOs
        usbdrv_flush_rx_fifo();
        usbdrv_flush_tx_fifo(USB_FLUSH_TX_FIFO_ALL);
    }

    // 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 global state
void usbdrv_init_global_state() {
    // clear state
    memset(&gs, 0, sizeof(USBDRV_GlobalState));

    // clear IN complete callbacks
    memset(&cbs, 0, sizeof(UsbDrv_IN_cb) * USB_NUM_OF_ENDPOINTS);

    // initialize receive buffer
    gs.rx_buf = rx_buf;
    gs.rx_buf_level = 0;
}

/**
 * Hook for initializing modules after the low-level driver has been initialized
 * but has not been connected to the bus yet.
 */
__weak void usbdrv_init_hook() {
    return;
}

// initialize USB subsystem
void usbdrv_init() {
    USB_IRQ_DISABLE(USB_IRQ_N); // disable USB interrupts

    usbdrv_init_global_state();
    usbdrv_init_intf();
    usbdrv_gpio_init();
    usbdrv_periph_init(false);
    usbdrv_initial_ep0_setup();
    usbdrv_init_hook(); // <---
    usbdrv_power_and_connect(true);

    USB_IRQ_SET_PRIORITY(USB_IRQ_N, USB_IRQ_PRIORITY);
    USB_IRQ_ENABLE(USB_IRQ_N);
}

void usbdrv_reset() {
    USB_IRQ_DISABLE(USB_IRQ_N); // disable USB interrupts

    usbdrv_hw_reset(); // hardware reset
    usbdrv_init_global_state();
    usbdrv_periph_init(true);
    usbdrv_initial_ep0_setup();
    usbdrv_power_and_connect(true);

    USB_IRQ_SET_PRIORITY(USB_IRQ_N, USB_IRQ_PRIORITY);
    USB_IRQ_ENABLE(USB_IRQ_N);
}

// connect to or disconnect from the bus
void usbdrv_power_and_connect(bool en) {
    if (en) { // ON
        CLEAR_BIT(USBD->DCTL, USB_OTG_DCTL_SDIS);
#ifdef USB_INTERNAL
        SET_BIT(USBG->GCCFG, USB_OTG_GCCFG_PWRDWN); // actually, this is power UP and affects only the built-in FS transciever
#endif
    } else { // OFF
        SET_BIT(USBD->DCTL, USB_OTG_DCTL_SDIS);
#ifdef USB_INTERNAL
        CLEAR_BIT(USBG->GCCFG, USB_OTG_GCCFG_PWRDWN);
#endif
    }
}

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

// flush specific or all Tx FIFOs
void usbdrv_flush_tx_fifo(uint8_t n) {
    WAIT_FOR_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_TXFFLSH);  // wait for previous request to conclude
    WRITE_FIELD(USBG->GRSTCTL, USB_OTG_GRSTCTL_TXFNUM, n); // issue flush
    WAIT_FOR_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_TXFFLSH);  // wait for our request to conclude
}

// flush the Rx FIFO
void usbdrv_flush_rx_fifo() {
    WAIT_FOR_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_RXFFLSH);
    SET_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_RXFFLSH); // issue flush
    WAIT_FOR_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_RXFFLSH);
}

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

// addresses of specific DIEPTXF registers, addresses from the RM
static uint32_t *USB_pDIEPTXF[USB_NUM_OF_ENDPOINTS] = {
    // FS
    (uint32_t *)(((uint32_t)USBG) + 0x028), // DIEPTXF0
    (uint32_t *)(((uint32_t)USBG) + 0x104), // DIEPTXF1
    (uint32_t *)(((uint32_t)USBG) + 0x108), // DIEPTXF2
    (uint32_t *)(((uint32_t)USBG) + 0x10C), // DIEPTXF3
    // HS
    (uint32_t *)(((uint32_t)USBG) + 0x120), // DIEPTXF4
    (uint32_t *)(((uint32_t)USBG) + 0x124), // DIEPTXF5
};

// configure USB endpoint
void usbdrv_configure_endpoint(uint8_t ep, uint8_t dir, const USBDRV_EpConfig *cfg) {
    if (dir == USB_OUT) { // ---- OUT ----

        if (ep == 0) {                                                      // SPECIAL handling on EP0
            WRITE_FIELD(USBOUTEP[0].DOEPCTL, USB_OTG_DOEPCTL_MPSIZ, 0);     // fix in 64 bytes
            WRITE_FIELD(USBOUTEP[0].DOEPTSIZ, USB_OTG_DOEPTSIZ_STUPCNT, 3); // SETUP transaction stands of three packets
        } else {
            WRITE_FIELD(USBOUTEP[ep].DOEPCTL, USB_OTG_DOEPCTL_EPTYP, cfg->type);            // program endpoint type
            WRITE_FIELD(USBOUTEP[ep].DOEPCTL, USB_OTG_DOEPCTL_MPSIZ, cfg->max_packet_size); // program maximum packet size
            SET_BIT(USBOUTEP[ep].DOEPCTL, USB_OTG_DOEPCTL_USBAEP);                          // the endpoint is active in the current configuration
        }

        // ---- common for all endpoints ----

        // enable interrupt
        SET_BIT(USBD->DAINTMSK, 1 << (USB_OTG_DAINTMSK_OEPM_Pos + ep));

        // NAK processing
        if (cfg->responding_NAK) {
            SET_BIT(USBOUTEP[ep].DOEPCTL, USB_OTG_DOEPCTL_SNAK); // send NAK
        } else {
            usbdrv_arm_OUT_endpoint(ep, cfg->max_packet_size);
        }
    } else { // ---- IN ----

        if (ep == 0) {                                                 // SPECIAL handling on EP0
            WRITE_FIELD(USBINEP[0].DIEPCTL, USB_OTG_DIEPCTL_MPSIZ, 0); // fix in 64 bytes
        } else {
            WRITE_FIELD(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_EPTYP, cfg->type);            // program endpoint type
            WRITE_FIELD(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_MPSIZ, cfg->max_packet_size); // program maximum packet size
            SET_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_USBAEP);                          // the endpoint is active in the current configuration
        }

        // ---- common for all endpoints ----

        // program FIFO corresponding FIFO number
        WRITE_FIELD(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_TXFNUM, ep);

        // store Tx FIFO size (both fields are WORD units, NOT bytes, RM is missing this information!)
        uint32_t tx_fifo_config = ((cfg->fifo_size >> 2) << USB_OTG_DIEPTXF_INEPTXFD_Pos) | (cfg->fifo_address >> 2); // combine size in DWORDs and address
        *(USB_pDIEPTXF[ep]) = tx_fifo_config;                                                                         // save

        // NAK processing
        if (cfg->responding_NAK) {
            SET_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_SNAK); // send NAK
        } else {
            SET_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_CNAK); // clear sending NAK
        }
    }
}

// deconfigure USB endpoint
void usbdrv_deconfigure_endpoint(uint8_t ep, uint8_t dir) {
    if (ep == 0) { // EP0 cannot be deconfigured
        return;
    }

    if (dir == USB_OUT) {                                                 // ---- OUT ----
        CLEAR_BIT(USBOUTEP[ep].DOEPCTL, USB_OTG_DOEPCTL_USBAEP);          // deactivate endpoint
        CLEAR_BIT(USBD->DAINTMSK, 1 << (USB_OTG_DAINTMSK_OEPM_Pos + ep)); // disable interrupt
    } else {                                                              // ---- IN ----
        CLEAR_BIT(USBD->DAINTMSK, 1 << ep);                               // disable interrupt
        usbdrv_flush_tx_fifo(ep);                                         // flush Tx FIFO
        SET_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_USBAEP);             // deactivate endpoint
    }
}

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

// stall endpoint
void usbdrv_stall_endpoint(uint8_t ep, uint8_t dir, bool stall) {
    USB_OTG_INEndpointTypeDef *inep = USBINEP + ep;
    // USB_OTG_OUTEndpointTypeDef *outep = USBOUTEP + ep;

    if (stall) {
        if (dir == USB_IN) {
            if (ep != 0) {                                     // special treatment for EP0
                SET_BIT(inep->DIEPCTL, USB_OTG_DIEPCTL_EPDIS); // disable endpoint
            }
            SET_BIT(inep->DIEPCTL, USB_OTG_DIEPCTL_STALL); // stall endpoint
        }

        if (ep != 0) { // EP0 cannot be disabled
            // wait for endpoint disable to get effective
            WAIT_FOR_nBIT(inep->DIEPINT, USB_OTG_DIEPINT_EPDISD);
            CLEAR_BIT(inep->DIEPINT, USB_OTG_DIEPINT_EPDISD);
        }

        // flush trnasmit FIFO
        usbdrv_flush_tx_fifo(ep);
    } else {
        if (dir == USB_IN) {
            if (ep != 0) {                                     // special treatment for EP0
                SET_BIT(inep->DIEPCTL, USB_OTG_DIEPCTL_EPENA); // enable endpoint
            }
            CLEAR_BIT(inep->DIEPCTL, USB_OTG_DIEPCTL_STALL); // clear endpoint stall
        }
    }
}

// set global NAK
void usbdrv_set_global_NAK(uint8_t dir, bool en) {
    if (en) {
        if (dir == USB_IN) {
            SET_BIT(USBD->DCTL, USB_OTG_DCTL_SGINAK);
        } else {
            SET_BIT(USBD->DCTL, USB_OTG_DCTL_SGONAK);
        }
    } else {
        if (dir == USB_IN) {
            SET_BIT(USBD->DCTL, USB_OTG_DCTL_CGINAK);
        } else {
            SET_BIT(USBD->DCTL, USB_OTG_DCTL_CGONAK);
        }
    }
}

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

// fetch received data from RX FIFO to receive buffer
void usbdrv_fetch_received_data(uint8_t ep, uint16_t len) {
    if (len > 0) {
        volatile uint32_t *p = USBFIFO(ep);
        uint16_t len_dwords = CEILDIV4(len);
        for (uint16_t i = 0; i < len_dwords; i++) {
            uint32_t dword = p[i];
            uint16_t i0 = i * 4;
            gs.rx_buf[i0] = dword & 0xFF;
            gs.rx_buf[i0 + 1] = (dword >> 8) & 0xFF;
            gs.rx_buf[i0 + 2] = (dword >> 16) & 0xFF;
            gs.rx_buf[i0 + 3] = (dword >> 24) & 0xFF;
        }
    }
    gs.rx_buf_level = len;
}

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

// write data to specific endpoint FIFO
uint32_t usbdrv_arm_IN_endpoint(uint8_t ep, const uint8_t *data, uint16_t len) {
    // determine if a transmission is in progress or not
    bool txp = READ_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_EPENA);

    uint16_t mps = gs.ep_IN[ep].max_packet_size; // fetch maximum packet size

    // determine final write size
    uint32_t freeSize = USBINEP[ep].DTXFSTS * sizeof(uint32_t); // get free transmit buffer size
    uint16_t writeSize = 0;

    if (txp) {                                                                    // transaction is in progress
        uint32_t remainingBytes = USBINEP[ep].DIEPTSIZ & USB_OTG_DIEPTSIZ_XFRSIZ; // acquire remaining bytes
        if (len >= mps) {                                                         // write only full-sized packets, or a short packet
            uint16_t mws = MIN(freeSize, len);                                    // maximum possible write size
            uint16_t fpc = mws / mps;                                             // full packet count
            writeSize = fpc * mps;                                                // form write size
        } else {                                                                  // length is less then a full packet size
            if (len <= freeSize) {                                                // packet can be written
                writeSize = len;
            } else {
                writeSize = 0;
            }
        }
        // adjust write size to remaining size
        writeSize = MIN(remainingBytes, writeSize);
    } else { // no transaction is in progress
        writeSize = MIN(freeSize, len);
    }

    // if no transmission is in progress
    if (!txp) {
        // calculate packet count based on max packet size
        uint16_t packet_count = 1;                                  // for ZLPs
        if (len > 0) {                                              // if length is nonzero
            packet_count = len / mps + (((len % mps) > 0) ? 1 : 0); // a transfer may contain multiple packets
        }

        // set zlp_next if transfer size is integer multiple of max packet size and auto ZLP is on
        gs.ep_IN[ep].zlp_next = (len > 0) && ((len % mps) == 0);

        // program DIEPTSIZ with transfer length
        USBINEP[ep].DIEPTSIZ = /*(packet_count << USB_OTG_DIEPTSIZ_MULCNT_Pos) |*/ (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | len;

        // enable endpoint and cancel responding NAK
        SET_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_EPENA | USB_OTG_DIEPCTL_CNAK);

        // enable endpoint interrupts
        SET_BIT(USBD->DAINTMSK, 1 << ep);
    }

    // turn on interrupt generation only, if this is NOT the last FIFO write considering the current transfer
    if (len > writeSize) {
        USBD->DIEPEMPMSK |= ((uint32_t)(1 << ep));
    }

    // disable ALL USB interrupts to prevent access to specific registers (see errata)
    CLEAR_BIT(USBG->GAHBCFG, USB_OTG_GAHBCFG_GINT);

    // WAIT_FOR_nBIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_TXFE);

    // https://github.com/iliasam/STM32F4_USB_MICROPHONE/blob/master/Libraries/STM32_USB_OTG_Driver/src/usb_dcd_int.c#L655
    // https://github.com/iliasam/STM32F4_USB_MICROPHONE/blob/master/Libraries/STM32_USB_OTG_Driver/src/usb_core.c#L168

    // push full dwords
    volatile uint32_t *p = (uint32_t *)USBFIFO(ep);
    uint32_t floorlen_dwords = writeSize >> 2;
    for (uint16_t i = 0; i < floorlen_dwords; i++) {
        uint16_t i0 = 4 * i;
        uint32_t dword = data[i0] | (data[i0 + 1] << 8) | (data[i0 + 2] << 16) | (data[i0 + 3] << 24);
        *p = dword;
    }

    // push the remaining partial dword
    uint8_t rem_bytes = writeSize & 0b11;
    if (rem_bytes > 0) {
        uint32_t rem_dword = 0;
        uint16_t rem_start = writeSize - rem_bytes;
        for (int16_t i = writeSize - 1; i >= rem_start; i--) {
            rem_dword = (rem_dword << 8) | data[i];
        }
        *p = rem_dword;
    }

    // unmask USB interrupts
    SET_BIT(USBG->GAHBCFG, USB_OTG_GAHBCFG_GINT);

    // return with written size
    return writeSize;
}

// arm OUT endpoint
uint32_t usbdrv_arm_OUT_endpoint(uint8_t ep, uint16_t size) {
    // arm endpoint only if it was not armed before
    if (READ_BIT(USBOUTEP[ep].DOEPCTL, USB_OTG_DOEPCTL_EPENA)) {
        return 0;
    }

    // cap size at max packet size defined for this endpoint
    size = MIN(gs.ep_OUT[ep].max_packet_size, size);

    // write registers
    uint32_t doeptsiz = USBOUTEP[ep].DOEPTSIZ;
    doeptsiz &= ~(USB_OTG_DOEPTSIZ_XFRSIZ);     // clear XFERSIZ bits
    doeptsiz |= USB_OTG_DOEPTSIZ_PKTCNT | size; // program DIEPTSIZ with maximum (expected) transfer length and set PCKTCNT to make ready for reception
    USBOUTEP[ep].DOEPTSIZ = doeptsiz;           // write value to the actual register

    SET_BIT(USBOUTEP[ep].DOEPCTL, USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK); // enable endpoint and clear NAK

    // return with armed size
    return size;
}

void usbdrv_autoarm_OUT_endpoint(uint8_t ep) {
    gs.ep_OUT[ep].autoarm = true;
}

void usbdrv_enable_endpoint_interrupt(uint8_t ep, uint8_t dir, bool en) {
    uint32_t mask = 1 << (ep + ((dir == USB_OUT) ? USB_OTG_DAINTMSK_OEPM_Pos : 0));
    if (en) {
        SET_BIT(USBD->DAINTMSK, mask);
    } else {
        CLEAR_BIT(USBD->DAINTMSK, mask);
    }
}

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

// set Rx FIFO size
void usbdrv_set_rx_fifo_size(uint16_t size) {
    USBG->GRXFSIZ = CEILDIV4(size);
}

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

// preload usb endpoint config
void usbdrv_preload_endpoint_config(uint8_t ep, uint8_t dir, const USBDRV_EpConfig *cfg) {
    USBMSG("PRELOAD: %u %s\n", ep, dir ? "IN" : "OUT");
    if (dir == USB_OUT) {
        gs.ep_OUT[ep] = *cfg;
        gs.ep_OUT[ep].is_configured = true;
    } else {
        gs.ep_IN[ep] = *cfg;
        gs.ep_IN[ep].is_configured = true;
    }
}

// clear endpoint config
void usbdrv_clear_endpoint_config() {
    memset(&gs.ep_OUT, 0, USB_NUM_OF_ENDPOINTS * sizeof(USBDRV_EpConfig));
    memset(&gs.ep_IN, 0, USB_NUM_OF_ENDPOINTS * sizeof(USBDRV_EpConfig));
}

// apply preloaded endpoint configuration
void usbdrv_apply_endpoint_config() {
    for (uint8_t i = 0; i < USB_NUM_OF_ENDPOINTS; i++) {
        // OUT EPs
        if (gs.ep_OUT[i].is_configured) {
            usbdrv_configure_endpoint(i, USB_OUT, gs.ep_OUT + i);
        }

        // IN EPs
        if (gs.ep_IN[i].is_configured) {
            usbdrv_configure_endpoint(i, USB_IN, gs.ep_IN + i);
        }
    }
}

// fetch endpoint configuration from descriptor dump
void usbdrv_fetch_endpoint_configuration(uint8_t config_index) {
    const uint8_t *fullConfDesc = (const uint8_t *)confDescs[config_index];                         // point an array to the beginning of the full configuration
    const USB_ConfigurationDesc *confDesc = (const USB_ConfigurationDesc *)confDescs[config_index]; // fetch the leading configuration descriptor

    // look up endpoint descriptors
    const uint8_t *iter = fullConfDesc;
    while (iter < (fullConfDesc + confDesc->wTotalLength)) {
        if (iter[1] == UD_Endpoint) { // Endpoint descriptor found
            USB_EndpointDesc epDesc;
            memcpy(&epDesc, iter, iter[0]); // fetch EP descriptor by copy, since desciptor start address is NOT aligned
            USBDRV_EpConfig cfg;            // fill-in configuration
            cfg.max_packet_size = epDesc.wMaxPacketSize;
            cfg.responding_NAK = false;
            cfg.type = epDesc.bmAttributes & 0b11;
            cfg.service_interval = epDesc.bInterval;

            // fetch endpoint address
            uint8_t dir = (epDesc.bEndpointAddress >> 7);
            uint8_t n = epDesc.bEndpointAddress & 0x7F;

            // apply configuration
            usbdrv_preload_endpoint_config(n, dir, &cfg);
        }

        // advance iterator using the bLength field (first byte in a descriptor)
        iter += iter[0];
    }

    usbdrv_build_fifo();

    usbdrv_apply_endpoint_config();
}

#define USB_MIN_FIFO_SIZE (64)
#define USB_FIFO_MARGIN (8)
#define USB_RX_FIFO_SETUP_RESERVATION_DWORDS (10)
#define USB_MIN_GROSS_TX_FIFO_SIZE (2 * USB_MIN_EP_FIFO_SIZE)

#if defined(USB_STM32F4)
#if USB_INTERNAL
#define USB_MIN_GROSS_RX_FIFO_SIZE (2 * USB_MIN_EP_FIFO_SIZE + USB_RX_FIFO_SETUP_RESERVATION_DWORDS * 4)
#else
#define USB_MIN_GROSS_RX_FIFO_SIZE (1024)
#endif
#elif defined(USB_STM32H7)
#define USB_MIN_GROSS_RX_FIFO_SIZE (256)
#endif

#define USBDRV_ADDR_TABLE_STR_LEN (255)
static char usbdrv_addr_table[USBDRV_ADDR_TABLE_STR_LEN + 1];

// build FIFO (compute addresses)
void usbdrv_build_fifo() {
    // ---- OUT ----
    uint16_t next_fifo_addr = 0x00;                      // Rx FIFO begins at address zero
    uint16_t largest_packet_size = 0;                    // largest packet size
    uint16_t control_ep_count = 0;                       // number of control endpoints
    uint16_t out_ep_count = 0;                           // count of OUT pipes
    for (uint8_t i = 0; i < USB_NUM_OF_ENDPOINTS; i++) { // gather config information
        // look for largest packet size
        if (gs.ep_OUT[i].is_configured) { // examine OUT EPs
            largest_packet_size = MAX(largest_packet_size, gs.ep_OUT[i].max_packet_size);
            out_ep_count++;
        }
        if (gs.ep_IN[i].is_configured) { // examine IN EPs
            largest_packet_size = MAX(largest_packet_size, gs.ep_IN[i].max_packet_size);
        }

        // count control endpoints
        if (((gs.ep_OUT[i].is_configured) && (gs.ep_OUT[i].type == UT_Control)) ||
            ((gs.ep_IN[i].is_configured) && (gs.ep_IN[i].type == UT_Control))) {
            control_ep_count++;
        }
    }

    // RX FIFO size calculation expression from the RM
    uint16_t fifo_size_dwords = (5 * control_ep_count + 8) + (CEILDIV4(largest_packet_size) + 1) + (2 * out_ep_count) + 1; // calculate RX FIFO size in DWORDS
    uint16_t fifo_size = fifo_size_dwords * 4;                                                                             // calculate RX FIFO size in bytes
    fifo_size = CEIL4(MAX(fifo_size, USB_MIN_GROSS_RX_FIFO_SIZE));                                                         // RX FIFO should be at least this large

    // fifo_size *= 2;                 // TODO:
    gs.rx_fifo_size = fifo_size;        // save Rx FIFO size for later
    next_fifo_addr += fifo_size;        // advance next FIFO address
    usbdrv_set_rx_fifo_size(fifo_size); // set Rx FIFO size in hardware

    uint32_t str_offset = SNPRINTF(usbdrv_addr_table, USBDRV_ADDR_TABLE_STR_LEN, "RX:  000-%03x (%u)\r\n", fifo_size - 1, fifo_size);

    // ---- IN ----
    for (uint8_t i = 0; i < USB_NUM_OF_ENDPOINTS; i++) {
        USBDRV_EpConfig *cfg = &gs.ep_IN[i];
        if (cfg->is_configured) {
            cfg->fifo_size = CEIL4(MAX(USB_MIN_GROSS_TX_FIFO_SIZE, cfg->max_packet_size + 64)); // correct FIFO size if necessary
            cfg->fifo_address = next_fifo_addr;                                                 // store FIFO address
            cfg->zlp_next = false;

            str_offset += SNPRINTF(usbdrv_addr_table + str_offset, USBDRV_ADDR_TABLE_STR_LEN - str_offset, "TX%u: %03x-%03x (%u)\r\n", i, next_fifo_addr, next_fifo_addr + cfg->fifo_size - 1, cfg->fifo_size);

            // cfg->txp = false;                                                                                    // no transfer is in progress
            next_fifo_addr += cfg->fifo_size; // advance next address
        }
    }
}

const char *usbdrv_get_fifo_addr_table() {
    return usbdrv_addr_table;
}

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

// create an initial setup for EP0 in both directions
void usbdrv_initial_ep0_setup() {
    // setup EP0 OUT and IN
    USBDRV_EpConfig ep_cfg;
    ep_cfg.max_packet_size = 64;
    ep_cfg.responding_NAK = false;
    usbdrv_preload_endpoint_config(0, USB_OUT, &ep_cfg);
    usbdrv_preload_endpoint_config(0, USB_IN, &ep_cfg);

    // build FIFO
    usbdrv_build_fifo();

    // configure endpoints
    usbdrv_apply_endpoint_config();

    // turn off global NAK
    usbdrv_set_global_NAK(USB_IN, false);
    usbdrv_set_global_NAK(USB_OUT, false);
}

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

void usbdrv_set_address(uint8_t addr) {
    gs.address = addr;
    WRITE_FIELD(USBD->DCFG, USB_OTG_DCFG_DAD, gs.address);
}

// get endpoint interrupt flag
bool usbdrv_get_endpoint_interrupt_flag(uint8_t ep, uint8_t dir) {
    return (USBD->DAINT & (1 << (ep + ((dir == USB_OUT) ? USB_OTG_DAINT_OEPINT_Pos : 0)))) != 0;
}

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

// receive packet
void usbdrv_process_rx_fifo_top(USBDRV_EventData *evt_data) {
    uint32_t rxstat = USBG->GRXSTSP; // POP (not just read) latest FIFO status word

    uint8_t pckt_status = READ_FIELD(rxstat, USB_OTG_GRXSTSP_PKTSTS); // read packet status
    uint8_t data_pid = READ_FIELD(rxstat, USB_OTG_GRXSTSP_DPID);      // read data PID
    uint8_t byte_count = READ_FIELD(rxstat, USB_OTG_GRXSTSP_BCNT);    // byte count
    uint8_t ep_num = READ_FIELD(rxstat, USB_OTG_GRXSTSP_EPNUM);       // read endpoint number

    // copy to output structure
    evt_data->rx.pckt_status = pckt_status;
    evt_data->rx.data_pid = data_pid;
    evt_data->rx.byte_count = byte_count;
    evt_data->rx.ep_num = ep_num;

    USBMSG("%s [%u] %u\n", FIFO_STATUS_STR[pckt_status - 1], ep_num, byte_count);
}

/**
 * Function prototype for processing SETUP packets. This function is expected to be
 * overridden by the application.
 * 
 * @param data pointer to the SETUP packet
 * @param size size of the packet
 * @param stage stage of the SETUP transaction
 */
__weak void usbcore_process_setup_pckt(const uint8_t *data, uint16_t size, uint8_t stage) {
    // always pass ALIGNED data!
    return;
}

/**
 * Function prototype for processing non-SETUP packets. This function is expected to be
 * overridden by the application.
 * 
 * @param cbcpd pointer to callback compound
 */
__weak void usbcore_process_nonsetup_event(UsbDrv_CallbackCompound *cbcpd) {
    return;
}

// process USB event
void usbdrv_process_event(uint8_t evt_code, USBDRV_EventData *evt_data) {
    if (evt_code == USB_EVT_USB_RESET) { // reset takes precedence over anything else TODO
        usbdrv_reset();

        // invoke Reset Done notification
        if (drvIntf.rst_notify != NULL) {
            drvIntf.rst_notify();
        }

        USBMSG("RESET\n");
        return;
    }

    switch (gs.state) {
    case USB_FSM_INITIAL_WAIT_SPEEDNEG: // wait for speed negotitation to conclude
        if (evt_code == USB_EVT_SPEEDNEG_DONE) {
            gs.state = USB_FSM_SETUP_OPERATE; // wait for speed negotiation
        }

        // invoke Enumeration Done notification
        if (drvIntf.enum_notify != NULL) {
            uint8_t spd;
            uint32_t dspd = READ_FIELD(USBD->DCFG, USB_OTG_DCFG_DSPD);
            switch (dspd) {
            case USB_LINESPEED_FULL_SPEED_ULPI:
            case USB_LINESPEED_FULL_SPEED:
                spd = USB_SPD_FULL;
                break;
            case USB_LINESPEED_HIGH_SPEED_ULPI:
                spd = USB_SPD_HIGH;
                break;
            default:
                spd = USB_SPD_UKWN;
                break;
            }
            drvIntf.enum_notify(spd);
        }

        break;
    case USB_FSM_SETUP_OPERATE: { // expect SETUP transactions first, then everything else as well
        switch (evt_code) {
        case USB_EVT_RECEPTION_DONE: { // reception done
            USBDRV_EventData evt_data = {0};
            usbdrv_process_rx_fifo_top(&evt_data); // process rx fifo top

            // fetch data if data are available
            if ((evt_data.rx.pckt_status == USB_PCKT_STATUS_SETUP_DATA_RECV) ||
                (evt_data.rx.pckt_status == USB_PCKT_STATUS_OUT_DATA_RECV)) {
                usbdrv_fetch_received_data(evt_data.rx.ep_num, evt_data.rx.byte_count); // fetch the data
            }

            // act according to what we have received
            if (evt_data.rx.ep_num == 0) { // EP0 special treatment
                int stage = -1;
                if (evt_data.rx.pckt_status == USB_PCKT_STATUS_SETUP_CPLT) {
                    stage = UST_SETUP;
                    USBMSG("--SETUP\n");
                } else if (evt_data.rx.pckt_status == USB_PCKT_STATUS_OUT_TRANSFER_CPLT) {
                    stage = UST_DATA;

                    USBMSG("--DATA\n");
                }

                // process setup packet
                if (stage != -1) {
                    usbcore_process_setup_pckt(gs.rx_buf, gs.rx_buf_level, stage);
                }

                // SET_BIT(USBG->GINTMSK, USB_OTG_GINTMSK_RXFLVLM); // unmask interrupt
            } else { // not EP0
                if (evt_data.rx.pckt_status == USB_PCKT_STATUS_OUT_DATA_RECV) {
                    UsbDrv_CallbackCompound cbcpd;
                    cbcpd.ep = evt_data.rx.ep_num;
                    cbcpd.dir = USB_OUT;
                    cbcpd.code = USB_CBC_OUT;
                    cbcpd.data = gs.rx_buf;
                    cbcpd.size = gs.rx_buf_level;
                    usbcore_process_nonsetup_event(&cbcpd);
                }
            }
            break;
        }

        case USB_EVT_OUT_DONE: { // some OUT operations have finished
            for (uint8_t ep = 0; ep < USB_NUM_OF_ENDPOINTS; ep++) {
                if (gs.ep_OUT[ep].is_configured) {                              // if the endpoint is running
                    if (READ_BIT(USBOUTEP[ep].DOEPINT, USB_OTG_DOEPINT_STUP)) { // setup done
                        SET_BIT(USBOUTEP[ep].DOEPINT, USB_OTG_DOEPINT_STUP);
                        USBMSG("SETUP\n");
                    }

                    if (READ_BIT(USBOUTEP[ep].DOEPINT, USB_OTG_DOEPINT_XFRC)) { // OUT transaction done
                        SET_BIT(USBOUTEP[ep].DOEPINT, USB_OTG_DOEPINT_XFRC);
                        USBMSG("OUT\n");

                        if ((ep == 0) || (gs.ep_OUT[ep].autoarm)) {                     // EP0 must always be armed
                            usbdrv_arm_OUT_endpoint(ep, gs.ep_OUT[ep].max_packet_size); // arm endpoint
                            gs.ep_OUT[ep].autoarm = false;                              // clear autoarm flag
                        }
                    }
                }
            }
            break;
        }

        case USB_EVT_IN_DONE: { // some IN operations have finished

            // callback compound
            UsbDrv_CallbackCompound cbcpd;
            cbcpd.dir = USB_IN;
            cbcpd.data = NULL;
            cbcpd.size = 0;

            for (uint8_t ep = 0; ep < USB_NUM_OF_ENDPOINTS; ep++) {
                cbcpd.ep = ep;
                bool ep_on = USBD->DAINTMSK & (1 << ep);                      // decide if this endpoint is currently enable for transmission based on its endpoint mask
                if (gs.ep_IN[ep].is_configured && ep_on) {                    // if the endpoint is running
                    if (READ_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_TOC)) { // timeout done
                        SET_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_TOC);
                        USBMSG("TO\n");
                    } else if ((USBD->DIEPEMPMSK & (1 << ep)) && READ_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_TXFE)) {
                        // disable FIFO empty interrupt
                        USBD->DIEPEMPMSK &= ~((uint32_t)(1 << ep));

                        cbcpd.code = USB_CBC_IN_FIFOEMPTY;
                        usbcore_process_nonsetup_event(&cbcpd);
                    } else if (READ_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_XFRC)) { // IN transaction done
                        SET_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_XFRC);

                        // disable FIFO empty interrupt
                        USBD->DIEPEMPMSK &= ~((uint32_t)(1 << ep));

                        // transfer finished
                        // gs.ep_IN[ep].txp = false;

                        // see if a ZLP transmission was queued
                        if (gs.ep_IN[ep].zlp_next) {
                            usbdrv_arm_IN_endpoint(ep, NULL, 0); // send ZLP
                        } else {                                 // no ZLP
                            USBMSG("IN [%d]\n", ep);

                            // invoke callback if registered
                            if (cbs[ep] != NULL) {
                                UsbDrv_IN_cb cb = cbs[ep]; // fetch function pointer
                                cbs[ep] = NULL;            // clear callback
                                cb(ep);                    // invoke the callback
                            }

                            cbcpd.code = USB_CBC_IN_DONE;
                            usbcore_process_nonsetup_event(&cbcpd);
                        }

                    } else if (READ_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_ITTXFE)) { // IN endpoint IN token received with Tx FIFO empty interrupt
                        SET_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_ITTXFE);

                        // disable endpoint interrupt
                        usbdrv_enable_endpoint_interrupt(ep, USB_IN, false);

                        cbcpd.code = USB_CBC_IN_FIFOEMPTY;
                        usbcore_process_nonsetup_event(&cbcpd);
                    }
                }
            }
        }

        default:
            break;
        }
    }

    default:
        break;
    }
}

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

#define PROCESS_EVENT(evt, data) usbdrv_process_event((evt), (data))

void USB_IRQ_HANDLER() {

    uint32_t ints = USBG->GINTSTS;

    // USB reset
    if (ints & USB_OTG_GINTSTS_USBRST) {
        SET_BIT(USBG->GINTSTS, USB_OTG_GINTSTS_USBRST); // clear interrupt
        //PROCESS_EVENT(USB_EVT_USB_RESET, NULL);         // process event
    }

    // End of enumeration (meaning NOT the USB ENUMERATION PROCESS,
    // ST calls speed negotiation the enumeration, normally this
    // interrupt fires only before even communication is commenced)
    if (ints & USB_OTG_GINTSTS_ENUMDNE) {
        SET_BIT(USBG->GINTSTS, USB_OTG_GINTSTS_ENUMDNE); // clear interrupt
        PROCESS_EVENT(USB_EVT_SPEEDNEG_DONE, NULL);      // process event
        // usbdrv_process_event(USB_EVT_SPEEDNEG_DONE, NULL); // process event
        //  usbdrv_push_event(USB_EVT_SPEEDNEG_DONE, NULL); // push event
    }

    // Start of Frame received (like Keep-Alive in LS mode)
    if (ints & USB_OTG_GINTSTS_SOF) {
        SET_BIT(USBG->GINTSTS, USB_OTG_GINTSTS_SOF); // clear interrupt
    }

    // USB Suspend
    if (ints & USB_OTG_GINTSTS_USBSUSP) {
        SET_BIT(USBG->GINTSTS, USB_OTG_GINTSTS_USBSUSP); // clear interrupt
        USBMSG("SUSPEND\n");
    }

    // OUT endpoint interrupt
    if (ints & USB_OTG_GINTSTS_OEPINT) {
        PROCESS_EVENT(USB_EVT_OUT_DONE, NULL);
        // usbdrv_push_event(USB_EVT_OUT_DONE, NULL);
        //  if (USBD->DAINT & (1 << 16)) {
        //      if (USBOUTEP[0].DOEPINT & USB_OTG_DOEPINT_STUP) {
        //          CLEAR_BIT(USBOUTEP[0].DOEPINT, USB_OTG_DOEPINT_STUP);
        //      }
        //  }
    }

    // RX FIFO non-empty interrupt
    if (ints & USB_OTG_GINTSTS_RXFLVL) {
        // SET_BIT(USBG->GINTSTS, USB_OTG_GINTSTS_RXFLVL); // clear interrupt
        USBMSG("RX DONE\n");
        // CLEAR_BIT(USBG->GINTMSK, USB_OTG_GINTMSK_RXFLVLM);  // mask interrupt until processing is done
        PROCESS_EVENT(USB_EVT_RECEPTION_DONE, NULL); // process event
        // usbdrv_push_event(USB_EVT_RECEPTION_DONE, NULL); // push event
    }

    // IN endpoint interrupt
    if (ints & USB_OTG_GINTSTS_IEPINT) {
        PROCESS_EVENT(USB_EVT_IN_DONE, NULL);
        // usbdrv_push_event(USB_EVT_IN_DONE, NULL);
    }

    return;
}