8 files changed, 48 insertions, 87 deletions

diff --git a/payloads/libpayload/drivers/usb/ehci.c b/payloads/libpayload/drivers/usb/ehci.c
index 0c3e32ddcc..b83712c463 100644
--- a/payloads/libpayload/drivers/usb/ehci.c
+++ b/payloads/libpayload/drivers/usb/ehci.c
@@ -176,16 +176,17 @@ static int ehci_set_periodic_schedule(ehci_t *ehcic, int enable)
 static void ehci_shutdown (hci_t *controller)
 {
 	detach_controller(controller);
+
 	/* Make sure periodic schedule is disabled */
 	ehci_set_periodic_schedule(EHCI_INST(controller), 0);
-	/* Free periodic frame list */
-	free(phys_to_virt(EHCI_INST(controller)->operation->periodiclistbase));
-
-	/* Free dummy QH */
-	free((void *)EHCI_INST(controller)->dummy_qh);
 
+	/* Give all ports back to companion controller */
 	EHCI_INST(controller)->operation->configflag = 0;
 
+	/* Free all dynamic allocations */
+	free(EHCI_INST(controller)->dma_buffer);
+	free(phys_to_virt(EHCI_INST(controller)->operation->periodiclistbase));
+	free((void *)EHCI_INST(controller)->dummy_qh);
 	free(EHCI_INST(controller));
 	free(controller);
 }
