MLK-20599-2 usb: cdns3: gadget: fix link test TD 7.37

USB3 device should accept LGO_U1 request after receiving SET_CONFIGURATION.

Signed-off-by: Peter Chen <peter.chen@nxp.com>
Signed-off-by: Arulpandiyan Vadivel <arulpandiyan_vadivel@mentor.com>
Signed-off-by: Shrikant Bobade <Shrikant_Bobade@mentor.com>
(cherry picked from commit 4e1fc76ac36ebde235abe7dc0dc78a73ff9544b4)

diff --git a/drivers/usb/cdns3/gadget.c b/drivers/usb/cdns3/gadget.c
index 0bcfbd4..5884533 100644 (file)
--- a/drivers/usb/cdns3/gadget.c
+++ b/drivers/usb/cdns3/gadget.c
@@ -102,6 +102,8 @@ static void cdns_prepare_setup_packet(struct usb_ss_dev *usb_ss);
 static void cdns_ep_config(struct usb_ss_endpoint *usb_ss_ep);
 static void cdns_enable_l1(struct usb_ss_dev *usb_ss, int enable);
 static void __pending_setup_status_handler(struct usb_ss_dev *usb_ss);
+static void cdns_enable_u1(struct usb_ss_dev *usb_ss, int enable);
+static void cdns_enable_u2(struct usb_ss_dev *usb_ss, int enable);
 
 static struct usb_endpoint_descriptor cdns3_gadget_ep0_desc = {
        .bLength        = USB_DT_ENDPOINT_SIZE,
@@ -616,14 +618,12 @@ static int cdns_req_ep0_handle_feature(struct usb_ss_dev *usb_ss,
                        if (usb_ss->gadget.speed != USB_SPEED_SUPER)
                                return -EINVAL;
 
-                       reg = gadget_readl(usb_ss, &usb_ss->regs->usb_conf);
                        if (set)
                                /* set U1EN */
-                               reg |= USB_CONF__U1EN__MASK;
+                               cdns_enable_u1(usb_ss, 1);
                        else
                                /* set U1 disable */
-                               reg |= USB_CONF__U1DS__MASK;
-                       gadget_writel(usb_ss, &usb_ss->regs->usb_conf, reg);
+                               cdns_enable_u1(usb_ss, 0);
                        break;
                case USB_DEVICE_U2_ENABLE:
                        if (usb_ss->gadget.state != USB_STATE_CONFIGURED)
@@ -631,14 +631,12 @@ static int cdns_req_ep0_handle_feature(struct usb_ss_dev *usb_ss,
                        if (usb_ss->gadget.speed != USB_SPEED_SUPER)
                                return -EINVAL;
 
-                       reg = gadget_readl(usb_ss, &usb_ss->regs->usb_conf);
                        if (set)
                                /* set U2EN */
-                               reg |= USB_CONF__U2EN__MASK;
+                               cdns_enable_u2(usb_ss, 1);
                        else
                                /* set U2 disable */
-                               reg |= USB_CONF__U2DS__MASK;
-                       gadget_writel(usb_ss, &usb_ss->regs->usb_conf, reg);
+                               cdns_enable_u2(usb_ss, 0);
                        break;
                case USB_DEVICE_A_ALT_HNP_SUPPORT:
                        break;
@@ -799,6 +797,26 @@ static void cdns_enable_l1(struct usb_ss_dev *usb_ss, int enable)
                                USB_CONF__L1DS__MASK);
 }
 
+static void cdns_enable_u1(struct usb_ss_dev *usb_ss, int enable)
+{
+       if (enable)
+               gadget_writel(usb_ss, &usb_ss->regs->usb_conf,
+                               USB_CONF__U1EN__MASK);
+       else
+               gadget_writel(usb_ss, &usb_ss->regs->usb_conf,
+                               USB_CONF__U1DS__MASK);
+}
+
+static void cdns_enable_u2(struct usb_ss_dev *usb_ss, int enable)
+{
+       if (enable)
+               gadget_writel(usb_ss, &usb_ss->regs->usb_conf,
+                               USB_CONF__U2EN__MASK);
+       else
+               gadget_writel(usb_ss, &usb_ss->regs->usb_conf,
+                               USB_CONF__U2DS__MASK);
+}
+
 /**
  * cdns_req_ep0_set_configuration - Handling of SET_CONFIG standard USB request
  * @usb_ss: extended gadget object
@@ -841,6 +859,10 @@ static int cdns_req_ep0_set_configuration(struct usb_ss_dev *usb_ss,
                                        USB_STS__CFGSTS__MASK, 100);
                                usb_ss->hw_configured_flag = 1;
                                cdns_enable_l1(usb_ss, 1);
+                               if (usb_ss->gadget.speed == USB_SPEED_SUPER) {
+                                       cdns_enable_u1(usb_ss, 1);
+                                       cdns_enable_u2(usb_ss, 1);
+                               }
 
                                list_for_each_entry(ep,
                                        &usb_ss->gadget.ep_list,
@@ -1400,6 +1422,11 @@ static int usb_ss_gadget_ep0_queue(struct usb_ep *ep,
                        erdy_sent = 1;
                        usb_ss->hw_configured_flag = 1;
                        cdns_enable_l1(usb_ss, 1);
+                       /* Enable U1/U2 at Configuration state */
+                       if (usb_ss->gadget.speed == USB_SPEED_SUPER) {
+                               cdns_enable_u1(usb_ss, 1);
+                               cdns_enable_u2(usb_ss, 1);
+                       }
 
                        list_for_each_entry(ep,
                                &usb_ss->gadget.ep_list,