1 files changed, 29 insertions, 3 deletions

diff --git a/drivers/usb/dwc2/gadget.c b/drivers/usb/dwc2/gadget.c
index b2f6da5b65cc..e7bf9cc635be 100644
--- a/drivers/usb/dwc2/gadget.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -885,10 +885,10 @@ static void dwc2_gadget_config_nonisoc_xfer_ddma(struct dwc2_hsotg_ep *hs_ep,
 	}
 
 	/* DMA sg buffer */
-	for_each_sg(ureq->sg, sg, ureq->num_sgs, i) {
+	for_each_sg(ureq->sg, sg, ureq->num_mapped_sgs, i) {
 		dwc2_gadget_fill_nonisoc_xfer_ddma_one(hs_ep, &desc,
 			sg_dma_address(sg) + sg->offset, sg_dma_len(sg),
-			sg_is_last(sg));
+			(i == (ureq->num_mapped_sgs - 1)));
 		desc_count += hs_ep->desc_count;
 	}
 
@@ -3424,8 +3424,11 @@ void dwc2_hsotg_core_init_disconnected(struct dwc2_hsotg *hsotg,
 
 	dwc2_hsotg_init_fifo(hsotg);
 
-	if (!is_usb_reset)
+	if (!is_usb_reset) {
 		dwc2_set_bit(hsotg, DCTL, DCTL_SFTDISCON);
+		if (hsotg->params.eusb2_disc)
+			dwc2_set_bit(hsotg, GOTGCTL, GOTGCTL_EUSB2_DISC_SUPP);
+	}
 
 	dcfg |= DCFG_EPMISCNT(1);
 
@@ -5316,6 +5319,8 @@ void dwc2_gadget_program_ref_clk(struct dwc2_hsotg *hsotg)
 int dwc2_gadget_enter_hibernation(struct dwc2_hsotg *hsotg)
 {
 	u32 gpwrdn;
+	u32 gusbcfg;
+	u32 pcgcctl;
 	int ret = 0;
 
 	/* Change to L2(suspend) state */
@@ -5335,6 +5340,22 @@ int dwc2_gadget_enter_hibernation(struct dwc2_hsotg *hsotg)
 	}
 
 	gpwrdn = GPWRDN_PWRDNRSTN;
+	udelay(10);
+	gusbcfg = dwc2_readl(hsotg, GUSBCFG);
+	if (gusbcfg & GUSBCFG_ULPI_UTMI_SEL) {
+		/* ULPI interface */
+		gpwrdn |= GPWRDN_ULPI_LATCH_EN_DURING_HIB_ENTRY;
+	}
+	dwc2_writel(hsotg, gpwrdn, GPWRDN);
+	udelay(10);
+
+	/* Suspend the Phy Clock */
+	pcgcctl = dwc2_readl(hsotg, PCGCTL);
+	pcgcctl |= PCGCTL_STOPPCLK;
+	dwc2_writel(hsotg, pcgcctl, PCGCTL);
+	udelay(10);
+
+	gpwrdn = dwc2_readl(hsotg, GPWRDN);
 	gpwrdn |= GPWRDN_PMUACTV;
 	dwc2_writel(hsotg, gpwrdn, GPWRDN);
 	udelay(10);
@@ -5435,6 +5456,11 @@ int dwc2_gadget_exit_hibernation(struct dwc2_hsotg *hsotg,
 	if (reset)
 		dwc2_clear_bit(hsotg, DCFG, DCFG_DEVADDR_MASK);
 
+	/* Reset ULPI latch */
+	gpwrdn = dwc2_readl(hsotg, GPWRDN);
+	gpwrdn &= ~GPWRDN_ULPI_LATCH_EN_DURING_HIB_ENTRY;
+	dwc2_writel(hsotg, gpwrdn, GPWRDN);
+
 	/* De-assert Wakeup Logic */
 	gpwrdn = dwc2_readl(hsotg, GPWRDN);
 	gpwrdn &= ~GPWRDN_PMUACTV;