[ARM] 3829/1: iop3xx: optimise irq entry macros

Squeeze three instructions out of the iop32x irq demuxer, and nine
out of the iop33x irq demuxer by using the hardware vector generator.
Signed-off-by: Lennert Buytenhek <buytenh@wantstofly.org>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

arch/arm/mach-iop33x/irq.c

@@ -57,6 +57,20 @@ static inline void intstr_write1(u32 val)
 	iop3xx_cp6_disable();
 }
 
+static inline void intbase_write(u32 val)
+{
+	iop3xx_cp6_enable();
+	asm volatile("mcr p6, 0, %0, c12, c0, 0" : : "r" (val));
+	iop3xx_cp6_disable();
+}
+
+static inline void intsize_write(u32 val)
+{
+	iop3xx_cp6_enable();
+	asm volatile("mcr p6, 0, %0, c13, c0, 0" : : "r" (val));
+	iop3xx_cp6_disable();
+}
+
 static void
 iop331_irq_mask1 (unsigned int irq)
 {
@@ -107,6 +121,8 @@ void __init iop331_init_irq(void)
     	intctl_write1(0);
 	intstr_write0(0);		// treat all as IRQ
     	intstr_write1(0);
+	intbase_write(0);
+	intsize_write(1);
 	if(machine_is_iq80331()) 	// all interrupts are inputs to chip
 		*IOP3XX_PCIIRSR = 0x0f;
 

include/asm-arm/arch-iop32x/entry-macro.S

@@ -16,13 +16,9 @@
 		 * Note: only deal with normal interrupts, not FIQ
 		 */
 		.macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
-		mov     \irqnr, #0
 		ldr	\base, =IOP3XX_REG_ADDR(0x07D8)
 		ldr	\irqstat, [\base]		@ Read IINTSRC
-		cmp     \irqstat, #0
-		beq     1001f
-		clz     \irqnr, \irqstat
-		mov     \base, #31
-		subs    \irqnr,\base,\irqnr
-1001:
+		cmp	\irqstat, #0
+		clzne	\irqnr, \irqstat
+		rsbne	\irqnr, \irqnr, #31
 		.endm

include/asm-arm/arch-iop33x/entry-macro.S

@@ -12,23 +12,11 @@
 		.macro  disable_fiq
 		.endm
 
-		/*
-		 * Note: only deal with normal interrupts, not FIQ
-		 */
 		.macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
-		mov     \irqnr, #0
-		ldr	\base, =IOP3XX_REG_ADDR(0x7A0)
-		ldr	\irqstat, [\base]		@ Read IINTSRC0
-		cmp     \irqstat, #0
-		bne     1002f
-		ldr	\irqstat, [\base, #4]		@ Read IINTSRC1
+		ldr	\base, =IOP3XX_REG_ADDR(0x07C8)
+		ldr	\irqstat, [\base]		@ Read IINTVEC
 		cmp	\irqstat, #0
-		beq	1001f
-		clz	\irqnr, \irqstat
-		rsbs    \irqnr,\irqnr,#31   @ recommend by RMK
-		add	\irqnr,\irqnr,#IRQ_IOP331_XINT8
-		b	1001f
-1002:		clz     \irqnr, \irqstat
-		rsbs    \irqnr,\irqnr,#31   @ recommend by RMK
-1001:
+		ldreq	\irqstat, [\base]		@ erratum 63 workaround
+		adds	\irqnr, \irqstat, #1
+		movne	\irqnr, \irqstat, lsr #2
 		.endm