openwrtv4/target/linux/brcm2708/patches-3.10/0027-Add-FIQ-patch-to-dwc_otg-driver.-Enable-with-dwc_otg.patch

From 5e4c6e64d0c5fd6adc6c68cc1366f0f7c0ad84ba Mon Sep 17 00:00:00 2001
From: popcornmix <popcornmix@gmail.com>
Date: Wed, 3 Jul 2013 00:46:42 +0100
Subject: [PATCH 027/196] Add FIQ patch to dwc_otg driver. Enable with
 dwc_otg.fiq_fix_enable=1. Should give about 10% more ARM performance. Thanks
 to Gordon and Costas

---
 arch/arm/Kconfig                              |   1 +
 arch/arm/include/asm/fiq.h                    |   1 +
 arch/arm/kernel/fiq.c                         |   1 +
 arch/arm/kernel/fiqasm.S                      |   7 ++
 arch/arm/mach-bcm2708/armctrl.c               |  19 ++-
 arch/arm/mach-bcm2708/bcm2708.c               |  29 ++++-
 arch/arm/mach-bcm2708/include/mach/irqs.h     | 159 +++++++++++++-------------
 arch/arm/mach-bcm2708/include/mach/platform.h |   2 +
 drivers/usb/host/dwc_otg/Makefile             |   1 +
 drivers/usb/host/dwc_otg/dwc_otg_cil_intr.c   |  14 ++-
 drivers/usb/host/dwc_otg/dwc_otg_dbg.h        |   1 +
 drivers/usb/host/dwc_otg/dwc_otg_driver.c     |  37 +++++-
 drivers/usb/host/dwc_otg/dwc_otg_hcd.c        |   5 +
 drivers/usb/host/dwc_otg/dwc_otg_hcd.h        |   2 +-
 drivers/usb/host/dwc_otg/dwc_otg_hcd_if.h     |   5 +
 drivers/usb/host/dwc_otg/dwc_otg_hcd_intr.c   | 154 ++++++++++++++++++++++---
 drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c  |  45 ++++++++
 drivers/usb/host/dwc_otg/dwc_otg_hcd_queue.c  |  20 +++-
 drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.c   | 113 ++++++++++++++++++
 drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.h   |  36 ++++++
 drivers/usb/host/dwc_otg/dwc_otg_os_dep.h     |   3 +
 21 files changed, 545 insertions(+), 110 deletions(-)
 create mode 100755 drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.c
 create mode 100755 drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.h

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index eb291c7..5468f1f 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -373,6 +373,7 @@ config ARCH_BCM2708
 	select ARM_ERRATA_411920
 	select MACH_BCM2708
 	select VC4
+	select FIQ
 	help
 	  This enables support for Broadcom BCM2708 boards.
 
diff --git a/arch/arm/include/asm/fiq.h b/arch/arm/include/asm/fiq.h
index d493d0b..f1a131b 100644
--- a/arch/arm/include/asm/fiq.h
+++ b/arch/arm/include/asm/fiq.h
@@ -42,6 +42,7 @@ extern void disable_fiq(int fiq);
 /* helpers defined in fiqasm.S: */
 extern void __set_fiq_regs(unsigned long const *regs);
 extern void __get_fiq_regs(unsigned long *regs);
+extern void __FIQ_Branch(unsigned long *regs);
 
 static inline void set_fiq_regs(struct pt_regs const *regs)
 {
diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c
index 25442f4..74ff4ba 100644
--- a/arch/arm/kernel/fiq.c
+++ b/arch/arm/kernel/fiq.c
@@ -145,6 +145,7 @@ void disable_fiq(int fiq)
 EXPORT_SYMBOL(set_fiq_handler);
 EXPORT_SYMBOL(__set_fiq_regs);	/* defined in fiqasm.S */
 EXPORT_SYMBOL(__get_fiq_regs);	/* defined in fiqasm.S */
+EXPORT_SYMBOL(__FIQ_Branch);	/* defined in fiqasm.S */
 EXPORT_SYMBOL(claim_fiq);
 EXPORT_SYMBOL(release_fiq);
 EXPORT_SYMBOL(enable_fiq);
diff --git a/arch/arm/kernel/fiqasm.S b/arch/arm/kernel/fiqasm.S
index 207f9d6..93eddfe 100644
--- a/arch/arm/kernel/fiqasm.S
+++ b/arch/arm/kernel/fiqasm.S
@@ -25,6 +25,9 @@
 ENTRY(__set_fiq_regs)
 	mov	r2, #PSR_I_BIT | PSR_F_BIT | FIQ_MODE
 	mrs	r1, cpsr
+@@@@@@@@@@@@@@@ hack: enable the fiq here to keep usb driver happy
+	and	r1, #~PSR_F_BIT
+@@@@@@@@@@@@@@@ endhack: (need to find better place for this to happen)
 	msr	cpsr_c, r2	@ select FIQ mode
 	mov	r0, r0		@ avoid hazard prior to ARMv4
 	ldmia	r0!, {r8 - r12}
@@ -47,3 +50,7 @@ ENTRY(__get_fiq_regs)
 	mov	r0, r0		@ avoid hazard prior to ARMv4
 	mov	pc, lr
 ENDPROC(__get_fiq_regs)
+
+ENTRY(__FIQ_Branch)
+	mov pc, r8
+ENDPROC(__FIQ_Branch)
diff --git a/arch/arm/mach-bcm2708/armctrl.c b/arch/arm/mach-bcm2708/armctrl.c
index da18725..274aa30 100644
--- a/arch/arm/mach-bcm2708/armctrl.c
+++ b/arch/arm/mach-bcm2708/armctrl.c
@@ -52,8 +52,12 @@ static void armctrl_mask_irq(struct irq_data *d)
 		0
 	};
 
-	unsigned int data = (unsigned int)irq_get_chip_data(d->irq);
-	writel(1 << (data & 0x1f), __io_address(disables[(data >> 5) & 0x3]));
+	if (d->irq >= FIQ_START) {
+		writel(0, __io_address(ARM_IRQ_FAST));
+	} else {
+		unsigned int data = (unsigned int)irq_get_chip_data(d->irq);
+		writel(1 << (data & 0x1f), __io_address(disables[(data >> 5) & 0x3]));
+	}
 }
 
 static void armctrl_unmask_irq(struct irq_data *d)
@@ -65,8 +69,14 @@ static void armctrl_unmask_irq(struct irq_data *d)
 		0
 	};
 
-	unsigned int data = (unsigned int)irq_get_chip_data(d->irq);
-	writel(1 << (data & 0x1f), __io_address(enables[(data >> 5) & 0x3]));
+	if (d->irq >= FIQ_START) {
+		unsigned int data =
+		    (unsigned int)irq_get_chip_data(d->irq) - FIQ_START;
+		writel(0x80 | data, __io_address(ARM_IRQ_FAST));
+	} else {
+		unsigned int data = (unsigned int)irq_get_chip_data(d->irq);
+		writel(1 << (data & 0x1f), __io_address(enables[(data >> 5) & 0x3]));
+	}
 }
 
 #if defined(CONFIG_PM)
@@ -204,5 +214,6 @@ int __init armctrl_init(void __iomem * base, unsigned int irq_start,
 	}
 
 	armctrl_pm_register(base, irq_start, resume_sources);
+	init_FIQ(FIQ_START);
 	return 0;
 }
