MXC: Lets handle IRQ by priority, defined with exported API function

Signed-off-by: NDarius Augulis <augulis.darius@gmail.com>
Signed-off-by: NSascha Hauer <s.hauer@pengutronix.de>

arch/arm/plat-mxc/Kconfig

@@ -23,4 +23,15 @@ source "arch/arm/mach-mx3/Kconfig"
 
 endmenu
 
+config MXC_IRQ_PRIOR
+	bool "Use IRQ priority"
+	depends on ARCH_MXC
+	help
+	  Select this if you want to use prioritized IRQ handling.
+	  This feature prevents higher priority ISR to be interrupted
+	  by lower priority IRQ even IRQF_DISABLED flag is not set.
+	  This may be useful in embedded applications, where are strong
+	  requirements for timing.
+	  Say N here, unless you have a specialized requirement.
+
 endif

arch/arm/plat-mxc/include/mach/entry-macro.S

@@ -9,11 +9,17 @@
  * published by the Free Software Foundation.
  */
 
+#define AVIC_NIMASK	0x04
+
 	@ this macro disables fast irq (not implemented)
 	.macro	disable_fiq
 	.endm
 
 	.macro  get_irqnr_preamble, base, tmp
+	ldr	\base, =AVIC_IO_ADDRESS(AVIC_BASE_ADDR)
+#ifdef CONFIG_MXC_IRQ_PRIOR
+	ldr	r4, [\base, #AVIC_NIMASK]
+#endif
 	.endm
 
 	.macro  arch_ret_to_user, tmp1, tmp2
@@ -23,7 +29,6 @@
 	@ and returns its number in irqnr
 	@ and returns if an interrupt occured in irqstat
 	.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
-	ldr	\base, =AVIC_IO_ADDRESS(AVIC_BASE_ADDR)
 	@ Load offset & priority of the highest priority
 	@ interrupt pending from AVIC_NIVECSR
 	ldr	\irqstat, [\base, #0x40]
@@ -32,6 +37,11 @@
 	mov	\irqnr, \irqstat, asr #16
 	@ set zero flag if IRQ + 1 == 0
 	adds	\tmp, \irqnr, #1
+#ifdef CONFIG_MXC_IRQ_PRIOR
+	bicne	\tmp, \irqstat, #0xFFFFFFE0
+	strne	\tmp, [\base, #AVIC_NIMASK]
+	streq	r4, [\base, #AVIC_NIMASK]
+#endif
 	.endm
 
 	@ irq priority table (not used)

arch/arm/plat-mxc/include/mach/irqs.h

@@ -12,5 +12,6 @@
 #define __ASM_ARCH_MXC_IRQS_H__
 
 #include <mach/hardware.h>
+extern void imx_irq_set_priority(unsigned char irq, unsigned char prio);
 
 #endif /* __ASM_ARCH_MXC_IRQS_H__ */

arch/arm/plat-mxc/irq.c

@@ -30,14 +30,7 @@
 #define AVIC_INTENABLEL		(AVIC_BASE + 0x14)	/* int enable reg low */
 #define AVIC_INTTYPEH		(AVIC_BASE + 0x18)	/* int type reg high */
 #define AVIC_INTTYPEL		(AVIC_BASE + 0x1C)	/* int type reg low */
-#define AVIC_NIPRIORITY7	(AVIC_BASE + 0x20)	/* norm int priority lvl7 */
-#define AVIC_NIPRIORITY6	(AVIC_BASE + 0x24)	/* norm int priority lvl6 */
-#define AVIC_NIPRIORITY5	(AVIC_BASE + 0x28)	/* norm int priority lvl5 */
-#define AVIC_NIPRIORITY4	(AVIC_BASE + 0x2C)	/* norm int priority lvl4 */
-#define AVIC_NIPRIORITY3	(AVIC_BASE + 0x30)	/* norm int priority lvl3 */
-#define AVIC_NIPRIORITY2	(AVIC_BASE + 0x34)	/* norm int priority lvl2 */
-#define AVIC_NIPRIORITY1	(AVIC_BASE + 0x38)	/* norm int priority lvl1 */
-#define AVIC_NIPRIORITY0	(AVIC_BASE + 0x3C)	/* norm int priority lvl0 */
+#define AVIC_NIPRIORITY(x)	(AVIC_BASE + (0x20 + 4 * (7 - (x)))) /* int priority */
 #define AVIC_NIVECSR		(AVIC_BASE + 0x40)	/* norm int vector/status */
 #define AVIC_FIVECSR		(AVIC_BASE + 0x44)	/* fast int vector/status */
 #define AVIC_INTSRCH		(AVIC_BASE + 0x48)	/* int source reg high */
@@ -54,6 +47,24 @@
 #define IIM_PROD_REV_SH		3
 #define IIM_PROD_REV_LEN	5
 
+#ifdef CONFIG_MXC_IRQ_PRIOR
+void imx_irq_set_priority(unsigned char irq, unsigned char prio)
+{
+	unsigned int temp;
+	unsigned int mask = 0x0F << irq % 8 * 4;
+
+	if (irq > 63)
+		return;
+
+	temp = __raw_readl(AVIC_NIPRIORITY(irq / 8));
+	temp &= ~mask;
+	temp |= prio & mask;
+
+	__raw_writel(temp, AVIC_NIPRIORITY(irq / 8));
+}
+EXPORT_SYMBOL(imx_irq_set_priority);
+#endif
+
 /* Disable interrupt number "irq" in the AVIC */
 static void mxc_mask_irq(unsigned int irq)
 {
@@ -101,10 +112,14 @@ void __init mxc_init_irq(void)
 		set_irq_flags(i, IRQF_VALID);
 	}
 
+	/* Set default priority value (0) for all IRQ's */
+	for (i = 0; i < 8; i++)
+		__raw_writel(0, AVIC_NIPRIORITY(i));
+
 	/* Set WDOG2's interrupt the highest priority level (bit 28-31) */
-	reg = __raw_readl(AVIC_NIPRIORITY6);
+	reg = __raw_readl(AVIC_NIPRIORITY(6));
 	reg |= (0xF << 28);
-	__raw_writel(reg, AVIC_NIPRIORITY6);
+	__raw_writel(reg, AVIC_NIPRIORITY(6));
 
 	/* init architectures chained interrupt handler */
 	mxc_register_gpios();