13 files changed, 684 insertions(+), 233 deletions(-)

M Makefile
M include/delay.h
M include/pin.h
M include/queue.h
M include/usb_device.h
M include/usb_device_cdc.h
M src/delay.c
M src/main.c
M src/pin.c
M src/queue.c
M src/usb.c
M src/usb_device.c
M src/usb_device_cdc.c

@@ 22,7 22,7 @@ OBJS=$(patsubst %.c,$(OBJDIR)/%.o,$(SRCS))
 DEPFILES=$(patsubst %.c,$(DEPDIR)/%.d,$(SRCS))
 
 DEPFLAGS = -MT $@ -MMD -MP -MF $(DEPDIR)/$*.d
-CFLAGS=-I$(INCDIR) \
+CFLAGS=-I$(INCDIR) -Wall \
 				-I$(DEVICEDIR)/$(INCDIR) \
 				-Ilibs/CMSIS_6/CMSIS/Core/Include \
 				-Ilibs/cmsis_device_h7/Include \

@@ 1,26 1,32 @@
 #ifndef DELAY_H
 #define DELAY_H
 
+#include <stdint.h>
+
 // TODO: define system clock well
 /* #define SYSTEM_CLOCK    ((uint32_t)360000000UL) */
 #define SYSTEM_CLOCK    ((uint32_t)64000000UL)
 
-#define SYSTICK_CALIB 0x3E8
-#define SYSTICK_LOAD (SYSTEM_CLOCK/1000000UL)
+/* #define SYSTICK_CALIB 0x3E8 */
+// Underflow every 1 us
+#define SYSTICK_LOAD (SYSTEM_CLOCK/1000UL-1)
 #define SYSTICK_DELAY_CALIB (SYSTICK_LOAD >> 1)
 
+extern uint32_t us_ticks;
+
 #define DELAY_US(us) \
     do { \
-         uint32_t start = SysTick->VAL; \
-         uint32_t ticks = (us * SYSTICK_LOAD)-SYSTICK_DELAY_CALIB;  \
-         while((start - SysTick->VAL) < ticks); \
+      for (int i = 0; i < 500000; i++); \
+         /* uint32_t start = us_ticks; \ */ \
+         /* while((us_ticks - start) < us); \ */ \
     } while (0)
 
 #define DELAY_MS(ms) \
     do { \
-        for (uint32_t i = 0; i < ms; ++i) { \
-            DELAY_US(1000); \
-        } \
+      for (int i = 0; i < 500000; i++); \
+        /* for (uint32_t i = 0; i < ms; ++i) { \ */ \
+            /* DELAY_US(1000); \ */ \
+        /* } \ */ \
     } while (0)
 
 void systick_configure();

@@ 39,7 39,7 @@ void pin_speed(pin_t* pin, pin_speedmode_t mode);
 
 // Reads input, not output!
 uint8_t pin_read(pin_t* pin);
-uint8_t pin_write(pin_t* pin, uint8_t val);
+void pin_write(pin_t* pin, uint8_t val);
 void pin_toggle(pin_t* pin);
 void pin_set(pin_t* pin);
 void pin_reset(pin_t* pin);

@@ 75,4 75,6 @@ void* queue_peek(queue_t* queue);
  */
 uint16_t queue_count(queue_t* queue);
 
+uint16_t queue_space(queue_t* queue);
+
 #endif // QUEUE_H

