[PATCH 6/9] usb: dwc2: gadget: Configure the core to enable LPM.

[Sevak Arakelyan <SEVAK.ARAKELYAN@synopsys.com>]

Configure core in device mode to support LPM according to
programming guide.
Device will start giving valid responses for LPM tokens.
After this patch device side LPM will start working.

Signed-off-by: Sevak Arakelyan <sevaka@synopsys.com>
---
 drivers/usb/dwc2/core.h      |  2 ++
 drivers/usb/dwc2/core_intr.c |  1 +
 drivers/usb/dwc2/gadget.c    | 22 ++++++++++++++++++++++
 3 files changed, 25 insertions(+)

diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index 13e0cb643678..04cc77224354 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -1201,6 +1201,7 @@ int dwc2_restore_device_registers(struct dwc2_hsotg *hsotg);
 int dwc2_hsotg_tx_fifo_count(struct dwc2_hsotg *hsotg);
 int dwc2_hsotg_tx_fifo_total_depth(struct dwc2_hsotg *hsotg);
 int dwc2_hsotg_tx_fifo_average_depth(struct dwc2_hsotg *hsotg);
+void dwc2_gadget_init_lpm(struct dwc2_hsotg *hsotg);
 #else
 static inline int dwc2_hsotg_remove(struct dwc2_hsotg *dwc2)
 { return 0; }
@@ -1228,6 +1229,7 @@ static inline int dwc2_hsotg_tx_fifo_total_depth(struct dwc2_hsotg *hsotg)
 { return 0; }
 static inline int dwc2_hsotg_tx_fifo_average_depth(struct dwc2_hsotg *hsotg)
 { return 0; }
+static inline void dwc2_gadget_init_lpm(struct dwc2_hsotg *hsotg) {}
 #endif
 
 #if IS_ENABLED(CONFIG_USB_DWC2_HOST) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
diff --git a/drivers/usb/dwc2/core_intr.c b/drivers/usb/dwc2/core_intr.c
index ea4a6efc33b0..8c820dbbf082 100644
--- a/drivers/usb/dwc2/core_intr.c
+++ b/drivers/usb/dwc2/core_intr.c
@@ -368,6 +368,7 @@ static void dwc2_wakeup_from_lpm_l1(struct dwc2_hsotg *hsotg)
 			dev_err(hsotg->dev, "Failed to exit L1 sleep state in 200us.\n");
 			return;
 		}
+		dwc2_gadget_init_lpm(hsotg);
 	} else {
 		/* TODO */
 		dev_err(hsotg->dev, "Host side LPM is not supported.\n");
diff --git a/drivers/usb/dwc2/gadget.c b/drivers/usb/dwc2/gadget.c
index c763e0e7d64f..487397d571a8 100644
--- a/drivers/usb/dwc2/gadget.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -3427,6 +3427,9 @@ void dwc2_hsotg_core_init_disconnected(struct dwc2_hsotg *hsotg,
 		val |= DCTL_SFTDISCON;
 	__orr32(hsotg->regs + DCTL, val);
 
+	/* configure the core to support LPM */
+	dwc2_gadget_init_lpm(hsotg);
+
 	/* must be at-least 3ms to allow bus to see disconnect */
 	mdelay(3);
 
@@ -4648,6 +4651,7 @@ int dwc2_gadget_init(struct dwc2_hsotg *hsotg, int irq)
 	hsotg->gadget.max_speed = USB_SPEED_HIGH;
 	hsotg->gadget.ops = &dwc2_hsotg_gadget_ops;
 	hsotg->gadget.name = dev_name(dev);
+
 	if (hsotg->dr_mode == USB_DR_MODE_OTG)
 		hsotg->gadget.is_otg = 1;
 	else if (hsotg->dr_mode == USB_DR_MODE_PERIPHERAL)
@@ -4886,3 +4890,21 @@ int dwc2_restore_device_registers(struct dwc2_hsotg *hsotg)
 
 	return 0;
 }
+
+/**
+ * dwc2_gadget_init_lpm - Configure the core to support LPM in device mode
+ */
+void dwc2_gadget_init_lpm(struct dwc2_hsotg *hsotg)
+{
+	u32 val;
+
+	if (!hsotg->params.lpm)
+		return;
+
+	val = GLPMCFG_LPMCAP | GLPMCFG_APPL1RES;
+	val |= hsotg->params.hird_threshold_en ? GLPMCFG_HIRD_THRES_EN : 0;
+	val |= hsotg->params.lpm_clock_gating ? GLPMCFG_ENBLSLPM : 0;
+	dwc2_writel(val, hsotg->regs + GLPMCFG);
+	dev_dbg(hsotg->dev, "GLPMCFG=0x%08x\n", dwc2_readl(hsotg->regs
+		+ GLPMCFG));
+}
-- 
2.11.0