diff --git a/arch/arm/mach-bcm2708/bcm2708.c b/arch/arm/mach-bcm2708/bcm2708.c
index 18f7ede..b9aa2de 100644
--- a/arch/arm/mach-bcm2708/bcm2708.c
+++ b/arch/arm/mach-bcm2708/bcm2708.c
@@ -309,12 +309,32 @@ static struct resource bcm2708_usb_resources[] = {
 	       .flags = IORESOURCE_MEM,
 	       },
 	[1] = {
-	       .start = IRQ_USB,
-	       .end = IRQ_USB,
+		.start = MPHI_BASE,
+		.end = MPHI_BASE + SZ_4K - 1,
+		.flags = IORESOURCE_MEM,
+	      },
+	[2] = {
+	       .start = IRQ_HOSTPORT,
+	       .end = IRQ_HOSTPORT,
 	       .flags = IORESOURCE_IRQ,
 	       },
 };
 
+bool fiq_fix_enable = true;
+
+static struct resource bcm2708_usb_resources_no_fiq_fix[] = {
+	[0] = {
+		.start = USB_BASE,
+		.end = USB_BASE + SZ_128K - 1,
+		.flags = IORESOURCE_MEM,
+		},
+	[1] = {
+		.start = IRQ_USB,
+		.end = IRQ_USB,
+		.flags = IORESOURCE_IRQ,
+		},
+};
+
 static u64 usb_dmamask = DMA_BIT_MASK(DMA_MASK_BITS_COMMON);
 
 static struct platform_device bcm2708_usb_device = {
@@ -642,6 +662,11 @@ void __init bcm2708_init(void)
 #endif
 	bcm_register_device(&bcm2708_systemtimer_device);
 	bcm_register_device(&bcm2708_fb_device);
+	if (!fiq_fix_enable)
+	{
+		bcm2708_usb_device.resource = bcm2708_usb_resources_no_fiq_fix;
+		bcm2708_usb_device.num_resources = ARRAY_SIZE(bcm2708_usb_resources_no_fiq_fix);
+	}
 	bcm_register_device(&bcm2708_usb_device);
 	bcm_register_device(&bcm2708_uart1_device);
 	bcm_register_device(&bcm2708_powerman_device);
diff --git a/arch/arm/mach-bcm2708/include/mach/irqs.h b/arch/arm/mach-bcm2708/include/mach/irqs.h
index e8bb068..9aaedf1 100644
--- a/arch/arm/mach-bcm2708/include/mach/irqs.h
+++ b/arch/arm/mach-bcm2708/include/mach/irqs.h
@@ -106,91 +106,94 @@
 #define IRQ_PENDING1          (IRQ_ARMCTRL_START + INTERRUPT_PENDING1)
 #define IRQ_PENDING2          (IRQ_ARMCTRL_START + INTERRUPT_PENDING2)
 
+#define FIQ_START             HARD_IRQS
+
 /*
  *  FIQ interrupts definitions are the same as the INT definitions.
  */
-#define FIQ_TIMER0            INT_TIMER0
-#define FIQ_TIMER1            INT_TIMER1
-#define FIQ_TIMER2            INT_TIMER2
-#define FIQ_TIMER3            INT_TIMER3
-#define FIQ_CODEC0            INT_CODEC0
-#define FIQ_CODEC1            INT_CODEC1
-#define FIQ_CODEC2            INT_CODEC2
-#define FIQ_JPEG              INT_JPEG
-#define FIQ_ISP               INT_ISP
-#define FIQ_USB               INT_USB
-#define FIQ_3D                INT_3D
-#define FIQ_TRANSPOSER        INT_TRANSPOSER
-#define FIQ_MULTICORESYNC0    INT_MULTICORESYNC0
-#define FIQ_MULTICORESYNC1    INT_MULTICORESYNC1
-#define FIQ_MULTICORESYNC2    INT_MULTICORESYNC2
-#define FIQ_MULTICORESYNC3    INT_MULTICORESYNC3
-#define FIQ_DMA0              INT_DMA0
-#define FIQ_DMA1              INT_DMA1
-#define FIQ_DMA2              INT_DMA2
-#define FIQ_DMA3              INT_DMA3
-#define FIQ_DMA4              INT_DMA4
-#define FIQ_DMA5              INT_DMA5
-#define FIQ_DMA6              INT_DMA6
-#define FIQ_DMA7              INT_DMA7
-#define FIQ_DMA8              INT_DMA8
-#define FIQ_DMA9              INT_DMA9
-#define FIQ_DMA10             INT_DMA10
-#define FIQ_DMA11             INT_DMA11
-#define FIQ_DMA12             INT_DMA12
-#define FIQ_AUX               INT_AUX
-#define FIQ_ARM               INT_ARM
-#define FIQ_VPUDMA            INT_VPUDMA
-#define FIQ_HOSTPORT          INT_HOSTPORT
-#define FIQ_VIDEOSCALER       INT_VIDEOSCALER
-#define FIQ_CCP2TX            INT_CCP2TX
-#define FIQ_SDC               INT_SDC
-#define FIQ_DSI0              INT_DSI0
-#define FIQ_AVE               INT_AVE
-#define FIQ_CAM0              INT_CAM0
-#define FIQ_CAM1              INT_CAM1
-#define FIQ_HDMI0             INT_HDMI0
-#define FIQ_HDMI1             INT_HDMI1
-#define FIQ_PIXELVALVE1       INT_PIXELVALVE1
-#define FIQ_I2CSPISLV         INT_I2CSPISLV
-#define FIQ_DSI1              INT_DSI1
-#define FIQ_PWA0              INT_PWA0
-#define FIQ_PWA1              INT_PWA1
-#define FIQ_CPR               INT_CPR
-#define FIQ_SMI               INT_SMI
-#define FIQ_GPIO0             INT_GPIO0
-#define FIQ_GPIO1             INT_GPIO1
-#define FIQ_GPIO2             INT_GPIO2
-#define FIQ_GPIO3             INT_GPIO3
-#define FIQ_I2C               INT_I2C
-#define FIQ_SPI               INT_SPI
-#define FIQ_I2SPCM            INT_I2SPCM
-#define FIQ_SDIO              INT_SDIO
-#define FIQ_UART              INT_UART
-#define FIQ_SLIMBUS           INT_SLIMBUS
-#define FIQ_VEC               INT_VEC
-#define FIQ_CPG               INT_CPG
-#define FIQ_RNG               INT_RNG
-#define FIQ_ARASANSDIO        INT_ARASANSDIO
-#define FIQ_AVSPMON           INT_AVSPMON
+#define FIQ_TIMER0            (FIQ_START+INTERRUPT_TIMER0)
+#define FIQ_TIMER1            (FIQ_START+INTERRUPT_TIMER1)
+#define FIQ_TIMER2            (FIQ_START+INTERRUPT_TIMER2)
+#define FIQ_TIMER3            (FIQ_START+INTERRUPT_TIMER3)
+#define FIQ_CODEC0            (FIQ_START+INTERRUPT_CODEC0)
+#define FIQ_CODEC1            (FIQ_START+INTERRUPT_CODEC1)
+#define FIQ_CODEC2            (FIQ_START+INTERRUPT_CODEC2)
+#define FIQ_JPEG              (FIQ_START+INTERRUPT_JPEG)
+#define FIQ_ISP               (FIQ_START+INTERRUPT_ISP)
+#define FIQ_USB               (FIQ_START+INTERRUPT_USB)
+#define FIQ_3D                (FIQ_START+INTERRUPT_3D)
+#define FIQ_TRANSPOSER        (FIQ_START+INTERRUPT_TRANSPOSER)
+#define FIQ_MULTICORESYNC0    (FIQ_START+INTERRUPT_MULTICORESYNC0)
+#define FIQ_MULTICORESYNC1    (FIQ_START+INTERRUPT_MULTICORESYNC1)
+#define FIQ_MULTICORESYNC2    (FIQ_START+INTERRUPT_MULTICORESYNC2)
+#define FIQ_MULTICORESYNC3    (FIQ_START+INTERRUPT_MULTICORESYNC3)
+#define FIQ_DMA0              (FIQ_START+INTERRUPT_DMA0)
+#define FIQ_DMA1              (FIQ_START+INTERRUPT_DMA1)
+#define FIQ_DMA2              (FIQ_START+INTERRUPT_DMA2)
+#define FIQ_DMA3              (FIQ_START+INTERRUPT_DMA3)
+#define FIQ_DMA4              (FIQ_START+INTERRUPT_DMA4)
+#define FIQ_DMA5              (FIQ_START+INTERRUPT_DMA5)
+#define FIQ_DMA6              (FIQ_START+INTERRUPT_DMA6)
+#define FIQ_DMA7              (FIQ_START+INTERRUPT_DMA7)
+#define FIQ_DMA8              (FIQ_START+INTERRUPT_DMA8)
+#define FIQ_DMA9              (FIQ_START+INTERRUPT_DMA9)
+#define FIQ_DMA10             (FIQ_START+INTERRUPT_DMA10)
+#define FIQ_DMA11             (FIQ_START+INTERRUPT_DMA11)
+#define FIQ_DMA12             (FIQ_START+INTERRUPT_DMA12)
+#define FIQ_AUX               (FIQ_START+INTERRUPT_AUX)
+#define FIQ_ARM               (FIQ_START+INTERRUPT_ARM)
+#define FIQ_VPUDMA            (FIQ_START+INTERRUPT_VPUDMA)
+#define FIQ_HOSTPORT          (FIQ_START+INTERRUPT_HOSTPORT)
+#define FIQ_VIDEOSCALER       (FIQ_START+INTERRUPT_VIDEOSCALER)
+#define FIQ_CCP2TX            (FIQ_START+INTERRUPT_CCP2TX)
+#define FIQ_SDC               (FIQ_START+INTERRUPT_SDC)
+#define FIQ_DSI0              (FIQ_START+INTERRUPT_DSI0)
+#define FIQ_AVE               (FIQ_START+INTERRUPT_AVE)
+#define FIQ_CAM0              (FIQ_START+INTERRUPT_CAM0)
+#define FIQ_CAM1              (FIQ_START+INTERRUPT_CAM1)
+#define FIQ_HDMI0             (FIQ_START+INTERRUPT_HDMI0)
+#define FIQ_HDMI1             (FIQ_START+INTERRUPT_HDMI1)
+#define FIQ_PIXELVALVE1       (FIQ_START+INTERRUPT_PIXELVALVE1)
+#define FIQ_I2CSPISLV         (FIQ_START+INTERRUPT_I2CSPISLV)
+#define FIQ_DSI1              (FIQ_START+INTERRUPT_DSI1)
+#define FIQ_PWA0              (FIQ_START+INTERRUPT_PWA0)
+#define FIQ_PWA1              (FIQ_START+INTERRUPT_PWA1)
+#define FIQ_CPR               (FIQ_START+INTERRUPT_CPR)
+#define FIQ_SMI               (FIQ_START+INTERRUPT_SMI)
+#define FIQ_GPIO0             (FIQ_START+INTERRUPT_GPIO0)
+#define FIQ_GPIO1             (FIQ_START+INTERRUPT_GPIO1)
+#define FIQ_GPIO2             (FIQ_START+INTERRUPT_GPIO2)
+#define FIQ_GPIO3             (FIQ_START+INTERRUPT_GPIO3)
+#define FIQ_I2C               (FIQ_START+INTERRUPT_I2C)
+#define FIQ_SPI               (FIQ_START+INTERRUPT_SPI)
+#define FIQ_I2SPCM            (FIQ_START+INTERRUPT_I2SPCM)
+#define FIQ_SDIO              (FIQ_START+INTERRUPT_SDIO)
+#define FIQ_UART              (FIQ_START+INTERRUPT_UART)
+#define FIQ_SLIMBUS           (FIQ_START+INTERRUPT_SLIMBUS)
+#define FIQ_VEC               (FIQ_START+INTERRUPT_VEC)
+#define FIQ_CPG               (FIQ_START+INTERRUPT_CPG)
+#define FIQ_RNG               (FIQ_START+INTERRUPT_RNG)
+#define FIQ_ARASANSDIO        (FIQ_START+INTERRUPT_ARASANSDIO)
+#define FIQ_AVSPMON           (FIQ_START+INTERRUPT_AVSPMON)
 
-#define FIQ_ARM_TIMER         INT_ARM_TIMER
-#define FIQ_ARM_MAILBOX       INT_ARM_MAILBOX
-#define FIQ_ARM_DOORBELL_0    INT_ARM_DOORBELL_0
-#define FIQ_ARM_DOORBELL_1    INT_ARM_DOORBELL_1
-#define FIQ_VPU0_HALTED       INT_VPU0_HALTED
-#define FIQ_VPU1_HALTED       INT_VPU1_HALTED
-#define FIQ_ILLEGAL_TYPE0     INT_ILLEGAL_TYPE0
-#define FIQ_ILLEGAL_TYPE1     INT_ILLEGAL_TYPE1
-#define FIQ_PENDING1          INT_PENDING1
-#define FIQ_PENDING2          INT_PENDING2
+#define FIQ_ARM_TIMER         (FIQ_START+INTERRUPT_ARM_TIMER)
+#define FIQ_ARM_MAILBOX       (FIQ_START+INTERRUPT_ARM_MAILBOX)
+#define FIQ_ARM_DOORBELL_0    (FIQ_START+INTERRUPT_ARM_DOORBELL_0)
+#define FIQ_ARM_DOORBELL_1    (FIQ_START+INTERRUPT_ARM_DOORBELL_1)
+#define FIQ_VPU0_HALTED       (FIQ_START+INTERRUPT_VPU0_HALTED)
+#define FIQ_VPU1_HALTED       (FIQ_START+INTERRUPT_VPU1_HALTED)
+#define FIQ_ILLEGAL_TYPE0     (FIQ_START+INTERRUPT_ILLEGAL_TYPE0)
+#define FIQ_ILLEGAL_TYPE1     (FIQ_START+INTERRUPT_ILLEGAL_TYPE1)
+#define FIQ_PENDING1          (FIQ_START+INTERRUPT_PENDING1)
+#define FIQ_PENDING2          (FIQ_START+INTERRUPT_PENDING2)
 
-#define HARD_IRQS	      (64 + 21)
-#define GPIO_IRQ_START	      HARD_IRQS
+#define GPIO_IRQ_START	      (HARD_IRQS + FIQ_IRQS)
 
-#define GPIO_IRQS	      32*5
+#define HARD_IRQS	      (64 + 21)
+#define FIQ_IRQS              (64 + 21)
+#define GPIO_IRQS	      (32*5)
 
-#define NR_IRQS		      HARD_IRQS+GPIO_IRQS
+#define NR_IRQS		      HARD_IRQS+FIQ_IRQS+GPIO_IRQS
 
 
 #endif /* _BCM2708_IRQS_H_ */
diff --git a/arch/arm/mach-bcm2708/include/mach/platform.h b/arch/arm/mach-bcm2708/include/mach/platform.h
index 4d3c15d..89e72d1 100644
--- a/arch/arm/mach-bcm2708/include/mach/platform.h
+++ b/arch/arm/mach-bcm2708/include/mach/platform.h
@@ -56,7 +56,9 @@
  */
 
 #define BCM2708_PERI_BASE        0x20000000
+#define IC0_BASE                 (BCM2708_PERI_BASE + 0x2000)
 #define ST_BASE                  (BCM2708_PERI_BASE + 0x3000)   /* System Timer */
+#define MPHI_BASE		 (BCM2708_PERI_BASE + 0x6000)	/* Message -based Parallel Host Interface */
 #define DMA_BASE		 (BCM2708_PERI_BASE + 0x7000)	/* DMA controller */
 #define ARM_BASE                 (BCM2708_PERI_BASE + 0xB000)	 /* BCM2708 ARM control block */
 #define PM_BASE			 (BCM2708_PERI_BASE + 0x100000) /* Power Management, Reset controller and Watchdog registers */
diff --git a/drivers/usb/host/dwc_otg/Makefile b/drivers/usb/host/dwc_otg/Makefile
index c4c6e4e..6bd6a2e 100644
--- a/drivers/usb/host/dwc_otg/Makefile
+++ b/drivers/usb/host/dwc_otg/Makefile
@@ -36,6 +36,7 @@ dwc_otg-objs	+= dwc_otg_cil.o dwc_otg_cil_intr.o
 dwc_otg-objs	+= dwc_otg_pcd_linux.o dwc_otg_pcd.o dwc_otg_pcd_intr.o 
 dwc_otg-objs	+= dwc_otg_hcd.o dwc_otg_hcd_linux.o dwc_otg_hcd_intr.o dwc_otg_hcd_queue.o dwc_otg_hcd_ddma.o
 dwc_otg-objs	+= dwc_otg_adp.o
+dwc_otg-objs	+= dwc_otg_mphi_fix.o
 ifneq ($(CFI),)
 dwc_otg-objs	+= dwc_otg_cfi.o
 endif
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_cil_intr.c b/drivers/usb/host/dwc_otg/dwc_otg_cil_intr.c
index 21804c4..b861b55 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_cil_intr.c
+++ b/drivers/usb/host/dwc_otg/dwc_otg_cil_intr.c
@@ -45,6 +45,9 @@
 #include "dwc_otg_driver.h"
 #include "dwc_otg_pcd.h"
 #include "dwc_otg_hcd.h"
+#include "dwc_otg_mphi_fix.h"
+
+extern bool fiq_fix_enable;
 
 #ifdef DEBUG
 inline const char *op_state_str(dwc_otg_core_if_t * core_if)
@@ -1351,10 +1354,15 @@ static inline uint32_t dwc_otg_read_common_intr(dwc_otg_core_if_t * core_if)
 			    gintsts.d32, gintmsk.d32);
 	}
 #endif
