usb: ums: add ums exit feature by ctrl+c or by detach usb cable

This patch allows exiting from UMS mode to u-boot prompt
by detaching usb cable or by pressing ctrl+c.

Add new config: CONFIG_USB_CABLE_CHECK. If defined then board
file should provide function: usb_cable_connected() (include/usb.h)
that return 1 if cable is connected and 0 otherwise.

Changes v2:
- add a note to the README

Signed-off-by: Przemyslaw Marczak <p.marczak@samsung.com>
Cc: Marek Vasut <marex@denx.de>
diff --git a/README b/README
index 91c3ac0..b0c2fdc 100644
--- a/README
+++ b/README
@@ -1367,6 +1367,13 @@
 			for your device
 			- CONFIG_USBD_PRODUCTID 0xFFFF
 
+		Some USB device drivers may need to check USB cable attachment.
+		In this case you can enable following config in BoardName.h:
+			CONFIG_USB_CABLE_CHECK
+			This enables function definition:
+			- usb_cable_connected() in include/usb.h
+			Implementation of this function is board-specific.
+
 - ULPI Layer Support:
 		The ULPI (UTMI Low Pin (count) Interface) PHYs are supported via
 		the generic ULPI layer. The generic layer accesses the ULPI PHY
diff --git a/common/cmd_usb_mass_storage.c b/common/cmd_usb_mass_storage.c
index 4d3bbd8..99487f4 100644
--- a/common/cmd_usb_mass_storage.c
+++ b/common/cmd_usb_mass_storage.c
@@ -5,6 +5,7 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+#include <errno.h>
 #include <common.h>
 #include <command.h>
 #include <g_dnl.h>
@@ -42,16 +43,20 @@
 	g_dnl_register("ums");
 
 	while (1) {
-		/* Handle control-c and timeouts */
-		if (ctrlc()) {
-			error("The remote end did not respond in time.");
+		usb_gadget_handle_interrupts();
+
+		rc = fsg_main_thread(NULL);
+		if (rc) {
+			/* Check I/O error */
+			if (rc == -EIO)
+				printf("\rCheck USB cable connection\n");
+
+			/* Check CTRL+C */
+			if (rc == -EPIPE)
+				printf("\rCTRL+C - Operation aborted\n");
+
 			goto exit;
 		}
-
-		usb_gadget_handle_interrupts();
-		/* Check if USB cable has been detached */
-		if (fsg_main_thread(NULL) == EIO)
-			goto exit;
 	}
 exit:
 	g_dnl_unregister();
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index be6b418..b1fe8bd 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -243,6 +243,7 @@
 #include <config.h>
 #include <malloc.h>
 #include <common.h>
+#include <usb.h>
 
 #include <linux/err.h>
 #include <linux/usb/ch9.h>
@@ -675,6 +676,18 @@
 			k++;
 		}
 
+		if (k == 10) {
+			/* Handle CTRL+C */
+			if (ctrlc())
+				return -EPIPE;
+#ifdef CONFIG_USB_CABLE_CHECK
+			/* Check cable connection */
+			if (!usb_cable_connected())
+				return -EIO;
+#endif
+			k = 0;
+		}
+
 		usb_gadget_handle_interrupts();
 	}
 	common->thread_wakeup_needed = 0;
@@ -2387,6 +2400,7 @@
 
 int fsg_main_thread(void *common_)
 {
+	int ret;
 	struct fsg_common	*common = the_fsg_common;
 	/* The main loop */
 	do {
@@ -2396,12 +2410,16 @@
 		}
 
 		if (!common->running) {
-			sleep_thread(common);
+			ret = sleep_thread(common);
+			if (ret)
+				return ret;
+
 			continue;
 		}
 
-		if (get_next_command(common))
-			continue;
+		ret = get_next_command(common);
+		if (ret)
+			return ret;
 
 		if (!exception_in_progress(common))
 			common->state = FSG_STATE_DATA_PHASE;
diff --git a/include/usb.h b/include/usb.h
index d9fedee..736730e 100644
--- a/include/usb.h
+++ b/include/usb.h
@@ -197,6 +197,16 @@
  */
 int board_usb_cleanup(int index, enum usb_init_type init);
 
+/*
+ * If CONFIG_USB_CABLE_CHECK is set then this function
+ * should be defined in board file.
+ *
+ * @return 1 if cable is connected and 0 otherwise.
+ */
+#ifdef CONFIG_USB_CABLE_CHECK
+int usb_cable_connected(void);
+#endif
+
 #ifdef CONFIG_USB_STORAGE
 
 #define USB_MAX_STOR_DEV 5