#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,
                      char *vendor_name, char *product_name,
                      uint16_t serial_number, char* serial_name) {
  usb_device_t* device = &usb_devices[slot];
  void* peripheral_address = usb_periph_addresses[slot];

  device->state = INIT;
  device->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->received_setup_commands_count = 0;
  device->received_setup_commands_index = 0;
  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) {
  RCC->AHB1ENR |= RCC_AHB1ENR_USB1OTGHSEN | RCC_AHB1ENR_USB1OTGHSULPIEN;

  volatile uint32_t dummy;
  dummy = RCC->AHB1ENR;
  dummy = RCC->AHB1ENR;

  NVIC_SetPriority(OTG_HS_IRQn, 1);
  NVIC_EnableIRQ(OTG_HS_IRQn);

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

  device->device->DCTL |= USB_OTG_DCTL_SDIS;

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

  device->core->GCCFG |= USB_OTG_GCCFG_PWRDWN;

  device->core->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;

  DELAY_US(3);
  usb_device_reset(device_ptr);

  // 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 | USB_OTG_GUSBCFG_TRDT_3 | USB_OTG_GUSBCFG_TRDT_2;

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

  // 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) {} */
}

typedef enum {
  PACKET_GLOBAL_OUT_NAK = 1,
  PACKET_OUT_DATA_PACKET_RCVD = 2,
  PACKET_OUT_TRANSFER_COMPLETED = 3,
  PACKET_SETUP_TRANSACTION_COMPLETED = 4,
  PACKET_SETUP_DATA_PACKET_RECEIVED = 6,
} packet_status_t;

typedef enum {
  DATA0 = 0,
  DATA1 = 1,
  DATA2 = 2,
  MDATA = 3
} packet_dpid_t;

typedef struct {
  packet_status_t packet_status;
  packet_dpid_t dpid;
  uint8_t byte_count;
  uint8_t endpoint_num;
} packet_info_t;

