1 files changed, 80 insertions, 13 deletions

diff --git a/hw/intc/arm_gic.c b/hw/intc/arm_gic.c
index 955b8d494..c37a857cb 100644
--- a/hw/intc/arm_gic.c
+++ b/hw/intc/arm_gic.c
@@ -22,7 +22,7 @@
 #include "gic_internal.h"
 #include "qom/cpu.h"
 
-//#define DEBUG_GIC
+#define DEBUG_GIC
 
 #ifdef DEBUG_GIC
 #define DPRINTF(fmt, ...) \
@@ -52,14 +52,15 @@ void gic_update(GICState *s)
     int best_irq;
     int best_prio;
     int irq;
-    int level;
+    int irq_level;
+    int fiq_level;
     int cpu;
     int cm;
 
     for (cpu = 0; cpu < NUM_CPU(s); cpu++) {
         cm = 1 << cpu;
         s->current_pending[cpu] = 1023;
-        if (!s->enabled || !s->cpu_enabled[cpu]) {
+        if (!s->enabled_grp0 || !s->cpu_enabled[cpu]) {
             qemu_irq_lower(s->parent_irq[cpu]);
             return;
         }
@@ -73,15 +74,25 @@ void gic_update(GICState *s)
                 }
             }
         }
-        level = 0;
+        irq_level = 0;
         if (best_prio < s->priority_mask[cpu]) {
             s->current_pending[cpu] = best_irq;
             if (best_prio < s->running_priority[cpu]) {
                 DPRINTF("Raised pending IRQ %d (cpu %d)\n", best_irq, cpu);
-                level = 1;
+                irq_level = 1;
             }
         }
-        qemu_set_irq(s->parent_irq[cpu], level);
+
+        fiq_level = 0;
+        if (s->revision >= 2) {
+            if (s->enabled_fiq && !GIC_TEST_GROUP(best_irq)) {
+                fiq_level = irq_level;
+                irq_level = 0;
+            }
+        }
+
+        qemu_set_irq(s->parent_irq[cpu], irq_level);
+        qemu_set_irq(s->parent_fiq[cpu], fiq_level);
     }
 }
 
@@ -294,15 +305,35 @@ static uint32_t gic_dist_readb(void *opaque, hwaddr offset)
     cpu = gic_get_current_cpu(s);
     cm = 1 << cpu;
     if (offset < 0x100) {
-        if (offset == 0)
-            return s->enabled;
+        if (offset == 0) {
+            if (s->revision < 2) {
+                return s->enabled_grp0;
+            } else {
+                return (s->enabled_grp0) | (s->enabled_grp1 << 1) |
+                       (s->enabled_fiq << 3);
+            }
+        }
         if (offset == 4)
             return ((s->num_irq / 32) - 1) | ((NUM_CPU(s) - 1) << 5);
         if (offset < 0x08)
             return 0;
         if (offset >= 0x80) {
-            /* Interrupt Security , RAZ/WI */
-            return 0;
+            /* Interrupt Group. */
+            if (s->revision < 2) {
+                /* GICv1 (or security extensions), RAZ/WI */
+                res = 0;
+            } else {
+                /* GICv2+ without security extensions */
+                irq = (offset - 0x80) * 8 + GIC_BASE_IRQ;
+                if (irq >= s->num_irq)
+                    goto bad_reg;
+                res = 0;
+                for (i = 0; i < 8; i++) {
+                    if (GIC_TEST_GROUP(irq + i)) {
+                        res |= (1 << i);
+                    }
+                }
+            }
         }
         goto bad_reg;
     } else if (offset < 0x200) {
@@ -441,12 +472,45 @@ static void gic_dist_writeb(void *opaque, hwaddr offset,
     cpu = gic_get_current_cpu(s);
     if (offset < 0x100) {
         if (offset == 0) {
-            s->enabled = (value & 1);
-            DPRINTF("Distribution %sabled\n", s->enabled ? "En" : "Dis");
+            s->enabled_grp0 = (value & (1 << 0));
+            if (s->revision < 2) {
+                DPRINTF("Distribution %sabled\n",
+                        s->enabled_grp0 ? "En" : "Dis");
+            } else {
+                s->enabled_grp1 = (value & (1 << 1));
+                s->enabled_fiq = (value & (1 << 3));
+                DPRINTF("Interrupt distribution: "
+                        "group0 %sabled; group1 %sabled; fiq %sabled\n",
+                        s->enabled_grp0 ? "En" : "Dis",
+                        s->enabled_grp1 ? "En" : "Dis",
+                        s->enabled_fiq ? "En" : "Dis");
+            }
         } else if (offset < 4) {
             /* ignored.  */
         } else if (offset >= 0x80) {
-            /* Interrupt Security Registers, RAZ/WI */
+            /* Interrupt Group. */
+            if (s->revision < 2) {
+                /* GICv1 (or security extensions), RAZ/WI */
+            } else {
+                /* GICv2+ without security extensions */
+                irq = (offset - 0x80) * 8 + GIC_BASE_IRQ;
+                irq += GIC_BASE_IRQ;
+                if (irq >= s->num_irq)
+                    goto bad_reg;
+                for (i = 0; i < 8; i++) {
+                    if (value & (1 << i)) {
+                        if (!GIC_TEST_GROUP(irq + i)) {
+                            DPRINTF("Set IRQ %d to group1\n", irq + i);
+                        }
+                        GIC_SET_GROUP(irq + i);
+                    } else {
+                        if (GIC_TEST_GROUP(irq + i)) {
+                            DPRINTF("Set IRQ %d to group0\n", irq + i);
+                        }
+                        GIC_CLEAR_GROUP(irq + i);
+                    }
+                }
+            }
         } else {
             goto bad_reg;
         }
@@ -786,6 +850,9 @@ void gic_init_irqs_and_distributor(GICState *s, int num_irq)
     for (i = 0; i < NUM_CPU(s); i++) {
         sysbus_init_irq(sbd, &s->parent_irq[i]);
     }
+    for (i = 0; i < NUM_CPU(s); i++) {
+        sysbus_init_irq(sbd, &s->parent_fiq[i]);
+    }
     memory_region_init_io(&s->iomem, OBJECT(s), &gic_dist_ops, s,
                           "gic_dist", 0x1000);
 }