#include "generic.h"
#include "stm32h747xx.h"
#include "usb.h"
#include "usb_device.h"
#include "usb_device_cdc.h"
#include "registers.h"
#include <stdlib.h>
#include <stdint.h>
#include <string.h>

usb_class_header_t* usb_device_cdc_init(usb_device_t *device,
                uint16_t id_vendor, uint16_t id_product,
                char16_t *vendor_name, char16_t *product_name,
                uint16_t serial_number, char16_t* serial_name);

task_result_t usb_device_cdc_send_configuration(usb_device_t* device, usb_setup_command_t* cmd);
void usb_device_cdc_setup_endpoints(usb_device_t* device, uint8_t configuration);
void usb_device_cdc_reset_endpoints(usb_device_t* device);
void usb_device_cdc_reset_callback(usb_device_t* device);
task_result_t usb_device_cdc_setup_packet_callback(usb_device_t* device, usb_setup_command_t* cmd);
void usb_device_cdc_enumeration_done_callback(usb_device_t* device);
void usb_device_cdc_txfifo_empty_callback(usb_device_t* device, uint8_t endpoint);
void usb_device_cdc_rxfifo_full_callback(usb_device_t* device, uint8_t endpoint);
void usb_device_cdc_rx_done_callback(usb_device_t* device, uint8_t endpoint);
void usb_device_cdc_tx_done_callback(usb_device_t* device, uint8_t endpoint);
void usb_device_cdc_nak_callback(usb_device_t* device, uint8_t endpoint);
void usb_device_cdc_nyet_callback(usb_device_t* device, uint8_t endpoint);
void usb_device_cdc_received_data_callback(usb_device_t* device, packet_info_t* packet);

#define MAX_PACKET_SIZE 512

uint16_t usb_cdc_lang_descriptors[] = {
  USB_LANG_ENGLISH | (USB_SUBLANG_ENGLISH_US << 10)
};

usb_class_vtable_t USB_CLASS_CDC_ACM = {
  .init = usb_device_cdc_init,
  .send_configuration = usb_device_cdc_send_configuration,
  .setup_endpoints = usb_device_cdc_setup_endpoints,
  .reset_endpoints = usb_device_cdc_reset_endpoints,
  .reset_callback = usb_device_cdc_reset_callback,

  .setup_packet_callback = usb_device_cdc_setup_packet_callback,
  .enumeration_done_callback = usb_device_cdc_enumeration_done_callback,
  .txfifo_empty_callback = usb_device_cdc_txfifo_empty_callback,
  .rxfifo_full_callback = usb_device_cdc_rxfifo_full_callback,
  .rx_done_callback = usb_device_cdc_rx_done_callback,
  .tx_done_callback = usb_device_cdc_tx_done_callback,
  .nak_callback = usb_device_cdc_nak_callback,
  .nyet_callback = usb_device_cdc_nyet_callback,
  .received_data_callback = usb_device_cdc_received_data_callback,
};

static inline size_t strlen16(register const char16_t * string) {
    if (!string) return 0;
    register size_t len = 0;
    while(string[len++]);
    return len;
}