-	if (gahbcfg.b.glblintrmsk)	
+	if (!fiq_fix_enable){
+		if (gahbcfg.b.glblintrmsk)
+			return ((gintsts.d32 & gintmsk.d32) & gintmsk_common.d32);
+		else
+			return 0;
+	}
+	else {
 		return ((gintsts.d32 & gintmsk.d32) & gintmsk_common.d32);
-	else
-		return 0;
+	}
 
 }
 
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_dbg.h b/drivers/usb/host/dwc_otg/dwc_otg_dbg.h
index 8681aa9..2ff1532 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_dbg.h
+++ b/drivers/usb/host/dwc_otg/dwc_otg_dbg.h
@@ -49,6 +49,7 @@ static inline uint32_t SET_DEBUG_LEVEL(const uint32_t new)
 	return old;
 }
 
+#define DBG_USER	(0x1)
 /** When debug level has the DBG_CIL bit set, display CIL Debug messages. */
 #define DBG_CIL		(0x2)
 /** When debug level has the DBG_CILV bit set, display CIL Verbose debug
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_driver.c b/drivers/usb/host/dwc_otg/dwc_otg_driver.c
index e7f99e1..3ac720b 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_driver.c
+++ b/drivers/usb/host/dwc_otg/dwc_otg_driver.c
@@ -64,6 +64,8 @@ bool microframe_schedule=true;
 
 static const char dwc_driver_name[] = "dwc_otg";
 
+extern void* dummy_send;
+
 extern int pcd_init(
 #ifdef LM_INTERFACE
 			   struct lm_device *_dev
@@ -238,6 +240,10 @@ static struct dwc_otg_driver_module_params dwc_otg_module_params = {
 	.adp_enable = -1,
 };
 
+//Global variable to switch the fiq fix on or off (declared in bcm2708.c)
+extern bool fiq_fix_enable;
+
+
 /**
  * This function shows the Driver Version.
  */
