libpayload: Detach unresponsive usb mass storage devices

This enables logical detachment of unresponsive usb devices (i.e.
devices not responding to control transfers) in the usb mass storage
driver. Without the detection of unresponsive devices we wait way too
long for the device to become ready.

Change-Id: I8b8cf327f49dde25afaca4d3066f16ea86b99d3d
Signed-off-by: Nico Huber <nico.huber@secunet.com>
Reviewed-on: http://review.coreboot.org/1121
Tested-by: build bot (Jenkins)
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>

3 files changed, 116 insertions, 60 deletions

diff --git a/payloads/libpayload/drivers/usb/usb.c b/payloads/libpayload/drivers/usb/usb.c
index 42c73f345e..99b65e03f2 100644
--- a/payloads/libpayload/drivers/usb/usb.c
+++ b/payloads/libpayload/drivers/usb/usb.c
@@ -206,7 +206,7 @@ set_configuration (usbdev_t *dev)
 	dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0);
 }
 
-void
+int
 clear_feature (usbdev_t *dev, int endp, int feature, int rtype)
 {
 	dev_req_t dr;
@@ -217,7 +217,7 @@ clear_feature (usbdev_t *dev, int endp, int feature, int rtype)
 	dr.wValue = feature;
 	dr.wIndex = endp;
 	dr.wLength = 0;
-	dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0);
+	return dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0);
 }
 
 int
@@ -228,9 +228,9 @@ clear_stall (endpoint_t *ep)
 	int rtype = gen_bmRequestType (host_to_device, standard_type,
 					endp ? endp_recp : dev_recp);
 
-	clear_feature (dev, endp, ENDPOINT_HALT, rtype);
+	int ret = clear_feature (dev, endp, ENDPOINT_HALT, rtype);
 	ep->toggle = 0;
-	return 0;
+	return ret;
 }
 
 /* returns free address or -1 */
@@ -459,12 +459,21 @@ set_address (hci_t *controller, int speed, int hubport, int hubaddr)
 	return adr;
 }
 
+/*
+ * Should be called by the hub drivers whenever a physical detach occurs
+ * and can be called by usb class drivers if they are unsatisfied with a
+ * malfunctioning device.
+ */
 void
 usb_detach_device(hci_t *controller, int devno)
 {
-	controller->devices[devno]->destroy (controller->devices[devno]);
-	free(controller->devices[devno]);
-	controller->devices[devno] = 0;
+	/* check if device exists, as we may have
+	   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;
+	}
 }
 
 int
diff --git a/payloads/libpayload/drivers/usb/usbmsc.c b/payloads/libpayload/drivers/usb/usbmsc.c
index fd6517a5e3..d1e33022c3 100644
--- a/payloads/libpayload/drivers/usb/usbmsc.c
+++ b/payloads/libpayload/drivers/usb/usbmsc.c
@@ -106,10 +106,22 @@ typedef struct {
 	unsigned char bCSWStatus;
 } __attribute__ ((packed)) csw_t;
 
+enum {
+	/*
+	 * MSC commands can be
+	 *   successful,
+	 *   fail with proper response or
+	 *   fail totally, which results in detaching of the usb device.
+	 * In the latter case the caller has to make sure, that he won't
+	 * use the device any more.
+	 */
+	MSC_COMMAND_OK = 0, MSC_COMMAND_FAIL, MSC_COMMAND_DETACHED
+};
+
 static int
 request_sense (usbdev_t *dev);
 
-static void
+static int
 reset_transport (usbdev_t *dev)
 {
 	dev_req_t dr;
@@ -124,9 +136,17 @@ reset_transport (usbdev_t *dev)
 	dr.wValue = 0;
 	dr.wIndex = 0;
 	dr.wLength = 0;
-	dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0);
-	clear_stall (MSC_INST (dev)->bulk_in);
-	clear_stall (MSC_INST (dev)->bulk_out);
+
+	/* if any of these fails, detach device, as we are lost */
+	if (dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0) ||
+			clear_stall (MSC_INST (dev)->bulk_in) ||
+			clear_stall (MSC_INST (dev)->bulk_out)) {
+		printf ("Detaching unresponsive device.\n");
+		usb_detach_device (dev->controller, dev->address);
+		return MSC_COMMAND_DETACHED;
+	}
+	/* return fail as we are only called in case of failure */
+	return MSC_COMMAND_FAIL;
 }
 
 /* device may stall this command, so beware! */
