#include "usb_driver.h"

#include <memory.h>

#include "usb_common.h"
#include "usb_core_types.h"

#include "desc/usb_desc.h"

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

#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#define MIN(a, b) (((a) < (b)) ? (a) : (b))

#ifdef USBDBGMSG
#include "../cli/stdio_uart.h"
#define USBMSG(...) MSG(__VA_ARGS__)
#else
#define USBMSG(...)
#endif

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

static USBDRV_GlobalState gs; // global USB state

static uint8_t rx_buf[USB_MAX_FS_PCKT_SIZE_NON_ISOCHRONOUS] DWORD_ALIGN; // receive buffer

#define USB_EVENT_QUEUE_LENGTH (16)

// static uint8_t event_queue_mem[Q_REQ_MEM_SIZE_T(USB_EVENT_QUEUE_LENGTH, USBDRV_EventCompound)] DWORD_ALIGN; // backing memory for the event queue

#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

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

// USB pin low level, early peripheral initialization
// PA12: D+, PA11: D-
void usbdrv_gpio_init() {
    // turn GPIO-s into AF mode
    __HAL_RCC_GPIOA_CLK_ENABLE(); // turn ON GPIOA clocks

    GPIO_InitTypeDef gpio_init;
    gpio_init.Mode = GPIO_MODE_AF_PP;
    gpio_init.Pin = GPIO_PIN_12;
    gpio_init.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    gpio_init.Pull = GPIO_NOPULL;
    gpio_init.Alternate = GPIO_AF10_OTG_FS;

    HAL_GPIO_Init(GPIOA, &gpio_init); // USB D+

    // HAL_GPIO_WritePin(GPIOA, GPIO_PIN_11, GPIO_PIN_SET);

    gpio_init.Pin = GPIO_PIN_11;
    gpio_init.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOA, &gpio_init); // USB D-

    // gpio_init.Mode = GPIO_MODE_INPUT;
    // gpio_init.Pin = GPIO_PIN_9;
    // gpio_init.Speed = GPIO_SPEED_FREQ_HIGH;
    // gpio_init.Pull = GPIO_NOPULL;
    // gpio_init.Alternate = 0;

    // HAL_GPIO_Init(GPIOA, &gpio_init); // USB VBUSSENSE
}

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

// initialize USB subsystem
void usbdrv_init() {
    NVIC_DisableIRQ(OTG_FS_IRQn);

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

    NVIC_SetPriority(OTG_FS_IRQn, 6);
    NVIC_EnableIRQ(OTG_FS_IRQn);
}

void usbdrv_reset() {
    usbdrv_init();
}

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

// initialize global state
void usbdrv_init_global_state() {
    // clear state
    memset(&gs, 0, sizeof(USBDRV_GlobalState));

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

    // initialize event queue
    // gs.event_queue = Q_CREATE_T(USB_EVENT_QUEUE_LENGTH, USBDRV_EventCompound, event_queue_mem);
}

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

#define USB_LINESPEED_FULL_SPEED (0b11)

// initialize USB peripheral
void usbdrv_periph_init() {
    __HAL_RCC_USB_OTG_FS_CLK_ENABLE(); // enable clock on USB peripheral
    //__HAL_RCC_USB_OTG_FS_FORCE_RESET();
    //__HAL_RCC_USB_OTG_FS_RELEASE_RESET();

    CLEAR_BIT(USBG->GCCFG, USB_OTG_GCCFG_PWRDWN); // power down the 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, 0x06);                    // set TRDT according to the RM
    WRITE_FIELD(USBG->GUSBCFG, USB_OTG_GUSBCFG_TOCAL, 0x07);                   // set TOCAL
    SET_BIT(USBG->GUSBCFG, USB_OTG_GUSBCFG_FDMOD);                             // force Device mode
    SET_BIT(USBG->GCCFG, USB_OTG_GCCFG_NOVBUSSENS);                            // turn off VBUSSENSE

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

    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)
    WRITE_FIELD(USBD->DCFG, USB_OTG_DCFG_DSPD, USB_LINESPEED_FULL_SPEED); // there's no other possible option

    // 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);

    // 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);
}

// 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);
        SET_BIT(USBG->GCCFG, USB_OTG_GCCFG_PWRDWN); // actually, this is power UP
    } else {                                        // OFF
        SET_BIT(USBD->DCTL, USB_OTG_DCTL_SDIS);
        CLEAR_BIT(USBG->GCCFG, USB_OTG_GCCFG_PWRDWN);
    }
}

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

// 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 (USB_MIN_EP_FIFO_SIZE + USB_FIFO_MARGIN)
#define USB_MIN_GROSS_RX_FIFO_SIZE (2 * USB_MIN_EP_FIFO_SIZE + USB_RX_FIFO_SETUP_RESERVATION_DWORDS * 4)

// build FIFO (compute addresses)
void usbdrv_build_fifo() {
    // ---- OUT ----
    uint16_t fifo_size = USB_MIN_GROSS_RX_FIFO_SIZE;     // at least this large
    uint16_t next_fifo_addr = 0x00;                      // Rx FIFO begins at address zero
    for (uint8_t i = 0; i < USB_NUM_OF_ENDPOINTS; i++) { // look for greatest FIFO size
        if (gs.ep_OUT[i].is_configured) {
            fifo_size = CEIL4(MAX(fifo_size, gs.ep_OUT[i].max_packet_size)); // compare and replace if necessary
        }
    }
    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

    // ---- 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, 2 * cfg->max_packet_size + USB_FIFO_MARGIN)); // correct FIFO size if necessary
            cfg->fifo_address = next_fifo_addr;                                                                  // store FIFO address
            cfg->zlp_next = false;                                                                               // clear ZLP next
            next_fifo_addr += cfg->fifo_size;                                                                    // advance next address
        }
    }
}