@@ -779,17 +785,33 @@ static int dwc_otg_driver_probe(
                     _dev->resource->start,
                     _dev->resource->end - _dev->resource->start + 1);
 #if 1
-        if (!request_mem_region(_dev->resource->start,
-                                _dev->resource->end - _dev->resource->start + 1,
+        if (!request_mem_region(_dev->resource[0].start,
+                                _dev->resource[0].end - _dev->resource[0].start + 1,
                                 "dwc_otg")) {
           dev_dbg(&_dev->dev, "error reserving mapped memory\n");
           retval = -EFAULT;
           goto fail;
         }
 
-	dwc_otg_device->os_dep.base = ioremap_nocache(_dev->resource->start,
-                                                      _dev->resource->end -
-                                                      _dev->resource->start+1);
+	dwc_otg_device->os_dep.base = ioremap_nocache(_dev->resource[0].start,
+                                                      _dev->resource[0].end -
+                                                      _dev->resource[0].start+1);
+	if (fiq_fix_enable)
+	{
+		if (!request_mem_region(_dev->resource[1].start,
+	                                _dev->resource[1].end - _dev->resource[1].start + 1,
+	                                "dwc_otg")) {
+	          dev_dbg(&_dev->dev, "error reserving mapped memory\n");
+	          retval = -EFAULT;
+	          goto fail;
+	}
+
+		dwc_otg_device->os_dep.mphi_base = ioremap_nocache(_dev->resource[1].start,
+							    _dev->resource[1].end -
+							    _dev->resource[1].start + 1);
+		dummy_send = (void *) kmalloc(16, GFP_ATOMIC);
+	}
+
 #else
         {
                 struct map_desc desc = {
@@ -1063,6 +1085,7 @@ static int __init dwc_otg_driver_init(void)
 		printk(KERN_ERR "%s retval=%d\n", __func__, retval);
 		return retval;
 	}
+	printk(KERN_DEBUG "dwc_otg: FIQ %s\n", fiq_fix_enable ? "enabled":"disabled");
 
 	error = driver_create_file(drv, &driver_attr_version);
 #ifdef DEBUG
@@ -1343,6 +1366,10 @@ MODULE_PARM_DESC(otg_ver, "OTG revision supported 0=OTG 1.3 1=OTG 2.0");
 module_param(microframe_schedule, bool, 0444);
 MODULE_PARM_DESC(microframe_schedule, "Enable the microframe scheduler");
 
+
+module_param(fiq_fix_enable, bool, 0444);
+MODULE_PARM_DESC(fiq_fix_enable, "Enable the fiq fix");
+
 /** @page "Module Parameters"
  *
  * The following parameters may be specified when starting the module.
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_hcd.c b/drivers/usb/host/dwc_otg/dwc_otg_hcd.c
index 434d0c4..20f989e 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_hcd.c
+++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd.c
@@ -53,6 +53,8 @@ static int last_sel_trans_num_avail_hc_at_start = 0;
 static int last_sel_trans_num_avail_hc_at_end = 0;
 #endif /* DEBUG_HOST_CHANNELS */
 
+extern int g_next_sched_frame, g_np_count, g_np_sent;
+
 dwc_otg_hcd_t *dwc_otg_hcd_alloc_hcd(void)
 {
 	return DWC_ALLOC(sizeof(dwc_otg_hcd_t));
@@ -407,6 +409,7 @@ static int dwc_otg_hcd_sleep_cb(void *p)
 }
 #endif
 
+
 /**
  * HCD Callback function for Remote Wakeup.
  *
@@ -1330,6 +1333,8 @@ dwc_otg_transaction_type_e dwc_otg_hcd_select_transactions(dwc_otg_hcd_t * hcd)
 				   &qh->qh_list_entry);
 		DWC_SPINUNLOCK_IRQRESTORE(channel_lock, flags);
 
+		g_np_sent++;
+
 		if (ret_val == DWC_OTG_TRANSACTION_NONE) {
 			ret_val = DWC_OTG_TRANSACTION_NON_PERIODIC;
 		} else {
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_hcd.h b/drivers/usb/host/dwc_otg/dwc_otg_hcd.h
index 8075595..dd30f47 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_hcd.h
+++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd.h
@@ -594,7 +594,7 @@ extern void dwc_otg_hcd_queue_transactions(dwc_otg_hcd_t * hcd,
 /** @name Interrupt Handler Functions */
 /** @{ */
 extern int32_t dwc_otg_hcd_handle_intr(dwc_otg_hcd_t * dwc_otg_hcd);
-extern int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * dwc_otg_hcd);
+extern int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * dwc_otg_hcd, int32_t);
 extern int32_t dwc_otg_hcd_handle_rx_status_q_level_intr(dwc_otg_hcd_t *
 							 dwc_otg_hcd);
 extern int32_t dwc_otg_hcd_handle_np_tx_fifo_empty_intr(dwc_otg_hcd_t *
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_hcd_if.h b/drivers/usb/host/dwc_otg/dwc_otg_hcd_if.h
index b3dc806..04ca4c2 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_if.h
+++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_if.h
@@ -113,6 +113,11 @@ extern void dwc_otg_hcd_remove(dwc_otg_hcd_t * hcd);
  */
 extern int32_t dwc_otg_hcd_handle_intr(dwc_otg_hcd_t * dwc_otg_hcd);
 
+/** This function is used to handle the fast interrupt
+ *
+ */
+extern void __attribute__ ((naked)) dwc_otg_hcd_handle_fiq(void);
+
 /**
  * Returns private data set by
  * dwc_otg_hcd_set_priv_data function.
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_hcd_intr.c b/drivers/usb/host/dwc_otg/dwc_otg_hcd_intr.c
index 63c1b55..f1658fa 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_intr.c
+++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_intr.c
@@ -34,6 +34,11 @@
 
 #include "dwc_otg_hcd.h"
 #include "dwc_otg_regs.h"
+#include "dwc_otg_mphi_fix.h"
+
+#include <linux/jiffies.h>
+#include <mach/hardware.h>
+
 
 extern bool microframe_schedule;
 
@@ -41,36 +46,105 @@ extern bool microframe_schedule;
  * This file contains the implementation of the HCD Interrupt handlers.
  */
 
+/*
+ * Some globals to communicate between the FIQ and INTERRUPT
+ */
+
+void * dummy_send;
+mphi_regs_t c_mphi_regs;
+int fiq_done, int_done;
+int g_next_sched_frame, g_np_count, g_np_sent, g_work_expected;
+static int mphi_int_count = 0 ;
+
+extern bool fiq_fix_enable;
+
+void __attribute__ ((naked)) dwc_otg_hcd_handle_fiq(void)
+{
+	gintsts_data_t gintsts;
+	hfnum_data_t hfnum;
+
+	/* entry takes care to store registers we will be treading on here */
+	asm __volatile__ (
+		"mov     ip, sp ;"
+		/* stash FIQ and normal regs */
+		"stmdb	sp!, {r0-r12,  lr};"
+		/* !! THIS SETS THE FRAME, adjust to > sizeof locals */
+		"sub     fp, ip, #256 ;"
+		);
+
+	fiq_done++;
+	gintsts.d32 = FIQ_READ_IO_ADDRESS(USB_BASE + 0x14) & FIQ_READ_IO_ADDRESS(USB_BASE + 0x18);
+	hfnum.d32 =   FIQ_READ_IO_ADDRESS(USB_BASE + 0x408);
+
+	if(gintsts.d32)
+	{
+		if(gintsts.b.sofintr && g_np_count == g_np_sent && dwc_frame_num_gt(g_next_sched_frame, hfnum.b.frnum))
+		{
+			/*
+			 * If np_count != np_sent that means we need to queue non-periodic (bulk) packets this packet
+			 * g_next_sched_frame is the next frame we have periodic packets for
+			 *
+			 * if neither of these are required for this frame then just clear the interrupt
+			 */
+			gintsts.d32 = 0;
+			gintsts.b.sofintr = 1;
+			FIQ_WRITE_IO_ADDRESS((USB_BASE + 0x14), gintsts.d32);
+
+			g_work_expected = 0;
+		}
+		else
+		{
+			g_work_expected = 1;
+			/* To enable the MPHI interrupt  (INT 32)
+			 */
+			FIQ_WRITE(  c_mphi_regs.outdda, (int) dummy_send);
+			FIQ_WRITE( c_mphi_regs.outddb, (1 << 29));
+
+			mphi_int_count++;
+			/* Clear the USB global interrupt so we don't just sit in the FIQ */
+			FIQ_MODIFY_IO_ADDRESS((USB_BASE + 0x8),1,0);
+
+		}
+	}
+	mb();
+
+	/* exit back to normal mode restoring everything */
+	asm __volatile__ (
+		/* return FIQ regs back to pristine state
+		 * and get normal regs back
+		 */
+		"ldmia	sp!, {r0-r12, lr};"
+
+		/* return */
+		"subs	pc, lr, #4;"
+	);
+}
+
 /** This function handles interrupts for the HCD. */
 int32_t dwc_otg_hcd_handle_intr(dwc_otg_hcd_t * dwc_otg_hcd)
 {
 	int retval = 0;
+	static int last_time;
 
 	dwc_otg_core_if_t *core_if = dwc_otg_hcd->core_if;
 	gintsts_data_t gintsts;
+	hfnum_data_t hfnum;
+
 #ifdef DEBUG
 	dwc_otg_core_global_regs_t *global_regs = core_if->core_global_regs;
 
-        //GRAYG: debugging
-        if (NULL == global_regs) {
-                DWC_DEBUGPL(DBG_HCD, "**** NULL regs: dwc_otg_hcd=%p "
-                            "core_if=%p\n",
-                            dwc_otg_hcd, global_regs);
-                return retval;
-        }
 #endif
 
 	/* Exit from ISR if core is hibernated */
 	if (core_if->hibernation_suspend == 1) {
-		return retval;
+		goto exit_handler_routine;
 	}
 	DWC_SPINLOCK(dwc_otg_hcd->lock);
 	/* Check if HOST Mode */
 	if (dwc_otg_is_host_mode(core_if)) {
 		gintsts.d32 = dwc_otg_read_core_intr(core_if);
 		if (!gintsts.d32) {
-			DWC_SPINUNLOCK(dwc_otg_hcd->lock);
-			return 0;
+			goto exit_handler_routine;
 		}
 #ifdef DEBUG
 		/* Don't print debug message in the interrupt handler on SOF */
@@ -88,9 +162,14 @@ int32_t dwc_otg_hcd_handle_intr(dwc_otg_hcd_t * dwc_otg_hcd)
 				    "DWC OTG HCD Interrupt Detected gintsts&gintmsk=0x%08x core_if=%p\n",
 				    gintsts.d32, core_if);
 #endif
-
-		if (gintsts.b.sofintr) {
-			retval |= dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd);
+		hfnum.d32 = DWC_READ_REG32(&dwc_otg_hcd->core_if->host_if->host_global_regs->hfnum);
+		if (gintsts.b.sofintr && g_np_count == g_np_sent && dwc_frame_num_gt(g_next_sched_frame, hfnum.b.frnum))
+		{
+			/* Note, we should never get here if the FIQ is doing it's job properly*/
+			retval |= dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd, g_work_expected);
+		}
+		else if (gintsts.b.sofintr) {
+			retval |= dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd, g_work_expected);
 		}
 		if (gintsts.b.rxstsqlvl) {
 			retval |=
@@ -138,11 +217,37 @@ int32_t dwc_otg_hcd_handle_intr(dwc_otg_hcd_t * dwc_otg_hcd)
 #endif
 
 	}
+
+exit_handler_routine:
+
+	if (fiq_fix_enable)
+	{
+		/* Clear the MPHI interrupt */
+		DWC_WRITE_REG32(c_mphi_regs.intstat, (1<<16));
+		if (mphi_int_count >= 60)
+		{
+			DWC_WRITE_REG32(c_mphi_regs.ctrl, ((1<<31) + (1<<16)));
+			DWC_WRITE_REG32(c_mphi_regs.ctrl, (1<<31));
+			mphi_int_count = 0;
+		}
+			int_done++;
+		if((jiffies / HZ) > last_time)
+		{
+			/* Once a second output the fiq and irq numbers, useful for debug */
+			last_time = jiffies / HZ;
+			DWC_DEBUGPL(DBG_USER, "int_done = %d fiq_done = %d\n", int_done, fiq_done);
+		}
+
+		/* Re-Enable FIQ interrupt from USB peripheral */
+		DWC_MODIFY_REG32((uint32_t *)IO_ADDRESS(USB_BASE + 0x8), 0 , 1);
+	}
+
 	DWC_SPINUNLOCK(dwc_otg_hcd->lock);
 	return retval;
 }
 
 #ifdef DWC_TRACK_MISSED_SOFS
