5 files changed, 802 insertions, 0 deletions

diff --git a/src/libs/usb/cdc-acm.c b/src/libs/usb/cdc-acm.c
new file mode 100644
index 0000000..711bc99
--- /dev/null
+++ b/src/libs/usb/cdc-acm.c
@@ -0,0 +1,184 @@
+#include "cdc-acm.h"
+#include "cusb.h"
+#include "proto.h"
+#include "../gpio.h"
+#include "../addrmap.h"
+
+void cdc_acm_ep1_in_handler(u8 *buffer, int len);
+void cdc_acm_ep2_in_handler(u8 *buffer, int len);
+void cdc_acm_ep3_out_handler(u8 *buffer, int len);
+void cdc_acm_setup_handler(UD_SETUP *pkt);
+void cdc_acm_setup_ack_handler(void);
+void cdc_acm_eoi(void);
+
+u8 configuration_descriptor[128];
+u8 *send_buffer;
+u8 send_buffer_size=0;
+u8 *recv_buffer;
+u8 recv_buffer_size=0;
+
+DeviceConfiguration CDC_ACM_CONFIG ={
+  .device_descriptor = {
+	.bDeviceClass = USB_DEVICE_CLASS_CDC,
+	.idProduct = 1,
+	.iManufacturer = 1, // String id 1
+	.iProduct = 2 // String id 2
+  },
+  .configuration_descriptor = {
+	.bNumInterfaces = 2
+  },
+  .interface_descriptors = {
+	{
+	  .bInterfaceClass = USB_INTERFACE_CLASS_COMMUNICATION,
+	  .bInterfaceSubClass = USB_INTERFACE_SUBCLASS_ACM,
+	  .bInterfaceProtocol = USB_INTERFACE_COMMUNICATION_PROTOCOL_NOPROTO,
+	  .bInterfaceNumber = 0,
+	  .bNumEndpoints = 1 // SHOULD BE 1 (IT IS 0 BECAUSE GET_CONFIG DESCRIPTOR MUST BE UPDATED)
+	},
+	{
+	  .bInterfaceClass = USB_INTERFACE_CLASS_DATA,
+	  .bInterfaceProtocol = USB_INTERFACE_COMMUNICATION_PROTOCOL_NOPROTO,
+	  .bInterfaceNumber = 1,
+	  .bNumEndpoints = 2
+	}
+  },
+  .endpoints = {
+	{
+	  .endpoint_descriptor = {
+		.bEndpointAddress = B_ENDPOINT_ADDRESS(1, USB_DIRECTION_IN),
+		.wMaxPacketSize=64,
+		.bmAttributes = USB_TRANSFERT_TYPE_INTERRUPT,
+		.bInterval = 0
+	  },
+	  .handler = cdc_acm_ep1_in_handler
+	},
+	{
+	  .endpoint_descriptor = {
+		.bEndpointAddress = B_ENDPOINT_ADDRESS(2, USB_DIRECTION_IN),
+		.wMaxPacketSize=64,
+		.bmAttributes = USB_TRANSFERT_TYPE_BULK,
+		.bInterval = 0
+	  },
+	  .handler = cdc_acm_ep2_in_handler
+	},
+	{
+	  .endpoint_descriptor = {
+		.bEndpointAddress = B_ENDPOINT_ADDRESS(3, USB_DIRECTION_OUT),
+		.wMaxPacketSize=64,
+		.bmAttributes = USB_TRANSFERT_TYPE_BULK,
+		.bInterval = 0
+	  },
+	  .handler = cdc_acm_ep3_out_handler
+	},
+  },
+  .descriptor_strings = {
+		"Raspberry Pi", // Vendor
+		"Pico RP2040" // Product
+  },
+  .supported_languages = {
+	.wLANGID0 = USB_LANG_ENGLISH_US
+  },
+  .setup_handler = cdc_acm_setup_handler,
+  .setup_ack_handler = cdc_acm_setup_ack_handler,
+  .eoi=cdc_acm_eoi
+};
+
+
+void usb_cdc_acm_init(void) {
+  cusb_init(&CDC_ACM_CONFIG);
+  USB_CDC_ACM_FUNCTIONAL_DESCRIPTOR f;
+  f.header_descriptor.bFunctionLength=sizeof(USB_CDC_HEADER_FUNCTIONAL_DESCRIPTOR);
+  f.header_descriptor.bDescriptorType=0x24;
+  f.header_descriptor.bcdCDC=0b0110;
+  f.acm_descriptor.bFunctionLength=sizeof(USB_ACM_FUNCTIONAL_DESCRIPTOR);
+  f.acm_descriptor.bDescriptorType=0x24;
+  f.acm_descriptor.bDescriptorSubtype=0x02;
+  f.acm_descriptor.bmCapabilities=0xf;
+  f.union_descriptor.bFunctionLength=sizeof(USB_UNION_INTERFACE_DESCRIPTOR);
+  f.union_descriptor.bDescriptorType=0x24;
+  f.union_descriptor.bDescriptorSubtype=0x06;
+  f.union_descriptor.bMasterInterface=0;
+  f.union_descriptor.bSlaveInterface0=1;
+
+  // ----- Setting-up full configuration descriptor -----
+  u8 *ptr=configuration_descriptor;
+  // Configuration descriptor
+  memcpy(ptr,(void*) &(CDC_ACM_CONFIG.configuration_descriptor), sizeof(USB_CONFIGURATION_DESCRIPTOR));
+  ptr+=sizeof(USB_CONFIGURATION_DESCRIPTOR);
+  // Communication interface descriptor
+  memcpy(ptr,(void*) &(CDC_ACM_CONFIG.interface_descriptors[0]), sizeof(USB_INTERFACE_DESCRIPTOR));
+  ptr+=sizeof(USB_INTERFACE_DESCRIPTOR);
+  memcpy(ptr,(void*) &f, sizeof(USB_CDC_ACM_FUNCTIONAL_DESCRIPTOR));   // Functional descriptor
+  ptr+=sizeof(USB_CDC_ACM_FUNCTIONAL_DESCRIPTOR);
+  memcpy(ptr,(void*) &(CDC_ACM_CONFIG.endpoints[0].endpoint_descriptor), sizeof(USB_ENDPOINT_DESCRIPTOR)); // Endpoint descriptor
+  ptr+=sizeof(USB_ENDPOINT_DESCRIPTOR);
+
+  // Data interface descriptor
+  memcpy(ptr,(void*) &(CDC_ACM_CONFIG.interface_descriptors[1]), sizeof(USB_INTERFACE_DESCRIPTOR));
+  ptr+=sizeof(USB_INTERFACE_DESCRIPTOR);
+  memcpy(ptr,(void*) &(CDC_ACM_CONFIG.endpoints[1].endpoint_descriptor), sizeof(USB_ENDPOINT_DESCRIPTOR));
+  ptr+=sizeof(USB_ENDPOINT_DESCRIPTOR);
+  memcpy(ptr,(void*) &(CDC_ACM_CONFIG.endpoints[2].endpoint_descriptor), sizeof(USB_ENDPOINT_DESCRIPTOR));
+  ptr+=sizeof(USB_ENDPOINT_DESCRIPTOR);
+
+  // Setup full descriptor length
+  USB_CONFIGURATION_DESCRIPTOR *conf=(USB_CONFIGURATION_DESCRIPTOR*)configuration_descriptor;
+  conf->wTotalLength=ptr-configuration_descriptor;
+
+  // Finalize
+  CDC_ACM_CONFIG.full_configuration_descriptor=configuration_descriptor;
+
+  // Start receiving from host
+  cusb_start_xfer(0,64,&(CDC_ACM_CONFIG.endpoints[2]));
+}
+
+void cdc_acm_ep1_in_handler(u8 *buffer, int len) {}
+
+void cdc_acm_ep2_in_handler(u8 *buffer, int len) {
+  send_buffer_size=0;
+}
+
+void cdc_acm_ep3_out_handler(u8 *buffer, int len) {
+  if(recv_buffer_size){
+	memcpy(recv_buffer, buffer, len);
+	recv_buffer_size=0; // Prevent further receive until usb_cdc_acm_recv() is called again
+	cusb_start_xfer(0,64,&(CDC_ACM_CONFIG.endpoints[2]));
+  }
+}
+
+void cdc_acm_setup_handler(UD_SETUP *pkt){
+  u8 direction=pkt->bmRequestType & BM_REQUEST_TYPE_DIR_BIT;
+  if(pkt->bRequest == USB_REQUEST_CODE_SET_LINE_CODING){
+	// TODO
+  }
+  else if(pkt->bRequest == USB_REQUEST_CODE_SET_CONTROL_LINE_STATE){
+	// TODO
+  }
+  else if(pkt->bRequest == USB_REQUEST_CODE_SEND_BREAK){
+	// TODO
+  }
+  else {
+	// TODO
+  }
+
+}
+void cdc_acm_setup_ack_handler(void){
+}
+
+void cdc_acm_eoi(void){
+  if(send_buffer_size)
+	cusb_start_xfer(send_buffer,send_buffer_size,&(CDC_ACM_CONFIG.endpoints[1]));
+}
+
+void usb_cdc_acm_send(char *data, int size){
+  send_buffer=data;
+  send_buffer_size=size;
+  cusb_isr_trigger();
+  while(send_buffer_size){};
+}
+
+void usb_cdc_acm_recv(char *data, int size){
+  recv_buffer=data;
+  recv_buffer_size=size;
+  while(recv_buffer_size){}
+}
diff --git a/src/libs/usb/cdc-acm.h b/src/libs/usb/cdc-acm.h
new file mode 100644
index 0000000..6262ee8
--- /dev/null
+++ b/src/libs/usb/cdc-acm.h
@@ -0,0 +1,10 @@
+#ifndef __CDC_ACM_H__
+#define __CDC_ACM_H__
+
+#include "proto.h"
+
+void usb_cdc_acm_init(void);
+void usb_cdc_acm_send(char *data, int size);
+void usb_cdc_acm_recv(char *data, int size);
+
+#endif
diff --git a/src/libs/usb/cusb.c b/src/libs/usb/cusb.c
new file mode 100644
index 0000000..249945a
--- /dev/null
+++ b/src/libs/usb/cusb.c
@@ -0,0 +1,309 @@
+#include "cusb.h"
+#include "../addrmap.h"
+#include "../bitmap.h"
+#include "../interrupts.h"
+#include "../gpio.h"
+#include "proto.h"
+
+u8 xferbuff[64]; // Free to use for transfer (IN)
+u8 *xferptr;
+u8 xferremain=0;
+DeviceConfiguration *cusb_dc;
+
+void cusb_init(DeviceConfiguration *dc){
+
+  // Reset USB controller
+  REG_WRITE_BITMAP_CLEAR(RESETS_RESET, 1<<24); // Reset USBCTRL
+  while((REG_READ(RESETS_DONE) & 1<<24) == 0){} // Wait controller reset
+
+  // Flush USB Controller DPSRAM
+  memset((void*)USBCTRL_DPSRAM_BASE, 0, USBCTRL_DPSRAM_SIZE);
+
+  // Enable usb controller interrupt
+  interrupts_enable(5);
+
+  // Setup controller
+  REG_WRITE(USBCTRL_MUXING, BIT_USBCTRL_MUXING_TO_PHY|BIT_USBCTRL_MUXING_SOFTCON);
+  REG_WRITE(USBCTRL_PWR, BIT_USBCTRL_PWR_VBUS_DETECT|BIT_USBCTRL_PWR_VBUS_DETECT_OVERRIDE_EN);
+  REG_WRITE(USBCTRL_MAINCTRL, BIT_USBCTRL_MAIN_CTRL_CONTROLLER_EN); // Set device mode + enable controller
+  REG_WRITE(USBCTRL_SIE_CTRL, BIT_USBCTRL_SIE_CTRL_EP0_INT_1BUF); // Enable usb interrupts on EP0
+  REG_WRITE(USBCTRL_INTE, BIT_USBCTRL_INTE_SETUP_REQ|BIT_USBCTRL_INTE_BUS_RESET|BIT_USBCTRL_INTE_BUFF_STATUS); // Setup interrupt granularity
+
+  // Initialize device configuration
+  cusb_dc=dc; // Store controller device config
+  cusb_init_device_configuration();
+
+  // Initialize endpoints
+  for(int i=0;i<USB_ENDPOINT_COUNT;i++){
+	EndPointConfiguration *ep=&(cusb_dc->endpoints[i]);
+	u32 dir=ep->endpoint_descriptor.bEndpointAddress & 0x80;
+	u32 id=ep->endpoint_descriptor.bEndpointAddress & 0xF;
+	// Endpoint and buffer control
+	u32 endpoint_control=USBCTRL_EP1_ENDP_CTRL_IN+0x8*(id-1); // -1 since start at 1 (ep0 already covered in cusb_init_device_configuration())
+	u32 buffer_control=USBCTRL_EP1_BUFFER_CTRL_IN+0x8*(id-1);
+	if(dir == USB_DIRECTION_OUT){
+	  endpoint_control+=4;
+	  buffer_control+=4;
+	}
+	ep->endpoint_control=(void*)(endpoint_control);
+	ep->buffer_control=(void*)(buffer_control);
+	// Init buffers
+	u32 buffer0=USBCTRL_DATA_BUFFER_START+(64*2)*(id-1); // 64 bits aligned warning see p388
+	u32 buffer1=buffer0+64;
+	ep->buffer0=(void*)buffer0;
+	ep->buffer1=(void*)buffer1;
+	// Init ep control
+	u32 epctrl=BIT_USBCTRL_ENDP_CTRL_ENABLE |
+	  BIT_USBCTRL_ENDP_CTRL_INT_PER_BUFF1 |
+	  ep->endpoint_descriptor.bmAttributes << BIT_USBCTRL_ENDP_CTRL_TYPE_LSB|
+	  USB_BUFFER_OFFSET(buffer0);
+	*ep->endpoint_control=epctrl; // Apply buffer control setting
+	ep->next_pid=0; // Maybe that is good?
+  }
+
+  // Pull-up usb line! (for the host :)
+  REG_WRITE_BITMAP_SET(USBCTRL_SIE_CTRL, BIT_USBCTRL_SIE_CTRL_PULLUP_EN);
+}
+
+void cusb_init_device_configuration(){
+
+  // Init device descriptor
+  cusb_dc->device_descriptor.bLength=sizeof(USB_DEVICE_DESCRIPTOR);
+  cusb_dc->device_descriptor.bDescriptorType=USB_DESCRIPTOR_TYPE_DEVICE;
+  cusb_dc->device_descriptor.bNumConfigurations = 1; // We support only 1
+  cusb_dc->device_descriptor.bcdUSB=BCD_USB;
+  cusb_dc->device_descriptor.bMaxPacketSize0=64;
+
+  // Init configuration descriptor
+  cusb_dc->configuration_descriptor.bLength=sizeof(USB_CONFIGURATION_DESCRIPTOR);
+  cusb_dc->configuration_descriptor.bDescriptorType=USB_DESCRIPTOR_TYPE_CONFIGURATION;
+  cusb_dc->configuration_descriptor.bConfigurationValue = 1; // Configuration id
+  cusb_dc->configuration_descriptor.iConfiguration = 0; // No string
+  cusb_dc->configuration_descriptor.bmAttributes = 0xC0; // attributes: self powered, no remote wakeup
+  cusb_dc->configuration_descriptor.bMaxPower = 0x32; // 100ma
+
+  // Init device state
+  cusb_dc->devaddr=0; // Just in case
+  cusb_dc->setdevaddr=0; // Just in case
+  cusb_dc->setdevaddr=0; // Just in case
+
+  // Init string zero descriptor
+  cusb_dc->supported_languages.bLength = sizeof(USB_STRING_DESCRIPTOR_ZERO);
+  cusb_dc->supported_languages.bDescriptorType = USB_DESCRIPTOR_TYPE_STRING;
+
+  // Init ep0 in
+  EndPointConfiguration *ep0_in= &(cusb_dc->endpoints[USB_ENDPOINT_COUNT]);
+  ep0_in->buffer0=(void*)USBCTRL_EP0_BUFFER0;
+  ep0_in->buffer1=(void*)USBCTRL_EP0_BUFFER0+0x40;
+  ep0_in->buffer_control=(void*)USBCTRL_EP0_BUFFER_CTRL_IN;
+  ep0_in->handler=cusb_ep0_in_handler;
+  ep0_in->endpoint_descriptor.bEndpointAddress = B_ENDPOINT_ADDRESS(0, USB_DIRECTION_IN);
+  ep0_in->endpoint_descriptor.bmAttributes = USB_TRANSFERT_TYPE_CONTROL;
+  ep0_in->endpoint_descriptor.wMaxPacketSize=64;
+
+  // Init ep0 out
+  EndPointConfiguration *ep0_out= &(cusb_dc->endpoints[USB_ENDPOINT_COUNT+1]);
+  ep0_out->buffer0=(void*)USBCTRL_EP0_BUFFER0;
+  ep0_out->buffer1=(void*)USBCTRL_EP0_BUFFER0+0x40;
+  ep0_out->buffer_control=(void*)USBCTRL_EP0_BUFFER_CTRL_OUT;
+  ep0_out->handler=cusb_ep0_out_handler;
+  ep0_out->endpoint_descriptor.bEndpointAddress = B_ENDPOINT_ADDRESS(0, USB_DIRECTION_OUT);
+  ep0_out->endpoint_descriptor.bmAttributes = USB_TRANSFERT_TYPE_CONTROL;
+  ep0_out->endpoint_descriptor.wMaxPacketSize=64;
+
+  // Init bLength
+  for(char i=0;i<USB_ENDPOINT_COUNT+2;i++){
+	cusb_dc->endpoints[i].endpoint_descriptor.bLength=sizeof(USB_ENDPOINT_DESCRIPTOR);
+	cusb_dc->endpoints[i].endpoint_descriptor.bDescriptorType=USB_DESCRIPTOR_TYPE_ENDPOINT;
+  }
+
+  // Init interfaces
+  for(char i=0;i<USB_INTERFACE_COUNT;i++){
+	cusb_dc->interface_descriptors[i].bLength=sizeof(USB_INTERFACE_DESCRIPTOR);
+	cusb_dc->interface_descriptors[i].bDescriptorType=USB_DESCRIPTOR_TYPE_INTERFACE;
+  }
+}
+
+int cusb_check_interrupt(int int_bit){
+  if(REG_READ(USBCTRL_INTS) & int_bit)
+	return 1;
+  return 0;
+}
+
+void cusb_handle_bus_reset(void){
+  // https://github.com/raspberrypi/pico-examples/blob/master/usb/device/dev_lowlevel/dev_lowlevel.c#L469
+  REG_WRITE(USBCTRL_ADDR_ENDP, 0); // Remove device address from controller
+  cusb_dc->devaddr=0; // No more device address
+  cusb_dc->setdevaddr=0; // Ensure no device address will be set
+  cusb_dc->configured=0;
+}
+
+EndPointConfiguration* cusb_get_endpoint(char num, u32 direction){
+  EndPointConfiguration *ep;
+  for(char i=0;i<(USB_ENDPOINT_COUNT+2);i++){
+	ep=&(cusb_dc->endpoints[i]);
+	u32 bEndpointAddress=ep->endpoint_descriptor.bEndpointAddress;
+	// Bit 7 (mask 0x80) is IN or OUT and first 4 bits is addr (see p269)
+	if((bEndpointAddress & 0x80) == direction && (bEndpointAddress & 0xF) == num)
+	  return ep;
+  }
+
+  return 0;
+}
+
+void cusb_handle_setup(void){
+  UD_SETUP *pkt=(UD_SETUP*)USBCTRL_DPSRAM_SETUP_PACKET;
+
+  // Always responds with DATA1 PID
+  EndPointConfiguration *ep=cusb_get_endpoint(0, USB_DIRECTION_IN);
+  ep->next_pid=1;
+
+  u8 direction=pkt->bmRequestType & BM_REQUEST_TYPE_DIR_BIT;
+  if(direction == USB_DIRECTION_OUT){
+	if (pkt->bRequest == USB_REQUEST_CODE_SET_ADDRESS){
+	  cusb_dc->devaddr= pkt->wValue & 0xff;
+	  cusb_dc->setdevaddr=1;
+	  // Since we should acknowledge (status phase) before setting the address
+	  // we use setdevaddr boolean. When status done, buffer_status interrupt will be triggered
+	  // and ep0_in_handler will set the address
+	  cusb_status_xfer(USB_DIRECTION_IN);
+	} else if (pkt->bRequest == USB_REQUEST_CODE_SET_CONFIGURATION) {
+	  cusb_status_xfer(USB_DIRECTION_IN);
+	  cusb_dc->configured=1;
+	} else {
+	  if(cusb_dc->setup_handler){
+		cusb_dc->setup_handler(pkt);
+	  }
+	  // Acknowledge whatever other requests
+	  cusb_status_xfer(USB_DIRECTION_IN);
+	}
+  } else if(direction == USB_DIRECTION_IN){
+	if(pkt->bRequest == USB_REQUEST_CODE_GET_DESCRIPTOR){
+	  int desc_type=pkt->wValue>>8; // See USB SPecification (wValue contains descriptor type+index)
+	  if(desc_type == USB_DESCRIPTOR_TYPE_DEVICE ){
+		// Send descriptor
+		cusb_start_xfer(&cusb_dc->device_descriptor, sizeof(USB_DEVICE_DESCRIPTOR), ep);
+	  } else if(desc_type == USB_DESCRIPTOR_TYPE_CONFIGURATION ){
+
+		USB_CONFIGURATION_DESCRIPTOR *conf=cusb_dc->full_configuration_descriptor;
+		int size=conf->bLength;
+
+		if(pkt->wLength > size)
+		  size=conf->wTotalLength;
+
+		// Send descriptors!!
+		cusb_start_xfer(conf, size, ep);
+	  } else if(desc_type == USB_DESCRIPTOR_TYPE_STRING ){
+		u8 id = pkt->wValue & 0xff; // Get string id
+		if(id==0){ // This is the special string descriptor for supported language
+		  cusb_start_xfer(&(cusb_dc->supported_languages), sizeof(USB_STRING_DESCRIPTOR_ZERO), ep);
+		}
+		else {
+		  char *str=cusb_dc->descriptor_strings[id-1]; // Remember id 0 taken by ZERO DESCriptor
+		  u8 *ptr=xferbuff;
+		  USB_UNICODE_STRING_DESCRIPTOR *u=(USB_UNICODE_STRING_DESCRIPTOR*)ptr;
+		  u->bLength = sizeof(USB_UNICODE_STRING_DESCRIPTOR);
+		  u->bDescriptorType=USB_DESCRIPTOR_TYPE_STRING;
+		  char c;
+		  ptr+=sizeof(USB_UNICODE_STRING_DESCRIPTOR); // String first 2 descriptor entries
+		  do {
+			c = *str++;
+			*ptr++ = c;
+			*ptr++ = 0; // Unicode
+			u->bLength+=2; // Unicode
+		  } while(c != '\0');
+		  cusb_start_xfer(xferbuff, u->bLength, ep);
+		}
+	  }
+	  else {
+		cusb_dc->setup_handler(pkt);
+	  }
+	}
+  }
+}
+
+void cusb_handle_buffer_status(void){
+  u32 status=REG_READ(USBCTRL_BUFF_STATUS);
+  for(u8 num=0;num<16;num++){
+	for(u8 i=0;i<2;i++){
+	  u32 bit=1u << (num*2+i); // Shift register for IN and OUT for endpoint num
+	  if(status & bit){
+		u32 dir=i ? USB_DIRECTION_OUT : USB_DIRECTION_IN;
+		EndPointConfiguration *ep=cusb_get_endpoint(num,dir);
+		REG_WRITE_BITMAP_CLEAR(USBCTRL_BUFF_STATUS, bit); // Clear buffer status
+		ep->handler((u8*)ep->buffer0,*(ep->buffer_control)&MASK_USBCTRL_BUFFER0_LENGTH);
+	  }
+	}
+  }
+}
+
+void cusb_status_xfer(u32 dir){
+  // If dir == USB_DIRECTION_OUT
+  // controller will receive the out token from host then send acknowledgement
+  // otherwise if we do not perform xfer when receiving out token, then controller do not send acknowledgement which do not complete the control transaction
+  // If dir == USB_DIRECTION_IN
+  // controller will receive the in token from host then send zlp and receive acknowledgement from host
+  EndPointConfiguration *ep=cusb_get_endpoint(0, dir);
+  cusb_start_xfer(0,0,ep);
+}
+
+void cusb_ep0_in_handler(u8 *buffer, int len) {
+  // This function is trigger when buff_status interrupt is handled
+  // it is called inside  cusb_handle_buffer_status
+  if(cusb_dc->setdevaddr){
+	REG_WRITE(USBCTRL_ADDR_ENDP, cusb_dc->devaddr);
+	cusb_dc->setdevaddr=0;
+  }
+  else if(xferremain>0){
+	EndPointConfiguration *ep=cusb_get_endpoint(0, USB_DIRECTION_IN);
+	cusb_start_xfer(xferptr,xferremain,ep);
+  }
+  else {
+	cusb_status_xfer(USB_DIRECTION_OUT); // Acknowledge with zlp when setup transaction ends
+  }
+}
+
+void cusb_ep0_out_handler(u8 *buffer, int len) {
+  // This function is trigger when buff_status interrupt is handled
+  // it is called inside  cusb_handle_buffer_status
+}
+
+
+
+void cusb_start_xfer(void* data, u32 size, EndPointConfiguration *ep){
+  u32 buffer_ctrl = size; // Set data size
+  buffer_ctrl|=ep->next_pid ? BIT_USBCTRL_BUFFER0_PID : 0; // Set DATA0 or DATA1
+  ep->next_pid ^= 1u; // For next transfert
+
+  // Move user data to usb buffer if needed
+  u32 direction=ep->endpoint_descriptor.bEndpointAddress & 0x80;
+  if(direction == USB_DIRECTION_IN){
+	memcpy((void *)ep->buffer0, data, size);
+	buffer_ctrl |= BIT_USBCTRL_BUFFER0_FULL; // Set buffer full for controller
+	// Support for data > 64 bytes (see also cusb_ep0_in_handler())
+	if(size>64){
+	  xferremain=size-64;
+	  xferptr=((u8*) data)+64;
+	}
+	else {
+	  xferremain=0;
+	}
+  }
+
+  // Setup buffer (not available bit yet)
+  *(ep->buffer_control)=buffer_ctrl;
+
+  // Now do available bit (because USB and cores have different clock speed (race condition see p389)
+  asm volatile("nop;nop;nop;"); // ceil(125MHz/48MHz) Should wait 3 cycles see warning p392
+  *(ep->buffer_control)=buffer_ctrl| BIT_USBCTRL_BUFFER0_AVAILABLE; // Apply available bit
+
+}
+
+void cusb_eoi(){
+  if(cusb_dc->eoi)
+	cusb_dc->eoi();
+}
+
+void cusb_isr_trigger(){
+  REG_WRITE(PPB_NVIC_ISPR, 1u<<5); // Trigger interrupt 5 (USB Controller)
+}
diff --git a/src/libs/usb/cusb.h b/src/libs/usb/cusb.h
new file mode 100644
index 0000000..b751dd5
--- /dev/null
+++ b/src/libs/usb/cusb.h
@@ -0,0 +1,72 @@
+#ifndef __USB_H__
+#define __USB_H__
+
+#include "../types.h"
+#include "../utils.h"
+#include "proto.h"
+
+#define USBCTRL_DPSRAM_SIZE (1024*4) // See p122
+#define USBCTRL_BUFFER_SIZE 64 // See p385
+#define USB_ENDPOINT_COUNT 3   // Number of endpoints for the device driver
+#define USB_INTERFACE_COUNT 2 // Number of interfaces for the device driver
+#define BCD_USB 0x0110
+#define USB_BUFFER_OFFSET(ADDR) (ADDR ^ USBCTRL_DPSRAM_BASE) // See here https://github.com/raspberrypi/pico-examples/blob/master/usb/device/dev_lowlevel/dev_lowlevel.c#L141
+
+typedef struct EndPoint {
+  void *buffer0;
+  void *buffer1;
+  void *rbc; // EP control register
+  volatile u32 *rbc_in; // EP buffer control in register
+  volatile u32 *rbc_out; // EP buffer control out register
+  int next_pid;
+  int is_in;
+} EndPoint;
+
+typedef  struct EndPointConfiguration{
+  USB_ENDPOINT_DESCRIPTOR endpoint_descriptor;
+  volatile u32* buffer0;
+  volatile u32* buffer1;
+  volatile u32* buffer_control;
+  volatile u32* endpoint_control;
+  u32 next_pid; // Next usb pkt pid (DATA0/DATA1)
+  void (*handler)(u8*,int);
+} EndPointConfiguration;
+
+typedef struct FunctionalDescriptors {
+  int size;
+  void* descriptors;
+} FunctionalDescriptors;
+
+typedef struct DeviceConfiguration {
+  u8 devaddr; // Current device address
+  u8 setdevaddr; // Set to one when devaddr is available
+  u8 configured; // Set to true when host triggered SET_CONFIGURATION
+  USB_DEVICE_DESCRIPTOR device_descriptor;
+  USB_CONFIGURATION_DESCRIPTOR configuration_descriptor;
+  USB_INTERFACE_DESCRIPTOR interface_descriptors[USB_INTERFACE_COUNT];
+  EndPointConfiguration endpoints[2+USB_ENDPOINT_COUNT]; // Endpoint 0 in and out always required so +2
+  USB_STRING_DESCRIPTOR_ZERO supported_languages;
+  FunctionalDescriptors functional_descriptors;
+  void *full_configuration_descriptor;
+  void (*setup_handler)(UD_SETUP *pkt); // Notify the driver in case of unhandled setup packet
+  void (*setup_ack_handler)(void); // Notify the driver
+  void (*eoi)(void); // Notify the driver for End Of Interrupt
+  char *descriptor_strings[]; // All string like vendor and product id
+} DeviceConfiguration;
+
+
+void cusb_init(DeviceConfiguration *dc);
+void cusb_init_device_configuration();
+void cusb_ep0_in_handler(u8 *buffer, int len);
+void cusb_ep0_out_handler(u8 *buffer, int len);
+void cusb_handle_setup(void);
+void cusb_handle_bus_reset(void);
+void cusb_handle_buffer_status(void);
+int cusb_check_interrupt(int int_bit);
+void usb_start_xfer(void* data, u32 size, int ep);
+void cusb_start_xfer(void *data, u32 size, EndPointConfiguration *ep);
+void cusb_status_xfer(u32 dir);
+void cusb_eoi(); // Trigger at end of each USB interrupt
+void cusb_isr_trigger(); // Manually trigger an usb interrupt
+
+#endif
diff --git a/src/libs/usb/proto.h b/src/libs/usb/proto.h
new file mode 100644
index 0000000..d313cb5
--- /dev/null
+++ b/src/libs/usb/proto.h
@@ -0,0 +1,227 @@
+#ifndef __PROTO_H__
+#define __PROTO_H__
+
+#include "../types.h"
+
+// ---------- USB Protocol ----------
+#define USB_DATA typedef struct __attribute__((__packed__))
+
+// USB token PIDs
+#define USB_PID_OUT   0b0001
+#define USB_PID_IN    0b1001
+#define USB_PID_SOF   0b0101
+#define USB_PID_SETUP 0b1101
+
+// USB data PIDs
+#define USB_PID_DATA0 0b0011
+#define USB_PID_DATA1 0b1011
+#define USB_PID_DATA2 0b0111
+#define USB_PID_MDATA 0b1111
+
+// Standard Requests Code (bRequest)
+#define USB_REQUEST_CODE_GET_STATUS    0
+#define USB_REQUEST_CODE_CLEAR_FEATURE 1
+#define USB_REQUEST_CODE_SET_FEATURE 3
+#define USB_REQUEST_CODE_SET_ADDRESS 5
+#define USB_REQUEST_CODE_GET_DESCRIPTOR 6
+#define USB_REQUEST_CODE_SET_DESCRIPTOR 7
+#define USB_REQUEST_CODE_GET_CONFIGURATION 8
+#define USB_REQUEST_CODE_SET_CONFIGURATION 9
+#define USB_REQUEST_CODE_GET_INTERFACE 10
+#define USB_REQUEST_CODE_SET_INTERFACE 11
+#define USB_REQUEST_CODE_SYNCH_FRAME 12
+
+// Descriptor Types (wValue)
+#define USB_DESCRIPTOR_TYPE_DEVICE 1
+#define USB_DESCRIPTOR_TYPE_CONFIGURATION 2
+#define USB_DESCRIPTOR_TYPE_STRING 3
+#define USB_DESCRIPTOR_TYPE_INTERFACE 4
+#define USB_DESCRIPTOR_TYPE_ENDPOINT 5
+#define USB_DESCRIPTOR_TYPE_DEVICE_QUALIFIER 6
+#define USB_DESCRIPTOR_TYPE_OTHER_SPEED_CONFIGURATION 7
+#define USB_DESCRIPTOR_TYPE_INTERFACE_POWER 8
+
+// Feature Selector (wvalue)
+#define USB_FEATURE_SELECTOR_DEVICE_REMOTE_WAKEUP 1
+#define USB_FEATURE_SELECTOR_ENDPOINT_HALT 0
+#define USB_FEATURE_SELECTOR_TEST_MODE 2
+
+// Device class code https://developerhelp.microchip.com/xwiki/bin/view/applications/usb/how-it-works/device-classes/
+#define USB_DEVICE_CLASS_DEVICE 0x00
+#define USB_DEVICE_CLASS_AUDIO 0x01
+#define USB_DEVICE_CLASS_CDC 0x02
+#define USB_DEVICE_CLASS_HID 0x03
+#define USB_DEVICE_CLASS_PHYSICAL 0x05
+
+#define USB_TRANSFERT_TYPE_CONTROL     0b00
+#define USB_TRANSFERT_TYPE_ISOCHRONOUS 0b01
+#define USB_TRANSFERT_TYPE_BULK        0b10
+#define USB_TRANSFERT_TYPE_INTERRUPT   0b11
+
+#define USB_DIRECTION_OUT 0x00
+#define USB_DIRECTION_IN 0x80
+
+// Supported languages http://www.baiheee.com/Documents/090518/090518112619/USB_LANGIDs.pdf
+#define USB_LANG_ENGLISH_US 0x0409
+#define USB_LANG_ENGLISH_UNITED_KINGDOM 0x0809
+
+#define B_ENDPOINT_ADDRESS(NUM, DIR) ((NUM)&0xF | (DIR))
+#define BM_REQUEST_TYPE_DIR_BIT (USB_DIRECTION_IN)
+
+#define USB_INTERFACE_CLASS_COMMUNICATION 0x02 // Communication interface class
+#define USB_INTERFACE_CLASS_DATA 0x0a // Data interface class
+#define USB_INTERFACE_SUBCLASS_ACM  0x02 // Abstract Control Mode
+#define USB_INTERFACE_COMMUNICATION_PROTOCOL_NOPROTO 0x00 // No protocol
+
+USB_DATA { // P250
+  u8 bmRequestType;
+  u8 bRequest;
+  u16 wValue;
+  u16 wIndex;
+  u16 wLength;
+} UD_SETUP;
+
+// ----- Descriptors -----
+
+USB_DATA { // P262
+  u8 bLength;
+  u8 bDescriptorType;
+  u16 bcdUSB;
+  u8 bDeviceClass;
+  u8 bDeviceSubClass;
+  u8 bDeviceProtocol;
+  u8 bMaxPacketSize0;
+  u16 idVendor;
+  u16 idProduct;
+  u16 bcdDevice;
+  u8 iManufacturer;
+  u8 iProduct;
+  u8 iSerialNumber;
+  u8 bNumConfigurations;
+} USB_DEVICE_DESCRIPTOR;
+
+
+USB_DATA { // P264 (may not be used in this code)
+  u8 bLength;
+  u8 bDescriptorType;
+  u16 bcdUSB;
+  u8 bDeviceClass;
+  u8 bDeviceSubClass;
+  u8 bDeviceProtocol;
+  u8 bMaxPacketSize0;
+  u8 bNumConfigurations;
+  u8 bReserved;
+} USB_DEVICE_QUALIFIER_DESCRIPTOR;
+
+USB_DATA { // P265
+  u8 bLength;
+  u8 bDescriptorType;
+  u16 wTotalLength;
+  u8 bNumInterfaces;
+  u8 bConfigurationValue;
+  u8 iConfiguration;
+  u8 bmAttributes;
+  u8 bMaxPower;
+} USB_CONFIGURATION_DESCRIPTOR;
+
+USB_DATA {
+  u8 bLength;
+  u8 bDescriptorType;
+  u16 wTotalLength;
+  u8 bNumInterfaces;
+  u8 bConfigurationValue;
+  u8 iConfiguration;
+  u8 bmAttributes;
+  u8 bMaxPower;
+} USB_OTHER_SPEED_CONFIGURATION_DESCRIPTOR;
+
+USB_DATA {
+  u8 bLength;
+  u8 bDescriptorType;
+  u8 bInterfaceNumber;
+  u8 bAlternateSetting;
+  u8 bNumEndpoints;
+  u8 bInterfaceClass;
+  u8 bInterfaceSubClass;
+  u8 bInterfaceProtocol;
+  u8 iInterface;
+} USB_INTERFACE_DESCRIPTOR;
+
+USB_DATA {
+  u8 bLength;
+  u8 bDescriptorType;
+  u8 bEndpointAddress;
+  u8 bmAttributes;
+  u16 wMaxPacketSize;
+  u8 bInterval;
+} USB_ENDPOINT_DESCRIPTOR;
+
+USB_DATA {
+  u8 bLength;
+  u8 bDescriptorType;
+  u16 wLANGID0;
+  // Can be more wLANGIDX but stop here!
+} USB_STRING_DESCRIPTOR_ZERO;
+
+USB_DATA {
+  u8 bLength;
+  u8 bDescriptorType;
+  // You add your unicode string there
+} USB_UNICODE_STRING_DESCRIPTOR;
+
+
+
+
+// ----- CDC Device Class -----
+// p10 PSTN120.pdf in CDC Specification
+// p10-..  CDC120-20101103-track.pdf
+// p10-..  usbcdc11.pdf
+#define USB_INTERFACE_CLASS_COMMUNICATION 0x02 // Communication interface class
+#define USB_INTERFACE_CLASS_DATA 0x0a // Data interface class
+#define USB_INTERFACE_SUBCLASS_ACM  0x02 // Abstract Control Mode
+#define USB_INTERFACE_COMMUNICATION_PROTOCOL_NOPROTO 0x00 // No protocol
+
+#define USB_RS232_BMCAPABILITIES 0b10
+
+#define USB_REQUEST_CODE_SET_LINE_CODING 0x20
+#define USB_REQUEST_CODE_SET_CONTROL_LINE_STATE 0x22
+#define USB_REQUEST_CODE_SEND_BREAK 0x23
+
+// p34
+USB_DATA {
+  u8 bFunctionLength;
+  u8 bDescriptorType;
+  u8 bDescriptorSubtype;
+  u16 bcdCDC;
+} USB_CDC_HEADER_FUNCTIONAL_DESCRIPTOR;
+
+// p35
+USB_DATA {
+  u8 bFunctionLength;
+  u8 bDescriptorType;
+  u8 bDescriptorSubtype;
+  u8 bmCapabilities;
+} USB_ACM_FUNCTIONAL_DESCRIPTOR;
+
+// p40
+USB_DATA {
+  u8 bFunctionLength;
+  u8 bDescriptorType;
+  u8 bDescriptorSubtype;
+  u8 bMasterInterface;
+  u8 bSlaveInterface0;
+  // Can be more! I guess for RS-232 it is enough
+} USB_UNION_INTERFACE_DESCRIPTOR;
+
+
+// SEE p49!!!! it is all there it seems
+// the last descriptor on that page is the "Call management functional
+// descriptor" (p34). It seems not required according to chatgpt for rs-232
+USB_DATA {
+  USB_CDC_HEADER_FUNCTIONAL_DESCRIPTOR header_descriptor;
+  USB_ACM_FUNCTIONAL_DESCRIPTOR acm_descriptor;
+  USB_UNION_INTERFACE_DESCRIPTOR union_descriptor;
+} USB_CDC_ACM_FUNCTIONAL_DESCRIPTOR;
+
+
+#endif