Assign unique, auto-incrementing addresses to USB devices

USBHost.h

 private:
 	static void isr();
 	static void claim_drivers(Device_t *dev);
+	static uint32_t assign_address(void);
 	static bool queue_Transfer(Pipe_t *pipe, Transfer_t *transfer);
 	static void init_Device_Pipe_Transfer_memory(void);
 	static Device_t * allocate_Device(void);

enumeration.cpp

 static Device_t  *devlist=NULL;
 
 
-static uint32_t assign_addr(void);
 static void pipe_set_maxlen(Pipe_t *pipe, uint32_t maxlen);
 static void pipe_set_addr(Pipe_t *pipe, uint32_t addr);
 
 		switch (dev->enum_state) {
 		case 0: // read 8 bytes of device desc, set max packet, and send set address
 			pipe_set_maxlen(dev->control_pipe, enumbuf[7]);
-			mk_setup(enumsetup, 0, 5, assign_addr(), 0, 0); // 5=SET_ADDRESS
+			mk_setup(enumsetup, 0, 5, assign_address(), 0, 0); // 5=SET_ADDRESS
 			queue_Control_Transfer(dev, &enumsetup, NULL, NULL);
 			dev->enum_state = 1;
 			return;
 	}
 }
 
-static uint32_t assign_addr(void)
+static bool address_in_use(uint32_t addr)
 {
-	return 29; // TODO: when multiple devices, assign a unique address
+	for (Device_t *p = devlist; p; p = p->next) {
+		if (p->address == addr) return true;
+	}
+	return false;
+}
+
+uint32_t USBHost::assign_address(void)
+{
+	static uint8_t last_assigned_address=0;
+	uint32_t addr = last_assigned_address;
+	while (1) {
+		if (++addr > 127) addr = 1;
+		if (!address_in_use(addr)) {
+			last_assigned_address = addr;
+			return addr;
+		}
+	}
 }
 
 static void pipe_set_maxlen(Pipe_t *pipe, uint32_t maxlen)