M include/pin.h => include/pin.h +4 -4
  
@@ 18,10 18,10 @@ typedef enum {
 } pin_pullmode_t;
 
 typedef enum {
-  INPUT,
-  OUTPUT,
-  ALTERNATE,
-  ANALOG
+  INPUT = 0,
+  OUTPUT = 1,
+  ALTERNATE = 2,
+  ANALOG = 3
 } pin_mode_t;
 
 typedef enum {
 
M include/usb.h => include/usb.h +7 -0
  
@@ 330,6 330,7 @@ void usb_generic_read(uint8_t *data,
  */
 void usb_send_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                          usb_descriptor_t *descriptor,
+                         uint16_t max_size,
                          volatile uint32_t *fifo_tx_target);
 
 /**
@@ 341,6 342,7 @@ void usb_send_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
  */
 void usb_send_device_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                 usb_device_descriptor_t *descriptor,
+                                uint16_t max_size,
                                 volatile uint32_t *fifo_tx_target);
 
 /**
@@ 351,6 353,7 @@ void usb_send_device_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
  * @param[out] fifo_tx_target Address of the fifo. All data are written to this exact address, no offsets are made.
  */
 void usb_send_device_qualifier_descriptor(USB_OTG_INEndpointTypeDef* endpoint,usb_device_qualifier_t *descriptor,
+                                          uint16_t max_size,
                                           volatile uint32_t *fifo_tx_target);
 
 /**
@@ 362,6 365,7 @@ void usb_send_device_qualifier_descriptor(USB_OTG_INEndpointTypeDef* endpoint,us
  */
 void usb_send_interface_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                    usb_interface_descriptor_t *descriptor,
+                                   uint16_t max_size,
                                    volatile uint32_t *fifo_tx_target);
 
 /**
@@ 373,6 377,7 @@ void usb_send_interface_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
  */
 void usb_send_endpoint_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                   usb_endpoint_descriptor_t *descriptor,
+                                  uint16_t max_size,
                                   volatile uint32_t *fifo_tx_target);
 
 /**
@@ 385,6 390,7 @@ void usb_send_endpoint_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
 void usb_send_string_descriptor_zero(
     USB_OTG_INEndpointTypeDef *endpoint,
     usb_string_descriptor_zero_t *string_descriptor,
+    uint16_t max_size,
     volatile uint32_t *fifo_tx_target);
 
 /**
@@ 397,6 403,7 @@ void usb_send_string_descriptor_zero(
 void usb_send_unicode_string_descriptor(
     USB_OTG_INEndpointTypeDef *endpoint,
     usb_unicode_string_descriptor_t *string_descriptor,
+    uint16_t max_size,
     volatile uint32_t *fifo_tx_target);
 
 #endif // USB_H
 
M src/main.c => src/main.c +6 -4
  
@@ 123,8 123,6 @@ void main()
 
   __enable_irq();
 
-  // TODO: ULPI inputs to alternate mode...
-
   pin_t ulpi_clk, ulpi_stp, ulpi_dir, ulpi_nxt,
     ulpi_d0, ulpi_d1, ulpi_d2, ulpi_d3, ulpi_d4,
     ulpi_d5, ulpi_d6, ulpi_d7, otg_hs_overcurrent;
@@ 159,9 157,13 @@ void main()
   // TODO: ?
   pin_into_input_highspeed(&otg_hs_overcurrent);
 
+  // TODO: why can't I send string descriptors?
+  /* void *usb_dev = usb_device_init(USB_OTG_HS1, &USB_CLASS_CDC_ACM, */
+  /*                                 0x1234, 0x1111, "Frantisek Bohacek", */
+  /*                                 "Display", 1, NULL); */
   void *usb_dev = usb_device_init(USB_OTG_HS1, &USB_CLASS_CDC_ACM,
-                                  0x1234, 0x1111, "Frantisek Bohacek",
-                                  "Display", 1, NULL);
+                                  0x1234, 0x1111, NULL,
+                                  NULL, 1, NULL);
   usb_device_cdc_acm_configure(usb_dev);
   usb_device_setup(usb_dev);
 
 
M src/pin.c => src/pin.c +2 -3
  
