#include "usb_device.h"
#include "usb.h"
#include <stdlib.h>
#include <stddef.h>
#include <stm32h747xx.h>
#include "registers.h"
#include "delay.h"

/* USB_OTG_GlobalTypeDef */

usb_device_t usb_devices[2];
void* usb_periph_addresses[] =
{ (void*)USB1_OTG_HS, (void*)USB2_OTG_FS };

void* usb_device_init(usb_device_slot_t slot, usb_class_vtable_t *class,
                      uint16_t id_vendor, uint16_t id_product,
                      char16_t *vendor_name, char16_t *product_name,
                      uint16_t serial_number, char16_t* serial_name) {
  usb_device_t* device = &usb_devices[slot];
  void* peripheral_address = usb_periph_addresses[slot];

  device->state = INIT;
  device->setup.detected_setup_errors = 0;
  device->core = peripheral_address + USB_OTG_GLOBAL_BASE;
  device->device = peripheral_address + USB_OTG_DEVICE_BASE;
  device->out = peripheral_address + USB_OTG_OUT_ENDPOINT_BASE;
  device->in = peripheral_address + USB_OTG_IN_ENDPOINT_BASE;
  device->fifos = (usb_fifo_t*)(((uint8_t*)device->core) + USB_OTG_FIFO_BASE);

  device->setup.received_setup_commands = queue_malloc(sizeof(usb_setup_command_t), MAX_SETUP_PACKETS);
  device->setup.rcvd_awaiting = 0;
  device->setup.rcvd_count = 0;
  device->setup.rcvd_data = NULL;
  queue_init(device->setup.received_setup_commands, sizeof(usb_setup_command_t), MAX_SETUP_PACKETS);
  device->vt = *class;

  device->class = device->
    vt.init(device, id_vendor, id_product,
            vendor_name, product_name, serial_number, serial_name);

  return device;
}