usb_class_header_t *usb_device_cdc_init(usb_device_t *device, 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_class_header_t* class = (usb_class_header_t*)calloc(1, sizeof(usb_device_cdc_t));

  class->device_descriptor.idProduct = id_product;
  class->device_descriptor.idVendor = id_vendor;
  class->device_descriptor.bcdDevice = serial_number;

  class->string_descriptor_zero.header.bDescriptorType = DESCRIPTOR_STRING;
  class->string_descriptor_zero.header.bLength = sizeof(usb_string_descriptor_zero_t);
  class->string_descriptor_zero.wLANGID = usb_cdc_lang_descriptors;

  uint8_t string_count = 0;

  if (vendor_name != NULL) {
    string_count++;
    class->device_descriptor.iManufacturer = string_count;
  }
  if (product_name != NULL) {
    string_count++;
    class->device_descriptor.iProduct = string_count;
  }
  if (serial_name != NULL) {
    string_count++;
    class->device_descriptor.iSerialNumber = string_count;
  }

  usb_unicode_string_descriptor_t *descriptor =
    (usb_unicode_string_descriptor_t*)malloc(sizeof(usb_unicode_string_descriptor_t) * string_count);
  class->string_descriptors = descriptor;

  if (vendor_name != NULL) {
    descriptor->header.bDescriptorType = DESCRIPTOR_STRING;
    descriptor->header.bLength = 2 + strlen16(vendor_name) * sizeof(char16_t);
    descriptor->bString = (uint16_t*)vendor_name;

    descriptor++;
  }
  if (product_name != NULL) {
    descriptor->header.bDescriptorType = DESCRIPTOR_STRING;
    descriptor->header.bLength = 2 + strlen16(product_name) * sizeof(char16_t);
    descriptor->bString = (uint16_t*)product_name;
    descriptor++;
  }
  if (serial_name != NULL) {
    descriptor->header.bDescriptorType = DESCRIPTOR_STRING;
    descriptor->header.bLength = 2 + strlen16(serial_name) * sizeof(char16_t);
    descriptor->bString = (uint16_t*)serial_name;
    descriptor++;
  }

  return class;
}