@@ 5,8 5,6 @@
 void pin_init(pin_t* pin, GPIO_TypeDef *gpio, uint8_t pin_index) {
   pin->gpio = gpio;
   pin->pin = pin_index;
-
-  return pin;
 }
 
 void pin_mode(pin_t *pin, pin_mode_t mode) {
@@ 65,6 63,7 @@ void pin_into_input_highspeed(pin_t *pin) {
 
 void pin_into_alternate(pin_t *pin, uint8_t alternate) {
   pin_mode(pin, ALTERNATE);
+  pin_pull(pin, NONE);
   uint8_t index = pin->pin >> 3;
   uint8_t pos = (pin->pin & 0x7) * 4;
   volatile uint32_t *afr = pin->gpio->AFR;
@@ 76,5 75,5 @@ void pin_into_alternate_highspeed(pin_t *pin, uint8_t alternate) {
 }
 
 void pin_into_highspeed(pin_t *pin) {
-  pin_speed(pin, HIGH_SPEED);
+  pin_speed(pin, VERY_HIGH_SPEED);
 }
 
M src/usb.c => src/usb.c +75 -22
  
@@ 42,7 42,7 @@ void usb_generic_fill_fifo_words(USB_OTG_INEndpointTypeDef *endpoint,
       tx_data.bytes[i + bytes] = *(data++);
     }
 
-    *sub_word_bytes = tx_data.word;
+    *sub_word_data = tx_data.word;
     return;
   }
 
@@ 72,6 72,36 @@ void usb_generic_fill_fifo_words(USB_OTG_INEndpointTypeDef *endpoint,
   *sub_word_bytes = subWordBytes;
 }
 
+
+void usb_generic_fill_fifo(USB_OTG_INEndpointTypeDef *endpoint,
+                      uint8_t *data,
+                      uint16_t size,
+                      volatile uint32_t *fifo_tx_target) {
+  size = ((size + 3) / 4) * 4;
+
+  uint32_t subWordData;
+  uint8_t subWordCount;
+
+  usb_generic_fill_fifo_words(endpoint, data, size, fifo_tx_target, &subWordData, &subWordCount);
+}
+
+void usb_generic_setup_in_endpoint(USB_OTG_INEndpointTypeDef *endpoint, uint16_t size, uint16_t max_packet_size) {
+  uint16_t packet_count = (size + max_packet_size - 1) / max_packet_size;
+
+  if (endpoint->DIEPCTL & USB_OTG_DIEPCTL_EPENA) {
+    // this is bad! Can't send the packet, this shouldn't get here, ever.
+    while (endpoint->DIEPCTL & USB_OTG_DIEPCTL_EPENA);
+    /* __nop(); */
+  }
+
+  if (size > 0) {
+    endpoint->DIEPTSIZ = (1 << USB_OTG_DIEPTSIZ_MULCNT_Pos) | (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);
+  }
+  endpoint->DIEPCTL = USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA;
+}
+
 void usb_generic_send(USB_OTG_INEndpointTypeDef *endpoint,
                       uint8_t *data,
                       uint16_t size,
@@ 79,12 109,16 @@ void usb_generic_send(USB_OTG_INEndpointTypeDef *endpoint,
   if (endpoint->DIEPCTL & USB_OTG_DIEPCTL_EPENA) {
     // this is bad! Can't send the packet, this shouldn't get here, ever.
     while (endpoint->DIEPCTL & USB_OTG_DIEPCTL_EPENA);
+    /* __nop(); */
   }
 
-  // TODO: generic max packet size
-  uint16_t packet_count = (size + 63) / 64;
+  uint32_t fifo_size;
+  while ((fifo_size = endpoint->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) < (size + 3) / 4) {
+    /* __nop(); */
+  }
 
-  endpoint->DIEPTSIZ = (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | (size << USB_OTG_DIEPTSIZ_XFRSIZ_Pos);
+  // TODO: generic max packet size
+  usb_generic_setup_in_endpoint(endpoint, size, 64);
 
   usb_data_t tx_data;
   uint8_t subWordBytes = size % 4;
@@ 105,7 139,8 @@ void usb_generic_send(USB_OTG_INEndpointTypeDef *endpoint,
     *fifo_tx_target = tx_data.word;
   }
 
-  endpoint->DIEPCTL |= USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA;
+  // After the fifo is filled...
+  endpoint->DIEPCTL = USB_OTG_DIEPCTL_CNAK;
 }
 
 void usb_generic_read(uint8_t *data, uint16_t size, volatile uint32_t *fifo_rx_source) {
@@ 130,8 165,14 @@ void usb_generic_read(uint8_t *data, uint16_t size, volatile uint32_t *fifo_rx_s
 
 void usb_send_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                          usb_descriptor_t *descriptor,
+                         uint16_t max_size,
                          volatile uint32_t *fifo_tx_target) {
-  usb_generic_send(endpoint, (uint8_t*)descriptor, descriptor->bLength, fifo_tx_target);
+  uint16_t size = descriptor->bLength;
+  if (max_size != 0 && size > max_size) {
+    size = max_size;
+  }
+
+  usb_generic_send(endpoint, (uint8_t*)descriptor, size, fifo_tx_target);
 }
 
 /* void usb_send_configuration_descriptor(USB_OTG_INEndpointTypeDef* endpoint, */
@@ 160,76 201,88 @@ void usb_send_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
 
 void usb_send_device_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                 usb_device_descriptor_t *descriptor,
+                                uint16_t max_size,
                                 volatile uint32_t *fifo_tx_target) {
   descriptor->header.bDescriptorType = DESCRIPTOR_DEVICE;
   descriptor->header.bLength = sizeof(usb_device_descriptor_t);
-  usb_send_descriptor(endpoint, &descriptor->header, fifo_tx_target);
+  usb_send_descriptor(endpoint, &descriptor->header, max_size, fifo_tx_target);
 }
 void usb_send_device_qualifier_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                           usb_device_qualifier_t *descriptor,
+                                          uint16_t max_size,
                                    volatile uint32_t *fifo_tx_target) {
   descriptor->header.bDescriptorType = DESCRIPTOR_DEVICE_QUALIFIER;
   descriptor->header.bLength = sizeof(usb_device_qualifier_t);
-  usb_send_descriptor(endpoint, &descriptor->header, fifo_tx_target);
+  usb_send_descriptor(endpoint, &descriptor->header, max_size, fifo_tx_target);
 }
 void usb_send_interface_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                    usb_interface_descriptor_t *descriptor,
+                                   uint16_t max_size,
                                    volatile uint32_t *fifo_tx_target) {
   descriptor->header.bDescriptorType = DESCRIPTOR_INTERFACE;
   descriptor->header.bLength = sizeof(usb_interface_descriptor_t);
-  usb_send_descriptor(endpoint, &descriptor->header, fifo_tx_target);
+  usb_send_descriptor(endpoint, &descriptor->header, max_size, fifo_tx_target);
 }
 void usb_send_endpoint_descriptor(USB_OTG_INEndpointTypeDef *endpoint,
                                   usb_endpoint_descriptor_t *descriptor,
+                                   uint16_t max_size,
                                   volatile uint32_t *fifo_tx_target) {
   descriptor->header.bDescriptorType = DESCRIPTOR_ENDPOINT;
   descriptor->header.bLength = sizeof(usb_endpoint_descriptor_t);
-  usb_send_descriptor(endpoint, &descriptor->header, fifo_tx_target);
+  usb_send_descriptor(endpoint, &descriptor->header, max_size, fifo_tx_target);
 }
 
 void usb_send_string_descriptor_zero(USB_OTG_INEndpointTypeDef *endpoint,
                                      usb_string_descriptor_zero_t *string_descriptor,
+                                     uint16_t max_size,
                               volatile uint32_t *fifo_tx_target) {
   usb_data_t data;
   data.word = 0;
 
+  usb_generic_setup_in_endpoint(endpoint, string_descriptor->header.bLength, 64);
+
   data.bytes[0] = string_descriptor->header.bLength;
   data.bytes[1] = string_descriptor->header.bDescriptorType;
 
   if (string_descriptor->header.bLength == 2) {
-    *fifo_tx_target = data.word;
+    usb_generic_fill_fifo(endpoint, &data.bytes[0], 2, fifo_tx_target);
     return;
   }
 
   data.halfwords[1] = string_descriptor->wLANGID[0];
 
-  *fifo_tx_target = data.word;
+  usb_generic_fill_fifo(endpoint, &data.bytes[0], 4, fifo_tx_target);
 
-  // TODO: Now the rest only from wLANGID[1..]
   if (string_descriptor->header.bLength > 4) {
-    usb_generic_send(endpoint,
-                     (uint8_t *)&string_descriptor->wLANGID[1],
-                     string_descriptor->header.bLength - 4,
-                     fifo_tx_target);
+    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;
 }
 void usb_send_unicode_string_descriptor(
     USB_OTG_INEndpointTypeDef *endpoint,
     usb_unicode_string_descriptor_t *string_descriptor,
+    uint16_t max_size,
     volatile uint32_t *fifo_tx_target) {
+
+  usb_generic_setup_in_endpoint(endpoint, string_descriptor->header.bLength, 64);
+
   usb_data_t data = { .word = 0 };
-  data.halfwords[0] = *((uint16_t*)string_descriptor);
+
+  data.bytes[0] = string_descriptor->header.bLength;
+  data.bytes[1] = string_descriptor->header.bDescriptorType;
+
   if (string_descriptor->bString != 0 && string_descriptor->header.bLength > 2) {
     // NOTE: if the string had length of just one, there would be two bytes
     // read here. That shouldn't usually matter much, so we don't care about
     // that here.
     data.halfwords[1] = *((uint16_t*)string_descriptor->bString);
   }
-  *fifo_tx_target = data.word;
+  usb_generic_fill_fifo(endpoint, &data.bytes[0], 4, fifo_tx_target);
 
   if (string_descriptor->header.bLength > 4) {
-    usb_generic_send(endpoint, &string_descriptor->bString[2],
-                     string_descriptor->header.bLength - 4,
-                     fifo_tx_target);
+    usb_generic_fill_fifo(endpoint, &string_descriptor->bString[2], string_descriptor->header.bLength - 4, fifo_tx_target);
   }
+
+  endpoint->DIEPCTL = USB_OTG_DIEPCTL_CNAK;
 }
 
M src/usb_device.c => src/usb_device.c +141 -78
  
@@ 71,23 71,28 @@ void usb_device_setup(void* device_ptr) {
   dummy = RCC->AHB1ENR;
   dummy = RCC->AHB1ENR;
 
-  NVIC_SetPriority(OTG_HS_IRQn, 1);
+  NVIC_SetPriority(OTG_HS_IRQn, 2);
   NVIC_EnableIRQ(OTG_HS_IRQn);
 
   // 1. core initialization
   usb_device_t* device = (usb_device_t*)device_ptr;
 
   device->device->DCTL |= USB_OTG_DCTL_SDIS;
+  /* DELAY_US(3); */
 
   PWR->CR3 |= PWR_CR3_USB33DEN;
   while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0);
 
-  device->core->GCCFG |= USB_OTG_GCCFG_PWRDWN;
+  device->core->GCCFG &= ~USB_OTG_GCCFG_PWRDWN;
+  device->core->GCCFG |= USB_OTG_GCCFG_VBDEN;
 
   device->core->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD;
 
-  DELAY_US(3);
+  /* DELAY_US(3); */
   usb_device_reset(device_ptr);
+  /* DELAY_US(3); */
+
+  *(uint32_t*)((uint8_t*)device->core + USB_OTG_PCGCCTL_BASE) = 0;
 
   // global interrupt mask
   // rx fifo non empty
@@ 97,7 102,7 @@ void usb_device_setup(void* device_ptr) {
 
   // 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;
+  device->core->GUSBCFG = USB_OTG_GUSBCFG_FDMOD | USB_OTG_GUSBCFG_ULPIEVBUSI | USB_OTG_GUSBCFG_ULPIEVBUSD | (9 << USB_OTG_GUSBCFG_TRDT_Pos);
 
   // TODO: are these needed? device mode is forced...
   // unmask otg interrupt mask
@@ 107,6 112,7 @@ void usb_device_setup(void* device_ptr) {
 
   // descdma, device speed, non-zero-length status out handshake, periodic frame interval
   device->device->DCFG = 0;
+  device->core->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL | USB_OTG_GOTGCTL_BVALOEN;
 
   // TODO device threshold control register IF DMA
 
@@ 143,6 149,7 @@ typedef struct {
   packet_dpid_t dpid;
   uint8_t byte_count;
   uint8_t endpoint_num;
+  uint8_t status_phase_start;
 } packet_info_t;
 
 void usb_handle_setup(usb_device_t *device, usb_setup_command_t* cmd) {
@@ 193,33 200,35 @@ void usb_handle_setup(usb_device_t *device, usb_setup_command_t* cmd) {
 
     switch (descriptor_type) {
     case DESCRIPTOR_DEVICE:
-      usb_send_device_descriptor(device->in, &device->class->device_descriptor, &device->fifos[0].data[0]);
+      usb_send_device_descriptor(device->in, &device->class->device_descriptor, cmd->wLength, &device->fifos[0].data[0]);
+      device->setup_stage = SETUP_STAGE_SENDING_RESPONSE;
       break;
     case DESCRIPTOR_CONFIGURATION:
       device->vt.send_configuration(device, cmd);
+      device->setup_stage = SETUP_STAGE_SENDING_RESPONSE;
       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]);
+        usb_send_string_descriptor_zero(device->in, &device->class->string_descriptor_zero, cmd->wLength, &device->fifos[0].data[0]);
       } else {
         uint8_t index = descriptor_index - 1;
         // NOTE: the user could potentially read different memory part!!
         // This has to be fixed, the length has to be stored somewhere.
-        usb_send_unicode_string_descriptor(device->in, &device->class->string_descriptors[index], &device->fifos[0].data[0]);
+        usb_send_unicode_string_descriptor(device->in, &device->class->string_descriptors[index], cmd->wLength, &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]);
+      usb_send_interface_descriptor(device->in, &device->class->interfaces[descriptor_index].interface_descriptor, cmd->wLength, &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]);
+      usb_send_endpoint_descriptor(device->in, &device->class->interfaces[0].endpoint_descriptors[descriptor_index], cmd->wLength, &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]);
+      usb_send_device_qualifier_descriptor(device->in, &device->class->device_qualifier, cmd->wLength, &device->fifos[0].data[0]);
       device->setup_stage = SETUP_STAGE_SENDING_RESPONSE;
       break;
     case DESCRIPTOR_OTHER_SPEED_CONFIGURATION:
@@ 320,6 329,8 @@ void usb_handle_setup(usb_device_t *device, usb_setup_command_t* cmd) {
     device->vt.setup_packet_callback(device, cmd);
     break;
   }
+
+  device->out[0].DOEPCTL = USB_OTG_DOEPCTL_CNAK;
 }
 
 void usb_handle_rxflvl_control_int(usb_device_t *device,
@@ 330,20 341,35 @@ void usb_handle_rxflvl_control_int(usb_device_t *device,
 
   if (packet_info->packet_status == PACKET_SETUP_TRANSACTION_COMPLETED) {
     // Nothing do to.
+    dummy = *fifo;
+
+    if (device->setup_stage != SETUP_STAGE_RCVD_SETUP_PACKET) {
+      // something went wrong. Let's continue, but this isn't looking good.
+      device->detected_setup_errors++;
+    }
+
+    device->received_setup_commands_index %= 3;
+    uint8_t setup_packets_count = reg_read_bits_pos(&device->out[0].DOEPTSIZ, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
+    usb_setup_command_t* command = &device->received_setup_commands[device->received_setup_commands_index++];
+    usb_handle_setup(device, command);
+
+    if (setup_packets_count == 0) {
+      reg_write_bits_pos(&device->out[0].DOEPTSIZ, 3, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
+    }
   } 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);
+    uint8_t setup_packets_count = reg_read_bits_pos(&device->out[0].DOEPTSIZ, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3);
+    usb_setup_command_t* command = &device
+      ->received_setup_commands[device->received_setup_commands_count++];
     device->received_setup_commands_count %= 3;
+
+    usb_generic_read(command, 8, fifo);
+
     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);
-    }
+    /* 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) {
@@ 369,6 395,7 @@ void usb_handle_rxflvl_int(usb_device_t *device) {
 
     /* uint8_t status_phase_start = reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_STSPHST); */
     packet_info_t packet_info = {
+      .status_phase_start = reg_read_bits_pos(&status_data, 27, 0x01),
       .packet_status = (packet_status_t)reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_PKTSTS_Pos, 0xF),
       .dpid = (packet_dpid_t)reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_DPID_Pos, 0x3),
       .byte_count = reg_read_bits_pos(&status_data, USB_OTG_GRXSTSP_BCNT_Pos, 0x7FF),
@@ 382,6 409,8 @@ 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
@@ 417,37 446,48 @@ void usb_handle_endpoint_in_int(usb_device_t* device) {
   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) {
+    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);
-  } else if (interrupt_reg & USB_OTG_DIEPINT_TXFIFOUDRN) {
+  }
+  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 (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) {
+  }
+  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) {
+  }
+  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);
-  } else if (interrupt_reg & USB_OTG_DIEPINT_ITTXFE) {
+  }
+  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) {
+  }
+  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) {
+  }
+  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) {
+  }
+  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) {
+  }
+  if (interrupt_reg & USB_OTG_DIEPINT_XFRC) {
     // Transfer is completed.
     if (device->state == ENUMERATED) {
       device->vt.transmit_done_callback(device, ep_id);
@@ 455,9 495,9 @@ void usb_handle_endpoint_in_int(usb_device_t* device) {
 
     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;
+      } else if (device->setup_stage == SETUP_STAGE_SENDING_ACK) {
+        device->setup_stage = SETUP_STAGE_NONE;
       }
     }
     reg_clear_bits(&device->in[ep_id].DIEPINT, USB_OTG_DIEPINT_XFRC);
@@ 484,68 524,82 @@ void usb_handle_endpoint_out_int(usb_device_t* device) {
   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) {
+  }
+  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) {
+  }
+  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);
-  } else if (interrupt_reg & USB_OTG_DOEPINT_BERR) {
+  }
+  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) {
+  }
+  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) {
+  }
+  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) {
+  }
+  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) {
+  }
+  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) {
+  }
+  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);
-    }
-
+  }
+  if (interrupt_reg & USB_OTG_DOEPINT_STUP) {
     reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_STUP);