// 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);
}

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

// addresses of specific DIEPTXF registers, addresses from the RM
static uint32_t *USB_pDIEPTXF[4] = {
    (uint32_t *)(USB_OTG_FS_PERIPH_BASE + 0x028), // DIEPTXF0
    (uint32_t *)(USB_OTG_FS_PERIPH_BASE + 0x104), // DIEPTXF1
    (uint32_t *)(USB_OTG_FS_PERIPH_BASE + 0x108), // DIEPTXF2
    (uint32_t *)(USB_OTG_FS_PERIPH_BASE + 0x10C), // DIEPTXF3
};

// 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 ----

        // program maximum packet size
        // WRITE_FIELD(USBOUTEP[ep].DOEPTSIZ, USB_OTG_DOEPTSIZ_XFRSIZ, cfg->max_packet_size); // TODO:

        // 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, this 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

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

        // NAK processing
        if (cfg->responding_NAK) {
            SET_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_SNAK); // send 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
    }
}

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

// 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);
}

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

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

// 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) {
    /*WRITE_FIELD(USBINEP[ep].DIEPTSIZ, USB_OTG_DIEPTSIZ_XFRSIZ, len);
    WRITE_FIELD(USBINEP[ep].DIEPTSIZ, USB_OTG_DIEPTSIZ_PKTCNT, 1);*/

    // transmission may only be commenced if last transmission has concluded
    if (READ_BIT(USBINEP[ep].DIEPCTL, USB_OTG_DIEPCTL_EPENA)) {
        return 0;
    }

    // determine final write size
    uint32_t freeSize = 0;
    // if (len > 0) { // only poll DTXFSTS if (len > 0) (see datasheet)
    freeSize = USBINEP[ep].DTXFSTS * sizeof(uint32_t); // get free transmit buffer size
    //}
    uint16_t writeSize = MIN(freeSize, len); // limit transmit size to free size

    // calculate packet count based on max packet size
    uint16_t mps = gs.ep_IN[ep].max_packet_size;
    uint16_t packet_count = 1; // for ZLPs
    if (writeSize > 0) {       // if length is nonzero
        packet_count = writeSize / mps + (((writeSize % mps) > 0) ? 1 : 0);
    }

    // set zlp_next if transmission size is integer multiple of max packet size6
    gs.ep_IN[ep].zlp_next = (writeSize > 0) && ((writeSize % mps) == 0);

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

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

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

    // 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[i] = 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
void usbdrv_arm_OUT_endpoint(uint8_t ep, uint8_t size) {
    USBOUTEP[ep].DOEPTSIZ |= USB_OTG_DOEPTSIZ_PKTCNT | size;                     // program DIEPTSIZ with maximum (expected) transfer length and set PCKTCNT to make ready for reception
    SET_BIT(USBOUTEP[ep].DOEPCTL, USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK); // enable endpoint and clear NAK
}

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

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

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

// push event onto the event queue
// void usbdrv_push_event(uint32_t evt_code, USBDRV_EventData *evt_data) {
//     USBDRV_EventCompound evt_cpd;
//     if (evt_data != NULL) {
//         evt_cpd.data = *evt_data;
//     }
//     evt_cpd.code = evt_code;
//     q_push(gs.event_queue, &evt_cpd);
// }

// // call this to process incoming events
// void usbdrv_periodic_processing() {
//     if ((gs.event_queue != NULL) && (q_avail(gs.event_queue))) {
//         USBDRV_EventCompound evt_cpd;
//         q_top(gs.event_queue, &evt_cpd);
//         q_pop(gs.event_queue);
//         usbdrv_process_event(evt_cpd.code, NULL);
//     }
// }

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

// 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);
}

// always pass ALIGNED data!
__weak void usbcore_process_setup_pckt(const uint8_t *data, uint16_t size, uint8_t stage) {
    return;
}

__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
        SET_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_CSRST);
        WAIT_FOR_BIT(USBG->GRSTCTL, USB_OTG_GRSTCTL_CSRST);
        usbdrv_reset();
        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
        }
        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) { // TODO maybe the
                    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");

                        usbdrv_arm_OUT_endpoint(ep, gs.ep_OUT[ep].max_packet_size);
                    }
                }
            }
            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;
                if (gs.ep_IN[ep].is_configured) {                             // 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");
                    }

                    if (READ_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_XFRC)) { // IN transaction done
                        SET_BIT(USBINEP[ep].DIEPINT, USB_OTG_DIEPINT_XFRC);

                        // 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);

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

                    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);

                        // USBMSG("IN FIFOEMPTY [%d]\n", ep);

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

            // // set new address if it's already waiting
            // if (gs.new_address != 0) {
            //     WRITE_FIELD(USBD->DCFG, USB_OTG_DCFG_DAD, gs.new_address);
            //     gs.new_address = 0;
            //     USBMSG("ADDR SET\n");
            // }
        }

        default:
            break;
        }
    }

    default:
        break;
    }
}

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

// 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;
}

void OTG_FS_IRQHandler() {
    uint32_t ints = USBG->GINTSTS;

    // USB reset
    if (ints & USB_OTG_GINTSTS_USBRST) {
        SET_BIT(USBG->GINTSTS, USB_OTG_GINTSTS_USBRST); // clear interrupt
        // usb_reset();                                    // reset the USB subsystem
        // return;
        // usbdrv_push_event(USB_EVT_USB_RESET, NULL);
        return;
    }

    // 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
        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) {
        usbdrv_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
        usbdrv_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) {
        usbdrv_process_event(USB_EVT_IN_DONE, NULL);
        // usbdrv_push_event(USB_EVT_IN_DONE, NULL);
    }

    return;
}