4 files changed, 21 insertions, 9 deletions

diff --git a/payloads/libpayload/drivers/usb/usb.c b/payloads/libpayload/drivers/usb/usb.c
index d9727f01b0..8ee997ceca 100644
--- a/payloads/libpayload/drivers/usb/usb.c
+++ b/payloads/libpayload/drivers/usb/usb.c
@@ -539,10 +539,14 @@ usb_detach_device(hci_t *controller, int devno)
 	   been called yet by the usb class driver */
 	if (controller->devices[devno]) {
 		controller->devices[devno]->destroy (controller->devices[devno]);
-		free(controller->devices[devno]);
-		controller->devices[devno] = NULL;
 		if (controller->destroy_device)
 			controller->destroy_device(controller, devno);
+		if (controller->devices[devno]->configuration)
+			free(controller->devices[devno]->configuration);
+		if (controller->devices[devno]->descriptor)
+			free(controller->devices[devno]->descriptor);
+		free(controller->devices[devno]);
+		controller->devices[devno] = NULL;
 	}
 }
 
diff --git a/payloads/libpayload/drivers/usb/usbhid.c b/payloads/libpayload/drivers/usb/usbhid.c
index 51c3d46c25..0785df7b58 100644
--- a/payloads/libpayload/drivers/usb/usbhid.c
+++ b/payloads/libpayload/drivers/usb/usbhid.c
@@ -468,15 +468,18 @@ usb_hid_init (usbdev_t *dev)
 			dev->destroy = usb_hid_destroy;
 			dev->poll = usb_hid_poll;
 			int i;
-			for (i = 0; i <= dev->num_endp; i++) {
-				if (dev->endpoints[i].endpoint == 0)
-					continue;
+			for (i = 1; i < dev->num_endp; i++) {
 				if (dev->endpoints[i].type != INTERRUPT)
 					continue;
 				if (dev->endpoints[i].direction != IN)
 					continue;
 				break;
 			}
+			if (i >= dev->num_endp) {
+				usb_debug ("Could not find HID endpoint\n");
+				usb_detach_device (dev->controller, dev->address);
+				return;
+			}
 			usb_debug ("  found endpoint %x for interrupt-in\n", i);
 			/* 20 buffers of 8 bytes, for every 10 msecs */
 			HID_INST(dev)->queue = dev->controller->create_intr_queue (&dev->endpoints[i], 8, 20, 10);
diff --git a/payloads/libpayload/drivers/usb/usbmsc.c b/payloads/libpayload/drivers/usb/usbmsc.c
index 037ead3e37..178f982e07 100644
--- a/payloads/libpayload/drivers/usb/usbmsc.c
+++ b/payloads/libpayload/drivers/usb/usbmsc.c
@@ -598,6 +598,7 @@ usb_msc_init (usbdev_t *dev)
 
 	if (interface->bInterfaceProtocol != 0x50) {
 		usb_debug ("  Protocol not supported.\n");
+		usb_detach_device (dev->controller, dev->address);
 		return;
 	}
 
@@ -606,6 +607,7 @@ usb_msc_init (usbdev_t *dev)
 		(interface->bInterfaceSubClass != 6)) {	// SCSI
 		/* Other protocols, such as ATAPI don't seem to be very popular. looks like ATAPI would be really easy to add, if necessary. */
 		usb_debug ("  Interface SubClass not supported.\n");
+		usb_detach_device (dev->controller, dev->address);
 		return;
 	}
 
@@ -632,11 +634,13 @@ usb_msc_init (usbdev_t *dev)
 	}
 
 	if (MSC_INST (dev)->bulk_in == 0) {
-		usb_debug("couldn't find bulk-in endpoint");
+		usb_debug("couldn't find bulk-in endpoint.\n");
+		usb_detach_device (dev->controller, dev->address);
 		return;
 	}
 	if (MSC_INST (dev)->bulk_out == 0) {
-		usb_debug("couldn't find bulk-out endpoint");
+		usb_debug("couldn't find bulk-out endpoint.\n");
+		usb_detach_device (dev->controller, dev->address);
 		return;
 	}
 	usb_debug ("  using endpoint %x as in, %x as out\n",
diff --git a/payloads/libpayload/drivers/usb/xhci_devconf.c b/payloads/libpayload/drivers/usb/xhci_devconf.c
index 18ba4e8104..f1064f0247 100644
--- a/payloads/libpayload/drivers/usb/xhci_devconf.c
+++ b/payloads/libpayload/drivers/usb/xhci_devconf.c
@@ -180,7 +180,7 @@ xhci_set_address (hci_t *controller, int speed, int hubport, int hubaddr)
 	di = &xhci->dev[slot_id];
 	void *dma_buffer = dma_memalign(64, NUM_EPS * ctxsize);
 	if (!dma_buffer)
-		goto _free_return;
+		goto _disable_return;
 	memset(dma_buffer, 0, NUM_EPS * ctxsize);
 	for (i = 0; i < NUM_EPS; i++, dma_buffer += ctxsize)
 		di->ctx.ep[i] = dma_buffer;
@@ -249,6 +249,7 @@ xhci_set_address (hci_t *controller, int speed, int hubport, int hubaddr)
 _disable_return:
 	xhci_cmd_disable_slot(xhci, slot_id);
 	xhci->dcbaa[slot_id] = 0;
+	usb_detach_device(controller, slot_id);
 _free_return:
 	if (tr)
 		free((void *)tr->ring);
@@ -425,7 +426,7 @@ xhci_destroy_dev(hci_t *const controller, const int slot_id)
 {
 	xhci_t *const xhci = XHCI_INST(controller);
 
-	if (slot_id <= 0 || xhci->max_slots_en > slot_id)
+	if (slot_id <= 0 || slot_id > xhci->max_slots_en)
 		return;
 
 	int i;