-  } else if (interrupt_reg & USB_OTG_DOEPINT_AHBERR) {
+    /* if (device->setup_stage != SETUP_STAGE_RCVD_SETUP_PACKET) { */
+    /*   // something went wrong. Let's continue, but this isn't looking good. */
+    /*   device->detected_setup_errors++; */
+    /* } */
+
+    /* device->received_setup_commands_index %= 3; */
+    /* uint8_t setup_packets_count = reg_read_bits_pos(&device->out[ep_id].DOEPTSIZ, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3); */
+    /* usb_setup_command_t* command = &device->received_setup_commands[device->received_setup_commands_index++]; */
+    /* usb_handle_setup(device, command); */
+
+    /* if (setup_packets_count == 0) { */
+    /*   reg_write_bits_pos(&device->out[0].DOEPTSIZ, 3, USB_OTG_DOEPTSIZ_STUPCNT_Pos, 3); */
+    /* } */
+  }
+  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) {
+  }
+  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 (interrupt_reg & USB_OTG_DOEPINT_XFRC) {
+    // TODO: handle data? - callback to device data handle?
+    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_XFRC);
+
     if (ep_id == 0 && device->setup_stage == SETUP_STAGE_AWAITING_ACK) {
       device->setup_stage = SETUP_STAGE_NONE;
     }