void cdc_acm_configure(usb_device_t* device, uint16_t rx_queue_size) {
  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
  usb_class_header_t* header = &cdc->header;

  cdc->endpoint_nak = false;
  cdc->lost_data = 0;

  queue_t* rx_queue = queue_malloc(1, rx_queue_size);
  cdc->rx_buffer = rx_queue;
  queue_init(rx_queue, 1, rx_queue_size);

  usb_device_descriptor_t device_descriptor =
    {
      .header = { .bDescriptorType = DESCRIPTOR_DEVICE, .bLength = sizeof(usb_device_descriptor_t) },
      .bcdUSB = 0x200,
      .bDeviceClass = USB_CLASS_CDC_CODE,
      .bDeviceSubClass = 0x00,
      .bDeviceProtocol = 0x00,
      .bMaxPacketSize0 = 64,
      .idVendor = header->device_descriptor.idVendor,
      .idProduct = header->device_descriptor.idProduct,
      .bcdDevice = 0x0000,
      .iManufacturer = header->device_descriptor.iManufacturer,
      .iProduct = header->device_descriptor.iProduct,
      .iSerialNumber = header->device_descriptor.iSerialNumber,
      .bNumConfigurations = 1,
    };
  header->device_descriptor = device_descriptor;

  usb_device_qualifier_t qualifier =
    {
      .header = { .bDescriptorType = DESCRIPTOR_DEVICE_QUALIFIER, .bLength = sizeof(usb_device_qualifier_t) },
      .bcdUSB = 0x20,
      .bDeviceClass = USB_CLASS_CDC_CODE,
      .bDeviceSubClass = 0x00,
      .bDeviceProtocol = 0x00,
      .bMaxPacketSize0 = 64,
      .bNumConfigurations = 0,
      .bReserved = 0
    };
  header->device_qualifier = qualifier;

  usb_configuration_descriptor_t configuration_descriptor =
    {
      .header = { .bDescriptorType = DESCRIPTOR_CONFIGURATION, .bLength = sizeof(usb_configuration_descriptor_t) },
      .bNumInterfaces = 2,
      .bConfigurationValue = 1,
      .iConfiguration = 0,
      .bmAttributes =
      {
        .self_powered = 0,
        .remote_wakeup = 0,
        .reserved1 = 1,
        .reserved_zeros = 0,
      },
      .bMaxPower = 50,
    };
  header->configuration_descriptor = configuration_descriptor;

  header->interfaces_count = 2;
  header->interfaces = malloc(2 * sizeof(usb_interface_t));

  static usb_endpoint_descriptor_t notification_endpoint =
  {.header = {.bDescriptorType = DESCRIPTOR_ENDPOINT,
              .bLength = sizeof(usb_endpoint_descriptor_t)},
   .bEndpointAddress = { .endpoint_number = 1, .direction = USB_ENDPOINT_IN, .reserved = 0 },
   .bInterval = 16,
   .bmAttributes = {
       .reserved_zeros = 0,
       .synchronization_type = USB_ENDPOINT_SYNC_NO_SYNC,
       .usage_type = USB_ENDPOINT_USAGE_DATA,
       .transfer_type = USB_ENDPOINT_TYPE_INTERRUPT,
   },
    .wMaxPacketSize = 64,
  };
  usb_interface_t communications_interface =
  {.interface_descriptor = {
       .header = {.bDescriptorType = DESCRIPTOR_INTERFACE,
                  .bLength = sizeof(usb_interface_descriptor_t)},
       .bInterfaceNumber = 0,
       .bAlternateSetting = 0,
       .bNumEndpoints = 1,
       .bInterfaceClass = USB_CLASS_CDC_CODE,
       .bInterfaceSubClass = USB_SUBCLASS_CDC_ACM_CODE,
       .bInterfaceProtocol = 0,
       .iInterface = 0
      },
    .endpoint_descriptors_count = 1,
    // NOTE: mind here, the endpoint is same for all devices,
    // so it's defined here as static variable, meaning it will
    // not be deallocated when this function exits.
    .endpoint_descriptors = &notification_endpoint,
  };
  static usb_endpoint_descriptor_t data_endpoints[2] = {
    {.header = {.bDescriptorType = DESCRIPTOR_ENDPOINT,
                .bLength = sizeof(usb_endpoint_descriptor_t)},
     .bEndpointAddress = { .endpoint_number = 2, .direction = USB_ENDPOINT_IN, .reserved = 0 },
     .bInterval = 1,
     .bmAttributes = {
       .reserved_zeros = 0,
       .synchronization_type = USB_ENDPOINT_SYNC_NO_SYNC,
       .usage_type = USB_ENDPOINT_USAGE_DATA,
       .transfer_type = USB_ENDPOINT_TYPE_BULK,
     },
     .wMaxPacketSize = MAX_PACKET_SIZE,
    },
    {.header = {.bDescriptorType = DESCRIPTOR_ENDPOINT,
                .bLength = sizeof(usb_endpoint_descriptor_t)},
     .bEndpointAddress = { .endpoint_number = 2, .direction = USB_ENDPOINT_OUT, .reserved = 0 },
     .bInterval = 1,
     .bmAttributes = {
       .reserved_zeros = 0,
       .synchronization_type = USB_ENDPOINT_SYNC_NO_SYNC,
       .usage_type = USB_ENDPOINT_USAGE_DATA,
       .transfer_type = USB_ENDPOINT_TYPE_BULK,
     },
     .wMaxPacketSize = MAX_PACKET_SIZE,
    },
  };
  usb_interface_t data_interface = {
    .interface_descriptor = {
      .header = {.bDescriptorType = DESCRIPTOR_INTERFACE,
                 .bLength = sizeof(usb_interface_descriptor_t)},
      .bInterfaceNumber = 1,
      .bAlternateSetting = 0,
      .bNumEndpoints = 2,
      .bInterfaceClass = USB_CLASS_DATA_CODE,
      .bInterfaceSubClass = 0x00,
      .bInterfaceProtocol = 0,
      .iInterface = 0
    },
    .endpoint_descriptors_count = 2,
    // NOTE: mind here, the endpoint is same for all devices,
    // so it's defined here as static variable, meaning it will
    // not be deallocated when this function exits.
    .endpoint_descriptors = data_endpoints,
  };

  cdc->functional_descriptors_count = 4;

  static usb_cdc_header_functional_decriptor_t header_function = {
    .header = { .bFunctionLength = sizeof(usb_cdc_header_functional_decriptor_t), .bDescriptorType = CS_INTERFACE, .bDescriptorSubType = HEADER_FUNCTIONAL_DESCRIPTOR_FUNCTIONAL_DESCRIPTOR },
    .bcdCDC = 0x0110,
  };
  static usb_cdc_acm_functional_decriptor_t acm_function = {
    .header = { .bFunctionLength = sizeof(usb_cdc_acm_functional_decriptor_t), .bDescriptorType = CS_INTERFACE, .bDescriptorSubType = ABSTRACT_CONTROL_MANAGEMENT_FUNCTIONAL_DESCRIPTOR },
    .bmCapabilities = 0x00,
  };
  static usb_cdc_union_functional_decriptor_t union_function = {
    .header = { .bFunctionLength = sizeof(usb_cdc_union_functional_decriptor_t), .bDescriptorType = CS_INTERFACE, .bDescriptorSubType = UNION_FUNCTIONAL_DESCRIPTOR },
    .bControlInterface = 0,
    .bSubordinateInterface0 = 1,
  };
  static usb_cdc_call_management_functional_decriptor_t call_function = {
    .header = { .bFunctionLength = sizeof(usb_cdc_call_management_functional_decriptor_t), .bDescriptorType = CS_INTERFACE, .bDescriptorSubType = CALL_MANAGEMENT_FUNCTIONAL_FUNCTIONAL_DESCRIPTOR },
    .bmCapabilities = 0x00
  };
  static usb_cdc_functional_descriptor_header_t *headers[4] =
    { &header_function.header, &acm_function.header, &union_function.header, &call_function.header };
  cdc->functional_descriptors = headers;

  header->interfaces[0] = communications_interface;
  header->interfaces[1] = data_interface;
}