@@ -788,16 +789,8 @@ ehci_init (unsigned long physical_bar)
 {
 	int i;
 	hci_t *controller = new_controller ();
-
-	if (!controller)
-		fatal("Could not create USB controller instance.\n");
-
-	controller->instance = malloc (sizeof (ehci_t));
-	if(!controller->instance)
-		fatal("Not enough memory creating USB controller instance.\n");
-
+	controller->instance = xzalloc(sizeof (ehci_t));
 	controller->type = EHCI;
-
 	controller->start = ehci_start;
 	controller->stop = ehci_stop;
 	controller->reset = ehci_reset;
@@ -811,13 +804,10 @@ ehci_init (unsigned long physical_bar)
 	controller->create_intr_queue = ehci_create_intr_queue;
 	controller->destroy_intr_queue = ehci_destroy_intr_queue;
 	controller->poll_intr_queue = ehci_poll_intr_queue;
-	for (i = 0; i < 128; i++) {
-		controller->devices[i] = 0;
-	}
 	init_device_entry (controller, 0);
 
-	EHCI_INST(controller)->capabilities = phys_to_virt(physical_bar);
-	EHCI_INST(controller)->operation = (hc_op_t *)(phys_to_virt(physical_bar) + EHCI_INST(controller)->capabilities->caplength);
+	EHCI_INST(controller)->capabilities = phys_to_virt(controller->reg_base);
+	EHCI_INST(controller)->operation = (hc_op_t *)(phys_to_virt(controller->reg_base) + EHCI_INST(controller)->capabilities->caplength);
 
 	/* Set the high address word (aka segment) if controller is 64-bit */
 	if (EHCI_INST(controller)->capabilities->hccparams & 1)
diff --git a/payloads/libpayload/drivers/usb/ehci_rh.c b/payloads/libpayload/drivers/usb/ehci_rh.c
index 67f63cc481..6109505c7f 100644
--- a/payloads/libpayload/drivers/usb/ehci_rh.c
+++ b/payloads/libpayload/drivers/usb/ehci_rh.c
@@ -48,6 +48,17 @@ typedef struct {
 static void
 ehci_rh_destroy (usbdev_t *dev)
 {
+	int port;
+
+	/* Tear down all devices below the root hub (in bottom-up order). */
+	for (port = 0; port < RH_INST(dev)->n_ports; port++) {
+		if (RH_INST(dev)->devices[port] != -1) {
+			usb_detach_device(dev->controller,
+					  RH_INST(dev)->devices[port]);
+			RH_INST(dev)->devices[port] = -1;
+		}
+	}
+
         free (RH_INST(dev)->devices);
         free (RH_INST(dev));
 }
@@ -156,10 +167,10 @@ ehci_rh_init (usbdev_t *dev)
 	dev->destroy = ehci_rh_destroy;
 	dev->poll = ehci_rh_poll;
 
-	dev->data = malloc(sizeof(rh_inst_t));
-
+	dev->data = xmalloc(sizeof(rh_inst_t));
 	RH_INST(dev)->n_ports = EHCI_INST(dev->controller)->capabilities->hcsparams & HCS_NPORTS_MASK;
 	RH_INST(dev)->ports = EHCI_INST(dev->controller)->operation->portsc;
+	RH_INST(dev)->devices = xmalloc(RH_INST(dev)->n_ports * sizeof(int));
 
 	usb_debug("root hub has %x ports\n", RH_INST(dev)->n_ports);
 
@@ -175,7 +186,6 @@ ehci_rh_init (usbdev_t *dev)
 	}
 	mdelay(20); // ehci spec 2.3.9
 
-	RH_INST(dev)->devices = malloc(RH_INST(dev)->n_ports * sizeof(int));
 	for (i=0; i < RH_INST(dev)->n_ports; i++) {
 		RH_INST(dev)->devices[i] = -1;
 		ehci_rh_scanport(dev, i);
diff --git a/payloads/libpayload/drivers/usb/ohci.c b/payloads/libpayload/drivers/usb/ohci.c
index e1664cca9b..143f738de3 100644
--- a/payloads/libpayload/drivers/usb/ohci.c
+++ b/payloads/libpayload/drivers/usb/ohci.c
@@ -172,16 +172,8 @@ ohci_init (unsigned long physical_bar)
 	int i;
 
 	hci_t *controller = new_controller ();
-
-	if (!controller)
-		fatal("Could not create USB controller instance.\n");
-
-	controller->instance = malloc (sizeof (ohci_t));
-	if(!controller->instance)
-		fatal("Not enough memory creating USB controller instance.\n");
-
+	controller->instance = xzalloc(sizeof (ohci_t));
 	controller->type = OHCI;
-
 	controller->start = ohci_start;
 	controller->stop = ohci_stop;
 	controller->reset = ohci_reset;
@@ -195,9 +187,6 @@ ohci_init (unsigned long physical_bar)
 	controller->create_intr_queue = ohci_create_intr_queue;
 	controller->destroy_intr_queue = ohci_destroy_intr_queue;
 	controller->poll_intr_queue = ohci_poll_intr_queue;
-	for (i = 0; i < 128; i++) {
-		controller->devices[i] = 0;
-	}
 	init_device_entry (controller, 0);
 	OHCI_INST (controller)->roothub = controller->devices[0];
 
@@ -279,8 +268,7 @@ ohci_shutdown (hci_t *controller)
 		return;
 	detach_controller (controller);
 	ohci_stop(controller);
-	OHCI_INST (controller)->roothub->destroy (OHCI_INST (controller)->
-						  roothub);
+	free (OHCI_INST (controller)->hcca);
 	free ((void *)OHCI_INST (controller)->periodic_ed);
 	free (OHCI_INST (controller));
 	free (controller);
@@ -289,13 +277,13 @@ ohci_shutdown (hci_t *controller)
 static void
 ohci_start (hci_t *controller)
 {
-// TODO: turn on all operation of OHCI, but assume that it's initialized.
+	OHCI_INST (controller)->opreg->HcControl |= PeriodicListEnable;
 }
 
 static void
 ohci_stop (hci_t *controller)
 {
-// TODO: turn off all operation of OHCI
+	OHCI_INST (controller)->opreg->HcControl &= ~PeriodicListEnable;
 }
 
 static int
diff --git a/payloads/libpayload/drivers/usb/ohci_rh.c b/payloads/libpayload/drivers/usb/ohci_rh.c
index b3bf7ef3cb..5d82bd59e0 100644
--- a/payloads/libpayload/drivers/usb/ohci_rh.c
+++ b/payloads/libpayload/drivers/usb/ohci_rh.c
@@ -83,6 +83,11 @@ ohci_rh_enable_port (usbdev_t *dev, int port)
 static void
 ohci_rh_disable_port (usbdev_t *dev, int port)
 {
+	if (RH_INST (dev)->port[port] != -1) {
+		usb_detach_device(dev->controller, RH_INST (dev)->port[port]);
+		RH_INST (dev)->port[port] = -1;
+	}
+
 	OHCI_INST (dev->controller)->opreg->HcRhPortStatus[port] = ClearPortEnable; // disable port
 	int timeout = 50; /* timeout after 50 * 100us == 5ms */
 	while ((OHCI_INST (dev->controller)->opreg->HcRhPortStatus[port]
@@ -180,12 +185,9 @@ ohci_rh_init (usbdev_t *dev)
 	dev->destroy = ohci_rh_destroy;
 	dev->poll = ohci_rh_poll;
 
-	dev->data = malloc (sizeof (rh_inst_t));
-	if (!dev->data)
-		fatal("Not enough memory for OHCI RH.\n");
-
+	dev->data = xmalloc (sizeof (rh_inst_t));
 	RH_INST (dev)->numports = OHCI_INST (dev->controller)->opreg->HcRhDescriptorA & NumberDownstreamPortsMask;
-	RH_INST (dev)->port = malloc(sizeof(int) * RH_INST (dev)->numports);
+	RH_INST (dev)->port = xmalloc(sizeof(int) * RH_INST (dev)->numports);
 	usb_debug("%d ports registered\n", RH_INST (dev)->numports);
 
 	for (i = 0; i < RH_INST (dev)->numports; i++) {
diff --git a/payloads/libpayload/drivers/usb/uhci.c b/payloads/libpayload/drivers/usb/uhci.c
index 3eff612129..2e3249ee74 100644
--- a/payloads/libpayload/drivers/usb/uhci.c
+++ b/payloads/libpayload/drivers/usb/uhci.c
@@ -153,16 +153,8 @@ uhci_pci_init (pcidev_t addr)
 	u16 reg16;
 
 	hci_t *controller = new_controller ();
-
-	if (!controller)
-		fatal("Could not create USB controller instance.\n");
-
-	controller->instance = malloc (sizeof (uhci_t));
-	if(!controller->instance)
-		fatal("Not enough memory creating USB controller instance.\n");
-
+	controller->instance = xzalloc(sizeof (uhci_t));
 	controller->type = UHCI;
-
 	controller->start = uhci_start;
 	controller->stop = uhci_stop;
 	controller->reset = uhci_reset;
@@ -176,9 +168,6 @@ uhci_pci_init (pcidev_t addr)
 	controller->create_intr_queue = uhci_create_intr_queue;
 	controller->destroy_intr_queue = uhci_destroy_intr_queue;
 	controller->poll_intr_queue = uhci_poll_intr_queue;
-	for (i = 0; i < 128; i++) {
-		controller->devices[i] = 0;
-	}
 	init_device_entry (controller, 0);
 	UHCI_INST (controller)->roothub = controller->devices[0];
 
@@ -257,8 +246,6 @@ uhci_shutdown (hci_t *controller)
 	if (controller == 0)
 		return;
 	detach_controller (controller);
-	UHCI_INST (controller)->roothub->destroy (UHCI_INST (controller)->
-						  roothub);
 	uhci_reg_write16(controller, USBCMD,
 			 uhci_reg_read16(controller, USBCMD) & 0);	// stop work
 	free (UHCI_INST (controller)->framelistptr);
diff --git a/payloads/libpayload/drivers/usb/uhci_rh.c b/payloads/libpayload/drivers/usb/uhci_rh.c
index 5cb18b9516..4668c4ecae 100644
--- a/payloads/libpayload/drivers/usb/uhci_rh.c
+++ b/payloads/libpayload/drivers/usb/uhci_rh.c
@@ -176,6 +176,8 @@ uhci_rh_report_port_changes (usbdev_t *dev)
 static void
 uhci_rh_destroy (usbdev_t *dev)
 {
+	usb_detach_device (dev->controller, 1);
+	usb_detach_device (dev->controller, 2);
 	uhci_rh_disable_port (dev, 1);
 	uhci_rh_disable_port (dev, 2);
 	free (RH_INST (dev));
@@ -197,9 +199,7 @@ uhci_rh_init (usbdev_t *dev)
 
 	uhci_rh_enable_port (dev, 1);
 	uhci_rh_enable_port (dev, 2);
-	dev->data = malloc (sizeof (rh_inst_t));
-	if (!dev->data)
-		fatal("Not enough memory for UHCI RH.\n");
+	dev->data = xmalloc (sizeof (rh_inst_t));
 
 	RH_INST (dev)->port[0] = -1;
 	RH_INST (dev)->port[1] = -1;
diff --git a/payloads/libpayload/drivers/usb/usb.c b/payloads/libpayload/drivers/usb/usb.c
index 4f21e0cf3b..eb23760033 100644
--- a/payloads/libpayload/drivers/usb/usb.c
+++ b/payloads/libpayload/drivers/usb/usb.c
@@ -39,15 +39,9 @@ hci_t *usb_hcs = 0;
 hci_t *
 new_controller (void)
 {
-	hci_t *controller = malloc (sizeof (hci_t));
-
-	if (controller) {
-		/* atomic */
-		controller->next = usb_hcs;
-		usb_hcs = controller;
-		/* atomic end */
-	}
-
+	hci_t *controller = xzalloc(sizeof (hci_t));
+	controller->next = usb_hcs;
+	usb_hcs = controller;
 	return controller;
 }
 
@@ -56,6 +50,9 @@ detach_controller (hci_t *controller)
 {
 	if (controller == NULL)
 		return;
+
+	usb_detach_device(controller, 0);	/* tear down root hub tree */
+
 	if (usb_hcs == controller) {
 		usb_hcs = controller->next;
 	} else {
diff --git a/payloads/libpayload/drivers/usb/xhci.c b/payloads/libpayload/drivers/usb/xhci.c
index 23314855ee..c60a967607 100644
--- a/payloads/libpayload/drivers/usb/xhci.c
+++ b/payloads/libpayload/drivers/usb/xhci.c
@@ -150,11 +150,6 @@ xhci_init (unsigned long physical_bar)
 	/* First, allocate and initialize static controller structures */
 
 	hci_t *const controller = new_controller();
-	if (!controller) {
-		xhci_debug("Could not create USB controller instance\n");
-		return controller;
-	}
-
 	controller->type		= XHCI;
 	controller->start		= xhci_start;
 	controller->stop		= xhci_stop;
@@ -170,17 +165,10 @@ xhci_init (unsigned long physical_bar)
 	controller->destroy_intr_queue	= xhci_destroy_intr_queue;
 	controller->poll_intr_queue	= xhci_poll_intr_queue;
 	controller->pcidev		= 0;
-	for (i = 0; i < 128; ++i) {
-		controller->devices[i] = NULL;
-	}
 
-	controller->instance = malloc(sizeof(xhci_t));
-	if (!controller->instance) {
-		xhci_debug("Out of memory creating xHCI controller instance\n");
-		goto _free_controller;
-	}
+	controller->reg_base = (u32)physical_bar;
+	controller->instance = xzalloc(sizeof(xhci_t));
 	xhci_t *const xhci = (xhci_t *)controller->instance;
-	memset(xhci, 0x00, sizeof(*xhci));
 
 	init_device_entry(controller, 0);
 	xhci->roothub = controller->devices[0];
@@ -282,6 +270,7 @@ xhci_init (unsigned long physical_bar)
 	return controller;
 
 _free_xhci_structs:
+	free(xhci->dma_buffer);
 	if (xhci->sp_ptrs) {
 		for (i = 0; i < max_sp_bufs; ++i) {
 			if (xhci->sp_ptrs[i])
@@ -410,13 +399,10 @@ xhci_shutdown(hci_t *const controller)
 
 	if (controller == 0)
 		return;
-	xhci_t *const xhci = XHCI_INST(controller);
 
 	detach_controller(controller);
 
-	/* Detach device hierarchy (starting at root hub) */
-	usb_detach_device(controller, 0);
-
+	xhci_t *const xhci = XHCI_INST(controller);
 	xhci_stop(controller);
 
         if (controller->pcidev)
@@ -430,6 +416,7 @@ xhci_shutdown(hci_t *const controller)
 		}
 	}
 	free(xhci->sp_ptrs);
+	free(xhci->dma_buffer);
 	free(xhci->dcbaa);
 	free(xhci->dev);
 	free((void *)xhci->ev_ring_table);