+
+    device->out[0].DOEPCTL = USB_OTG_DOEPCTL_CNAK | USB_OTG_DOEPCTL_EPENA;
     // Transfer has been completed
-    // TODO: handle data? - callback to device data handle?
-    reg_clear_bits(&device->out[ep_id].DOEPINT, USB_OTG_DOEPINT_XFRC);
   }
 }
 
@@ 553,9 607,13 @@ void usb_handle_endpoint_out_int(usb_device_t* device) {
 void otg_hs_handler(void) {
 
   usb_device_t* device = &usb_devices[USB_OTG_HS1];
+  uint8_t handled = 0;
 
   // Reset detected
   if (device->core->GINTSTS & USB_OTG_GINTSTS_USBRST) {
+    // clear it
+    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);
 
@@ 564,63 622,58 @@ void otg_hs_handler(void) {
       USB_OTG_OUTEndpointTypeDef *out = device->out + ep;
       out->DOEPCTL |= USB_OTG_DOEPCTL_SNAK;
     }
+
+    device->device->DCTL = USB_OTG_DCTL_CGINAK | USB_OTG_DCTL_CGONAK;
+
     device->device->DAINTMSK |= 1 << USB_OTG_DAINTMSK_IEPM_Pos;
     device->device->DAINTMSK |= 1 << USB_OTG_DAINTMSK_OEPM_Pos;
 
-    device->device->DOEPMSK |= USB_OTG_DOEPMSK_STUPM | USB_OTG_DOEPMSK_XFRCM;
-    device->device->DIEPMSK |= USB_OTG_DIEPMSK_XFRCM | USB_OTG_DIEPMSK_TOM;
+    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;
 
     // 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;
+    device->core->DIEPTXF[0] = (0) << USB_OTG_TX0FSA_Pos | (128 / 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 = USB_DEV_RESET;
-    // clear it
-    reg_set_bits(&device->core->GINTSTS,  USB_OTG_GINTSTS_USBRST);
 
     return;
   }
 
   // Reset ended
   if (device->core->GINTSTS & USB_OTG_GINTSTS_ENUMDNE) {
+    device->core->GINTSTS = USB_OTG_GINTSTS_ENUMDNE;
     // The enumerated speed
     /* (device->device->DSTS & USB_OTG_DSTS_ENUMSPD_Msk) >> USB_OTG_DSTS_ENUMSPD_Pos; */
     // We do not need to know the speed, it can be either full speed or high
     // speed, both have maximum size 64 bytes. Only low speed or super high
     // speed would differ, but these are not supported!
-    reg_write_bits_pos(&device->in->DIEPCTL, 64, USB_OTG_DIEPCTL_MPSIZ_Pos, 0x7FFUL);
+    // TODO: does 0 mean 64 bits here??? This is NOT in the spec! It's in the spec only for out endpoint, not for the in one!!!
+    reg_write_bits_pos(&device->in[0].DIEPCTL, 0, USB_OTG_DIEPCTL_MPSIZ_Pos, 0x7FFUL);
 
+    device->in->DIEPCTL |= USB_OTG_DOEPCTL_SNAK;
     device->out->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
 
     device->state = RESET_DONE;
-    // 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);
+    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);
+    device->core->GINTSTS = USB_OTG_GINTSTS_MMIS;
     return;
   }
 
@@ 641,5 694,15 @@ void otg_hs_handler(void) {
     return;
   }
 
-  device->state = UNKNOWN_INTERRUPT;
+  // Start of frame
+  if (device->core->GINTSTS & USB_OTG_GINTSTS_SOF) {
+    // Nothing to do?
+    device->core->GINTSTS = USB_OTG_GINTSTS_SOF;
+    handled = 1;
+    return;
+  }
+
+  if (handled == 0) {
+    device->state = UNKNOWN_INTERRUPT;
+  }
 }
 
