Track the number of lanes connected in gadget->num_lanes and track the
current sublink speed attribute ID for super-speed-plus operations.
Note: if the device is running in gen1x2, set the gadget->speed to
USB_SPEED_SUPER_PLUS.
Signed-off-by: Thinh Nguyen <thinhn@xxxxxxxxxxxx>
---
drivers/usb/dwc3/core.h | 2 ++
drivers/usb/dwc3/gadget.c | 21 ++++++++++++++++++++-
2 files changed, 22 insertions(+), 1 deletion(-)
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 92ab65a46771..63178549428d 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -464,6 +464,8 @@
#define DWC3_DEVTEN_USBRSTEN BIT(1)
#define DWC3_DEVTEN_DISCONNEVTEN BIT(0)
+#define DWC3_DSTS_CONNLANES(n) (((n) >> 30) & 0x3) /* DWC_usb32 only */
+
/* Device Status Register */
#define DWC3_DSTS_DCNRD BIT(29)
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 01bdd51f6b47..87a69a096b57 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -2306,6 +2306,10 @@ static void dwc3_gadget_set_speed(struct usb_gadget *g,
reg |= DWC3_DCFG_SUPERSPEED_PLUS;
}
}
+
+ if (DWC3_IP_IS(DWC32) && speed < USB_SPEED_SUPER_PLUS)
+ reg &= ~DWC3_DCFG_NUMLANES(~0);
+
dwc3_writel(dwc->regs, DWC3_DCFG, reg);
spin_unlock_irqrestore(&dwc->lock, flags);
@@ -3175,12 +3179,19 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
struct dwc3_ep *dep;
int ret;
u32 reg;
+ u8 lanes = 1;
u8 speed;
reg = dwc3_readl(dwc->regs, DWC3_DSTS);
speed = reg & DWC3_DSTS_CONNECTSPD;
dwc->speed = speed;
+ if (DWC3_IP_IS(DWC32))
+ lanes = DWC3_DSTS_CONNLANES(reg) + 1;
+
+ dwc->gadget.num_lanes = lanes;
+ dwc->gadget.speed_ssid = 0;
+
/*
* RAMClkSel is reset to 0 after USB reset, so it must be reprogrammed
* each time on Connect Done.
@@ -3195,6 +3206,7 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
dwc3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
dwc->gadget.ep0->maxpacket = 512;
dwc->gadget.speed = USB_SPEED_SUPER_PLUS;
+ dwc->gadget.speed_ssid = DWC3_SSP_SSID_GEN2;
break;
case DWC3_DSTS_SUPERSPEED:
/*
@@ -3215,7 +3227,13 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
dwc3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
dwc->gadget.ep0->maxpacket = 512;
- dwc->gadget.speed = USB_SPEED_SUPER;
+
+ if (lanes > 1) {
+ dwc->gadget.speed = USB_SPEED_SUPER_PLUS;
+ dwc->gadget.speed_ssid = DWC3_SSP_SSID_GEN1;
+ } else {
+ dwc->gadget.speed = USB_SPEED_SUPER;
+ }
break;
case DWC3_DSTS_HIGHSPEED:
dwc3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(64);
@@ -3696,6 +3714,7 @@ int dwc3_gadget_init(struct dwc3 *dwc)
dwc->gadget.sg_supported = true;
dwc->gadget.name = "dwc3-gadget";
dwc->gadget.lpm_capable = true;
+ dwc->gadget.num_lanes = 1;
/*
* FIXME We might be setting max_speed to <SUPER, however versions
--
2.11.0