void usb_device_reset(void* device_ptr) {
  usb_device_t* device = (usb_device_t*)device_ptr;

  // Idle
  while ((device->core->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0);
  // Reset
  device->core->GRSTCTL |= USB_OTG_GRSTCTL_CSRST;
  // Wait for reset
  while (device->core->GRSTCTL & USB_OTG_GRSTCTL_CSRST);
  // Wait for idle
  while ((device->core->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0);
  delay_us(3);
}

void usb_device_clear_interrupts(void* device_ptr) {
  usb_device_t* device = (usb_device_t*)device_ptr;
  device->core->GINTSTS = 0; // clear interrupts

  // for other interrupts the rx queue has to be read
}

void usb_device_wait_for_handshake(void* device_ptr) {
  usb_device_t* device = (usb_device_t*)device_ptr;

  while (device->state != ENUMERATED);
}

void usb_device_setup(void* device_ptr) {
  // hsi48 as source
  reg_write_bits(&RCC->D2CCIP2R, 0b11 << RCC_D2CCIP2R_USBSEL_Pos, RCC_D2CCIP2R_USBSEL_Msk);

  RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN | RCC_AHB1ENR_USB1OTGHSULPIEN;

  NVIC_SetPriority(OTG_HS_IRQn, 2);
  NVIC_EnableIRQ(OTG_HS_IRQn);

  // 1. core initialization
  usb_device_t* device = (usb_device_t*)device_ptr;

  device->device->DCTL |= USB_OTG_DCTL_SDIS;
  delay_us(3);

  PWR->CR3 |= PWR_CR3_USB33DEN;
  while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0);

  device->core->GCCFG &= ~USB_OTG_GCCFG_PWRDWN;
  device->core->GCCFG |= USB_OTG_GCCFG_VBDEN;

  device->core->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;

  delay_us(3);
  usb_device_reset(device_ptr);
  delay_us(3);

  *(uint32_t*)((uint8_t*)device->core + USB_OTG_PCGCCTL_BASE) = 0;

  // global interrupt mask
  // rx fifo non empty
  // periodic tx fifo empty level
  usb_device_clear_interrupts(device_ptr);
  device->core->GAHBCFG = USB_OTG_GAHBCFG_GINT | USB_OTG_GAHBCFG_TXFELVL | USB_OTG_GAHBCFG_HBSTLEN_2;

  // hnp capable, srp capable, otg hs timeout, usb turnaround
  // TODO: based on enumeration speed set TOCAL - fs timeout calibration
  device->core->GUSBCFG = USB_OTG_GUSBCFG_FDMOD | USB_OTG_GUSBCFG_ULPIEVBUSI | USB_OTG_GUSBCFG_ULPIEVBUSD | (9 << USB_OTG_GUSBCFG_TRDT_Pos);

  // TODO: are these needed? device mode is forced...
  // unmask otg interrupt mask
  /* device->core->GINTMSK |= USB_OTG_GINTMSK_OTGINT | USB_OTG_GINTMSK_MMISM; */

  // device initialization

  // descdma, device speed, non-zero-length status out handshake, periodic frame interval
  device->device->DCFG = 0;
  device->core->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL | USB_OTG_GOTGCTL_BVALOEN;

  // TODO device threshold control register IF DMA

  // unmask interrupts usb reset, enumeration done, early suspend, usb suspend, sof
  device->core->GINTMSK |= USB_OTG_GINTMSK_ENUMDNEM | USB_OTG_GINTMSK_RSTDEM | USB_OTG_GINTMSK_USBRST | USB_OTG_GINTMSK_ESUSPM | USB_OTG_GINTMSK_SOFM | USB_OTG_GINTMSK_RXFLVLM;

  // sdis bit
  device->device->DCTL = ~(USB_OTG_DCTL_SDIS);

  // wait for usbrst interrupt - the reset
  /* while (((device->core->GINTSTS) & USB_OTG_GINTSTS_USBRST) == 0) {} */

  // wait for enumdne interrupt - end of reset
  /* while (((device->core->GINTSTS) & USB_OTG_GINTSTS_ENUMDNE) == 0) {} */
}

task_result_t usb_handle_setup_command(usb_device_t *device, usb_setup_command_t* cmd) {
  task_result_t result = RES_ERROR;

  switch (cmd->bRequest) {
  case USB_SETUP_GET_STATUS: {
    uint8_t size;
    uint8_t* packet;
    usb_device_status_t device_status;
    usb_interface_status_t interface_status;
    usb_endpoint_status_t endpoint_status;

    switch (cmd->bmRequestType.recipient) {
    case USB_SETUP_DEVICE: {
      size = sizeof(usb_device_status_t);
      device_status.self_powered = 0;
      device_status.remote_wakeup = 0;
      device_status.reserved_zeros = 0;
      packet = (uint8_t*)&device_status;
    }
      break;
    case USB_SETUP_INTERFACE: {
      size = sizeof(usb_interface_status_t);
      interface_status.reserved = 0;
      packet = (uint8_t*)&interface_status;
    }
      break;
    case USB_SETUP_ENDPOINT: {
      size = sizeof(usb_endpoint_status_t);
      endpoint_status.halt = 0;
      endpoint_status.reserved_zeros = 0;
      packet = (uint8_t*)&endpoint_status;
    }
      break;
    }

    result = usb_generic_send(device->in, packet, size,
                              &device->fifos[0].data[0]);
    if (result == RES_OK) {
      device->setup.stage = SETUP_STAGE_SENDING_RESPONSE;
    }
  }
    break;
  case USB_SETUP_GET_DESCRIPTOR: {
    usb_descriptor_type_t descriptor_type = (cmd->wValue >> 8) & 0xFF;
    uint8_t descriptor_index = cmd->wValue & 0xFF;

    switch (descriptor_type) {
    case DESCRIPTOR_DEVICE:
      result = usb_send_device_descriptor(device->in, &device->class->device_descriptor, cmd->wLength, &device->fifos[0].data[0]);
      break;
    case DESCRIPTOR_CONFIGURATION:
      result = device->vt.send_configuration(device, cmd);
      break;
    case DESCRIPTOR_STRING: {
      if (descriptor_index == 0) {
        result = usb_send_string_descriptor_zero(device->in, &device->class->string_descriptor_zero, cmd->wLength, &device->fifos[0].data[0]);
      } else {
        uint8_t index = descriptor_index - 1;
        // NOTE: the user could potentially read different memory part!!
        // This has to be fixed, the length has to be stored somewhere.
        result = usb_send_unicode_string_descriptor(device->in, &device->class->string_descriptors[index], cmd->wLength, &device->fifos[0].data[0]);
      }
    }
      break;
    case DESCRIPTOR_INTERFACE:
      result = usb_send_interface_descriptor(device->in, &device->class->interfaces[descriptor_index].interface_descriptor, cmd->wLength, &device->fifos[0].data[0]);
      break;
    case DESCRIPTOR_ENDPOINT:
      // TODO: how to match the interface to the descriptor index?
      result = usb_send_endpoint_descriptor(device->in, &device->class->interfaces[0].endpoint_descriptors[descriptor_index], cmd->wLength, &device->fifos[0].data[0]);
      break;
    case DESCRIPTOR_DEVICE_QUALIFIER:
      result = usb_send_device_qualifier_descriptor(device->in, &device->class->device_qualifier, cmd->wLength, &device->fifos[0].data[0]);
      break;
    case DESCRIPTOR_OTHER_SPEED_CONFIGURATION:
    case DESCRIPTOR_INTERFACE_POWER:
      reg_set_bits(&device->out[0].DOEPCTL, USB_OTG_DOEPCTL_STALL);
      break;
    }
    if (result == RES_OK) {
      device->setup.stage = SETUP_STAGE_SENDING_RESPONSE;
    }
  }
    break;
  case USB_SETUP_GET_CONFIGURATION: {
    uint8_t value = device->class->configuration_descriptor.bConfigurationValue;
    if (device->state != ENUMERATED) {
      value = 0;
    }
    result = usb_generic_send(device->in, &value, sizeof(value), &device->fifos[0].data[0]);
    if (result == RES_OK) {
      device->setup.stage = SETUP_STAGE_SENDING_RESPONSE;
    }
  }
    break;
  case USB_SETUP_GET_INTERFACE: {
    result = usb_generic_send(device->in,
                              &device->class->interfaces[cmd->wIndex].interface_descriptor.bAlternateSetting,
                              sizeof(uint8_t),
                              &device->fifos[0].data[0]);
    if (result == RES_OK) {
      device->setup.stage = SETUP_STAGE_SENDING_RESPONSE;
    }
  }
    break;
  case USB_SETUP_SET_ADDRESS:
    reg_write_bits_pos(&device->device->DCFG,
                       cmd->wValue,
                       USB_OTG_DCFG_DAD_Pos,
                       0x7F);
    result = usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);
    if (result == RES_OK) {
      device->setup.stage = SETUP_STAGE_SENDING_ACK;
      device->state = SET_ADDRESS_RCVD;
    }
    break;
  case USB_SETUP_SET_CONFIGURATION: {
    if (cmd->wValue == 0) {
      device->vt.reset_endpoints(device);

      result = usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);

      if (result == RES_OK) {
        device->state = SET_ADDRESS_RCVD;
      }
    } else if (cmd->wValue == device->class->configuration_descriptor.bConfigurationValue) {

      device->vt.setup_endpoints(device, cmd->wValue);

      result = usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);

      if (result == RES_OK) {
        device->setup.stage = SETUP_STAGE_SENDING_ACK;
        device->state = ENUMERATED;
      }
    } else {
      reg_set_bits(&device->in[0].DIEPCTL, USB_OTG_DIEPCTL_STALL);
    }
  }
    break;
  case USB_SETUP_CLEAR_FEATURE:
    switch (cmd->wValue) {
    case USB_FEATURE_ENDPOINT_HALT: {
      uint8_t ep_id = cmd->wIndex;
      reg_clear_bits(&device->in[ep_id].DIEPCTL, USB_OTG_DIEPCTL_STALL);
      reg_clear_bits(&device->out[ep_id].DOEPCTL, USB_OTG_DOEPCTL_STALL);
    }
      break;
    case USB_FEATURE_TEST_MODE:
    case USB_FEATURE_DEVICE_REMOTE_WAKEUP:
      // Do not do anything...
      break;
    }

    result = usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);
    if (result == RES_OK) {
      device->setup.stage = SETUP_STAGE_SENDING_ACK;
    }
    break;
  case USB_SETUP_SET_FEATURE:
    switch (cmd->wValue) {
    case USB_FEATURE_ENDPOINT_HALT: {
      uint8_t ep_id = cmd->wIndex;
      reg_set_bits(&device->in[ep_id].DIEPCTL, USB_OTG_DIEPCTL_STALL);
      reg_set_bits(&device->out[ep_id].DOEPCTL, USB_OTG_DOEPCTL_STALL);
    }
      break;
    case USB_FEATURE_TEST_MODE:
    case USB_FEATURE_DEVICE_REMOTE_WAKEUP:
      // Do not do anything...
      break;
    }

    result = usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);
    if (result == RES_OK) {
      device->setup.stage = SETUP_STAGE_SENDING_ACK;
    }
    break;
  case USB_SETUP_SET_DESCRIPTOR:
  case USB_SETUP_SET_INTERFACE:
  case USB_SETUP_SYNCH_FRAME:
    device->in->DIEPCTL |= USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_STALL;
    device->state = UNKNOWN_CONTROL_COMMAND;
    break;
  case RESERVED1:
  case RESERVED2:
  default:
    return device->vt.setup_packet_callback(device, cmd);
    break;
  }

  return result;
}

