[ARM] pxa: initialize default interrupt priority and use ICHP for IRQ handling

Signed-off-by: Haojian Zhuang <haojian.zhuang@marvell.com>
Signed-off-by: Eric Miao <eric.y.miao@gmail.com>

arch/arm/mach-pxa/include/mach/entry-macro.S

@@ -24,34 +24,27 @@
 		mov	\tmp, \tmp, lsr #13
 		and	\tmp, \tmp, #0x7		@ Core G
 		cmp	\tmp, #1
-		bhi	1004f
+		bhi	1002f
 
+		@ Core Generation 1 (PXA25x)
 		mov	\base, #io_p2v(0x40000000)	@ IIR Ctl = 0x40d00000
 		add	\base, \base, #0x00d00000
 		ldr	\irqstat, [\base, #0]		@ ICIP
 		ldr	\irqnr, [\base, #4]		@ ICMR
-		b	1002f
 
-1004:
-		mrc	p6, 0, \irqstat, c6, c0, 0	@ ICIP2
-		mrc	p6, 0, \irqnr, c7, c0, 0	@ ICMR2
 		ands	\irqnr, \irqstat, \irqnr
-		beq	1003f
+		beq	1001f
 		rsb	\irqstat, \irqnr, #0
 		and	\irqstat, \irqstat, \irqnr
 		clz	\irqnr, \irqstat
-		rsb	\irqnr, \irqnr, #31
-		add	\irqnr, \irqnr, #(32 + PXA_IRQ(0))
+		rsb	\irqnr, \irqnr, #(31 + PXA_IRQ(0))
 		b	1001f
-1003:
-		mrc	p6, 0, \irqstat, c0, c0, 0	@ ICIP
-		mrc	p6, 0, \irqnr, c1, c0, 0	@ ICMR
 1002:
-		ands	\irqnr, \irqstat, \irqnr
+		@ Core Generation 2 (PXA27x) or Core Generation 3 (PXA3xx)
+		mrc	p6, 0, \irqstat, c5, c0, 0	@ ICHP
+		tst	\irqstat, #0x80000000
 		beq	1001f
-		rsb	\irqstat, \irqnr, #0
-		and	\irqstat, \irqstat, \irqnr
-		clz	\irqnr, \irqstat
-		rsb	\irqnr, \irqnr, #(31 + PXA_IRQ(0))
+		bic	\irqstat, \irqstat, #0x80000000
+		mov	\irqnr, \irqstat, lsr #16
 1001:
 		.endm

arch/arm/mach-pxa/irq.c

@@ -120,7 +120,7 @@ static void __init pxa_init_low_gpio_irq(set_wake_t fn)
 
 void __init pxa_init_irq(int irq_nr, set_wake_t fn)
 {
-	int irq;
+	int irq, i;
 
 	pxa_internal_irq_nr = irq_nr;
 
@@ -129,6 +129,12 @@ void __init pxa_init_irq(int irq_nr, set_wake_t fn)
 		_ICLR(irq) = 0;	/* all IRQs are IRQ, not FIQ */
 	}
 
+	/* initialize interrupt priority */
+	if (cpu_is_pxa27x() || cpu_is_pxa3xx()) {
+		for (i = 0; i < irq_nr; i++)
+			IPR(i) = i | (1 << 31);
+	}
+
 	/* only unmasked interrupts kick us out of idle */
 	ICCR = 1;