+
 #warning Compiling code to track missed SOFs
 #define FRAME_NUM_ARRAY_SIZE 1000
 /**
@@ -182,13 +287,15 @@ static inline void track_missed_sofs(uint16_t curr_frame_number)
  * (micro)frame. Periodic transactions may be queued to the controller for the
  * next (micro)frame.
  */
-int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * hcd)
+int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * hcd, int32_t work_expected)
 {
 	hfnum_data_t hfnum;
 	dwc_list_link_t *qh_entry;
 	dwc_otg_qh_t *qh;
 	dwc_otg_transaction_type_e tr_type;
 	gintsts_data_t gintsts = {.d32 = 0 };
+	int did_something = 0;
+	int32_t next_sched_frame = -1;
 
 	hfnum.d32 =
 	    DWC_READ_REG32(&hcd->core_if->host_if->host_global_regs->hfnum);
@@ -218,12 +325,30 @@ int32_t dwc_otg_hcd_handle_sof_intr(dwc_otg_hcd_t * hcd)
 			 */
 			DWC_LIST_MOVE_HEAD(&hcd->periodic_sched_ready,
 					   &qh->qh_list_entry);
+
+			did_something = 1;
+		}
+		else
+		{
+			if(next_sched_frame < 0 || dwc_frame_num_le(qh->sched_frame, next_sched_frame))
+			{
+				next_sched_frame = qh->sched_frame;
+			}
 		}
 	}