typedef enum {
  USB_UNKNOWN,
  USB_GOT_SETUP,
  USB_PROCESS_SETUP,
  USB_GOT_ACK,
  USB_GOT_DATA,
  USB_SENT_RESPONSE,
} usb_event_t;

task_result_t usb_handle_setup(usb_device_t *device, usb_event_t event) {
  switch (device->setup.stage) {
  case SETUP_STAGE_RCVD_SETUP_PACKET: {
    if (event == USB_GOT_SETUP) {
      return RES_OK;
    }

    usb_setup_command_t* command = queue_peek(device->setup.received_setup_commands);

    if (command == NULL) {
      device->setup.stage = SETUP_STAGE_NONE;
      break;
    }

    // pre-processing of data commands - first get the data, only then proceed with handle_setup_command
    if (command->bmRequestType.direction == USB_SETUP_HOST_TO_DEVICE && command->wLength > 0 && device->setup.rcvd_awaiting == 0) {
      device->setup.rcvd_awaiting = command->wLength;
      device->setup.rcvd_count = 0;
      device->setup.rcvd_data = (uint8_t*)malloc(device->setup.rcvd_count);

      device->setup.stage = SETUP_STAGE_AWAITING_DATA;
      break;
    }

    task_result_t result = usb_handle_setup_command(device, command);
    if (result == RES_OK) {
      queue_dequeue(device->setup.received_setup_commands);
      uint8_t setup_packets_count = reg_read_bits_pos(&device->out[0].DOEPTSIZ, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
      if (setup_packets_count == 0) {
        reg_write_bits_pos(&device->out[0].DOEPTSIZ, 3, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
      }

      if (device->setup.rcvd_data != NULL) {
        free(device->setup.rcvd_data);
        device->setup.rcvd_data = NULL;
      }

      device->setup.rcvd_awaiting = 0;
      device->setup.rcvd_count = 0;
    }
    return result;
  }
    break;
  case SETUP_STAGE_NONE:
    if (queue_peek(device->setup.received_setup_commands) != NULL) {
      device->setup.stage = SETUP_STAGE_RCVD_SETUP_PACKET;

      if (event == USB_PROCESS_SETUP) {
        usb_handle_setup(device, event);
      }
    }
    break;
  case SETUP_STAGE_AWAITING_DATA:
    if (event == USB_GOT_DATA) {
      if (device->setup.rcvd_count >= device->setup.rcvd_awaiting) {
        device->setup.stage = SETUP_STAGE_RCVD_SETUP_PACKET;
      }
    }
    break;
  case SETUP_STAGE_SENDING_RESPONSE:
    if (event == USB_SENT_RESPONSE) {
      device->setup.stage = SETUP_STAGE_AWAITING_ACK;
    }
    break;
  case SETUP_STAGE_SENDING_ACK:
    if (event == USB_SENT_RESPONSE) {
      device->setup.stage = SETUP_STAGE_NONE;
      return usb_handle_setup(device, event);
    }
    break;
  case SETUP_STAGE_AWAITING_ACK:
    if (event == USB_GOT_ACK) {
      device->setup.stage = SETUP_STAGE_NONE;
      return usb_handle_setup(device, event);
    }
    break;
  }

  return RES_OK;
}


void usb_handle_rxflvl_control_int(usb_device_t *device,
                                   packet_info_t *packet_info) {
  uint8_t data[64];
  volatile uint32_t *fifo = device->fifos[0].data;

  if (packet_info->packet_status == PACKET_SETUP_TRANSACTION_COMPLETED) {
    if (packet_info->byte_count > 0) {
      usb_generic_read(data, packet_info->byte_count, fifo);
    }
  } else if (packet_info->packet_status == PACKET_SETUP_DATA_PACKET_RECEIVED) {
    // SAVE data
    usb_setup_command_t command;
    usb_generic_read((uint8_t*)&command, 8, fifo);

    if (!queue_enqueue(device->setup.received_setup_commands, &command)) {
      // Got a problem, setup command lost!
      device->setup.detected_setup_errors++;
    }

    usb_handle_setup(device, USB_GOT_SETUP);
  } else if (packet_info->byte_count != 0) {
    if (device->setup.stage == SETUP_STAGE_AWAITING_DATA && device->setup.rcvd_data != NULL) {
      // TODO: check if still enough space
      usb_generic_read(&device->setup.rcvd_data[device->setup.rcvd_count],
                       packet_info->byte_count, fifo);
      device->setup.rcvd_count += packet_info->byte_count;
    }

    if (device->setup.stage == SETUP_STAGE_AWAITING_ACK) {
      // This is an error, since there is data in status phase...
      // TODO: How to handle?
      device->setup.detected_setup_errors++;
    }
  }
}

void usb_handle_rxflvl_int(usb_device_t *device) {
  // Disable the interrupt
  reg_clear_bits(&device->core->GINTMSK, USB_OTG_GINTMSK_RXFLVLM);

  /* uint32_t rx_data; */
  /* while ((rx_data = device->core->GRXSTSP) != 0) { */
  while (device->core->GINTSTS & USB_OTG_GINTSTS_RXFLVL) {
    uint32_t status_data = device->core->GRXSTSP;

    /* uint8_t status_phase_start = reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_STSPHST); */
    packet_info_t packet_info = {
      .status_phase_start = reg_read_bits_pos(&status_data, 27, 0x01),
      .packet_status = (packet_status_t)reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_PKTSTS_Pos, 0xF),
      .dpid = (packet_dpid_t)reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_DPID_Pos, 0x3),
      .byte_count = reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_BCNT_Pos, 0x7FF),
      .endpoint_num = reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_EPNUM_Pos, 0xF),
    };

    if (packet_info.endpoint_num == 0) {
      usb_handle_rxflvl_control_int(device, &packet_info);
    } else {
      device->vt.received_data_callback(device, &packet_info);
    }
  }

  // Re-enable the interrupt
  reg_set_bits(&device->core->GINTMSK, USB_OTG_GINTMSK_RXFLVLM);
}

