@@ -3018,6 +3018,17 @@ int usbssp_queue_ctrl_tx(struct usbssp_udc *usbssp_data,
USB_STATE_CONFIGURED);
}
+ if (usbssp_data->bos_event_detected) {
+ usbssp_data->bos_event_detected = 0;
+ usb_gadget_unmap_request_by_dev(usbssp_data->dev,
+ &req_priv->request,
+ dep->direction);
+ usbssp_set_usb2_hardware_lpm(usbssp_data,
+ &req_priv->request, 1);
+ ret = usb_gadget_map_request_by_dev(usbssp_data->dev,
+ &req_priv->request, dep->direction);
+ }
+
/* 1 TRB for data, 1 for status */
if (usbssp_data->three_stage_setup)
num_trbs = 2;
@@ -1701,6 +1701,54 @@ int usbssp_enable_device(struct usbssp_udc *usbssp_data)
return usbssp_setup_device(usbssp_data, SETUP_CONTEXT_ONLY);
}
+int usbssp_set_usb2_hardware_lpm(struct usbssp_udc *usbssp_data,
+ struct usb_request *req, int enable)
+{
+ __le32 __iomem *pm_addr;
+ u32 pm_val, field;
+ int besl;
+
+ struct usb_ext_cap_descriptor *usb_ext = req->buf + USB_DT_BOS_SIZE;
+
+ if (usbssp_data->port_major_revision >= 3 ||
+ !usbssp_data->hw_lpm_support
+ /*|| !usbssp_data->gadget->lpm_capable*/)
+ return -EPERM;
+
+ if (usb_ext->bDescriptorType != USB_DT_DEVICE_CAPABILITY ||
+ usb_ext->bDevCapabilityType != USB_CAP_TYPE_EXT) {
+ return -EPERM;
+ }
+ pm_addr = usbssp_data->usb2_ports + PORTPMSC;
+ pm_val = readl(pm_addr);
+ field = le32_to_cpu(usb_ext->bmAttributes);
+
+ usbssp_dbg(usbssp_data, "%s port %d USB2 hardware LPM\n",
+ enable ? "enable" : "disable", usbssp_data->devs.port_num);
+
+ if (enable) {
+ /* if device doesn't have a preferred BESL value use a
+ * default one . See USBSSP_DEFAULT_BESL definition in gadget.h
+ */
+ if ((field & USB_BESL_SUPPORT) &&
+ (field & USB_BESL_BASELINE_VALID))
+ besl = USB_GET_BESL_BASELINE(field);
+ else
+ besl = USBSSP_DEFAULT_BESL;
+
+ pm_val &= ~(PORT_BESL_MASK | PORT_HLE_MASK);
+ pm_val |= PORT_RBESL(besl) | PORT_HLE | 3 /*L1S set to 3*/;
+ pr_err("usbssp_set_usb2_hardware_lpm7 %08x\n", pm_val);
+ writel(pm_val, pm_addr);
+ /* flush write */
+ readl(pm_addr);
+ } else {
+ pm_val &= ~(PORT_HLE | PORT_BESL_MASK | PORT_L1S_MASK);
+ pm_val |= PORT_L1S_HLE0_STALL;
+ writel(pm_val, pm_addr);
+ }
+ return 0;
+}
int usbssp_get_frame(struct usbssp_udc *usbssp_data)
{
@@ -1740,6 +1740,10 @@ int usbssp_alloc_dev(struct usbssp_udc *usbssp_data);
void usbssp_free_dev(struct usbssp_udc *usbssp_data);
int usbssp_address_device(struct usbssp_udc *usbssp_data);
int usbssp_enable_device(struct usbssp_udc *usbssp_data);
+
+int usbssp_set_usb2_hardware_lpm(struct usbssp_udc *usbsssp_data,
+ struct usb_request *req, int enable);
+
/* USBSSP ring, segment, TRB, and TD functions */
dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
union usbssp_trb *trb);
Patch implements LPM functionality for port working in HS mode. Signed-off-by: Pawel Laszczak <pawell@cadence.com> --- drivers/usb/usbssp/gadget-ring.c | 11 ++++++++ drivers/usb/usbssp/gadget.c | 48 ++++++++++++++++++++++++++++++++ drivers/usb/usbssp/gadget.h | 4 +++ 3 files changed, 63 insertions(+)