uint16_t get_size(uint16_t size_to_send, uint16_t* remaining_size) {
  uint16_t size = size_to_send;

  if (*remaining_size < size_to_send) {
    size = *remaining_size;
    *remaining_size = 0;
  } else {
    *remaining_size -= size_to_send;
  }

  return size;
}

task_result_t usb_device_cdc_send_configuration(usb_device_t *device,
                                                usb_setup_command_t *cmd) {
  usb_device_cdc_t* dev = (usb_device_cdc_t*)device->class;
  USB_OTG_INEndpointTypeDef* enp0 = &device->in[0];
  volatile uint32_t* enp0fifo = &device->fifos[0].data[0];
  uint32_t sub_word_data;
  uint8_t  sub_word_count = 0;

  // first configure the size
  uint16_t size =
      sizeof(usb_configuration_descriptor_t) +
    dev->header.interfaces_count * sizeof(usb_interface_descriptor_t);

  for (uint8_t i = 0; i < dev->header.interfaces_count; i++) {
    usb_interface_t* interface = &dev->header.interfaces[i];
    size += interface->endpoint_descriptors_count * sizeof(usb_endpoint_descriptor_t);
  }

  for (uint8_t i = 0; i < dev->functional_descriptors_count; i++) {
    usb_cdc_functional_descriptor_header_t* descriptor = dev->functional_descriptors[i];
    size += descriptor->bFunctionLength;
  }

  dev->header.configuration_descriptor.wTotalLength = size;

  if (size > cmd->wLength) {
    size = cmd->wLength;
  }

  // TODO: what if there is not enough space for this?
  // I mean there should be... but that case should probably be handled to,
  // it depends a lot on how many functions and interfaces we do have...
  task_result_t result = usb_generic_setup_in_endpoint(enp0, size, 64);

  if (result != RES_OK) {
    return result;
  }

  // fill fifo with all configuration
  usb_generic_fill_fifo_words(enp0,
                              (uint8_t*)&dev->header.configuration_descriptor,
                              get_size(sizeof(usb_configuration_descriptor_t), &size),
                              enp0fifo, &sub_word_data, &sub_word_count);

  // NOTE: there is always one control interface and one data one.
  for (uint8_t i = 0; i < dev->header.interfaces_count; i++) {
    usb_interface_t* interface = &dev->header.interfaces[i];
    usb_generic_fill_fifo_words(enp0,
                                (uint8_t*)&interface->interface_descriptor,
                                get_size(sizeof(usb_interface_descriptor_t), &size),
                                enp0fifo, &sub_word_data, &sub_word_count);

    // Control interface has functional descriptors
    if (i == 0) {
      for (uint8_t j = 0; j < dev->functional_descriptors_count; j++) {
        usb_cdc_functional_descriptor_header_t* descriptor = dev->functional_descriptors[j];
        usb_generic_fill_fifo_words(enp0, (uint8_t *)descriptor,
                                    get_size(descriptor->bFunctionLength, &size),
                                    enp0fifo, &sub_word_data, &sub_word_count);
      }
    }

    for (uint8_t j = 0; j < interface->endpoint_descriptors_count; j++) {
      usb_generic_fill_fifo_words(enp0,
                                  (uint8_t*)&interface->endpoint_descriptors[j],
                                  get_size(sizeof(usb_endpoint_descriptor_t), &size),
                                  enp0fifo, &sub_word_data, &sub_word_count);
    }
  }

  // The fifo takes always 4 elements. We do not care what's written on the
  // last bytes, since the peripheral will know only about the first bytes
  // as per the size written to DIEPTSIZ
  if (sub_word_count > 0) {
    sub_word_count = 0;
    usb_generic_fill_fifo_words(enp0, (uint8_t *)&sub_word_data,
                                4, enp0fifo, &sub_word_data, &sub_word_count);
  }

  // After the fifo is filled...
  return result;
}