void usb_handle_setup(usb_device_t *device, usb_setup_command_t* cmd) {
  if (device->setup_stage != SETUP_STAGE_RCVD_SETUP_PACKET) {
    device->detected_setup_errors++;
  }

  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*)&interface_status;
    }
      break;
    }

    usb_generic_send(device->in, packet, size,
                     &device->fifos[0].data[0]);
    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:
      usb_send_device_descriptor(device->in, &device->class->device_descriptor, &device->fifos[0].data[0]);
      break;
    case DESCRIPTOR_CONFIGURATION:
      device->vt.send_configuration(device, cmd);
      break;
    case DESCRIPTOR_STRING: {
      if (descriptor_index == 0) {
        usb_send_string_descriptor_zero(device->in, &device->class->string_descriptor_zero, &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.
        usb_send_unicode_string_descriptor(device->in, &device->class->string_descriptors[index], &device->fifos[0].data[0]);
      }
    }
      break;
    case DESCRIPTOR_INTERFACE:
      usb_send_interface_descriptor(device->in, &device->class->interfaces[descriptor_index].interface_descriptor, &device->fifos[0].data[0]);
      device->setup_stage = SETUP_STAGE_SENDING_RESPONSE;
      break;
    case DESCRIPTOR_ENDPOINT:
      // TODO: how to match the interface to the descriptor index?
      usb_send_endpoint_descriptor(device->in, &device->class->interfaces[0].endpoint_descriptors[descriptor_index], &device->fifos[0].data[0]);
      device->setup_stage = SETUP_STAGE_SENDING_RESPONSE;
      break;
    case DESCRIPTOR_DEVICE_QUALIFIER:
      usb_send_device_qualifier_descriptor(device->in, &device->class->device_qualifier, &device->fifos[0].data[0]);
      device->setup_stage = SETUP_STAGE_SENDING_RESPONSE;
      break;
    case DESCRIPTOR_OTHER_SPEED_CONFIGURATION:
    case DESCRIPTOR_INTERFACE_POWER:
      reg_set_bits(&device->out[0].DOEPCTL, USB_OTG_DOEPCTL_STALL);
      device->setup_stage = SETUP_STAGE_NONE;
      break;
    }
    // TODO
  }
    break;
  case USB_SETUP_GET_CONFIGURATION: {
    uint8_t value = device->class->configuration_descriptor.bConfigurationValue;
    if (device->state != ENUMERATED) {
      value = 0;
    }
    usb_generic_send(device->in, &value, sizeof(value), &device->fifos[0].data[0]);
    device->setup_stage = SETUP_STAGE_SENDING_RESPONSE;
  }
    break;
  case USB_SETUP_GET_INTERFACE: {
    usb_generic_send(device->in,
                     &device->class->interfaces[cmd->wIndex].interface_descriptor.bAlternateSetting,
                     sizeof(uint8_t),
                     &device->fifos[0].data[0]);
    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);
    usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);
    device->setup_stage = SETUP_STAGE_SENDING_ACK;
    device->state = SET_ADDRESS_RCVD;
    break;
  case USB_SETUP_SET_CONFIGURATION: {
    if (cmd->wValue == 0) {
      device->state = SET_ADDRESS_RCVD;
      usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);

      // TODO disable endpoints
    } else if (cmd->wValue == device->class->configuration_descriptor.bConfigurationValue) {
      device->state = ENUMERATED;
      usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);
      device->setup_stage = SETUP_STAGE_SENDING_ACK;

      // TODO setup endpoints
    } 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;
    }

    usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);
    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;
    }

    usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);
    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:
    device->vt.setup_packet_callback(device, cmd);
    break;
  }
}

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

  if (packet_info->packet_status == PACKET_SETUP_TRANSACTION_COMPLETED) {
    // Nothing do to.
  } else if (packet_info->packet_status == PACKET_SETUP_DATA_PACKET_RECEIVED) {
    // SAVE data
    uint8_t setup_packets_count = 3 - reg_read_bits_pos(&device->out[0].DOEPTSIZ, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
    usb_generic_read(
        (uint8_t *)&device
            ->received_setup_commands[device->received_setup_commands_count++],
        8,
        fifo);
    device->received_setup_commands_count %= 3;
    device->setup_stage = SETUP_STAGE_RCVD_SETUP_PACKET;

    if (setup_packets_count == 0) {
      reg_write_bits_pos(&device->out[0].DOEPTSIZ, 3, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
    }

    dummy = *fifo; // the last that will trigger another interrupt
  } else if (packet_info->byte_count != 0) {
    usb_generic_read(data, packet_info->byte_count, fifo);

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

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 = {
      .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 {
      // TODO: ...
    }
  }

  // Read info about the endpoint etc.

  // 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) {
  uint8_t ep_id = usb_daint_get_endpoint_number(reg_read_bits_pos(&device->device->DAINT, USB_OTG_DAINT_IEPINT_Pos, 0xFFFF));

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

    if (ep_id == 0) {
      if (device->setup_stage == SETUP_STAGE_SENDING_RESPONSE) {
        device->setup_stage = SETUP_STAGE_NONE;
      } else if (device->setup_stage == SETUP_STAGE_SENDING_ACK) {
        device->setup_stage = SETUP_STAGE_AWAITING_ACK;
      }
    }
    reg_clear_bits(&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) {
  /* device->core->GRXSTSP; */
  /* device->core->GRXFSIZ; */

  uint8_t ep_id = usb_daint_get_endpoint_number(reg_read_bits_pos(&device->device->DAINT, USB_OTG_DAINT_OEPINT_Pos, 0xFFFF));

  if (ep_id == 0xFF) {
    device->state = ERROR;
    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
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_STPKTRX);
  } else if (interrupt_reg & USB_OTG_DOEPINT_NYET) {
    // We don't really care about this one for now
    device->vt.nyet_callback(device, ep_id);
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_NYET);
  } else if (interrupt_reg & USB_OTG_DOEPINT_NAK) {
    // We don't really care about this one for now
    device->vt.nak_callback(device, ep_id);
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_NAK);
  } else if (interrupt_reg & USB_OTG_DOEPINT_BERR) {
    // Uh? Babble much?
    device->state = CONTROL_ERROR;
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_BERR);
  } else if (interrupt_reg & USB_OTG_DOEPINT_BNA) {
    // NOTE This shoudln't be reached since DMA is not used
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_BNA);
  } else if (interrupt_reg & USB_OTG_DOEPINT_OUTPKTERR) {
    // NOTE thresholding not enabled, so this shouldn't be reached
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_OUTPKTERR);
  } else if (interrupt_reg & USB_OTG_DOEPINT_B2BSTUP) {
    // TODO: this is a problem! we couldn't capture all the packets!
    device->state = CONTROL_ERROR;
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_B2BSTUP);
  } else if (interrupt_reg & USB_OTG_DOEPINT_OTEPSPR) {
    // This is valid only for DATA phases where the host sends us data.
    // Those are not supported yet! After they are, here is the place to
    // set STALL or send zero length packet.
    // TODO: ack or stall the status phase?
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_OTEPSPR);
  } else if (interrupt_reg & USB_OTG_DOEPINT_OTEPDIS) {
    // NOTE: Can we handle this? a callback to application?
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_OTEPDIS);
  } else if (interrupt_reg & USB_OTG_DOEPINT_STUP) {
    if (device->setup_stage != SETUP_STAGE_NONE) {
      // something went wrong. Let's continue, but this isn't looking good.
      device->detected_setup_errors++;
    }

    uint8_t setup_packets_count = 3 - reg_read_bits_pos(&device->out[ep_id].DOEPTSIZ, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
    usb_handle_setup(device, &device->received_setup_commands[device->received_setup_commands_index]);

    device->received_setup_commands_index++;
    device->received_setup_commands_index %= 3;

    if (setup_packets_count == 0) {
      reg_write_bits_pos(&device->out[0].DOEPTSIZ, 3, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
    }

    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_STUP);
  } else if (interrupt_reg & USB_OTG_DOEPINT_AHBERR) {
    // NOTE This shoudln't be reached since DMA is not used
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_AHBERR);
  } else if (interrupt_reg & USB_OTG_DOEPINT_EPDISD) {
    // NOTE endpoint has been disabled, as was instructed. So this shoudln't
    // need handling?
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_EPDISD);
  } else if (interrupt_reg & USB_OTG_DOEPINT_XFRC) {
    if (ep_id == 0 && device->setup_stage == SETUP_STAGE_AWAITING_ACK) {
      device->setup_stage = SETUP_STAGE_NONE;
    }
    // Transfer has been completed
    // TODO: handle data? - callback to device data handle?
    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_XFRC);
  }
}

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

  usb_device_t* device = &usb_devices[USB_OTG_HS1];

  // Reset detected
  if (device->core->GINTSTS & USB_OTG_GINTSTS_USBRST) {
    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);

    // TODO: should this be done for ep 0 as well?
    for (int ep = 0; ep < 8; ep++) {
      USB_OTG_OUTEndpointTypeDef *out = device->out + ep;
      out->DOEPCTL |= USB_OTG_DOEPCTL_SNAK;
    }
    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 = 256 / 4;
    // 64 bytes, beginning of ram
    device->core->DIEPTXF[0] = (0) << USB_OTG_TX0FSA_Pos | (64 / 4) << USB_OTG_TX0FD_Pos;

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

    device->state = RESET;
    // clear it
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_USBRST);

    return;
  }

  // Reset ended
  if (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->DIEPCTL, 64, USB_OTG_DIEPCTL_MPSIZ_Pos, 0x7FFUL);

    device->out->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;

    device->state = RESET_DONE;
    // clear it
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_ENUMDNE);

    return;
  }

  // Start of frame
  if (device->core->GINTSTS & USB_OTG_GINTSTS_SOF) {
    // Nothing to do?
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_SOF);
    return;
  }

  // OTG interrupt, should not be triggered since forced device mode
  if (device->core->GINTSTS & USB_OTG_GINTSTS_OTGINT) {
    device->state = OTG_ERROR;
    reg_set_bits(&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;
    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_MMIS);
    return;
  }

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

  if (device->core->GINTSTS & USB_OTG_GINTSTS_IEPINT) {
    usb_handle_endpoint_in_int(device);
    return;
  }

  if (device->core->GINTSTS & (USB_OTG_GINTSTS_RXFLVL)) {
    // TODO
    usb_handle_rxflvl_int(device);
    return;
  }

  device->state = UNKNOWN_INTERRUPT;
}