2 files changed, 21 insertions, 7 deletions

diff --git a/payloads/libpayload/drivers/udc/chipidea.c b/payloads/libpayload/drivers/udc/chipidea.c
index 58bea063e793..850d0c05b934 100644
--- a/payloads/libpayload/drivers/udc/chipidea.c
+++ b/payloads/libpayload/drivers/udc/chipidea.c
@@ -138,6 +138,14 @@ static void chipidea_halt_ep(struct usbdev_ctrl *this, int ep, int in_dir)
 	while (readl(&p->opreg->epflush))
 		;
 	clrbits_le32(&p->opreg->epctrl[ep], 1 << (7 + (in_dir ? 16 : 0)));
+
+	while (!SIMPLEQ_EMPTY(&p->job_queue[ep][in_dir])) {
+		struct job *job = SIMPLEQ_FIRST(&p->job_queue[ep][in_dir]);
+		if (job->autofree)
+			free(job->data);
+
+		SIMPLEQ_REMOVE_HEAD(&p->job_queue[ep][in_dir], queue);
+	}
 }
 
 static void chipidea_start_ep(struct usbdev_ctrl *this,
diff --git a/payloads/libpayload/drivers/udc/udc.c b/payloads/libpayload/drivers/udc/udc.c
index 89a7d1dac255..9694ff8ec18c 100644
--- a/payloads/libpayload/drivers/udc/udc.c
+++ b/payloads/libpayload/drivers/udc/udc.c
@@ -81,12 +81,8 @@ static struct usbdev_configuration *fetch_config(struct usbdev_ctrl *this,
 	return NULL;
 }
 
-static void enable_interface(struct usbdev_ctrl *this, int iface_num)
+static void cease_operation(struct usbdev_ctrl *this)
 {
-	struct usbdev_configuration *config = this->current_config;
-	struct usbdev_interface *iface = &config->interfaces[iface_num];
-
-	/* first: shut down all endpoints except EP0 */
 	int i;
 	for (i = 1; i < 16; i++) {
 		/* disable endpoints */
@@ -94,8 +90,19 @@ static void enable_interface(struct usbdev_ctrl *this, int iface_num)
 		this->halt_ep(this, i, 1);
 	}
 
+}
+
+static void enable_interface(struct usbdev_ctrl *this, int iface_num)
+{
+	struct usbdev_configuration *config = this->current_config;
+	struct usbdev_interface *iface = &config->interfaces[iface_num];
+
+	/* first: shut down all endpoints except EP0 */
+	cease_operation(this);
+
 	/* now enable all configured endpoints */
 	int epcount = iface->descriptor.bNumEndpoints;
+	int i;
 	for (i = 0; i < epcount; i++) {
 		int ep = iface->eps[i].bEndpointAddress;
 		int mps = iface->eps[i].wMaxPacketSize;
@@ -139,8 +146,7 @@ static int setup_ep0(struct usbdev_ctrl *this, dev_req_t *dr)
 		this->current_config_id = dr->wValue;
 
 		if (dr->wValue == 0)
-			// TODO: reset device
-			return 1;
+			cease_operation(this);
 
 		if (config == NULL) {
 			this->stall(this, 0, 0, 1);