+
+	g_next_sched_frame = next_sched_frame;
+
 	tr_type = dwc_otg_hcd_select_transactions(hcd);
 	if (tr_type != DWC_OTG_TRANSACTION_NONE) {
 		dwc_otg_hcd_queue_transactions(hcd, tr_type);
+		did_something = 1;
 	}
+	if(work_expected && !did_something)
+		DWC_DEBUGPL(DBG_USER, "Nothing to do !! frame = %x, g_next_sched_frame = %x\n", (int) hfnum.b.frnum, g_next_sched_frame);
+	if(!work_expected && did_something)
+		DWC_DEBUGPL(DBG_USER, "Unexpected work done !! frame = %x, g_next_sched_frame = %x\n", (int) hfnum.b.frnum, g_next_sched_frame);
+
 
 	/* Clear interrupt */
 	gintsts.b.sofintr = 1;
@@ -2102,5 +2227,4 @@ int32_t dwc_otg_hcd_handle_hc_n_intr(dwc_otg_hcd_t * dwc_otg_hcd, uint32_t num)
 
 	return retval;
 }
-
 #endif /* DWC_DEVICE_ONLY */
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c b/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
index 4a985a6..9702f81 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
+++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
@@ -1,3 +1,4 @@
+
 /* ==========================================================================
  * $File: //dwh/usb_iip/dev/software/otg/linux/drivers/dwc_otg_hcd_linux.c $
  * $Revision: #20 $
@@ -50,6 +51,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/version.h>
 #include <asm/io.h>
+#include <asm/fiq.h>
 #include <linux/usb.h>
 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
 #include <../drivers/usb/core/hcd.h>
@@ -67,6 +69,8 @@
 #include "dwc_otg_dbg.h"
 #include "dwc_otg_driver.h"
 #include "dwc_otg_hcd.h"
+#include "dwc_otg_mphi_fix.h"
+
 /**
  * Gets the endpoint number from a _bEndpointAddress argument. The endpoint is
  * qualified with its direction (possible 32 endpoints per device).
@@ -76,6 +80,8 @@
 
 static const char dwc_otg_hcd_name[] = "dwc_otg_hcd";
 
+extern bool fiq_fix_enable;
+
 /** @name Linux HC Driver API Functions */
 /** @{ */
 /* manage i/o requests, device state */
@@ -366,6 +372,12 @@ static struct dwc_otg_hcd_function_ops hcd_fops = {
 	.get_b_hnp_enable = _get_b_hnp_enable,
 };
 