@@ 31,7 31,7 @@ typedef enum {
  * Every read value is popped, every written value is enqueued to the fifo.
  */
 typedef struct {
-  volatile uint32_t data[128]; /**< Data memory. Every address here is the same. */
+  volatile uint32_t data[1024]; /**< Data memory. Every address here is the same. */
 } usb_fifo_t;
 
 struct usb_class_header_t;


@@ 141,11 141,11 @@ typedef struct {
   void (*txfifo_empty_callback)(usb_device_t* device, uint8_t endpoint);
 
 /**
- * @brief Callback on rx fifo empty.
+ * @brief Callback on rx fifo full.
  * @param[in,out] device The usb device.
  * @param[in] endpoint The number of the endpoint.
  */
-  void (*rxfifo_empty_callback)(usb_device_t* device, uint8_t endpoint);
+  void (*rxfifo_full_callback)(usb_device_t* device, uint8_t endpoint);
 
 /**
  * @brief Callback on tx transmission done - all data were sent from configured size.


@@ 215,7 215,7 @@ typedef struct usb_class_header_t usb_class_header_t;
 typedef enum {
   SETUP_STAGE_NONE,              /**< No setup command being handled. */
   SETUP_STAGE_RCVD_SETUP_PACKET, /**< Received a setup packet, saved to memory. */
-  SETUP_STAGE_AWAITING_RESPONSE, /**< Handled a setup packet, waiting for data from the host. */
+  SETUP_STAGE_AWAITING_DATA, /**< Handled a setup packet, waiting for data from the host. */
   SETUP_STAGE_SENDING_RESPONSE,  /**< Handling setup packet by sending response to host. */
   SETUP_STAGE_SENDING_ACK,       /**< Handling setup packet by sending acknowledge. */
   SETUP_STAGE_AWAITING_ACK,      /**< Handled a setup packet, sent data, waiting for acknowledge */


@@ 225,6 225,10 @@ typedef enum {
 typedef struct {
   queue_t* received_setup_commands;
 
+  uint8_t* rcvd_data;
+  uint16_t rcvd_count;
+  uint16_t rcvd_awaiting;
+
   usb_setup_command_stage_t stage;          /**< Current stage status. */
 
   uint8_t detected_setup_errors;                  /**< Number of errors when handling setup commands. This should be zero. */

@@ 1,4 1,6 @@
 #include "usb_device.h"
+#include <stdbool.h>
+#include <stdatomic.h>
 
 #ifndef USB_DEVICE_CDC_H
 #define USB_DEVICE_CDC_H


@@ 94,27 96,35 @@ typedef struct {
 } usb_cdc_call_management_functional_decriptor_t;
 _Static_assert(sizeof(usb_cdc_call_management_functional_decriptor_t) == 5, "Size check");
 
+typedef void (*cdc_receive_callback_t)(usb_device_t* device, uint16_t count);
+
+/* typedef enum { */
+/*   CDC_STATE_NONE, */
+/*   CDC_STATE_GOT_DATA, */
+/*   CDC_STATE_FULL_FIFO, */
+/* } cdc_rx_state_t; */
+
+typedef enum {
+  CDC_TX_STATE_NONE,
+  CDC_TX_STATE_SENDING,
+  CDC_TX_STATE_,
+} cdc_tx_state_t;
+
 #pragma GCC diagnostic ignored "-Wpadded"
 typedef struct {
   usb_class_header_t header;
   uint8_t functional_descriptors_count;
   usb_cdc_functional_descriptor_header_t** functional_descriptors;
-} usb_device_cdc_t;
 
-usb_class_header_t* usb_device_cdc_init(usb_device_t *device,
-                uint16_t id_vendor, uint16_t id_product,
-                char *vendor_name, char *product_name,
-                uint16_t serial_number, char* serial_name);
+  cdc_receive_callback_t callback;
 
-task_result_t usb_device_cdc_send_configuration(usb_device_t* device, usb_setup_command_t* cmd);
+  // Internal state.
+  queue_t* rx_buffer;
 
-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_empty_callback(usb_device_t* device, uint8_t endpoint);
-void usb_device_cdc_transmit_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);
+  cdc_tx_state_t tx_state;
+
+  uint8_t got_setup;
+} usb_device_cdc_t;
 
 /**
  * @brief Configures cdc acm descriptors


@@ 122,10 132,18 @@ void usb_device_cdc_nyet_callback(usb_device_t* device, uint8_t endpoint);
  * That means that after call to this function, usb_device_setup can be safely
  * called, and all setup commands handled.
  * @param[in,out] device The usb device.
+ * @param[in] rx_queue_size The size, in bytes, of the rx queue. When the queue is full, data cannot be received. There should not be any data lost, but it's not a 100% guarantee for now.
  */
-void usb_device_cdc_acm_configure(usb_device_t *device);
+void cdc_acm_configure(usb_device_t *device, uint16_t rx_queue_size);
+
+void cdc_data_set_receive_callback(usb_device_t *device,
+                                   cdc_receive_callback_t callback);
 
+int32_t cdc_data_send_blocking(usb_device_t *device, uint8_t *data, uint16_t size);
+int32_t cdc_data_send(usb_device_t* device, uint8_t* data, uint16_t size);
 
+uint16_t cdc_data_receive(usb_device_t* device, uint8_t* buffer, uint16_t max_size);
 
+bool cdc_got_params(usb_device_t* device);
 
 #endif // USB_DEVICE_CDC_H

@@ 1,7 1,15 @@
 #include "delay.h"
 #include <stm32h7xx.h>
 
+uint32_t us_ticks;
+
 void systick_configure() {
   SysTick->LOAD = SYSTICK_LOAD;
-  SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk;
+  SysTick->VAL = 0;
+  SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk; //| SysTick_CTRL_TICKINT_Msk;
+  us_ticks = 0;
+}
+
+void systick_handler() {
+  us_ticks++;
 }

@@ 164,10 164,25 @@ void main()
   void *usb_dev = usb_device_init(USB_OTG_HS1, &USB_CLASS_CDC_ACM,
                                   0x1234, 0x1111, NULL,
                                   NULL, 1, NULL);
-  usb_device_cdc_acm_configure(usb_dev);
+  cdc_acm_configure(usb_dev, 512);
   usb_device_setup(usb_dev);
 
   usb_device_wait_for_handshake(usb_dev);
 
-  while(1) {}
+  uint8_t data[64];
+  while (1) {
+    uint16_t received = cdc_data_receive(usb_dev, data, 64);
+
+    for (uint16_t i = 0; i < received; i++) {
+      data[i] = data[i] + 1;
+    }
+
+    if (received > 0) {
+      cdc_data_send_blocking(usb_dev, data, received);
+    }
+
+    if (pin_read(&wkup)) {
+      cdc_data_send_blocking(usb_dev, (uint8_t*)"Hello world!\r\n", 0);
+    }
+  }
 }

@@ 1,5 1,6 @@
 #include "pin.h"
 #include "registers.h"
+#include "stm32h747xx.h"
 #include <stdlib.h>
 
 void pin_init(pin_t* pin, GPIO_TypeDef *gpio, uint8_t pin_index) {


@@ 18,20 19,24 @@ void pin_speed(pin_t *pin, pin_speedmode_t mode) {
 }
 
 uint8_t pin_read(pin_t *pin) {
-  reg_read_bits_pos(&pin->gpio->IDR, pin->pin, 1);
+  return reg_read_bits_pos(&pin->gpio->IDR, pin->pin, 1);
 }
 
-uint8_t pin_write(pin_t *pin, uint8_t val) {
-  reg_write_bits_pos(&pin->gpio->ODR, val, pin->pin, 1);
+void pin_write(pin_t *pin, uint8_t val) {
+  if (val) {
+    pin_set(pin);
+  } else {
+    pin_reset(pin);
+  }
 }
 void pin_toggle(pin_t *pin) {
   reg_toggle_bits_pos(&pin->gpio->ODR, pin->pin, 1);
 }
 void pin_set(pin_t *pin) {
-  pin->gpio->ODR = 1 << pin->pin;
+  pin->gpio->BSRR = 1 << pin->pin;
 }
 void pin_reset(pin_t *pin) {
-  pin->gpio->ODR = 1 << (pin->pin + 15);
+  pin->gpio->BSRR = 1 << (pin->pin + GPIO_BSRR_BR0_Pos);
 }
 
 void pin_into_output(pin_t *pin) {

@@ 77,3 77,7 @@ void *queue_peek(queue_t *queue) {
 uint16_t queue_count(queue_t *queue) {
   return queue->length - queue->space;
 }
+
+uint16_t queue_space(queue_t *queue) {
+  return queue->space;
+}

@@ 106,8 106,12 @@ task_result_t usb_generic_setup_in_endpoint(USB_OTG_INEndpointTypeDef *endpoint,
     return RES_WOULD_BLOCK;
   }
 
+  if (!usb_check_fifo_space(endpoint, size)) {
+    return RES_WOULD_BLOCK;
+  }
+
   if (size > 0) {
-    endpoint->DIEPTSIZ = (1 << USB_OTG_DIEPTSIZ_MULCNT_Pos) | (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | (size << USB_OTG_DIEPTSIZ_XFRSIZ_Pos);
+    endpoint->DIEPTSIZ = (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | (size << USB_OTG_DIEPTSIZ_XFRSIZ_Pos);
   } else { // just one packet, of zero length
     endpoint->DIEPTSIZ = (1 << USB_OTG_DIEPTSIZ_PKTCNT_Pos);
   }


@@ 125,10 129,12 @@ task_result_t usb_generic_send(USB_OTG_INEndpointTypeDef *endpoint,
     return result;
   }
 
-  usb_generic_fill_fifo(endpoint, data, size, fifo_tx_target);
+  if (size > 0) {
+    usb_generic_fill_fifo(endpoint, data, size, fifo_tx_target);
+    // After the fifo is filled...
+    /* endpoint->DIEPCTL |= USB_OTG_DIEPCTL_CNAK; */
+  }
 
-  // After the fifo is filled...
-  endpoint->DIEPCTL |= USB_OTG_DIEPCTL_CNAK;
   return RES_OK;
 }
 


@@ 145,7 151,7 @@ task_result_t usb_generic_read(uint8_t *data, uint16_t size, volatile uint32_t *
 
   // TODO: check where to put it... beginning, end...
   if (subWordBytes > 0) {
-    rx_data.word = 0;
+    rx_data.word = *fifo_rx_source;
     for (uint8_t i = 0; i < subWordBytes; i++) {
       *(data + i) = rx_data.bytes[i];
     }


@@ 227,7 233,7 @@ task_result_t usb_send_string_descriptor_zero(USB_OTG_INEndpointTypeDef *endpoin
     usb_generic_fill_fifo(endpoint, (uint8_t*)&string_descriptor->wLANGID[1], string_descriptor->header.bLength - 4, fifo_tx_target);
   }
 
-  endpoint->DIEPCTL |= USB_OTG_DIEPCTL_CNAK;
+  /* endpoint->DIEPCTL |= USB_OTG_DIEPCTL_CNAK; */
 
   return RES_OK;
 }


@@ 260,7 266,7 @@ task_result_t usb_send_unicode_string_descriptor(
     usb_generic_fill_fifo(endpoint, &string_descriptor->bString[2], string_descriptor->header.bLength - 4, fifo_tx_target);
   }
 
-  endpoint->DIEPCTL |= USB_OTG_DIEPCTL_CNAK;
+  /* endpoint->DIEPCTL |= USB_OTG_DIEPCTL_CNAK; */
   return RES_OK;
 }
 

@@ 28,6 28,9 @@ void* usb_device_init(usb_device_slot_t slot, usb_class_vtable_t *class,
   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;
 


@@ 79,7 82,7 @@ void usb_device_setup(void* device_ptr) {
   usb_device_t* device = (usb_device_t*)device_ptr;
 
   device->device->DCTL |= USB_OTG_DCTL_SDIS;
-  /* DELAY_US(3); */
+  DELAY_US(3);
 
   PWR->CR3 |= PWR_CR3_USB33DEN;
   while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0);


@@ 89,7 92,7 @@ void usb_device_setup(void* device_ptr) {
 
   device->core->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;
 
-  /* DELAY_US(3); */
+  DELAY_US(3);
   usb_device_reset(device_ptr);
   /* DELAY_US(3); */
 


@@ 248,16 251,23 @@ task_result_t usb_handle_setup_command(usb_device_t *device, usb_setup_command_t
     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]);
+      device->vt.reset_endpoints(device);
+
+      result = usb_generic_send(device->in, NULL, 0, &device->fifos[0].data[0]);
 
-      // TODO disable endpoints
+      if (result == RES_OK) {
+        device->state = SET_ADDRESS_RCVD;
+      }
     } 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
+      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);
     }


@@ 322,7 332,7 @@ typedef enum {
   USB_GOT_SETUP,
   USB_PROCESS_SETUP,
   USB_GOT_ACK,
-  USB_GOT_RESPONSE,
+  USB_GOT_DATA,
   USB_SENT_RESPONSE,
 } usb_event_t;
 


@@ 334,6 344,22 @@ task_result_t usb_handle_setup(usb_device_t *device, usb_event_t event) {
     }
 
     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);


@@ 341,6 367,14 @@ task_result_t usb_handle_setup(usb_device_t *device, usb_event_t event) {
       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;
   }


@@ 354,9 388,12 @@ task_result_t usb_handle_setup(usb_device_t *device, usb_event_t event) {
       }
     }
     break;
-  case SETUP_STAGE_AWAITING_RESPONSE:
-    // NOTE: Currently not really supported.
-    return RES_ERROR;
+  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) {


@@ 388,29 425,36 @@ void usb_handle_rxflvl_control_int(usb_device_t *device,
   volatile uint32_t *fifo = device->fifos[0].data;
 
   if (packet_info->packet_status == PACKET_SETUP_TRANSACTION_COMPLETED) {
-    // Nothing do to.
-    dummy = *fifo;
-    usb_handle_setup(device, USB_PROCESS_SETUP);
+    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);
-    usb_handle_setup(device, USB_GOT_SETUP);
+
+    if (command.wLength > 60000) {
+      dummy = 0;
+    }
 
     if (!queue_enqueue(device->setup.received_setup_commands, &command)) {
       // Got a problem, setup command lost!
       device->setup.detected_setup_errors++;
     }
 
-    dummy = *fifo; // the last that will trigger another interrupt
+    usb_handle_setup(device, USB_GOT_SETUP);
   } else if (packet_info->byte_count != 0) {
-    usb_generic_read(data, packet_info->byte_count, fifo);
+    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++;
-      usb_handle_setup(device, USB_GOT_RESPONSE);
     }
   }
 }


