usb: dwc2: gadget: Fix dynamic FIFO initialization flow

index bc3b3fda5000..0b2d9bf43283 100644
@@ -293,9 +293,13 @@ static void dwc2_hsotg_init_fifo(struct dwc2_hsotg *hsotg)
 	unsigned int ep;
 	unsigned int addr;
 	int timeout;
+	int fifo_count;
 	u32 val;
 	u32 *txfsz = hsotg->params.g_tx_fifo_size;
 
+	if (!hsotg->params.enable_dynamic_fifo)
+		return;
+
 	/* Reset fifo map if not correctly cleared during previous session */
 	WARN_ON(hsotg->fifo_map);
 	hsotg->fifo_map = 0;

@@ -321,9 +325,9 @@ static void dwc2_hsotg_init_fifo(struct dwc2_hsotg *hsotg)
 	 * them to endpoints dynamically according to maxpacket size value of
 	 * given endpoint.
 	 */
-	for (ep = 1; ep < MAX_EPS_CHANNELS; ep++) {
-		if (!txfsz[ep])
-			continue;
+	fifo_count = dwc2_hsotg_tx_fifo_count(hsotg);
+
+	for (ep = 1; ep <= fifo_count; ep++) {
 		val = addr;
 		val |= txfsz[ep] << FIFOSIZE_DEPTH_SHIFT;
 		WARN_ONCE(addr + txfsz[ep] > hsotg->fifo_mem,

index 0b2d9bf43283..46f38604c3b2 100644
@@ -4214,14 +4214,6 @@ static void dwc2_hsotg_init(struct dwc2_hsotg *hsotg)
 	/* Be in disconnected state until gadget is registered */
 	__orr32(hsotg->regs + DCTL, DCTL_SFTDISCON);
 
-	/* setup fifos */
-
-	dev_dbg(hsotg->dev, "GRXFSIZ=0x%08x, GNPTXFSIZ=0x%08x\n",
-		dwc2_readl(hsotg->regs + GRXFSIZ),
-		dwc2_readl(hsotg->regs + GNPTXFSIZ));
-
-	dwc2_hsotg_init_fifo(hsotg);
-
 	/* keep other bits untouched (so e.g. forced modes are not lost) */
 	usbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
 	usbcfg &= ~(GUSBCFG_TOUTCAL_MASK | GUSBCFG_PHYIF16 | GUSBCFG_SRPCAP |

index 46f38604c3b2..574562a0286d 100644
@@ -292,7 +292,6 @@ static void dwc2_hsotg_init_fifo(struct dwc2_hsotg *hsotg)
 {
 	unsigned int ep;
 	unsigned int addr;
-	int timeout;
 	int fifo_count;
 	u32 val;
 	u32 *txfsz = hsotg->params.g_tx_fifo_size;

@@ -341,33 +340,16 @@ static void dwc2_hsotg_init_fifo(struct dwc2_hsotg *hsotg)
 	dwc2_writel(hsotg->hw_params.total_fifo_size |
 		    addr << GDFIFOCFG_EPINFOBASE_SHIFT,
 		    hsotg->regs + GDFIFOCFG);
+
 	/*
-	 * according to p428 of the design guide, we need to ensure that
-	 * all fifos are flushed before continuing
+	 * Chapter 2.1.1 of the Programming Guide - The TxFIFOs and the RxFIFO
+	 * must be flushed after the RAM allocation is done, for proper FIFO
+	 * functioning.
+	 *
+	 * 0x10 - all TX FIFOs
 	 */
-
-	dwc2_writel(GRSTCTL_TXFNUM(0x10) | GRSTCTL_TXFFLSH |
-	       GRSTCTL_RXFFLSH, hsotg->regs + GRSTCTL);
-
-	/* wait until the fifos are both flushed */
-	timeout = 100;
-	while (1) {
-		val = dwc2_readl(hsotg->regs + GRSTCTL);
-
-		if ((val & (GRSTCTL_TXFFLSH | GRSTCTL_RXFFLSH)) == 0)
-			break;
-
-		if (--timeout == 0) {
-			dev_err(hsotg->dev,
-				"%s: timeout flushing fifos (GRSTCTL=%08x)\n",
-				__func__, val);
-			break;
-		}
-
-		udelay(1);
-	}
-
-	dev_dbg(hsotg->dev, "FIFOs reset, timeout at %d\n", timeout);
+	dwc2_flush_tx_fifo(hsotg, 0x10);
+	dwc2_flush_rx_fifo(hsotg);
 }
 
 /**

index 574562a0286d..0bf42a0a3213 100644
@@ -285,10 +285,9 @@ int dwc2_hsotg_tx_fifo_average_depth(struct dwc2_hsotg *hsotg)
 }
 
 /**
- * dwc2_hsotg_init_fifo - initialise non-periodic FIFOs
- * @hsotg: The device instance.
+ * dwc2_hsotg_init_periodic_fifos - initialize periodic FIFOs
  */
-static void dwc2_hsotg_init_fifo(struct dwc2_hsotg *hsotg)
+static void dwc2_hsotg_init_periodic_fifos(struct dwc2_hsotg *hsotg)
 {
 	unsigned int ep;
 	unsigned int addr;

@@ -303,27 +302,12 @@ static void dwc2_hsotg_init_fifo(struct dwc2_hsotg *hsotg)
 	WARN_ON(hsotg->fifo_map);
 	hsotg->fifo_map = 0;
 
-	/* set RX/NPTX FIFO sizes */
-	dwc2_writel(hsotg->params.g_rx_fifo_size, hsotg->regs + GRXFSIZ);
-	dwc2_writel((hsotg->params.g_rx_fifo_size << FIFOSIZE_STARTADDR_SHIFT) |
-		    (hsotg->params.g_np_tx_fifo_size << FIFOSIZE_DEPTH_SHIFT),
-		    hsotg->regs + GNPTXFSIZ);
-
 	/*
-	 * arange all the rest of the TX FIFOs, as some versions of this
-	 * block have overlapping default addresses. This also ensures
-	 * that if the settings have been changed, then they are set to
-	 * known values.
+	 * Arange periodic TX FIFOs to correctly calculated values.
+	 * Start at the end of the GNPTXFSIZ.
 	 */
-
-	/* start at the end of the GNPTXFSIZ, rounded up */
 	addr = hsotg->params.g_rx_fifo_size + hsotg->params.g_np_tx_fifo_size;
 
-	/*
-	 * Configure fifos sizes from provided configuration and assign
-	 * them to endpoints dynamically according to maxpacket size value of
-	 * given endpoint.
-	 */
 	fifo_count = dwc2_hsotg_tx_fifo_count(hsotg);
 
 	for (ep = 1; ep <= fifo_count; ep++) {

@@ -340,6 +324,21 @@ static void dwc2_hsotg_init_fifo(struct dwc2_hsotg *hsotg)
 	dwc2_writel(hsotg->hw_params.total_fifo_size |
 		    addr << GDFIFOCFG_EPINFOBASE_SHIFT,
 		    hsotg->regs + GDFIFOCFG);
+}
+
+/**
+ * dwc2_hsotg_init_non_periodic_fifos - initialize non-periodic FIFOs
+ */
+static void dwc2_hsotg_init_non_periodic_fifos(struct dwc2_hsotg *hsotg)
+{
+	if (!hsotg->params.enable_dynamic_fifo)
+		return;
+
+	/* set RX/NPTX FIFO sizes */
+	dwc2_writel(hsotg->params.g_rx_fifo_size, hsotg->regs + GRXFSIZ);
+	dwc2_writel((hsotg->params.g_rx_fifo_size << FIFOSIZE_STARTADDR_SHIFT) |
+		    (hsotg->params.g_np_tx_fifo_size << FIFOSIZE_DEPTH_SHIFT),
+		    hsotg->regs + GNPTXFSIZ);
 
 	/*
 	 * Chapter 2.1.1 of the Programming Guide - The TxFIFOs and the RxFIFO

@@ -3269,10 +3268,13 @@ void dwc2_hsotg_core_init_disconnected(struct dwc2_hsotg *hsotg,
 	}
 	dwc2_writel(usbcfg, hsotg->regs + GUSBCFG);
 
-	dwc2_hsotg_init_fifo(hsotg);
+	dwc2_hsotg_init_non_periodic_fifos(hsotg);
+
+	if (!is_usb_reset) {
+		dwc2_hsotg_init_periodic_fifos(hsotg);
 
-	if (!is_usb_reset)
 		__orr32(hsotg->regs + DCTL, DCTL_SFTDISCON);
+	}
 
 	dcfg |= DCFG_EPMISCNT(1);