Memory management support for over 32 USB buffers

firmware/usb_mem.c

@@ -36,7 +36,7 @@
 __attribute__ ((section(".usbbuffers"), used))
 unsigned char usb_buffer_memory[NUM_USB_BUFFERS * sizeof(usb_packet_t)];
 
-static uint32_t usb_buffer_available = 0xFFFFFFFF;
+static uint32_t usb_buffer_available[4] = { -1, -1, -1, -1 };
 
 // use bitmask and CLZ instruction to implement fast free list
 // http://www.archivum.info/gnu.gcc.help/2006-08/00148/Re-GCC-Inline-Assembly.html
@@ -45,26 +45,30 @@ static uint32_t usb_buffer_available = 0xFFFFFFFF;
 
 usb_packet_t * usb_malloc(void)
 {
-	unsigned int n, avail;
+	unsigned int n, avail, idx = 0;
 	uint8_t *p;
 
 	__disable_irq();
-	avail = usb_buffer_available;
-	n = __builtin_clz(avail); // clz = count leading zeros
+
+	do {
+		avail = usb_buffer_available[idx];
+		if (avail) {
+			break;
+		}
+		idx++;
+	} while (idx < sizeof(usb_buffer_available) / sizeof(usb_buffer_available[0]));
+
+	n = __builtin_clz(avail) + (idx << 5);
 	if (n >= NUM_USB_BUFFERS) {
 		__enable_irq();
 		return NULL;
 	}
-	//serial_print("malloc:");
-	//serial_phex(n);
-	//serial_print("\n");
 
-	usb_buffer_available = avail & ~(0x80000000 >> n);
+	usb_buffer_available[idx] = avail & ~(0x80000000 >> (n & 31));
 	__enable_irq();
+
 	p = usb_buffer_memory + (n * sizeof(usb_packet_t));
-	//serial_print("malloc:");
-	//serial_phex32((int)p);
-	//serial_print("\n");
+
 	*(uint32_t *)p = 0;
 	*(uint32_t *)(p + 4) = 0;
 	return (usb_packet_t *)p;
@@ -76,31 +80,22 @@ extern void usb_rx_memory(usb_packet_t *packet);
 
 void usb_free(usb_packet_t *p)
 {
-	unsigned int n, mask;
+	unsigned int n, mask, idx;
 
-	//serial_print("free:");
 	n = ((uint8_t *)p - usb_buffer_memory) / sizeof(usb_packet_t);
 	if (n >= NUM_USB_BUFFERS) return;
-	//serial_phex(n);
-	//serial_print("\n");
 
 	// if any endpoints are starving for memory to receive
 	// packets, give this memory to them immediately!
 	if (usb_rx_memory_needed && usb_configuration) {
-		//serial_print("give to rx:");
-		//serial_phex32((int)p);
-		//serial_print("\n");
 		usb_rx_memory(p);
 		return;
 	}
 
-	mask = (0x80000000 >> n);
+	idx = n >> 5;
+	mask = 0x80000000 >> (n & 31);
 	__disable_irq();
-	usb_buffer_available |= mask;
+	usb_buffer_available[idx] |= mask;
 	__enable_irq();
-
-	//serial_print("free:");
-	//serial_phex32((int)p);
-	//serial_print("\n");
 }