@@ 440,10 484,6 @@ void usb_handle_rxflvl_int(usb_device_t *device) {
     }
   }
 
-  device->out[0].DOEPCTL = USB_OTG_DOEPCTL_CNAK | USB_OTG_DOEPCTL_EPENA;
-
-  // Read info about the endpoint etc.
-
   // Re-enable the interrupt
   reg_set_bits(&device->core->GINTMSK, USB_OTG_GINTMSK_RXFLVLM);
 }


@@ 458,77 498,80 @@ uint8_t usb_daint_get_endpoint_number(uint32_t endpoints) {
 }
 
 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));
+  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) {
-    device->state = USB_DEV_ERROR;
-    return;
-  }
+    if (ep_id == 0xFF) {
+      return;
+    }
 
-  uint32_t interrupt_reg = device->in[ep_id].DIEPINT;
+    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_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);
-    device->in[ep_id].DIEPCTL = USB_OTG_DIEPCTL_CNAK;
-  }
-  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);
-  }
-  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);
-  }
-  if (interrupt_reg & USB_OTG_DIEPINT_TXFE) {
-    if (device->state == ENUMERATED) {
-      device->vt.txfifo_empty_callback(device, ep_id);
+    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;
     }
-    reg_clear_bits(&device->in[ep_id].DIEPINT, USB_OTG_DIEPINT_TXFE);
-  }
-  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);
-  }
-  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.
-    reg_clear_bits(&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
-    reg_clear_bits(&device->in[ep_id].DIEPINT, USB_OTG_DIEPINT_ITTXFE);
-  }
-  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);
-  }
-  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);
-  }
-  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);
-  }
-  if (interrupt_reg & USB_OTG_DIEPINT_XFRC) {
-    // Transfer is completed.
-    if (device->state == ENUMERATED) {
-      device->vt.rx_done_callback(device, ep_id);
+    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_GOT_RESPONSE);
-      usb_handle_setup(device, USB_PROCESS_SETUP);
+      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;
     }