M src/usb_device_cdc.c => src/usb_device_cdc.c +47 -22
  
@@ 27,6 27,12 @@ usb_class_header_t *usb_device_cdc_init(usb_device_t *device, uint16_t id_vendor
                           char *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;
@@ 80,14 86,14 @@ void usb_device_cdc_acm_configure(usb_device_t* device) {
       .header = { .bDescriptorType = DESCRIPTOR_DEVICE, .bLength = sizeof(usb_device_descriptor_t) },
       .bcdUSB = 0x20,
       .bDeviceClass = USB_CLASS_CDC_CODE,
-      .bDeviceSubClass = USB_SUBCLASS_CDC_ACM_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.iManufacturer,
+      .iProduct = header->device_descriptor.iProduct,
       .iSerialNumber = header->device_descriptor.iSerialNumber,
       .bNumConfigurations = 1,
     };
@@ 144,7 150,7 @@ void usb_device_cdc_acm_configure(usb_device_t* device) {
        .header = {.bDescriptorType = DESCRIPTOR_INTERFACE,
                   .bLength = sizeof(usb_interface_descriptor_t)},
        .bInterfaceNumber = 0,
-       .bAlternateSetting = 1,
+       .bAlternateSetting = 0,
        .bNumEndpoints = 1,
        .bInterfaceClass = USB_CLASS_CDC_CODE,
        .bInterfaceSubClass = USB_SUBCLASS_CDC_ACM_CODE,
@@ 187,8 193,8 @@ void usb_device_cdc_acm_configure(usb_device_t* device) {
     .interface_descriptor = {
       .header = {.bDescriptorType = DESCRIPTOR_INTERFACE,
                  .bLength = sizeof(usb_interface_descriptor_t)},
-      .bInterfaceNumber = 1,
-      .bAlternateSetting = 2,
+      .bInterfaceNumber = 2,
+      .bAlternateSetting = 0,
       .bNumEndpoints = 1,
       .bInterfaceClass = USB_CLASS_DATA_CODE,
       .bInterfaceSubClass = 0x00,
@@ 229,6 235,19 @@ void usb_device_cdc_acm_configure(usb_device_t* device) {
   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;
+}
+
 void 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;
@@ 243,7 262,7 @@ void usb_device_cdc_send_configuration(usb_device_t *device,
   }
 
   // first configure the size
-  uint32_t size =
+  uint16_t size =
       sizeof(usb_configuration_descriptor_t) +
     dev->header.interfaces_count * sizeof(usb_interface_descriptor_t);
 
@@ 259,16 278,23 @@ void usb_device_cdc_send_configuration(usb_device_t *device,
 
   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...
   uint16_t packet_count = (size + 63) / 64;
-  enp0->DIEPTSIZ = (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | (size << USB_OTG_DIEPTSIZ_XFRSIZ_Pos);
+  // configure size
+  enp0->DIEPTSIZ = (1 << USB_OTG_DIEPTSIZ_MULCNT_Pos) | (packet_count << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | (size << USB_OTG_DIEPTSIZ_XFRSIZ_Pos);
+  // enable endpoint
+  enp0->DIEPCTL |= USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA;
 
   // fill fifo with all configuration
   usb_generic_fill_fifo_words(enp0,
                               (uint8_t*)&dev->header.configuration_descriptor,
-                              sizeof(usb_configuration_descriptor_t),
+                              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.
@@ 276,7 302,7 @@ void usb_device_cdc_send_configuration(usb_device_t *device,
     usb_interface_t* interface = &dev->header.interfaces[i];
     usb_generic_fill_fifo_words(enp0,
                                 (uint8_t*)&interface->interface_descriptor,
-                                sizeof(usb_interface_descriptor_t),
+                                get_size(sizeof(usb_interface_descriptor_t), &size),
                                 enp0fifo, &sub_word_data, &sub_word_count);
 
     // Control interface has functional descriptors
@@ 284,7 310,7 @@ void usb_device_cdc_send_configuration(usb_device_t *device,
       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,
-                                    descriptor->bFunctionLength,
+                                    get_size(descriptor->bFunctionLength, &size),
                                     enp0fifo, &sub_word_data, &sub_word_count);
       }
     }
@@ 292,23 318,22 @@ void usb_device_cdc_send_configuration(usb_device_t *device,
     for (uint8_t j = 0; j < interface->endpoint_descriptors_count; j++) {
       usb_generic_fill_fifo_words(enp0,
                                   (uint8_t*)&interface->endpoint_descriptors[j],
-                                  sizeof(usb_endpoint_descriptor_t),
+                                  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);
-    }
-
+  // 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);
   }
 
-  // enable endpoint
-  enp0->DIEPCTL |= USB_OTG_DIEPCTL_CNAK | USB_OTG_DIEPCTL_EPENA;
+  // After the fifo is filled...
+  enp0->DIEPCTL = USB_OTG_DIEPCTL_CNAK;
 }
 
 uint8_t usb_device_cdc_setup_packet_callback(usb_device_t *device,