@@ -177,20 +197,18 @@ get_csw (endpoint_t *ep, csw_t *csw)
 		clear_stall (ep);
 		if (ep->dev->controller->bulk
 				(ep, sizeof (csw_t), (u8 *) csw, 1)) {
-			reset_transport (ep->dev);
-			return 1;
+			return reset_transport (ep->dev);
 		}
 	}
 	if (csw->dCSWTag != tag) {
-		reset_transport (ep->dev);
-		return 1;
+		return reset_transport (ep->dev);
 	}
-	return 0;
+	return MSC_COMMAND_OK;
 }
 
 static int
 execute_command (usbdev_t *dev, cbw_direction dir, const u8 *cb, int cblen,
-		 u8 *buf, int buflen)
+		 u8 *buf, int buflen, int residue_ok)
 {
 	cbw_t cbw;
 	csw_t csw;
@@ -202,43 +220,48 @@ execute_command (usbdev_t *dev, cbw_direction dir, const u8 *cb, int cblen,
 	wrap_cbw (&cbw, buflen, dir, cb, cblen);
 	if (dev->controller->
 	    bulk (MSC_INST (dev)->bulk_out, sizeof (cbw), (u8 *) &cbw, 0)) {
-		reset_transport (dev);
-		return 1;
+		return reset_transport (dev);
 	}
 	if (buflen > 0) {
 		if (dir == cbw_direction_data_in) {
 			if (dev->controller->
 			    bulk (MSC_INST (dev)->bulk_in, buflen, buf, 0)) {
 				clear_stall (MSC_INST (dev)->bulk_in);
-				return 1;
+				return MSC_COMMAND_FAIL;
 			}
 		} else {
 			if (dev->controller->
 			    bulk (MSC_INST (dev)->bulk_out, buflen, buf, 0)) {
 				clear_stall (MSC_INST (dev)->bulk_out);
-				return 1;
+				return MSC_COMMAND_FAIL;
 			}
 		}
 	}
-	if (get_csw (MSC_INST (dev)->bulk_in, &csw))
-		return 1;
-	if (always_succeed == 1) {
-		// return success, regardless of message
-		return 0;
-	}
-	if (csw.bCSWStatus == 2) {
-		// phase error, reset transport
-		reset_transport (dev);
-		return 1;
-	}
-	if (csw.bCSWStatus == 0) {
-		// no error, exit
-		return 0;
+	int ret = get_csw (MSC_INST (dev)->bulk_in, &csw);
+	if (ret) {
+		return ret;
+	} else if (always_succeed == 1) {
+		/* return success, regardless of message */
+		return MSC_COMMAND_OK;
+	} else if (csw.bCSWStatus == 2) {
+		/* phase error, reset transport */
+		return reset_transport (dev);
+	} else if (csw.bCSWStatus == 0) {
+		if ((csw.dCSWDataResidue == 0) || residue_ok)
+			/* no error, exit */
+			return MSC_COMMAND_OK;
+		else
+			/* missed some bytes */
+			return MSC_COMMAND_FAIL;
+	} else {
+		if (cb[0] == 0x03)
+			/* requesting sense failed, that's bad */
+			return MSC_COMMAND_FAIL;
+		/* error "check condition" or reserved error */
+		ret = request_sense (dev);
+		/* return fail or the status of request_sense if it's worse */
+		return ret ? ret : MSC_COMMAND_FAIL;
 	}
-	if (cb[0] != 0x03) /* 0x03 == request sense */
-		// error "check condition" or reserved error
-		request_sense (dev);
-	return 1;
 }
 
 typedef struct {
@@ -309,7 +332,8 @@ readwrite_blocks (usbdev_t *dev, int start, int n, cbw_direction dir, u8 *buf)
 	cb.numblocks = htonw (n);
 
 	return execute_command (dev, dir, (u8 *) &cb, sizeof (cb), buf,
-				n * MSC_INST(dev)->blocksize);
+				n * MSC_INST(dev)->blocksize, 0)
+		!= MSC_COMMAND_OK ? 1 : 0;
 }
 
 /* Only request it, we don't interpret it.
@@ -324,7 +348,7 @@ request_sense (usbdev_t *dev)
 	cb.command = 0x3;
 
 	return execute_command (dev, cbw_direction_data_in, (u8 *) &cb,
-				sizeof (cb), buf, 19);
+				sizeof (cb), buf, 19, 1);
 }
 
 static int
@@ -333,7 +357,7 @@ test_unit_ready (usbdev_t *dev)
 	cmdblock6_t cb;
 	memset (&cb, 0, sizeof (cb));	// full initialization for T-U-R
 	return execute_command (dev, cbw_direction_data_out, (u8 *) &cb,
-				sizeof (cb), 0, 0);
+				sizeof (cb), 0, 0, 0);
 }
 
 static int
@@ -344,10 +368,10 @@ spin_up (usbdev_t *dev)
 	cb.command = 0x1b;
 	cb.lun = 1;
 	return execute_command (dev, cbw_direction_data_out, (u8 *) &cb,
-				sizeof (cb), 0, 0);
+				sizeof (cb), 0, 0, 0);
 }
 
-static void
+static int
 read_capacity (usbdev_t *dev)
 {
 	cmdblock_t cb;
@@ -356,12 +380,20 @@ read_capacity (usbdev_t *dev)
 	u8 buf[8];
 
 	debug ("Reading capacity of mass storage device.\n");
-	int count = 0;
-	while ((count++ < 20)
-	       &&
-	       (execute_command
-		(dev, cbw_direction_data_in, (u8 *) &cb, sizeof (cb), buf,
-		 8) == 1));
+	int count = 0, ret;
+	while (count++ < 20) {
+		switch (ret = execute_command
+				(dev, cbw_direction_data_in, (u8 *) &cb,
+				 sizeof (cb), buf, 8, 0)) {
+		case MSC_COMMAND_OK:
+			break;
+		case MSC_COMMAND_FAIL:
+			continue;
+		default: /* if it's worse return */
+			return ret;
+		}
+		break;
+	}
 	if (count >= 20) {
 		// still not successful, assume 2tb in 512byte sectors, which is just the same garbage as any other number, but probably more usable.
 		printf ("  assuming 2 TB with 512-byte sectors as READ CAPACITY didn't answer.\n");
@@ -377,6 +409,7 @@ read_capacity (usbdev_t *dev)
 		MSC_INST (dev)->numblocks > 1000000
 			? (MSC_INST (dev)->numblocks / 1000) * MSC_INST (dev)->blocksize / 1000 :
 		MSC_INST (dev)->numblocks * MSC_INST (dev)->blocksize / 1000 / 1000);