+static struct fiq_handler fh = {
+  .name = "usb_fiq",
+};
+static uint8_t fiqStack[1024];
+
+extern mphi_regs_t c_mphi_regs;
 /**
  * Initializes the HCD. This function allocates memory for and initializes the
  * static parts of the usb_hcd and dwc_otg_hcd structures. It also registers the
@@ -379,6 +391,7 @@ int hcd_init(dwc_bus_dev_t *_dev)
 	dwc_otg_device_t *otg_dev = DWC_OTG_BUSDRVDATA(_dev);
 	int retval = 0;
         u64 dmamask;
+	struct pt_regs regs;
 
 	DWC_DEBUGPL(DBG_HCD, "DWC OTG HCD INIT otg_dev=%p\n", otg_dev);
 
@@ -396,6 +409,18 @@ int hcd_init(dwc_bus_dev_t *_dev)
         pci_set_consistent_dma_mask(_dev, dmamask);
 #endif
 
+	if (fiq_fix_enable)
+	{
+		// Set up fiq
+		claim_fiq(&fh);
+		set_fiq_handler(__FIQ_Branch, 4);
+		memset(&regs,0,sizeof(regs));
+		regs.ARM_r8 = (long)dwc_otg_hcd_handle_fiq;
+		regs.ARM_r9 = (long)0;
+		regs.ARM_sp = (long)fiqStack + sizeof(fiqStack) - 4;
+		set_fiq_regs(&regs);
+		}
+
 	/*
 	 * Allocate memory for the base HCD plus the DWC OTG HCD.
 	 * Initialize the base HCD.
@@ -415,6 +440,26 @@ int hcd_init(dwc_bus_dev_t *_dev)
 
 	hcd->regs = otg_dev->os_dep.base;
 
+	if (fiq_fix_enable)
+	{
+		//Set the mphi periph to  the required registers
+		c_mphi_regs.base    = otg_dev->os_dep.mphi_base;
+		c_mphi_regs.ctrl    = otg_dev->os_dep.mphi_base + 0x4c;
+		c_mphi_regs.outdda  = otg_dev->os_dep.mphi_base + 0x28;
+		c_mphi_regs.outddb  = otg_dev->os_dep.mphi_base + 0x2c;
+		c_mphi_regs.intstat = otg_dev->os_dep.mphi_base + 0x50;
+
+		//Enable mphi peripheral
+		writel((1<<31),c_mphi_regs.ctrl);
+#ifdef DEBUG
+		if (readl(c_mphi_regs.ctrl) & 0x80000000)
+			DWC_DEBUGPL(DBG_USER, "MPHI periph has been enabled\n");
+		else
+			DWC_DEBUGPL(DBG_USER, "MPHI periph has NOT been enabled\n");
+#endif
+		// Enable FIQ interrupt from USB peripheral
+		enable_fiq(INTERRUPT_VC_USB);
+	}
 	/* Initialize the DWC OTG HCD. */
 	dwc_otg_hcd = dwc_otg_hcd_alloc_hcd();
 	if (!dwc_otg_hcd) {
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_hcd_queue.c b/drivers/usb/host/dwc_otg/dwc_otg_hcd_queue.c
index 08c1669..ac10323 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_queue.c
+++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_queue.c
@@ -572,6 +572,9 @@ static int check_max_xfer_size(dwc_otg_hcd_t * hcd, dwc_otg_qh_t * qh)
 	return status;
 }
 
+
+extern int g_next_sched_frame, g_np_count, g_np_sent;
+
 /**
  * Schedules an interrupt or isochronous transfer in the periodic schedule.
  *
@@ -630,8 +633,13 @@ static int schedule_periodic(dwc_otg_hcd_t * hcd, dwc_otg_qh_t * qh)
 		DWC_LIST_INSERT_TAIL(&hcd->periodic_sched_ready, &qh->qh_list_entry);
 	}
 	else {
-	/* Always start in the inactive schedule. */
-	DWC_LIST_INSERT_TAIL(&hcd->periodic_sched_inactive, &qh->qh_list_entry);
+		if(DWC_LIST_EMPTY(&hcd->periodic_sched_inactive) || dwc_frame_num_le(qh->sched_frame, g_next_sched_frame))
+		{
+			g_next_sched_frame = qh->sched_frame;
+
+		}
+		/* Always start in the inactive schedule. */
+		DWC_LIST_INSERT_TAIL(&hcd->periodic_sched_inactive, &qh->qh_list_entry);
 	}
 
 	if (!microframe_schedule) {
@@ -645,6 +653,7 @@ static int schedule_periodic(dwc_otg_hcd_t * hcd, dwc_otg_qh_t * qh)
 	return status;
 }
 
+
 /**
  * This function adds a QH to either the non periodic or periodic schedule if
  * it is not already in the schedule. If the QH is already in the schedule, no
@@ -667,6 +676,7 @@ int dwc_otg_hcd_qh_add(dwc_otg_hcd_t * hcd, dwc_otg_qh_t * qh)
 		/* Always start in the inactive schedule. */
 		DWC_LIST_INSERT_TAIL(&hcd->non_periodic_sched_inactive,
 				     &qh->qh_list_entry);
