[PATCH 31 v2] LEON3: added IRQAMP support

[Daniel Hellstrom]

---
 c/src/lib/libbsp/sparc/leon3/amba/amba.c       |   16 ++++++++++++++++
 c/src/lib/libbsp/sparc/shared/include/ambapp.h |    7 ++++---
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/c/src/lib/libbsp/sparc/leon3/amba/amba.c b/c/src/lib/libbsp/sparc/leon3/amba/amba.c
index e334aec..84230c8 100644
--- a/c/src/lib/libbsp/sparc/leon3/amba/amba.c
+++ b/c/src/lib/libbsp/sparc/leon3/amba/amba.c
@@ -39,6 +39,7 @@ extern int scan_uarts(void);
 void amba_initialize(void)
 {
   int i;
+  int icsel;
   amba_apb_device dev;
 
   /* Scan the AMBA Plug&Play info at the default LEON3 area */
@@ -55,6 +56,21 @@ void amba_initialize(void)
   }
 
   LEON3_IrqCtrl_Regs = (volatile LEON3_IrqCtrl_Regs_Map *) dev.start;
+  if ((LEON3_IrqCtrl_Regs->ampctrl >> 28) > 0) {
+    /* IRQAMP Controller may appear as multiple IRQMP Controllers, the
+     * CPUs can be routed to separate IRQMP controllers. We find out which
+     * controller the running CPU is routed to by looking at the IRQCTRL
+     * Select Register for this CPU. It is assumed that in a SMP system all
+     * CPUs are routed to the same controller.
+     * The Nth IRQMP Controller is located at Nx4KByte offset.
+     */
+    icsel = LEON3_IrqCtrl_Regs->icsel[LEON3_Cpu_Index/8];
+    icsel = (icsel >> ((7 - (LEON3_Cpu_Index & 0x7)) * 4)) & 0xf;
+    LEON3_IrqCtrl_Regs += icsel;
+    LEON3_IrqCtrl_Regs->mask[LEON3_Cpu_Index] = 0;
+    LEON3_IrqCtrl_Regs->force[LEON3_Cpu_Index] = 0;
+    LEON3_IrqCtrl_Regs->iclear = 0xffffffff;
+  }
 
   /* Init Extended IRQ controller if available */
   leon3_ext_irq_init();
diff --git a/c/src/lib/libbsp/sparc/shared/include/ambapp.h b/c/src/lib/libbsp/sparc/shared/include/ambapp.h
index cc8e120..7ca2d13 100644
--- a/c/src/lib/libbsp/sparc/shared/include/ambapp.h
+++ b/c/src/lib/libbsp/sparc/shared/include/ambapp.h
@@ -262,9 +262,8 @@ typedef struct {
   volatile unsigned int notused01;
   volatile unsigned int notused02;
   volatile unsigned int notused03;
-  volatile unsigned int notused10;
-  volatile unsigned int notused11;
-  volatile unsigned int notused12;
+  volatile unsigned int ampctrl;
+  volatile unsigned int icsel[2];
   volatile unsigned int notused13;
   volatile unsigned int notused20;
   volatile unsigned int notused21;
@@ -274,6 +273,8 @@ typedef struct {
   volatile unsigned int force[16];
   /* Extended IRQ registers */
   volatile unsigned int intid[16];
+  /* 0x100, align to 4Kb boundary */
+  volatile unsigned int resv1[(0x1000-0x100)/4];
 } LEON3_IrqCtrl_Regs_Map;
 
 /*****************************/
-- 
1.7.0.4