diff --git a/drivers/usb/musb-new/mtk-musb.c b/drivers/usb/musb-new/mtk-musb.c
index ee8b2c0..12068ea 100644
--- a/drivers/usb/musb-new/mtk-musb.c
+++ b/drivers/usb/musb-new/mtk-musb.c
@@ -22,6 +22,13 @@
 #define IDDIG_INT_STATUS     (1<<9)
 #define DRVVBUS_INT_STATUS   (1<<10)
 
+#define USB_DMA_BURST_MODE_3	(0x3 << 9)
+#define USB_DMA_ENDPNT_OFFSET	(4)
+#define USB_DMA_EN		(1 << 0)
+#define USB_DMA_ADDR(chan)	(0x0208 + 0x10*(chan-1))
+#define USB_DMA_COUNT(chan)	(0x020c + 0x10*(chan-1))
+#define USB_DMA_CNTL(chan)	(0x0204 + 0x10*(chan-1))
+
 /*EP Fifo Config*/
 static struct musb_fifo_cfg fifo_cfg[] __initdata = {
 { .hw_ep_num =  1, .style = FIFO_TX,   .maxpacket = 512, .mode = BUF_DOUBLE},
@@ -203,6 +210,22 @@
     udelay(800);
 }
 
+void musb_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst) {
+	struct musb *musb = hw_ep->musb;
+	void __iomem *fifo = hw_ep->fifo;
+
+	if (hw_ep->epnum == 0) {
+		readsb(fifo, dst, len);
+	} else {
+		flush_cache((uintptr_t)dst, len);
+		musb_writel(musb->mregs, USB_DMA_ADDR(hw_ep->epnum), (uintptr_t)dst);
+		musb_writel(musb->mregs, USB_DMA_COUNT(hw_ep->epnum), len);
+		u16 dma_cntl = USB_DMA_BURST_MODE_3 | (hw_ep->epnum << USB_DMA_ENDPNT_OFFSET) | USB_DMA_EN;
+		musb_writew(musb->mregs, USB_DMA_CNTL(hw_ep->epnum), dma_cntl);
+		while (musb_readw(musb->mregs, USB_DMA_CNTL(hw_ep->epnum)) & USB_DMA_EN);
+	}
+}
+
 static int mt8xx_musb_init(struct musb *musb)
 {
 	int ret;
diff --git a/drivers/usb/musb-new/musb_core.c b/drivers/usb/musb-new/musb_core.c
index afea9fb..c2abf4d 100644
--- a/drivers/usb/musb-new/musb_core.c
+++ b/drivers/usb/musb-new/musb_core.c
@@ -251,7 +251,7 @@
 	}
 }
 
-#if !defined(CONFIG_USB_MUSB_AM35X) && !defined(CONFIG_USB_MUSB_PIC32)
+#if !defined(CONFIG_USB_MUSB_AM35X) && !defined(CONFIG_USB_MUSB_PIC32) && !defined(CONFIG_USB_MUSB_MTK)
 /*
  * Unload an endpoint's FIFO
  */