void usb_device_cdc_setup_endpoints(usb_device_t *device,
                                    uint8_t configuration) {
  // NOTE: currently it is assumed one notification endpoint (1),
  //       and data endpoint (2). This is valid for acm, if other
  //       are implemented in the future, this will have to be changed

  // FIFOs sizes
  uint16_t fifo_zero_pos = 0;
  uint16_t fifo_zero_size = reg_read_bits_pos(&device->core->DIEPTXF0_HNPTXFSIZ, USB_OTG_DIEPTXF_INEPTXFD_Pos, 0xFFFF);
  uint16_t curr_pos = fifo_zero_pos + fifo_zero_size;
  uint16_t sizes = 512 / 4;
  device->core->DIEPTXF[1] = (curr_pos << USB_OTG_DIEPTXF_INEPTXSA_Pos) | (sizes << USB_OTG_DIEPTXF_INEPTXFD_Pos);
  curr_pos += sizes;
  device->core->DIEPTXF[2] = (curr_pos << USB_OTG_DIEPTXF_INEPTXSA_Pos) | (sizes << USB_OTG_DIEPTXF_INEPTXFD_Pos);

  // Enable endpoint 1 IN
  //   it's the notification endpoint
  // Configure the endpoint (size, fifo, type)
  reg_write_bits_pos(&device->in[1].DIEPCTL, MAX_PACKET_SIZE, USB_OTG_DIEPCTL_MPSIZ_Pos, 0x7FFUL);
  reg_write_bits_pos(&device->in[1].DIEPCTL, 1, USB_OTG_DIEPCTL_TXFNUM_Pos, 0xF);
  reg_write_bits_pos(&device->in[1].DIEPCTL, 3, USB_OTG_DIEPCTL_EPTYP_Pos, 0x3);
  // Enable interrupts
  device->device->DAINTMSK |= 1 << (USB_OTG_DAINTMSK_IEPM_Pos + 1);
  // The in endpoint should be enabled only if there is something to send,
  // so not enablling.
  reg_set_bits_pos(&device->in[1].DIEPCTL, USB_OTG_DIEPCTL_USBAEP_Pos, 1);

  // Enable endpoint 2 IN
  // Enable endpoint 2 OUT
  //   Those are the data endpoints, used for transferring
  //   serial data, directly, no protocol is utilized.
  // Configure the endpoint (size, fifo, type)
  reg_write_bits_pos(&device->in[2].DIEPCTL, MAX_PACKET_SIZE, USB_OTG_DIEPCTL_MPSIZ_Pos, 0x7FFUL);
  reg_write_bits_pos(&device->in[2].DIEPCTL, 2, USB_OTG_DIEPCTL_TXFNUM_Pos, 0xF);
  reg_write_bits_pos(&device->in[2].DIEPCTL, 2, USB_OTG_DIEPCTL_EPTYP_Pos, 0x3);
  reg_write_bits_pos(&device->out[2].DOEPCTL, 2, USB_OTG_DOEPCTL_EPTYP_Pos, 0x3);
  reg_write_bits_pos(&device->out[2].DOEPCTL, MAX_PACKET_SIZE, USB_OTG_DOEPCTL_MPSIZ_Pos, 0x7FFUL);
  /* reg_write_bits_pos(&device->out[2].DOEPTSIZ, MAX_PACKET_SIZE, USB_OTG_DOEPTSIZ_XFRSIZ_Pos, 0xFFFF); */
  reg_write_bits_pos(&device->out[2].DOEPTSIZ, 1, USB_OTG_DOEPTSIZ_XFRSIZ_Pos, 0xFFFF);
  reg_write_bits_pos(&device->out[2].DOEPTSIZ, 1, USB_OTG_DOEPTSIZ_PKTCNT_Pos, 0x3FF);
  // Enable interrupts
  device->device->DAINTMSK |= 1 << (USB_OTG_DAINTMSK_IEPM_Pos + 2);
  device->device->DAINTMSK |= 1 << (USB_OTG_DAINTMSK_OEPM_Pos + 2);
  // Enable the out endpoint
  reg_set_bits_pos(&device->in[2].DIEPCTL, USB_OTG_DIEPCTL_USBAEP_Pos, 1);
  reg_set_bits_pos(&device->out[2].DOEPCTL, USB_OTG_DOEPCTL_USBAEP_Pos, 1);
  device->out[2].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
}

