[PATCH v3 10/14] usb: dwc2: Add function to flush tx fifo

[Jules Maselbas]

Signed-off-by: Jules Maselbas <jmaselbas at kalray.eu>
---
 drivers/usb/dwc2/core.c | 33 +++++++++++++++++++++++++++++++++
 drivers/usb/dwc2/dwc2.h |  1 +
 2 files changed, 34 insertions(+)

diff --git a/drivers/usb/dwc2/core.c b/drivers/usb/dwc2/core.c
index 6e8d30c22..a8693097d 100644
--- a/drivers/usb/dwc2/core.c
+++ b/drivers/usb/dwc2/core.c
@@ -294,6 +294,39 @@ void dwc2_flush_all_fifo(struct dwc2 *dwc2)
 	udelay(1);
 }
 
+/**
+ * dwc2_flush_tx_fifo() - Flushes a Tx FIFO
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ * @idx: The fifo index (0..15)
+ */
+void dwc2_flush_tx_fifo(struct dwc2 *dwc2, const int idx)
+{
+	u32 greset;
+
+	if (idx > 15)
+		return;
+
+	dwc2_dbg(dwc2, "Flush Tx FIFO %d\n", idx);
+
+	/* Wait for AHB master IDLE state */
+	if (dwc2_wait_bit_set(dwc2, GRSTCTL, GRSTCTL_AHBIDLE, 10000)) {
+		dwc2_warn(dwc2, "%s: Timeout waiting for AHB Idle\n", __func__);
+		return;
+	}
+
+	greset = GRSTCTL_TXFFLSH;
+	greset |= GRSTCTL_TXFNUM(idx) & GRSTCTL_TXFNUM_MASK;
+	dwc2_writel(dwc2, greset, GRSTCTL);
+
+	if (dwc2_wait_bit_clear(dwc2, GRSTCTL, GRSTCTL_TXFFLSH, 10000))
+		dwc2_warn(dwc2, "%s: Timeout flushing tx fifo (GRSTCTL=%08x)\n",
+			 __func__, dwc2_readl(dwc2, GRSTCTL));
+
+	/* Wait for at least 3 PHY Clocks */
+	udelay(1);
+}
+
 static int dwc2_fs_phy_init(struct dwc2 *dwc2, bool select_phy)
 {
 	u32 usbcfg, ggpio, i2cctl;
diff --git a/drivers/usb/dwc2/dwc2.h b/drivers/usb/dwc2/dwc2.h
index c10385475..f620175df 100644
--- a/drivers/usb/dwc2/dwc2.h
+++ b/drivers/usb/dwc2/dwc2.h
@@ -17,6 +17,7 @@ void dwc2_get_hwparams(struct dwc2 *dwc2);
 
 void dwc2_init_fs_ls_pclk_sel(struct dwc2 *dwc2);
 void dwc2_flush_all_fifo(struct dwc2 *dwc2);
+void dwc2_flush_tx_fifo(struct dwc2 *dwc2, const int idx);
 
 int dwc2_phy_init(struct dwc2 *dwc2, bool select_phy);
 int dwc2_gahbcfg_init(struct dwc2 *dwc2);
-- 
2.17.1