[v1,06/14] usb: dwc2: Add part. power down exit from dwc2_conn_id_status_change().

[Artur Petrosyan <Arthur.Petrosyan@synopsys.com>]

Before changing to connector B exiting from Partial
Power Down is required.

- Added exiting from Partial Power Down mode when
  connector ID status changes to "connId B".
  Because if connector ID status changed to B connector
  while core was in partial power down mode, HANG would
  accrue from a soft reset.

Signed-off-by: Artur Petrosyan <arturp@synopsys.com>
---
 drivers/usb/dwc2/hcd.c | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

-- 
2.11.0

diff --git a/drivers/usb/dwc2/hcd.c b/drivers/usb/dwc2/hcd.c
index 8367902a47eb..54450fa352cf 100644
--- a/drivers/usb/dwc2/hcd.c
+++ b/drivers/usb/dwc2/hcd.c
@@ -3348,6 +3348,7 @@ static void dwc2_conn_id_status_change(struct work_struct *work)
 						wf_otg);
 	u32 count = 0;
 	u32 gotgctl;
+	u32 pcgcctl;
 	unsigned long flags;
 
 	dev_dbg(hsotg->dev, "%s()\n", __func__);
@@ -3387,6 +3388,23 @@ static void dwc2_conn_id_status_change(struct work_struct *work)
 		if (count > 250)
 			dev_err(hsotg->dev,
 				"Connection id status change timed out\n");
+
+		if (hsotg->params.power_down ==
+		    DWC2_POWER_DOWN_PARAM_PARTIAL &&
+		    hsotg->lx_state == DWC2_L2) {
+			pcgcctl = dwc2_readl(hsotg, PCGCTL);
+			pcgcctl &= ~PCGCTL_STOPPCLK;
+			dwc2_writel(hsotg, pcgcctl, PCGCTL);
+
+			pcgcctl = dwc2_readl(hsotg, PCGCTL);
+			pcgcctl &= ~PCGCTL_PWRCLMP;
+			dwc2_writel(hsotg, pcgcctl, PCGCTL);
+
+			pcgcctl = dwc2_readl(hsotg, PCGCTL);
+			pcgcctl &= ~PCGCTL_RSTPDWNMODULE;
+			dwc2_writel(hsotg, pcgcctl, PCGCTL);
+		}
+
 		hsotg->op_state = OTG_STATE_B_PERIPHERAL;
 		dwc2_core_init(hsotg, false);
 		dwc2_enable_global_interrupts(hsotg);