[ARM] 4440/1: PXA: enable the checking of ICIP2 for IRQs

ICIP2 is not examined during IRQ entrance, this patch add the
checking if the processor is PXA27x or later, with CoreG bits
in CPUID (Core Generation) > 1

Signed-off-by: eric miao <eric.miao@marvell.com>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

diff --git a/include/asm-arm/arch-pxa/entry-macro.S b/include/asm-arm/arch-pxa/entry-macro.S
index b7bf0dd4fa4470b73b2b5edd50dbe02a3eacd9b2..b7e730851461d635f38eff192b9ab6011053f98f 100644 (file)
--- a/include/asm-arm/arch-pxa/entry-macro.S
+++ b/include/asm-arm/arch-pxa/entry-macro.S
@@ -20,15 +20,33 @@
                .endm
 
                .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
-#ifdef CONFIG_PXA27x
-               mrc     p6, 0, \irqstat, c0, c0, 0              @ ICIP
-               mrc     p6, 0, \irqnr, c1, c0, 0                @ ICMR
-#else
+               mrc     p15, 0, \tmp, c0, c0, 0         @ CPUID
+               mov     \tmp, \tmp, lsr #13
+               and     \tmp, \tmp, #0x7                @ Core G
+               cmp     \tmp, #1
+               bhi     1004f
+
                mov     \base, #io_p2v(0x40000000)      @ IIR Ctl = 0x40d00000
                add     \base, \base, #0x00d00000
                ldr     \irqstat, [\base, #0]           @ ICIP
                ldr     \irqnr, [\base, #4]             @ ICMR
-#endif
+               b       1002f
+
+1004:
+               mrc     p6, 0, \irqstat, c6, c0, 0      @ ICIP2
+               mrc     p6, 0, \irqnr, c7, c0, 0        @ ICMR2
+               ands    \irqstat, \irqstat, \irqnr
+               beq     1003f
+               rsb     \irqstat, \irqnr, #0
+               and     \irqstat, \irqstat, \irqnr
+               clz     \irqnr, \irqstat
+               rsb     \irqnr, \irqnr, #31
+               add     \irqnr, \irqnr, #32
+               b       1001f
+1003:
+               mrc     p6, 0, \irqstat, c0, c0, 0      @ ICIP
+               mrc     p6, 0, \irqnr, c1, c0, 0        @ ICMR
+1002:
                ands    \irqnr, \irqstat, \irqnr
                beq     1001f
                rsb     \irqstat, \irqnr, #0