+	return MSC_COMMAND_OK;
 }
 
 void
@@ -444,12 +477,21 @@ usb_msc_init (usbdev_t *dev)
 
 	printf ("  Waiting for device to become ready...");
 	timeout = 30 * 10; /* SCSI/ATA specs say we have to wait up to 30s. Ugh */
-	while (test_unit_ready (dev) && --timeout) {
-		mdelay (100);
-		if (!(timeout % 10))
-			printf (".");
+	while (timeout--) {
+		switch (test_unit_ready (dev)) {
+		case MSC_COMMAND_OK:
+			break;
+		case MSC_COMMAND_FAIL:
+			mdelay (100);
+			if (!(timeout % 10))
+				printf (".");
+			continue;
+		default: /* if it's worse return */
+			return;
+		}
+		break;
 	}
-	if (test_unit_ready (dev)) {
+	if (timeout < 0) {
 		printf ("timeout. Device not ready. Still trying...\n");
 	} else {
 		printf ("ok.\n");
@@ -458,15 +500,20 @@ usb_msc_init (usbdev_t *dev)
 	debug ("  spin up");
 	for (i = 0; i < 30; i++) {
 		debug (".");
-		if (!spin_up (dev)) {
+		switch (spin_up (dev)) {
+		case MSC_COMMAND_OK:
 			debug (" OK.");
 			break;
+		case MSC_COMMAND_FAIL:
+			mdelay (100);
+			continue;
+		default: /* if it's worse return */
+			return;
 		}
-		mdelay (100);
+		break;
 	}
 	debug ("\n");
 
-	read_capacity (dev);
-	if (usbdisk_create)
+	if ((read_capacity (dev) == MSC_COMMAND_OK) && usbdisk_create)
 		usbdisk_create (dev);
 }
diff --git a/payloads/libpayload/include/usb/usb.h b/payloads/libpayload/include/usb/usb.h
index ee9c50bb85..ecfee54551 100644
--- a/payloads/libpayload/include/usb/usb.h
+++ b/payloads/libpayload/include/usb/usb.h
@@ -215,7 +215,7 @@ void init_device_entry (hci_t *controller, int num);
 
 void set_feature (usbdev_t *dev, int endp, int feature, int rtype);
 void get_status (usbdev_t *dev, int endp, int rtype, int len, void *data);
-void clear_feature (usbdev_t *dev, int endp, int feature, int rtype);
+int clear_feature (usbdev_t *dev, int endp, int feature, int rtype);
 int clear_stall (endpoint_t *ep);
 
 void usb_nop_init (usbdev_t *dev);