@@ -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");
@@ -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);
+}
@@ -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);
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(+)