-    reg_clear_bits(&device->in[ep_id].DIEPINT, USB_OTG_DIEPINT_XFRC);
   }
 }
 


@@ 537,84 580,97 @@ void usb_handle_endpoint_in_int(usb_device_t* device) {
 #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 = USB_DEV_ERROR;
-    return;
-  }
+  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;
 
-  uint32_t interrupt_reg = device->out[ep_id].DOEPINT;
+      if (device->state == ENUMERATED) {
+        device->vt.rx_done_callback(device, ep_id);
+      }
 
-  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);
-  }
-  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);
-  }
-  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[0].DOEPCTL = USB_OTG_DOEPCTL_CNAK | USB_OTG_DOEPCTL_EPENA;
-    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_NAK);
-  }
-  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);
-  }
-  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);
-  }
-  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);
-  }
-  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);
-  }
-  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?
-    reg_clear_bits(&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?
-    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_OTEPDIS);
-  }
-  if (interrupt_reg & USB_OTG_DOEPINT_STUP) {
-    usb_handle_setup(device, USB_PROCESS_SETUP);
-    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_STUP);
-  }
-  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);
-  }
-  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);
-  }
-  if (interrupt_reg & USB_OTG_DOEPINT_XFRC) {
-    device->out[0].DOEPCTL = USB_OTG_DOEPCTL_CNAK | USB_OTG_DOEPCTL_EPENA;
-    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_XFRC);
+      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);
+        }
 
