[PATCH v2 3/3] usb: dwc2: Add dwc2_enable_acg function

[Razmik Karapetyan]

Added function for supporting Active Clock Gating functionality.
This function checks GHWCFG4 register and if ACG supported,
it sets GATEEN bit in PCGCCTL1 register and enables ACG.

According to ACG functional specification, enabling of ACG feature
in host mode done in host initialization, before turning Vbus on,
specifically in dwc2_core_host_init function.

Enabling of ACG feature in device mode done in device initialization,
before clearing the SftDiscon bit in DCTL.
This bit was cleared in dwc2_hsotg_core_connect() function.So
dwc2_enable_acg() called before dwc2_core_connect() calls.

Signed-off-by: Razmik Karapetyan <razmik@xxxxxxxxxxxx>
---
 drivers/usb/dwc2/core.c   | 16 ++++++++++++++++
 drivers/usb/dwc2/core.h   |  2 ++
 drivers/usb/dwc2/gadget.c | 12 ++++++++++--
 drivers/usb/dwc2/hcd.c    |  5 +++++
 4 files changed, 33 insertions(+), 2 deletions(-)

diff --git a/drivers/usb/dwc2/core.c b/drivers/usb/dwc2/core.c
index 42ac47f..21fa39e 100644
--- a/drivers/usb/dwc2/core.c
+++ b/drivers/usb/dwc2/core.c
@@ -491,6 +491,22 @@ void dwc2_force_dr_mode(struct dwc2_hsotg *hsotg)
 	}
 }
 
+/*
+ * dwc2_enable_acg - enable active clock gating feature
+ */
+void dwc2_enable_acg(struct dwc2_hsotg *hsotg)
+{
+	u32 hwcfg4 = dwc2_readl(hsotg->regs + GHWCFG4);
+
+	if (hwcfg4 & GHWCFG4_ACG_SUPPORTED) {
+		u32 pcgcctl1 = dwc2_readl(hsotg->regs + PCGCCTL1);
+
+		dev_dbg(hsotg->dev, "Enabling Active Clock Gating\n");
+		pcgcctl1 |= PCGCCTL1_GATEEN;
+		dwc2_writel(pcgcctl1, hsotg->regs + PCGCCTL1);
+	}
+}
+
 /**
  * dwc2_dump_host_registers() - Prints the host registers
  *
diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index 67ca757..a20d166 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -1115,6 +1115,8 @@ void dwc2_flush_rx_fifo(struct dwc2_hsotg *hsotg);
 void dwc2_enable_global_interrupts(struct dwc2_hsotg *hcd);
 void dwc2_disable_global_interrupts(struct dwc2_hsotg *hcd);
 
+void dwc2_enable_acg(struct dwc2_hsotg *hsotg);
+
 /* This function should be called on every hardware interrupt. */
 irqreturn_t dwc2_handle_common_intr(int irq, void *dev);
 
diff --git a/drivers/usb/dwc2/gadget.c b/drivers/usb/dwc2/gadget.c
index 98a4a79..7c772f1 100644
--- a/drivers/usb/dwc2/gadget.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -4357,6 +4357,8 @@ static int dwc2_hsotg_pullup(struct usb_gadget *gadget, int is_on)
 	if (is_on) {
 		hsotg->enabled = 1;
 		dwc2_hsotg_core_init_disconnected(hsotg, false);
+		/* Enable ACG feature in device mode,if supported */
+		dwc2_enable_acg(hsotg);
 		dwc2_hsotg_core_connect(hsotg);
 	} else {
 		dwc2_hsotg_core_disconnect(hsotg);
@@ -4389,8 +4391,11 @@ static int dwc2_hsotg_vbus_session(struct usb_gadget *gadget, int is_active)
 		hsotg->op_state = OTG_STATE_B_PERIPHERAL;
 
 		dwc2_hsotg_core_init_disconnected(hsotg, false);
-		if (hsotg->enabled)
+		if (hsotg->enabled) {
+			/* Enable ACG feature in device mode,if supported */
+			dwc2_enable_acg(hsotg);
 			dwc2_hsotg_core_connect(hsotg);
+		}
 	} else {
 		dwc2_hsotg_core_disconnect(hsotg);
 		dwc2_hsotg_disconnect(hsotg);
@@ -4755,8 +4760,11 @@ int dwc2_hsotg_resume(struct dwc2_hsotg *hsotg)
 
 		spin_lock_irqsave(&hsotg->lock, flags);
 		dwc2_hsotg_core_init_disconnected(hsotg, false);
-		if (hsotg->enabled)
+		if (hsotg->enabled) {
+			/* Enable ACG feature in device mode,if supported */
+			dwc2_enable_acg(hsotg);
 			dwc2_hsotg_core_connect(hsotg);
+		}
 		spin_unlock_irqrestore(&hsotg->lock, flags);
 	}
 
diff --git a/drivers/usb/dwc2/hcd.c b/drivers/usb/dwc2/hcd.c
index 4fedc0c..d60545e 100644
--- a/drivers/usb/dwc2/hcd.c
+++ b/drivers/usb/dwc2/hcd.c
@@ -2413,6 +2413,9 @@ static void dwc2_core_host_init(struct dwc2_hsotg *hsotg)
 		}
 	}
 
+	/* Enable ACG feature in host mode, if supported */
+	dwc2_enable_acg(hsotg);
+
 	/* Turn on the vbus power */
 	dev_dbg(hsotg->dev, "Init: Port Power? op_state=%d\n", hsotg->op_state);
 	if (hsotg->op_state == OTG_STATE_A_HOST) {
@@ -3279,6 +3282,8 @@ static void dwc2_conn_id_status_change(struct work_struct *work)
 		spin_lock_irqsave(&hsotg->lock, flags);
 		dwc2_hsotg_core_init_disconnected(hsotg, false);
 		spin_unlock_irqrestore(&hsotg->lock, flags);
+		/* Enable ACG feature in device mode,if supported */
+		dwc2_enable_acg(hsotg);
 		dwc2_hsotg_core_connect(hsotg);
 	} else {
 host:
-- 
2.7.4