[PATCH 30/31] usb: usbssp: added support for TEST_MODE.

[Pawel Laszczak <pawell@cadence.com>]

Patch implements TEST_MODE feature that puts the device port
into test mode. This feature is used only when device works
in HS mode

Signed-off-by: Pawel Laszczak <pawell@cadence.com>
---
 drivers/usb/usbssp/gadget-ep0.c  | 39 ++++++++++++++
 drivers/usb/usbssp/gadget-port.c | 93 ++++++++++++++++++++++++++++++++
 drivers/usb/usbssp/gadget.h      |  6 +++
 3 files changed, 138 insertions(+)

diff --git a/drivers/usb/usbssp/gadget-ep0.c b/drivers/usb/usbssp/gadget-ep0.c
index 3b7066875d46..e03a34d11dab 100644
--- a/drivers/usb/usbssp/gadget-ep0.c
+++ b/drivers/usb/usbssp/gadget-ep0.c
@@ -245,6 +245,41 @@ static int usbssp_ep0_handle_feature_u2(struct usbssp_udc *usbssp_data,
 	return 0;
 }
 
+static int usbssp_ep0_handle_feature_test(struct usbssp_udc *usbssp_data,
+					  enum usb_device_state state,
+					  u32 wIndex, int set)
+{
+	int test_mode;
+	__le32 __iomem *port_regs;
+	u32 temp;
+	unsigned long flags;
+	int retval;
+
+	if (usbssp_data->port_major_revision == 0x03)
+		return -EINVAL;
+
+	dev_info(usbssp_data->dev, "Test mode; %d\n", wIndex);
+
+	port_regs = usbssp_get_port_io_addr(usbssp_data);
+
+
+	test_mode = (wIndex & 0xff00) >> 8;
+
+	temp = readl(port_regs);
+	temp = usbssp_port_state_to_neutral(temp);
+
+	if (test_mode > TEST_FORCE_EN || test_mode < TEST_J) {
+		/* "stall" on error */
+		retval = -EPIPE;
+	}
+
+	usbssp_status_stage(usbssp_data);
+	retval = usbssp_enter_test_mode(usbssp_data, test_mode, &flags);
+	usbssp_exit_test_mode(usbssp_data);
+
+	return 0;
+}
+
 static int usbssp_ep0_handle_feature_device(struct usbssp_udc *usbssp_data,
 					    struct usb_ctrlrequest *ctrl,
 					    int set)
@@ -275,6 +310,10 @@ static int usbssp_ep0_handle_feature_device(struct usbssp_udc *usbssp_data,
 	case USB_DEVICE_LTM_ENABLE:
 		ret = -EINVAL;
 		break;
+	case USB_DEVICE_TEST_MODE:
+		ret = usbssp_ep0_handle_feature_test(usbssp_data, state,
+						wIndex, set);
+		break;
 	default:
 		dev_err(usbssp_data->dev, "%s Feature Request not supported\n",
 			(set) ? "Set" : "Clear");
diff --git a/drivers/usb/usbssp/gadget-port.c b/drivers/usb/usbssp/gadget-port.c
index 05a4eb2fd8bf..ebbff47e9455 100644
--- a/drivers/usb/usbssp/gadget-port.c
+++ b/drivers/usb/usbssp/gadget-port.c
@@ -192,3 +192,96 @@ void usbssp_test_and_clear_bit(struct usbssp_udc *usbssp_data,
 		writel(temp, port_regs);
 	}
 }
