@@ -385,6 +385,8 @@
#define DWC3_GUCTL2_RST_ACTBITLATER BIT(14)
/* Device Configuration Register */
+#define DWC3_DCFG_NUMLANES(n) (((n) & 0x3) << 30) /* DWC_usb32 only */
+
#define DWC3_DCFG_DEVADDR(addr) ((addr) << 3)
#define DWC3_DCFG_DEVADDR_MASK DWC3_DCFG_DEVADDR(0x7f)
@@ -2384,6 +2384,62 @@ static void dwc3_gadget_set_speed(struct usb_gadget *g,
spin_unlock_irqrestore(&dwc->lock, flags);
}
+static void dwc3_gadget_set_num_lanes_and_speed(struct usb_gadget *g,
+ unsigned int num_lanes,
+ unsigned int ssid)
+{
+ struct dwc3 *dwc = gadget_to_dwc(g);
+ struct usb_sublink_speed *ssa = NULL;
+ unsigned int lanes;
+ unsigned long flags;
+ u32 reg;
+ int i;
+
+ spin_lock_irqsave(&dwc->lock, flags);
+ if (dwc->maximum_speed < USB_SPEED_SUPER_PLUS)
+ goto done;
+
+ for (i = 0; i < g->ssac + 1; i++) {
+ if (g->sublink_speed[i].id == ssid) {
+ ssa = &g->sublink_speed[i];
+ break;
+ }
+ }
+
+ if (!ssa) {
+ dev_err(dwc->dev, "SSID not found (%d)\n", ssid);
+ goto done;
+ }
+
+ reg = dwc3_readl(dwc->regs, DWC3_DCFG);
+ reg &= ~DWC3_DCFG_SPEED_MASK;
+
+ switch (ssa->mantissa) {
+ case 5:
+ reg |= DWC3_DCFG_SUPERSPEED;
+ break;
+ case 10:
+ reg |= DWC3_DCFG_SUPERSPEED_PLUS;
+ break;
+ default:
+ dev_err(dwc->dev, "invalid lane speed mantissa (%d)\n",
+ ssa->mantissa);
+ goto done;
+ }
+
+ /* Lane configuration is only available to DWC_usb32 */
+ if (DWC3_IP_IS(DWC32)) {
+ lanes = clamp_t(unsigned int, num_lanes, 1,
+ dwc->maximum_num_lanes);
+ reg &= ~DWC3_DCFG_NUMLANES(~0);
+ reg |= DWC3_DCFG_NUMLANES(lanes - 1);
+ }
+
+ dwc3_writel(dwc->regs, DWC3_DCFG, reg);
+done:
+ spin_unlock_irqrestore(&dwc->lock, flags);
+}
+
static const struct usb_gadget_ops dwc3_gadget_ops = {
.get_frame = dwc3_gadget_get_frame,
.wakeup = dwc3_gadget_wakeup,
@@ -2392,6 +2448,7 @@ static const struct usb_gadget_ops dwc3_gadget_ops = {
.udc_start = dwc3_gadget_start,
.udc_stop = dwc3_gadget_stop,
.udc_set_speed = dwc3_gadget_set_speed,
+ .udc_set_num_lanes_and_speed = dwc3_gadget_set_num_lanes_and_speed,
.get_config_params = dwc3_gadget_config_params,
};
@@ -3844,7 +3901,12 @@ int dwc3_gadget_init(struct dwc3 *dwc)
goto err5;
}
- dwc3_gadget_set_speed(dwc->gadget, dwc->maximum_speed);
+ if (DWC3_IP_IS(DWC32) && dwc->maximum_speed == USB_SPEED_SUPER_PLUS)
+ dwc3_gadget_set_num_lanes_and_speed(dwc->gadget,
+ dwc->maximum_num_lanes,
+ dwc->gadget->max_speed_ssid);
+ else
+ dwc3_gadget_set_speed(dwc->gadget, dwc->maximum_speed);
return 0;
Implement gadget ops udc_set_num_lanes_and_speed. This allows the gadget/core driver to select number of lanes to use and the sublink speed the controller supports. Signed-off-by: Thinh Nguyen <Thinh.Nguyen@synopsys.com> --- Changes in v5: - Rebase on Felipe's testing/next branch - Changed Signed-off-by email to match From: email header Changes in v4: - None Changes in v3: - None Changes in v2: - None drivers/usb/dwc3/core.h | 2 ++ drivers/usb/dwc3/gadget.c | 64 ++++++++++++++++++++++++++++++++++++++- 2 files changed, 65 insertions(+), 1 deletion(-)