+		g_np_count++;
 	} else {
 		status = schedule_periodic(hcd, qh);
 		if ( !hcd->periodic_qh_count ) {
@@ -767,6 +777,7 @@ void dwc_otg_hcd_qh_deactivate(dwc_otg_hcd_t * hcd, dwc_otg_qh_t * qh,
 			if (sched_next_periodic_split) {
 
 				qh->sched_frame = frame_number;
+
 				if (dwc_frame_num_le(frame_number,
 						     dwc_frame_num_inc
 						     (qh->start_split_frame,
@@ -815,6 +826,11 @@ void dwc_otg_hcd_qh_deactivate(dwc_otg_hcd_t * hcd, dwc_otg_qh_t * qh,
 				DWC_LIST_MOVE_HEAD(&hcd->periodic_sched_ready,
 						   &qh->qh_list_entry);
 			} else {
+				if(!dwc_frame_num_le(g_next_sched_frame, qh->sched_frame))
+				{
+					g_next_sched_frame = qh->sched_frame;
+				}
+
 				DWC_LIST_MOVE_HEAD
 				    (&hcd->periodic_sched_inactive,
 				     &qh->qh_list_entry);
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.c b/drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.c
new file mode 100755
index 0000000..b70ca68
--- /dev/null
+++ b/drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.c
@@ -0,0 +1,113 @@
+#include "dwc_otg_regs.h"
+#include "dwc_otg_dbg.h"
+
+void dwc_debug_print_core_int_reg(gintsts_data_t gintsts, const char* function_name)
+{
+	DWC_DEBUGPL(DBG_USER,   "*** Debugging from within the %s  function: ***\n"
+				"curmode:     %1i    Modemismatch: %1i    otgintr:    %1i    sofintr:    %1i\n"
+				"rxstsqlvl:   %1i    nptxfempty  : %1i    ginnakeff:  %1i    goutnakeff: %1i\n"
+				"ulpickint:   %1i    i2cintr:      %1i    erlysuspend:%1i    usbsuspend: %1i\n"
+				"usbreset:    %1i    enumdone:     %1i    isooutdrop: %1i    eopframe:   %1i\n"
+				"restoredone: %1i    epmismatch:   %1i    inepint:    %1i    outepintr:  %1i\n"
+				"incomplisoin:%1i    incomplisoout:%1i    fetsusp:    %1i    resetdet:   %1i\n"
+				"portintr:    %1i    hcintr:       %1i    ptxfempty:  %1i    lpmtranrcvd:%1i\n"
+				"conidstschng:%1i    disconnect:   %1i    sessreqintr:%1i    wkupintr:   %1i\n",
+				function_name,
+				gintsts.b.curmode,
+				gintsts.b.modemismatch,
+				gintsts.b.otgintr,
+				gintsts.b.sofintr,
+				gintsts.b.rxstsqlvl,
+				gintsts.b.nptxfempty,
+				gintsts.b.ginnakeff,
+				gintsts.b.goutnakeff,
+				gintsts.b.ulpickint,
+				gintsts.b.i2cintr,
+				gintsts.b.erlysuspend,
+				gintsts.b.usbsuspend,
+				gintsts.b.usbreset,
+				gintsts.b.enumdone,
+				gintsts.b.isooutdrop,
+				gintsts.b.eopframe,
+				gintsts.b.restoredone,
+				gintsts.b.epmismatch,
+				gintsts.b.inepint,
+				gintsts.b.outepintr,
+				gintsts.b.incomplisoin,
+				gintsts.b.incomplisoout,
+				gintsts.b.fetsusp,
+				gintsts.b.resetdet,
+				gintsts.b.portintr,
+				gintsts.b.hcintr,
+				gintsts.b.ptxfempty,
+				gintsts.b.lpmtranrcvd,
+				gintsts.b.conidstschng,
+				gintsts.b.disconnect,
+				gintsts.b.sessreqintr,
+				gintsts.b.wkupintr);
+	return;
+}
+
+void dwc_debug_core_int_mask(gintmsk_data_t gintmsk, const char* function_name)
+{
+	DWC_DEBUGPL(DBG_USER,	"Interrupt Mask status (called from %s) :\n"
+				"modemismatch: %1i     otgintr:    %1i    sofintr:    %1i    rxstsqlvl:   %1i\n"
+				"nptxfempty:   %1i     ginnakeff:  %1i    goutnakeff: %1i    ulpickint:   %1i\n"
+				"i2cintr:      %1i     erlysuspend:%1i    usbsuspend: %1i    usbreset:    %1i\n"
+				"enumdone:     %1i     isooutdrop: %1i    eopframe:   %1i    restoredone: %1i\n"
+				"epmismatch:   %1i     inepintr:   %1i    outepintr:  %1i    incomplisoin:%1i\n"
+				"incomplisoout:%1i     fetsusp:    %1i    resetdet:   %1i    portintr:    %1i\n"
+				"hcintr:       %1i     ptxfempty:  %1i    lpmtranrcvd:%1i    conidstschng:%1i\n"
+				"disconnect:   %1i     sessreqintr:%1i    wkupintr:   %1i\n",
+				function_name,
+				gintmsk.b.modemismatch,
+				gintmsk.b.otgintr,
+				gintmsk.b.sofintr,
+				gintmsk.b.rxstsqlvl,
+				gintmsk.b.nptxfempty,
+				gintmsk.b.ginnakeff,
+				gintmsk.b.goutnakeff,
+				gintmsk.b.ulpickint,
+				gintmsk.b.i2cintr,
+				gintmsk.b.erlysuspend,
+				gintmsk.b.usbsuspend,
+				gintmsk.b.usbreset,
+				gintmsk.b.enumdone,
+				gintmsk.b.isooutdrop,
+				gintmsk.b.eopframe,
+				gintmsk.b.restoredone,
+				gintmsk.b.epmismatch,
+				gintmsk.b.inepintr,
+				gintmsk.b.outepintr,
+				gintmsk.b.incomplisoin,
+				gintmsk.b.incomplisoout,
+				gintmsk.b.fetsusp,
+				gintmsk.b.resetdet,
+				gintmsk.b.portintr,
+				gintmsk.b.hcintr,
+				gintmsk.b.ptxfempty,
+				gintmsk.b.lpmtranrcvd,
+				gintmsk.b.conidstschng,
+				gintmsk.b.disconnect,
+				gintmsk.b.sessreqintr,
+				gintmsk.b.wkupintr);
+	return;
+}
+ 
+void dwc_debug_otg_int(gotgint_data_t gotgint, const char* function_name)
+{
+	DWC_DEBUGPL(DBG_USER,	"otg int register (from %s function):\n"
+				"sesenddet:%1i    sesreqsucstschung:%2i    hstnegsucstschng:%1i\n"
+				"hstnegdet:%1i    adevtoutchng:     %2i    debdone:         %1i\n"
+				"mvic:     %1i\n",
+				function_name,
+				gotgint.b.sesenddet,
+				gotgint.b.sesreqsucstschng,
+				gotgint.b.hstnegsucstschng,
+				gotgint.b.hstnegdet,
+				gotgint.b.adevtoutchng,
+				gotgint.b.debdone,
+				gotgint.b.mvic);
+
+	return;
+}
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.h b/drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.h
new file mode 100755
index 0000000..22f28e1
--- /dev/null
+++ b/drivers/usb/host/dwc_otg/dwc_otg_mphi_fix.h
@@ -0,0 +1,36 @@
+#ifndef __DWC_OTG_MPHI_FIX_H__
+#define __DWC_OTG_MPHI_FIX_H__
+
+#define FIQ_WRITE_IO_ADDRESS(_addr_,_data_) *(volatile uint32_t *) IO_ADDRESS(_addr_) = _data_
+#define FIQ_READ_IO_ADDRESS(_addr_) *(volatile uint32_t *) IO_ADDRESS(_addr_) 
+#define FIQ_MODIFY_IO_ADDRESS(_addr_,_clear_,_set_) FIQ_WRITE_IO_ADDRESS(_addr_ , (FIQ_READ_IO_ADDRESS(_addr_)&~_clear_)|_set_)
+#define FIQ_WRITE(_addr_,_data_) *(volatile uint32_t *) _addr_ = _data_
+
+typedef struct {
+	volatile void* base;
+	volatile void* ctrl;
+	volatile void* outdda;
+	volatile void* outddb;
+	volatile void* intstat;
+} mphi_regs_t; 
+
+void dwc_debug_print_core_int_reg(gintsts_data_t gintsts, const char* function_name);
+void dwc_debug_core_int_mask(gintsts_data_t gintmsk, const char* function_name);
+void dwc_debug_otg_int(gotgint_data_t gotgint, const char* function_name);
+
+
+
+#ifdef DEBUG
+#define DWC_DBG_PRINT_CORE_INT(_arg_) dwc_debug_print_core_int_reg(_arg_,__func__)
+#define DWC_DBG_PRINT_CORE_INT_MASK(_arg_) dwc_debug_core_int_mask(_arg_,__func__)
+#define DWC_DBG_PRINT_OTG_INT(_arg_) dwc_debug_otg_int(_arg_,__func__)
+
+#else
+#define DWC_DBG_PRINT_CORE_INT(_arg_)
+#define DWC_DBG_PRINT_CORE_INT_MASK(_arg_)
+#define DWC_DBG_PRINT_OTG_INT(_arg_)
+
+
+#endif
+
+#endif
diff --git a/drivers/usb/host/dwc_otg/dwc_otg_os_dep.h b/drivers/usb/host/dwc_otg/dwc_otg_os_dep.h
index a7e9076..bb1c42d 100644
--- a/drivers/usb/host/dwc_otg/dwc_otg_os_dep.h
+++ b/drivers/usb/host/dwc_otg/dwc_otg_os_dep.h
@@ -97,6 +97,9 @@ typedef struct os_dependent {
 	/** Register offset for Diagnostic API */
 	uint32_t reg_offset;
 
+	/** Base address for MPHI peripheral */
+	void *mphi_base;
+
 #ifdef LM_INTERFACE
 	struct lm_device *lmdev;
 #elif  defined(PCI_INTERFACE)
-- 
1.9.1