+
+static void usbssp_set_port_power(struct usbssp_udc *usbssp_data,
+				  bool on, unsigned long *flags)
+{
+	__le32 __iomem *addr;
+	u32 temp;
+
+	addr = usbssp_get_port_io_addr(usbssp_data);
+	temp = readl(addr);
+	temp = usbssp_port_state_to_neutral(temp);
+	if (on) {
+		/* Power on */
+		writel(temp | PORT_POWER, addr);
+		temp = readl(addr);
+		dev_dbg(usbssp_data->dev,
+			"set port power, actual port status = 0x%x\n",
+			temp);
+	} else {
+		/* Power off */
+		writel(temp & ~PORT_POWER, addr);
+		dev_dbg(usbssp_data->dev,
+			"clear port power, actual port status = 0x%x\n",
+			 temp);
+	}
+}
+
+static void usbssp_port_set_test_mode(struct usbssp_udc *usbssp_data,
+				      u16 test_mode)
+{
+	u32 temp;
+	__le32 __iomem *addr;
+
+	/* USBSSP only supports test mode for usb2 ports, */
+	addr = usbssp_get_port_io_addr(usbssp_data);
+	temp = readl(addr + PORTPMSC);
+	temp |= test_mode << PORT_TEST_MODE_SHIFT;
+	writel(temp, addr + PORTPMSC);
+	usbssp_data->test_mode = test_mode;
+	if (test_mode == TEST_FORCE_EN)
+		usbssp_start(usbssp_data);
+}
+
+int usbssp_enter_test_mode(struct usbssp_udc *usbssp_data,
+			   u16 test_mode, unsigned long *flags)
+{
+	int retval;
+
+	retval = usbssp_disable_slot(usbssp_data);
+	if (retval) {
+		dev_err(usbssp_data->dev,
+			"Failed to disable slot %d, %d. Enter test mode anyway\n",
+			usbssp_data->slot_id, retval);
+		return retval;
+	}
+
+	/* Put port to the Disable state by clear PP */
+	usbssp_set_port_power(usbssp_data, false, flags);
+
+	/* Stop the controller */
+	retval = usbssp_halt(usbssp_data);
+	if (retval)
+		return retval;
+
+	/* Disable runtime PM for test mode */
+	pm_runtime_forbid(usbssp_data->dev);
+
+	/* Set PORTPMSC.PTC field to enter selected test mode */
+	/* Port is selected by wIndex. port_id = wIndex + 1 */
+	dev_dbg(usbssp_data->dev, "Enter Test Mode: _id=%d\n",
+		test_mode);
+	usbssp_port_set_test_mode(usbssp_data, test_mode);
+
+	return retval;
+}
+
+int usbssp_exit_test_mode(struct usbssp_udc *usbssp_data)
+{
+	int retval;
+
+	if (!usbssp_data->test_mode) {
+		dev_err(usbssp_data->dev, "Not in test mode, do nothing.\n");
+		return 0;
+	}
+	if (usbssp_data->test_mode == TEST_FORCE_EN &&
+		!(usbssp_data->usbssp_state & USBSSP_STATE_HALTED)) {
+		retval = usbssp_halt(usbssp_data);
+		if (retval)
+			return retval;
+	}
+	pm_runtime_allow(usbssp_data->dev);
+	usbssp_data->test_mode = 0;
+	return usbssp_reset(usbssp_data);
+}
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 59d7ef573d96..418bc5ad2a22 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1744,6 +1744,7 @@ void usbssp_stop(struct usbssp_udc *usbssp_data);
 int usbssp_handshake(void __iomem *ptr, u32 mask, u32 done, int usec);
 void usbssp_quiesce(struct usbssp_udc *usbssp_data);
 int usbssp_halt(struct usbssp_udc *usbssp_data);
+int usbssp_start(struct usbssp_udc *usbssp_data);
 extern int usbssp_reset(struct usbssp_udc *usbssp_data);
 int usbssp_disable_slot(struct usbssp_udc *usbssp_data);
 
@@ -1836,6 +1837,8 @@ void usbssp_test_and_clear_bit(struct usbssp_udc *usbssp_data,
 			__le32 __iomem *port_regs, u32 port_bit);
 
 void usbssp_udc_died(struct usbssp_udc *usbssp_data);
+u32 usbssp_port_state_to_neutral(u32 state);
+
 /* USBSSP DC contexts */
 struct usbssp_input_control_ctx *usbssp_get_input_control_ctx(
 					struct usbssp_container_ctx *ctx);
@@ -1867,6 +1870,9 @@ int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data,
 			struct usbssp_ep *dep, int value);
 int usbssp_cmd_stop_ep(struct usbssp_udc *usbssp_data, struct usb_gadget *g,
 		struct usbssp_ep *ep_priv);
+int usbssp_enter_test_mode(struct usbssp_udc *usbssp_data,
+			u16 test_mode, unsigned long *flags);
+int usbssp_exit_test_mode(struct usbssp_udc *usbssp_data);
 int usbssp_setup_analyze(struct usbssp_udc *usbssp_data);
 int usbssp_status_stage(struct usbssp_udc *usbssp_data);
 
-- 
2.17.1