void usb_device_cdc_reset_endpoints(usb_device_t *device) {
  reg_set_bits(&device->in[1].DIEPCTL, USB_OTG_DIEPCTL_EPDIS_Msk);
  reg_set_bits(&device->in[2].DIEPCTL, USB_OTG_DIEPCTL_EPDIS_Msk);
  reg_set_bits(&device->out[2].DOEPCTL, USB_OTG_DOEPCTL_EPDIS_Msk);
  reg_clear_bits(&device->in[1].DIEPCTL, USB_OTG_DIEPCTL_USBAEP_Msk);
  reg_clear_bits(&device->in[2].DIEPCTL, USB_OTG_DIEPCTL_USBAEP_Msk);
  reg_clear_bits(&device->out[2].DOEPCTL, USB_OTG_DIEPCTL_USBAEP_Msk);

  reg_clear_bits(&device->device->DAINTMSK,
                 (USB_OTG_DAINTMSK_IEPM_Msk << 1) |
                 (USB_OTG_DAINTMSK_IEPM_Msk << 2) |
                 (USB_OTG_DAINTMSK_OEPM_Msk << 2));

  usb_flush_tx_fifo(device->core, 1);
  usb_flush_tx_fifo(device->core, 2);
}

void usb_device_cdc_reset_callback(usb_device_t *device) {

  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
  while (queue_dequeue(cdc->rx_buffer) != NULL);

  // NOTE: endpoint deinitialization and such is handled
  // by the usb device itself.
}

task_result_t usb_device_cdc_setup_packet_callback(usb_device_t *device,
                                                   usb_setup_command_t *cmd) {
  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
  cdc->got_setup = 1;
  reg_set_bits(&device->out[0].DOEPCTL, USB_OTG_DOEPCTL_STALL);
  reg_set_bits(&device->in[0].DIEPCTL, USB_OTG_DIEPCTL_STALL);
  return RES_OK;
  /* // TODO: only for some commands. For now, we just ignore them. */
  /* if (cmd->bRequest == 34) { */
  /*   task_result_t result = usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]); */
  /*   if (result == RES_OK) { */
  /*     device->setup.stage = SETUP_STAGE_SENDING_ACK; */
  /*   } */
  /*   return result; */
  /* } */

  /* return RES_OK; */
}