-    device->vt.rx_done_callback(device, ep_id);
+        // Process next packet / data
+        usb_handle_setup(device, USB_PROCESS_SETUP);
 
-    if (ep_id == 0) {
-      usb_handle_setup(device, USB_GOT_ACK);
-      usb_handle_setup(device, USB_PROCESS_SETUP);
+        device->out[ep_id].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
+      }
     }
   }
 }


@@ 625,15 681,25 @@ 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);
 
-    // 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;


@@ 644,8 710,8 @@ void otg_hs_handler(void) {
     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 | USB_OTG_DOEPMSK_NAKM;
-    device->device->DIEPMSK |= USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_TOM | USB_OTG_DIEPEACHMSK1_NAKM | USB_OTG_DIEPEACHMSK1_INEPNEM;
+    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;


@@ 695,26 761,33 @@ void otg_hs_handler(void) {
   // Setup, data...
   if (device->core->GINTSTS & USB_OTG_GINTSTS_OEPINT) {
     usb_handle_endpoint_out_int(device);
-    return;
+    handled = 1;
   }
 
   if (device->core->GINTSTS & USB_OTG_GINTSTS_IEPINT) {
     usb_handle_endpoint_in_int(device);
-    return;
+    handled = 1;
   }
 
   if (device->core->GINTSTS & (USB_OTG_GINTSTS_RXFLVL)) {
     usb_handle_rxflvl_int(device);
-    return;
+    handled = 1;
   }
 
-  // Start of frame
-  if (device->core->GINTSTS & USB_OTG_GINTSTS_SOF) {
-    device->core->GINTSTS = USB_OTG_GINTSTS_SOF;
+  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;
+  }
 
-    usb_handle_setup(device, USB_PROCESS_SETUP);
-    return;
+  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) {

@@ 1,10 1,34 @@
+#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,
+                char *vendor_name, char *product_name,
+                uint16_t serial_number, char* 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)
 };