uint8_t usb_daint_get_endpoint_number(uint32_t endpoints) {
  for (int i = 0; i < 15; i++) {
    if (endpoints & (1 << i)) {
      return i;
    }
  }
  return 0xFF;
}

void usb_handle_endpoint_in_int(usb_device_t* device) {
  uint32_t daint = reg_read_bits_pos(&device->device->DAINT, USB_OTG_DAINT_IEPINT_Pos, 0xFFFF);
  uint8_t ep_id = 0;
  while (ep_id != 0xFF) {
    ep_id = usb_daint_get_endpoint_number(daint);
    daint &= ~(1 << ep_id);

    if (ep_id == 0xFF) {
      return;
    }

    uint32_t interrupt_reg = device->in[ep_id].DIEPINT;

    if (interrupt_reg & USB_OTG_DIEPINT_PKTDRPSTS) {
      // Not generated by interrupt! So if we get this one
      // here, it's not from the interrupt. Just clear it,
      // isochronous pipes can drop data from time to time
      reg_clear_bits(&device->in[ep_id].DIEPINT, USB_OTG_DIEPINT_PKTDRPSTS);
    }

    if (interrupt_reg & USB_OTG_DIEPINT_NAK) {
      // NOTE no need to do much. Hardware will resend
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_NAK;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_BNA) {
      // NOTE this should not be reached as DMA is not used
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_BNA;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_TXFIFOUDRN) {
      // NOTE this should not be reached as thresholding is not used
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_TXFIFOUDRN;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_TXFE) {
      if (device->state == ENUMERATED) {
        device->vt.txfifo_empty_callback(device, ep_id);
      }
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_TXFE;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_INEPNE) {
      // NAK effective. Okay, ack, go on.
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_INEPNE;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_INEPNM) {
      device->state = USB_DEV_ERROR; // data on top of TxFIFO belong to endpoint other
      // than the one for which in token was received.
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_INEPNM;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_ITTXFE) {
      // In token when no data. How to proceed? TODO
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_ITTXFE;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_TOC) {
      // Timeout condition. Skip? TODO how to proceed?
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_TOC;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_AHBERR) {
      // NOTE this should not be reached as thresholding is not used
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_AHBERR;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_EPDISD) {
      // Endpoint is disabled, per application's request. Okay.
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_EPDISD;
    }
    if (interrupt_reg & USB_OTG_DIEPINT_XFRC) {
      // Transfer is completed.
      if (device->state == ENUMERATED) {
        device->vt.rx_done_callback(device, ep_id);
      }

      if (ep_id == 0) {
        usb_handle_setup(device, USB_SENT_RESPONSE);
        usb_handle_setup(device, USB_PROCESS_SETUP);
      }
      device->in[ep_id].DIEPINT = USB_OTG_DIEPINT_XFRC;
    }
  }
}

