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@xxxxxxxxxxx>
---
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
--
To unsubscribe from this list: send the line "unsubscribe linux-usb" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
