From 691835f2a10b41cce4664c5fe7046f49ad3e0535 Mon Sep 17 00:00:00 2001
From: Ian Campbell <ian.campbell@citrix.com>
Date: Wed, 17 Apr 2013 13:52:32 +0100
Subject: [PATCH] arm: gic: implement IPIs using SGI mechanism

Signed-off-by: Ian Campbell <ian.campbell@citrix.com>
Acked-by: Stefano Stabellini <stefano.stabellini@eu.citrix.com>
---
 xen/arch/arm/arm32/mode_switch.S |  2 +-
 xen/arch/arm/gic.c               | 85 +++++++++++++++++++++++++++++---
 xen/arch/arm/smp.c               | 14 ++----
 xen/include/asm-arm/gic.h        | 22 ++++++++-
 4 files changed, 105 insertions(+), 18 deletions(-)

diff --git a/xen/arch/arm/arm32/mode_switch.S b/xen/arch/arm/arm32/mode_switch.S
index bc2be7472a..d6741d0bf4 100644
--- a/xen/arch/arm/arm32/mode_switch.S
+++ b/xen/arch/arm/arm32/mode_switch.S
@@ -43,7 +43,7 @@ kick_cpus:
         mov   r2, #0x1
         str   r2, [r0, #(GICD_CTLR * 4)]      /* enable distributor */
         mov   r2, #0xfe0000
-        str   r2, [r0, #(GICD_SGIR * 4)]      /* send IPI to everybody */
+        str   r2, [r0, #(GICD_SGIR * 4)]      /* send IPI to everybody, SGI0 = Event check */
         dsb
         str   r2, [r0, #(GICD_CTLR * 4)]      /* disable distributor */
         mov   pc, lr
diff --git a/xen/arch/arm/gic.c b/xen/arch/arm/gic.c
index 3124da3c7a..0d1ab5a1cc 100644
--- a/xen/arch/arm/gic.c
+++ b/xen/arch/arm/gic.c
@@ -358,10 +358,52 @@ void __init gic_init(void)
     spin_unlock(&gic.lock);
 }
 
+void send_SGI_mask(const cpumask_t *cpumask, enum gic_sgi sgi)
+{
+    unsigned long mask = cpumask_bits(cpumask)[0];
+
+    ASSERT(sgi < 16); /* There are only 16 SGIs */
+
+    mask &= cpumask_bits(&cpu_online_map)[0];
+
+    ASSERT(mask < 0x100); /* The target bitmap only supports 8 CPUs */
+
+    dsb();
+
+    GICD[GICD_SGIR] = GICD_SGI_TARGET_LIST
+        | (mask<<GICD_SGI_TARGET_SHIFT)
+        | sgi;
+}
+
+void send_SGI_one(unsigned int cpu, enum gic_sgi sgi)
+{
+    ASSERT(cpu < 7);  /* Targets bitmap only supports 8 CPUs */
+    send_SGI_mask(cpumask_of(cpu), sgi);
+}
+
+void send_SGI_self(enum gic_sgi sgi)
+{
+    ASSERT(sgi < 16); /* There are only 16 SGIs */
+
+    dsb();
+
+    GICD[GICD_SGIR] = GICD_SGI_TARGET_SELF
+        | sgi;
+}
+
+void send_SGI_allbutself(enum gic_sgi sgi)
+{
+   ASSERT(sgi < 16); /* There are only 16 SGIs */
+
+   dsb();
+
+   GICD[GICD_SGIR] = GICD_SGI_TARGET_OTHERS
+       | sgi;
+}
+
 void smp_send_state_dump(unsigned int cpu)
 {
-    printk("WARNING: unable to send state dump request to CPU%d\n", cpu);
-    /* XXX TODO -- send an SGI */
+    send_SGI_one(cpu, GIC_SGI_DUMP_STATE);
 }
 
 /* Set up the per-CPU parts of the GIC for a secondary CPU */
@@ -594,6 +636,28 @@ out:
     return retval;
 }
 
+static void do_sgi(struct cpu_user_regs *regs, int othercpu, enum gic_sgi sgi)
+{
+    /* Lower the priority */
+    GICC[GICC_EOIR] = sgi;
+
+    switch (sgi)
+    {
+    case GIC_SGI_EVENT_CHECK:
+        /* Nothing to do, will check for events on return path */
+        break;
+    case GIC_SGI_DUMP_STATE:
+        dump_execstate(regs);
+        break;
+    default:
+        panic("Unhandled SGI %d on CPU%d\n", sgi, smp_processor_id());
+        break;
+    }
+
+    /* Deactivate */
+    GICC[GICC_DIR] = sgi;
+}
+
 /* Accept an interrupt from the GIC and dispatch its handler */
 void gic_interrupt(struct cpu_user_regs *regs, int is_fiq)
 {
@@ -604,14 +668,23 @@ void gic_interrupt(struct cpu_user_regs *regs, int is_fiq)
     do  {
         intack = GICC[GICC_IAR];
         irq = intack & GICC_IA_IRQ;
-        local_irq_enable();
 
-        if (likely(irq < 1021))
+        if ( likely(irq >= 16 && irq < 1021) )
+        {
+            local_irq_enable();
             do_IRQ(regs, irq, is_fiq);
+            local_irq_disable();
+        }
+        else if (unlikely(irq < 16))
+        {
+            unsigned int cpu = (intack & GICC_IA_CPU_MASK) >> GICC_IA_CPU_SHIFT;
+            do_sgi(regs, cpu, irq);
+        }
         else
+        {
+            local_irq_disable();
             break;
-
-        local_irq_disable();
+        }
     } while (1);
 }
 
diff --git a/xen/arch/arm/smp.c b/xen/arch/arm/smp.c
index 12260f4fae..2a429bdb39 100644
--- a/xen/arch/arm/smp.c
+++ b/xen/arch/arm/smp.c
@@ -3,10 +3,11 @@
 #include <asm/smp.h>
 #include <asm/cpregs.h>
 #include <asm/page.h>
+#include <asm/gic.h>
 
 void flush_tlb_mask(const cpumask_t *mask)
 {
-    /* XXX IPI other processors */
+    /* No need to IPI other processors on ARM, the processor takes care of it. */
     flush_xen_data_tlb();
 }
 
@@ -15,17 +16,12 @@ void smp_call_function(
     void *info,
     int wait)
 {
-    /* TODO: No SMP just now, does not include self so nothing to do.
-       cpumask_t allbutself = cpu_online_map;
-       cpu_clear(smp_processor_id(), allbutself);
-       on_selected_cpus(&allbutself, func, info, wait);
-    */
+    printk("%s not implmented\n", __func__);
 }
+
 void smp_send_event_check_mask(const cpumask_t *mask)
 {
-    /* TODO: No SMP just now, does not include self so nothing to do.
-       send_IPI_mask(mask, EVENT_CHECK_VECTOR);
-    */
+    send_SGI_mask(mask, GIC_SGI_EVENT_CHECK);
 }
 
 /*
diff --git a/xen/include/asm-arm/gic.h b/xen/include/asm-arm/gic.h
index 6bf50bbd95..24c0d5cf7b 100644
--- a/xen/include/asm-arm/gic.h
+++ b/xen/include/asm-arm/gic.h
@@ -51,6 +51,13 @@
 #define GICD_SPENDSGIRN (0xF2C/4)
 #define GICD_ICPIDR2    (0xFE8/4)
 
+#define GICD_SGI_TARGET_LIST   (0UL<<24)
+#define GICD_SGI_TARGET_OTHERS (1UL<<24)
+#define GICD_SGI_TARGET_SELF   (2UL<<24)
+#define GICD_SGI_TARGET_SHIFT  (16)
+#define GICD_SGI_TARGET_MASK   (0xFFUL<<GICD_SGI_TARGET_SHIFT)
+#define GICD_SGI_GROUP1        (1UL<<15)
+
 #define GICC_CTLR       (0x0000/4)
 #define GICC_PMR        (0x0004/4)
 #define GICC_BPR        (0x0008/4)
@@ -83,8 +90,9 @@
 #define GICC_CTL_ENABLE 0x1
 #define GICC_CTL_EOI    (0x1 << 9)
 
-#define GICC_IA_IRQ     0x03ff
-#define GICC_IA_CPU     0x1c00
+#define GICC_IA_IRQ       0x03ff
+#define GICC_IA_CPU_MASK  0x1c00
+#define GICC_IA_CPU_SHIFT 10
 
 #define GICH_HCR_EN       (1 << 0)
 #define GICH_HCR_UIE      (1 << 1)
@@ -157,6 +165,16 @@ extern int gicv_setup(struct domain *d);
 extern void gic_save_state(struct vcpu *v);
 extern void gic_restore_state(struct vcpu *v);
 
+/* SGI (AKA IPIs) */
+enum gic_sgi {
+    GIC_SGI_EVENT_CHECK = 0,
+    GIC_SGI_DUMP_STATE  = 1,
+};
+extern void send_SGI_mask(const cpumask_t *cpumask, enum gic_sgi sgi);
+extern void send_SGI_one(unsigned int cpu, enum gic_sgi sgi);
+extern void send_SGI_self(enum gic_sgi sgi);
+extern void send_SGI_allbutself(enum gic_sgi sgi);
+
 /* print useful debug info */
 extern void gic_dump_info(struct vcpu *v);
 
-- 
2.39.5