#define USB_OTG_DOEPINT_BNA_Pos              (9U)
#define USB_OTG_DOEPINT_BNA_Msk              (0x1UL << USB_OTG_DOEPINT_B2BSTUP_Pos) /*!< 0x00000040 */
#define USB_OTG_DOEPINT_BNA                  USB_OTG_DOEPINT_B2BSTUP_Msk   /*!< Back-to-back SETUP packets received */

void usb_handle_endpoint_out_int(usb_device_t* device) {
  uint32_t daint = reg_read_bits_pos(&device->device->DAINT, USB_OTG_DAINT_OEPINT_Pos, 0xFFFF);
  uint8_t ep_id = 0;
  while (ep_id != 0xFF) {
    ep_id = usb_daint_get_endpoint_number(daint);
    daint &= ~(1 << ep_id);

    if (ep_id == 0xFF) {
      return;
    }

    uint32_t interrupt_reg = device->out[ep_id].DOEPINT;

    if (interrupt_reg & USB_OTG_DOEPINT_STPKTRX) {
      // NOTE This shouldn't be reached since DMA is not used
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_STPKTRX;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_NYET) {
      // We don't really care about this one for now
      device->vt.nyet_callback(device, ep_id);
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_NYET;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_NAK) {
      // We don't really care about this one for now
      device->vt.nak_callback(device, ep_id);
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_NAK;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_BERR) {
      // Uh? Babble much?
      device->state = CONTROL_ERROR;
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_BERR;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_BNA) {
      // NOTE This shoudln't be reached since DMA is not used
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_BNA;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_OUTPKTERR) {
      // NOTE thresholding not enabled, so this shouldn't be reached
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_OUTPKTERR;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_B2BSTUP) {
      // TODO: this is a problem! we couldn't capture all the packets!
      device->state = CONTROL_ERROR;
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_B2BSTUP;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_OTEPSPR) {
      // TODO: is there someting to do? The data sending is usually handled prior
      //       to this, in handling of setup / data stages. Maybe it should be
      //       moved here instead, since it's possible other packet will be
      //       received in the meantime?
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_OTEPSPR;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_OTEPDIS) {
      // NOTE: Can we handle this? a callback to application?
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_OTEPDIS;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_STUP) {
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_STUP;

      if (ep_id == 0) {
        usb_handle_setup(device, USB_PROCESS_SETUP);
        device->out[ep_id].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
      }
    }
    if (interrupt_reg & USB_OTG_DOEPINT_AHBERR) {
      // NOTE This shoudln't be reached since DMA is not used
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_AHBERR;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_EPDISD) {
      // NOTE endpoint has been disabled, as was instructed. So this shoudln't
      // need handling?
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_EPDISD;
    }
    if (interrupt_reg & USB_OTG_DOEPINT_XFRC) {
      device->out[ep_id].DOEPINT = USB_OTG_DOEPINT_XFRC;

      if (device->state == ENUMERATED) {
        device->vt.rx_done_callback(device, ep_id);
      }

      if (ep_id == 0) {
        if (device->setup.rcvd_count > 0) {
          usb_handle_setup(device, USB_GOT_DATA);
        } else {
          usb_handle_setup(device, USB_GOT_ACK);
        }

        // Process next packet / data
        usb_handle_setup(device, USB_PROCESS_SETUP);

        device->out[ep_id].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
      }
    }
  }
}

