From a31e1ba443ff061c4bf4abbac46ca8e197da3f0c Mon Sep 17 00:00:00 2001
From: P33M <P33M@github.com>
Date: Thu, 22 Jan 2015 18:45:23 +0000
Subject: [PATCH 096/114] bcm2709/dwc_otg: Setup FIQ on core 1 if >1 core
 active

---
 arch/arm/mach-bcm2709/armctrl.c              | 10 +++++++++-
 drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c | 15 +++++++++++----
 2 files changed, 20 insertions(+), 5 deletions(-)

--- a/arch/arm/mach-bcm2709/armctrl.c
+++ b/arch/arm/mach-bcm2709/armctrl.c
@@ -89,7 +89,15 @@ static void armctrl_unmask_irq(struct ir
 	};
 	int i;
 	if (d->irq >= FIQ_START) {
-		unsigned int data = (unsigned int)irq_get_chip_data(d->irq) - FIQ_START;
+		unsigned int data;
+		if (num_online_cpus() > 1) {
+			data = readl(__io_address(ARM_LOCAL_GPU_INT_ROUTING));
+			data &= ~0xc;
+			data |= (1 << 2);
+			writel(data, __io_address(ARM_LOCAL_GPU_INT_ROUTING));
+		}
+		/* Unmask in ARMCTRL block after routing it properly */
+		data = (unsigned int)irq_get_chip_data(d->irq) - FIQ_START;
 		writel(0x80 | data, __io_address(ARM_IRQ_FAST));
 	} else if (d->irq >= IRQ_ARM_LOCAL_CNTPSIRQ && d->irq < IRQ_ARM_LOCAL_CNTPSIRQ + 4) {
 #if 1
--- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
+++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
@@ -407,7 +407,9 @@ static void hcd_init_fiq(void *cookie)
 		DWC_ERROR("Can't claim FIQ");
 		BUG();
 	}
-	DWC_WARN("FIQ at 0x%08x", (fiq_fsm_enable ? (int)&dwc_otg_fiq_fsm : (int)&dwc_otg_fiq_nop));
+	DWC_WARN("FIQ on core %d at 0x%08x",
+				smp_processor_id(),
+				(fiq_fsm_enable ? (int)&dwc_otg_fiq_fsm : (int)&dwc_otg_fiq_nop));
 	DWC_WARN("FIQ ASM at 0x%08x length %d", (int)&_dwc_otg_fiq_stub, (int)(&_dwc_otg_fiq_stub_end - &_dwc_otg_fiq_stub));
 		set_fiq_handler((void *) &_dwc_otg_fiq_stub, &_dwc_otg_fiq_stub_end - &_dwc_otg_fiq_stub);
 	memset(&regs,0,sizeof(regs));
@@ -510,9 +512,14 @@ int hcd_init(dwc_bus_dev_t *_dev)
 		goto error2;
 	}
 
-	if (fiq_enable)
-		smp_call_function_single(0, hcd_init_fiq, otg_dev, 1);
-
+	if (fiq_enable) {
+		if (num_online_cpus() > 1) {
+			/* bcm2709: can run the FIQ on a separate core to IRQs */
+			smp_call_function_single(1, hcd_init_fiq, otg_dev, 1);
+		} else {
+			smp_call_function_single(0, hcd_init_fiq, otg_dev, 1);
+		}
+	}
 
 	otg_dev->hcd->otg_dev = otg_dev;
 	hcd->self.otg_port = dwc_otg_hcd_otg_port(dwc_otg_hcd);