void usb_device_cdc_enumeration_done_callback(usb_device_t *device) {
  // Now receiving send, receive calls.
  // Nothing to do.
}

typedef enum {
  CDC_TX_FIFO_EMPTY,
  CDC_RX_FIFO_FULL,
  CDC_RX_DONE,
  CDC_TX_DONE,
  CDC_RX_DATA,
  CDC_TX_DATA,

  CDC_RX_REQ,
  CDC_TX_REQ,
} cdc_event_t;

#define DATA_ENDPOINT 2

task_result_t cdc_transmit(usb_device_t *device, uint8_t* data, uint16_t* size) {
  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;

  USB_OTG_INEndpointTypeDef* enp = &device->in[DATA_ENDPOINT];
  volatile uint32_t* fifo = device->fifos[DATA_ENDPOINT].data;

  uint32_t fifo_size = ((enp->DTXFSTS & (USB_OTG_DTXFSTS_INEPTFSAV_Msk)) >> USB_OTG_DTXFSTS_INEPTFSAV_Pos);

  if (fifo_size * 4 < *size) {
    *size = fifo_size * 4;
  }

  task_result_t result = usb_generic_setup_in_endpoint(enp, *size, MAX_PACKET_SIZE);

  if (result != RES_OK) {
    return result;
  }

  cdc->tx_state = CDC_TX_STATE_SENDING;

  usb_generic_fill_fifo(enp, data, *size, fifo);

  return RES_OK;
}

void cdc_process_tx(usb_device_t *device, cdc_event_t event) {
  switch (event) {
  case CDC_TX_REQ:
    break;
  case CDC_TX_DONE:
  case CDC_TX_FIFO_EMPTY:
    // Try reading from the queue. If size is 0,
    // notify application?
    break;
  default:
    // Should not happen.
    break;
  }
}

void cdc_process_rx(usb_device_t *device, cdc_event_t event) {
  /* usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class; */
  /* queue_t* queue = cdc->rx_buffer; */
  /* switch (event) { */
  /* case CDC_RX_REQ: */
  /*   // TODO: read from peripheral fifo to queue */
  /*   if (queue_space(queue) > 0 && cdc->pending_rx_bytes > 0) { */
  /*     cdc_read(device); */
  /*   } */
  /*   break; */
  /* case CDC_RX_DONE: */
  /* case CDC_RX_FIFO_FULL: */
  /*   // Try reading from peripheral to the queue, */
  /*   // if there is space. Check for space first, */
  /*   // when data are read, we don't have where to */
  /*   // store them! */

  /*   if (queue_space(queue) > 0) { */
  /*     cdc_read(device); */
  /*   } */
  /*   break; */
  /*   break; */
  /* default: */
  /*   // Should not happen. */
  /*   break; */
  /* } */
}

void usb_device_cdc_txfifo_empty_callback(usb_device_t *device,
                                          uint8_t endpoint) {
  cdc_process_tx(device, CDC_TX_FIFO_EMPTY);
}
void usb_device_cdc_rxfifo_full_callback(usb_device_t *device,
                                          uint8_t endpoint) {
  cdc_process_rx(device, CDC_RX_FIFO_FULL);
}
void usb_device_cdc_rx_done_callback(usb_device_t *device,
                                           uint8_t endpoint) {
  cdc_process_rx(device, CDC_RX_DONE);
}

void usb_device_cdc_tx_done_callback(usb_device_t *device,
                                           uint8_t endpoint) {
  cdc_process_tx(device, CDC_TX_DONE);
}

void usb_device_cdc_enable_endpoint_maybe(usb_device_t* device, usb_device_cdc_t *cdc) {
  if (queue_space(cdc->rx_buffer) >= MAX_PACKET_SIZE) {
    reg_write_bits_pos(&device->out[DATA_ENDPOINT].DOEPTSIZ, MAX_PACKET_SIZE, USB_OTG_DOEPTSIZ_XFRSIZ_Pos, 0xFFFF);
    reg_write_bits_pos(&device->out[DATA_ENDPOINT].DOEPTSIZ, 1, USB_OTG_DOEPTSIZ_PKTCNT_Pos, 0x3FF);
    device->out[DATA_ENDPOINT].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
    cdc->endpoint_nak = false;
  }
}

