openwrtv4/target/linux/brcm2708/patches-3.18/0091-dwc_otg-FIQ-support-on-SMP.-Set-up-FIQ-stack-and-han.patch

From e9898a39fce7db84ae56329d4f90da92af3bd584 Mon Sep 17 00:00:00 2001
From: P33M <P33M@github.com>
Date: Wed, 24 Sep 2014 11:57:51 +0100
Subject: [PATCH 091/114] dwc_otg: FIQ support on SMP. Set up FIQ stack and
 handler on Core 0 only.

---
 drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c | 96 ++++++++++++++--------------
 1 file changed, 49 insertions(+), 47 deletions(-)

--- a/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
+++ b/drivers/usb/host/dwc_otg/dwc_otg_hcd_linux.c
@@ -397,7 +397,55 @@ static struct fiq_handler fh = {
   .name = "usb_fiq",
 };
 
+static void hcd_init_fiq(void *cookie)
+{
+	dwc_otg_device_t *otg_dev = cookie;
+	dwc_otg_hcd_t *dwc_otg_hcd = otg_dev->hcd;
+	struct pt_regs regs;
+
+	if (claim_fiq(&fh)) {
+		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 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));
+
+	regs.ARM_r8 = (long) dwc_otg_hcd->fiq_state;
+	if (fiq_fsm_enable) {
+		regs.ARM_r9 = dwc_otg_hcd->core_if->core_params->host_channels;
+		//regs.ARM_r10 = dwc_otg_hcd->dma;
+		regs.ARM_fp = (long) dwc_otg_fiq_fsm;
+	} else {
+		regs.ARM_fp = (long) dwc_otg_fiq_nop;
+	}
+
+	regs.ARM_sp = (long) dwc_otg_hcd->fiq_stack + (sizeof(struct fiq_stack) - 4);
+
+//		__show_regs(&regs);
+	set_fiq_regs(&regs);
 
+	//Set the mphi periph to  the required registers
+	dwc_otg_hcd->fiq_state->mphi_regs.base    = otg_dev->os_dep.mphi_base;
+	dwc_otg_hcd->fiq_state->mphi_regs.ctrl    = otg_dev->os_dep.mphi_base + 0x4c;
+	dwc_otg_hcd->fiq_state->mphi_regs.outdda  = otg_dev->os_dep.mphi_base + 0x28;
+	dwc_otg_hcd->fiq_state->mphi_regs.outddb  = otg_dev->os_dep.mphi_base + 0x2c;
+	dwc_otg_hcd->fiq_state->mphi_regs.intstat = otg_dev->os_dep.mphi_base + 0x50;
+	dwc_otg_hcd->fiq_state->dwc_regs_base = otg_dev->os_dep.base;
+	DWC_WARN("MPHI regs_base at 0x%08x", (int)dwc_otg_hcd->fiq_state->mphi_regs.base);
+	//Enable mphi peripheral
+	writel((1<<31),dwc_otg_hcd->fiq_state->mphi_regs.ctrl);
+#ifdef DEBUG
+	if (readl(dwc_otg_hcd->fiq_state->mphi_regs.ctrl) & 0x80000000)
+		DWC_WARN("MPHI periph has been enabled");
+	else
+		DWC_WARN("MPHI periph has NOT been enabled");
+#endif
+	// Enable FIQ interrupt from USB peripheral
+	enable_fiq(INTERRUPT_VC_USB);
+	local_fiq_enable();
+}
 
 /**
  * Initializes the HCD. This function allocates memory for and initializes the
@@ -412,7 +460,6 @@ 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);
 
@@ -464,52 +511,7 @@ int hcd_init(dwc_bus_dev_t *_dev)
 	}
 
 	if (fiq_enable)
-	{
-		if (claim_fiq(&fh)) {
-			DWC_ERROR("Can't claim FIQ");
-			goto error2;
-		}
-
-		DWC_WARN("FIQ at 0x%08x", (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));
-
-		regs.ARM_r8 = (long) dwc_otg_hcd->fiq_state;
-		if (fiq_fsm_enable) {
-			regs.ARM_r9 = dwc_otg_hcd->core_if->core_params->host_channels;
-			//regs.ARM_r10 = dwc_otg_hcd->dma;
-			regs.ARM_fp = (long) dwc_otg_fiq_fsm;
-		} else {
-			regs.ARM_fp = (long) dwc_otg_fiq_nop;
-		}
-
-		regs.ARM_sp = (long) dwc_otg_hcd->fiq_stack + (sizeof(struct fiq_stack) - 4);
-
-//		__show_regs(&regs);
-		set_fiq_regs(&regs);
-
-		//Set the mphi periph to  the required registers
-		dwc_otg_hcd->fiq_state->mphi_regs.base    = otg_dev->os_dep.mphi_base;
-		dwc_otg_hcd->fiq_state->mphi_regs.ctrl    = otg_dev->os_dep.mphi_base + 0x4c;
-		dwc_otg_hcd->fiq_state->mphi_regs.outdda  = otg_dev->os_dep.mphi_base + 0x28;
-		dwc_otg_hcd->fiq_state->mphi_regs.outddb  = otg_dev->os_dep.mphi_base + 0x2c;
-		dwc_otg_hcd->fiq_state->mphi_regs.intstat = otg_dev->os_dep.mphi_base + 0x50;
-		dwc_otg_hcd->fiq_state->dwc_regs_base = otg_dev->os_dep.base;
-		DWC_WARN("MPHI regs_base at 0x%08x", (int)dwc_otg_hcd->fiq_state->mphi_regs.base);
-		//Enable mphi peripheral
-		writel((1<<31),dwc_otg_hcd->fiq_state->mphi_regs.ctrl);
-#ifdef DEBUG
-		if (readl(dwc_otg_hcd->fiq_state->mphi_regs.ctrl) & 0x80000000)
-			DWC_WARN("MPHI periph has been enabled");
-		else
-			DWC_WARN("MPHI periph has NOT been enabled");
-#endif
-		// Enable FIQ interrupt from USB peripheral
-		enable_fiq(INTERRUPT_VC_USB);
-		local_fiq_enable();
-	}
+		smp_call_function_single(0, hcd_init_fiq, otg_dev, 1);
 
 
 	otg_dev->hcd->otg_dev = otg_dev;