// NOTE: this is irq handler
void otg_hs_handler(void) {

  usb_device_t* device = &usb_devices[USB_OTG_HS1];
  uint8_t handled = 0;

  // Start of frame
  if (device->core->GINTSTS & USB_OTG_GINTSTS_SOF) {
    device->core->GINTSTS = USB_OTG_GINTSTS_SOF;
    handled = 1;

    /* usb_handle_setup(device, USB_PROCESS_SETUP); */
  }

  // Reset detected
  if (device->core->GINTSTS & USB_OTG_GINTSTS_USBRST) {
    // clear it
    device->core->GINTSTS = USB_OTG_GINTSTS_USBRST;

    device->setup.stage = SETUP_STAGE_NONE;
    while (queue_dequeue(device->setup.received_setup_commands) != NULL);

    reg_set_bits(&device->core->GINTMSK, USB_OTG_GINTMSK_IEPINT | USB_OTG_GINTMSK_OEPINT);
    reg_set_bits(&device->device->DOEPMSK, USB_OTG_GINTMSK_IEPINT | USB_OTG_GINTMSK_OEPINT);

    for (int ep = 0; ep < 8; ep++) {
      USB_OTG_OUTEndpointTypeDef *out = device->out + ep;
      out->DOEPCTL |= USB_OTG_DOEPCTL_SNAK;
    }

    device->device->DCTL = USB_OTG_DCTL_CGINAK | USB_OTG_DCTL_CGONAK;

    device->device->DAINTMSK |= 1 << USB_OTG_DAINTMSK_IEPM_Pos;
    device->device->DAINTMSK |= 1 << USB_OTG_DAINTMSK_OEPM_Pos;

    device->device->DOEPMSK |= USB_OTG_DOEPMSK_STUPM | USB_OTG_DOEPMSK_XFRCM;
    device->device->DIEPMSK |= USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_TOM;

    // 512 bytes
    device->core->GRXFSIZ = 1024 / 4;
    // 64 bytes, beginning of ram
    device->core->DIEPTXF[0] = (0 << USB_OTG_DIEPTXF_INEPTXSA_Pos) | ((128 / 4) << USB_OTG_DIEPTXF_INEPTXFD_Pos);

    // 3 packets to receive as setup
    reg_write_bits_pos(&device->out->DOEPTSIZ, 3, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);

    device->state = USB_DEV_RESET;

    return;
  }

  // Reset ended
  if (device->core->GINTSTS & USB_OTG_GINTSTS_ENUMDNE) {
    device->core->GINTSTS = USB_OTG_GINTSTS_ENUMDNE;
    // The enumerated speed
    /* (device->device->DSTS & USB_OTG_DSTS_ENUMSPD_Msk) >> USB_OTG_DSTS_ENUMSPD_Pos; */
    // We do not need to know the speed, it can be either full speed or high
    // speed, both have maximum size 64 bytes. Only low speed or super high
    // speed would differ, but these are not supported!
    reg_write_bits_pos(&device->in[0].DIEPCTL, 0, USB_OTG_DIEPCTL_MPSIZ_Pos, 0x7FFUL);

    device->in->DIEPCTL |= USB_OTG_DOEPCTL_SNAK;
    device->out->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;

    device->state = RESET_DONE;

    return;
  }

  // OTG interrupt, should not be triggered since forced device mode
  if (device->core->GINTSTS & USB_OTG_GINTSTS_OTGINT) {
    device->state = OTG_ERROR;
    device->core->GINTSTS = USB_OTG_GINTSTS_OTGINT;
    return;
  }

  // OTG interrupt, should not be triggered since forced device mode
  if (device->core->GINTSTS & USB_OTG_GINTSTS_MMIS) {
    device->state = OTG_ERROR;
    device->core->GINTSTS = USB_OTG_GINTSTS_MMIS;
    return;
  }

  // Setup, data...
  if (device->core->GINTSTS & USB_OTG_GINTSTS_OEPINT) {
    usb_handle_endpoint_out_int(device);
    handled = 1;
  }

  if (device->core->GINTSTS & USB_OTG_GINTSTS_IEPINT) {
    usb_handle_endpoint_in_int(device);
    handled = 1;
  }

  if (device->core->GINTSTS & (USB_OTG_GINTSTS_RXFLVL)) {
    usb_handle_rxflvl_int(device);
    handled = 1;
  }

  if (device->core->GINTSTS & (USB_OTG_GINTSTS_RSTDET)) {
    // Soft reset, meaning the usb is likely not connected
    device->core->GINTSTS = USB_OTG_GINTSTS_RSTDET;
    device->setup.stage = SETUP_STAGE_NONE;
    while (queue_dequeue(device->setup.received_setup_commands) != NULL);
    handled = 1;
  }

  if (device->core->GINTSTS & (USB_OTG_GINTSTS_ESUSP)) {
    // The usb has probably been disconnected
    device->core->GINTSTS = USB_OTG_GINTSTS_ESUSP;
    device->setup.stage = SETUP_STAGE_NONE;
    while (queue_dequeue(device->setup.received_setup_commands) != NULL);
    handled = 1;
  }

  if (handled == 0) {
    device->state = UNKNOWN_INTERRUPT;
  }
}