void usb_device_cdc_received_data_callback(usb_device_t *device, packet_info_t *packet) {
  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
  uint8_t data[2*MAX_PACKET_SIZE];
  uint16_t len = packet->byte_count;

  volatile uint32_t* fifo = &device->fifos[packet->endpoint_num].data[0];

  if (packet->packet_status == PACKET_OUT_DATA_PACKET_RCVD) {
    if (cdc->receive_location_callback != NULL) {
      uint8_t* receive_location = cdc->receive_location_callback(device, len);
      if (receive_location != NULL) {
        usb_generic_read(receive_location, len, fifo);
        return;
      }
    }

    usb_generic_read(data, len, fifo);

    if (cdc->receive_callback != NULL) {
      uint16_t app_read = cdc->receive_callback(device, data, len);
      len -= app_read;
    }

    // TODO: could be optimized until pointer has to wrap
    if (queue_space(cdc->rx_buffer) < len) {
      cdc->lost_data += len - queue_space(cdc->rx_buffer);
    }

    for (uint16_t i = 0; i < len; i++) {
       queue_enqueue(cdc->rx_buffer, (void*)&data[i]);
    }
  } else if(packet->packet_status == PACKET_OUT_TRANSFER_COMPLETED) {
    if (!(device->out[DATA_ENDPOINT].DOEPINT & USB_OTG_DOEPINT_NAK)) {
      usb_device_cdc_enable_endpoint_maybe(device, cdc);
    }
  } else {
    usb_generic_read(data, packet->byte_count, fifo);
  }
}

void usb_device_cdc_nak_callback(usb_device_t *device, uint8_t endpoint) {
  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
  if (queue_space(cdc->rx_buffer) > MAX_PACKET_SIZE) {
    usb_device_cdc_enable_endpoint_maybe(device, cdc);
  } else {
    cdc->endpoint_nak = true;
  }
}
void usb_device_cdc_nyet_callback(usb_device_t *device, uint8_t endpoint) {
  // Nothing to do for now
}

void cdc_data_set_receive_callback(usb_device_t *device,
                                   cdc_receive_callback_t callback) {
  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
  cdc->receive_callback = callback;
}

void cdc_data_set_receive_location_callback(usb_device_t *device,
                                   cdc_receive_location_callback_t callback) {
  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
  cdc->receive_location_callback = callback;
}

int32_t cdc_data_send_blocking(usb_device_t *device, uint8_t *data, uint16_t size) {
  int32_t sent = 0;
  if (size == 0) {
    size = strlen((char*)data);
  }

  while (size > 0) {
    int32_t res = cdc_data_send(device, data, size);

    if (res == -1) {
      return -1;
    }

    size -= res;
    data += res;
    sent += res;
  }

  return sent;
}

int32_t cdc_data_send(usb_device_t *device, uint8_t* data, uint16_t size) {
  if (size == 0) {
    size = strlen((char*)data);
  }

  task_result_t result = cdc_transmit(device, data, &size);

  // If would block, just send back nothing was sent
  // That means no transaction has begun.
  if (result == RES_WOULD_BLOCK) {
    size = 0;
  } else if (result == RES_ERROR) {
    size = -1;
  }

  return size;
}

uint16_t cdc_data_receive(usb_device_t *device, uint8_t* buffer,
                          uint16_t max_size) {
  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
  uint16_t size = max_size;

  for (uint16_t i = 0; i < max_size; i++) {
    if (!queue_dequeue_safely(cdc->rx_buffer, buffer + i)) {
      size = i;
      break;
    }
  }

  if (cdc->endpoint_nak) {
    usb_device_cdc_enable_endpoint_maybe(device, cdc);
  }

  return size;
}

/* bool cdc_got_params(usb_device_t *device) { */
/*   usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class; */
/*   return cdc->got_setup != 0; */
/* } */