@@ 12,13 36,19 @@ uint16_t usb_cdc_lang_descriptors[] = {
 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_empty_callback = usb_device_cdc_rxfifo_empty_callback,
-  .transmit_done_callback = usb_device_cdc_transmit_done_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,
 };
 
 usb_class_header_t *usb_device_cdc_init(usb_device_t *device, uint16_t id_vendor,


@@ 77,14 107,18 @@ usb_class_header_t *usb_device_cdc_init(usb_device_t *device, uint16_t id_vendor
   return class;
 }
 
-void usb_device_cdc_acm_configure(usb_device_t* device) {
+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;
 
+  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 = 0x20,
+      .bcdUSB = 0x200,
       .bDeviceClass = USB_CLASS_CDC_CODE,
       .bDeviceSubClass = 0x00,
       .bDeviceProtocol = 0x00,


@@ 174,28 208,28 @@ void usb_device_cdc_acm_configure(usb_device_t* device) {
        .usage_type = USB_ENDPOINT_USAGE_DATA,
        .transfer_type = USB_ENDPOINT_TYPE_BULK,
      },
-     .wMaxPacketSize = 64,
+     .wMaxPacketSize = MAX_PACKET_SIZE,
     },
     {.header = {.bDescriptorType = DESCRIPTOR_ENDPOINT,
                 .bLength = sizeof(usb_endpoint_descriptor_t)},
-     .bEndpointAddress = { .endpoint_number = 1, .direction = USB_ENDPOINT_OUT, .reserved = 0 },
-     .bInterval = 16,
+     .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 = 64,
+     .wMaxPacketSize = MAX_PACKET_SIZE,
     },
   };
   usb_interface_t data_interface = {
     .interface_descriptor = {
       .header = {.bDescriptorType = DESCRIPTOR_INTERFACE,
                  .bLength = sizeof(usb_interface_descriptor_t)},
-      .bInterfaceNumber = 2,
+      .bInterfaceNumber = 1,
       .bAlternateSetting = 0,
-      .bNumEndpoints = 1,
+      .bNumEndpoints = 2,
       .bInterfaceClass = USB_CLASS_DATA_CODE,
       .bInterfaceSubClass = 0x00,
       .bInterfaceProtocol = 0,


@@ 328,33 362,309 @@ task_result_t usb_device_cdc_send_configuration(usb_device_t *device,
   }
 
   // After the fifo is filled...
-  enp0->DIEPCTL = USB_OTG_DIEPCTL_CNAK;
   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) {
-  // TODO - is there something to do? maybe just the multiplexed commands?
+  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) {
-  // TODO start the application somehow
+  // 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) {
+  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
+  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) {
-// TODO the application
+  cdc_process_tx(device, CDC_TX_FIFO_EMPTY);
 }
-void usb_device_cdc_rxfifo_empty_callback(usb_device_t *device,
+void usb_device_cdc_rxfifo_full_callback(usb_device_t *device,
                                           uint8_t endpoint) {
-// TODO the application
+  cdc_process_rx(device, CDC_RX_FIFO_FULL);
 }
-void usb_device_cdc_transmit_done_callback(usb_device_t *device,
+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) {
-// TODO the application
+  cdc_process_tx(device, CDC_TX_DONE);
 }
+
+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[4*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) {
+    // TODO: could be optimized until pointer has to wrap
+    usb_generic_read(data, len, fifo);
+
+    for (uint16_t i = 0; i < len; i++) {
+      queue_enqueue(cdc->rx_buffer, (void*)&data[i]);
+    }
+
+    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_PKTCNT_Pos, 0x3FF);
+    device->out[DATA_ENDPOINT].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
+  } else if(packet->packet_status == PACKET_OUT_TRANSFER_COMPLETED) {
+      if (queue_space(cdc->rx_buffer)) {
+        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_PKTCNT_Pos, 0x3FF);
+        device->out[DATA_ENDPOINT].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
+      }
+  } else {
+    usb_generic_read(data, packet->byte_count, fifo);
+  }
+}
+
 void usb_device_cdc_nak_callback(usb_device_t *device, uint8_t endpoint) {
   // Nothing to do for now
 }
 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->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);
+  }
+
+  usb_device_cdc_t* cdc = (usb_device_cdc_t*)device->class;
+
+  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++) {
+    uint8_t element;
+    if (!queue_dequeue_safely(cdc->rx_buffer, buffer + i)) {
+      size = i;
+      break;
+    }
+  }
+
+  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;
+}