ARM: imx: add support for MSCM interrupt router

This adds support for Vybrids interrupt router for the shared
peripherals. The interrupt router is between the peripherals
and the GIC (IRQ controller of the Cortex-A5 CPU) or NVIC (IRQ
controller of the Cortex-M4 CPU) respectively. Hence, for every
IRQ we enable, we need to make sure the interrupt router routes
the IRQ to the current CPU. This driver currently only support
MSCM on the Cortex-A5 (in conjunction with GIC). Also, most
U-Boot versions for Vybrid configure the MSCM to route all
interupts to Cortex-A5 by default.

5 files changed, 140 insertions, 0 deletions

diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
index dd765bc63abd..286c7ffd1006 100644
--- a/arch/arm/mach-imx/Kconfig
+++ b/arch/arm/mach-imx/Kconfig
@@ -58,6 +58,9 @@ config HAVE_IMX_SRC
 	def_bool y if SMP
 	select ARCH_HAS_RESET_CONTROLLER
 
+config HAVE_VF610_MSCM
+	bool
+
 config IMX_HAVE_IOMUX_V1
 	bool
 
@@ -631,6 +634,7 @@ config SOC_IMX6SX
 
 config SOC_VF610
 	bool "Vybrid Family VF610 support"
+	select HAVE_VF610_MSCM
 	select ARM_GIC
 	select PINCTRL_VF610
 	select HAVE_IMX_GPC
diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 2b17bb35d0bb..302fe835ecd0 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -109,6 +109,7 @@ obj-$(CONFIG_SOC_IMX50) += mach-imx50.o
 obj-$(CONFIG_SOC_IMX51) += mach-imx51.o
 obj-$(CONFIG_SOC_IMX53) += mach-imx53.o
 
+obj-$(CONFIG_HAVE_VF610_MSCM) += mscm-vf610.o
 obj-$(CONFIG_SOC_VF610) += clk-vf610.o mach-vf610.o
 
 obj-$(CONFIG_SOC_LS1021A) += mach-ls1021a.o
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index c0a536f821ea..58b59643f868 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -111,6 +111,7 @@ static inline void imx_smp_prepare(void) {}
 void imx_src_init(void);
 void imx6_gpc_init(void);
 void vf610_gpc_init(void);
+void vf610_mscm_init(void);
 void imx_gpc_pre_suspend(bool arm_power_off);
 void imx_gpc_post_resume(void);
 void imx_gpc_mask_all(void);
diff --git a/arch/arm/mach-imx/mach-vf610.c b/arch/arm/mach-imx/mach-vf610.c
index 4ba460263349..f9335384c6aa 100644
--- a/arch/arm/mach-imx/mach-vf610.c
+++ b/arch/arm/mach-imx/mach-vf610.c
@@ -16,6 +16,7 @@
 static void __init vf610_init_irq(void)
 {
 	vf610_gpc_init();
+	vf610_mscm_init();
 	irqchip_init();
 }
 
diff --git a/arch/arm/mach-imx/mscm-vf610.c b/arch/arm/mach-imx/mscm-vf610.c
new file mode 100644
index 000000000000..6dc45b52b811
--- /dev/null
+++ b/arch/arm/mach-imx/mscm-vf610.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright 2014 Stefan Agner
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/cpu_pm.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/irqchip/arm-gic.h>
+#include "common.h"
+
+#define MSCM_CPxNUM		0x4
+#define MSCM_IRSPRC(n)		(0x880 + 2 * (n))
+#define MSCM_IRSPRC_CPEN_MASK	0x3
+
+#define MSCM_IRSPRC_NUM		112
+
+#define MSCM_IRQ_OFFSET		32
+
+static void __iomem *mscm_base;
+static u16 mscm_saved_irsprc[MSCM_IRSPRC_NUM];
+static u16 cpu_id;
+
+static int vf610_mscm_notifier(struct notifier_block *self, unsigned long cmd,
+			       void *v)
+{
+	int i;
+
+	/* Only the primary (boot CPU) should do suspend/resume */
+	if (cpu_id > 0)
+		return NOTIFY_OK;
+
+	switch (cmd) {
+	case CPU_CLUSTER_PM_ENTER:
+		for (i = 0; i < MSCM_IRSPRC_NUM; i++)
+			mscm_saved_irsprc[i] =
+				readw_relaxed(mscm_base + MSCM_IRSPRC(i));
+		break;
+	case CPU_CLUSTER_PM_ENTER_FAILED:
+	case CPU_CLUSTER_PM_EXIT:
+		for (i = 0; i < MSCM_IRSPRC_NUM; i++)
+			writew_relaxed(mscm_saved_irsprc[i],
+				       mscm_base + MSCM_IRSPRC(i));
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block mscm_notifier_block = {
+	.notifier_call = vf610_mscm_notifier,
+};
+
+static int vf610_mscm_domain_map(struct irq_domain *d, unsigned int irq,
+			       irq_hw_number_t hw)
+{
+	u16 irsprc;
+
+	/* Do not handle non Interrupt Router IRQs */
+	if (hw < MSCM_IRQ_OFFSET)
+		return 0;
+
+	hw -= MSCM_IRQ_OFFSET;
+	irsprc = readw_relaxed(mscm_base + MSCM_IRSPRC(hw));
+	irsprc &= MSCM_IRSPRC_CPEN_MASK;
+
+	/* Warn if interrupt is enabled on another CPU */
+	WARN_ON(irsprc & ~(0x1 << cpu_id));
+
+	writew_relaxed(0x1 << cpu_id, mscm_base + MSCM_IRSPRC(hw));
+
+	return 0;
+}
+
+static void vf610_mscm_domain_unmap(struct irq_domain *d, unsigned int irq)
+{
+	irq_hw_number_t hw = irq_get_irq_data(irq)->hwirq;
+	u16 irsprc;
+
+	/* Do not handle non Interrupt Router IRQs */
+	if (hw < MSCM_IRQ_OFFSET)
+		return;
+
+	hw -= MSCM_IRQ_OFFSET;
+	irsprc = readw_relaxed(mscm_base + MSCM_IRSPRC(hw));
+	irsprc &= MSCM_IRSPRC_CPEN_MASK;
+
+	writew_relaxed(0x1 << cpu_id, mscm_base + MSCM_IRSPRC(hw));
+}
+
+static int vf610_mscm_domain_xlate(struct irq_domain *d,
+				struct device_node *controller,
+				const u32 *intspec, unsigned int intsize,
+				unsigned long *out_hwirq,
+				unsigned int *out_type)
+{
+	*out_hwirq += 16;
+	return 0;
+}
+static const struct irq_domain_ops routable_irq_domain_ops = {
+	.map = vf610_mscm_domain_map,
+	.unmap = vf610_mscm_domain_unmap,
+	.xlate = vf610_mscm_domain_xlate,
+};
+
+void __init vf610_mscm_init(void)
+{
+	struct device_node *np;
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,vf610-mscm");
+	mscm_base = of_iomap(np, 0);
+
+	if (!mscm_base) {
+		WARN_ON(1);
+		return;
+	}
+
+	cpu_id = readl_relaxed(mscm_base + MSCM_CPxNUM);
+
+	/* Register MSCM as interrupt router */
+	register_routable_domain_ops(&routable_irq_domain_ops);
+
+	cpu_pm_register_notifier(&mscm_notifier_block);
+}