1 files changed, 74 insertions, 29 deletions

diff --git a/hw/intc/arm_gic.c b/hw/intc/arm_gic.c
index 34f781f189..9daa8cd44f 100644
--- a/hw/intc/arm_gic.c
+++ b/hw/intc/arm_gic.c
@@ -245,15 +245,72 @@ static int gic_get_group_priority(GICState *s, int cpu, int irq)
     return GIC_GET_PRIORITY(irq, cpu) & mask;
 }
 
-static void gic_set_running_irq(GICState *s, int cpu, int irq)
+static void gic_activate_irq(GICState *s, int cpu, int irq)
 {
-    s->running_irq[cpu] = irq;
-    if (irq == 1023) {
-        s->running_priority[cpu] = 0x100;
+    /* Set the appropriate Active Priority Register bit for this IRQ,
+     * and update the running priority.
+     */
+    int prio = gic_get_group_priority(s, cpu, irq);
+    int preemption_level = prio >> (GIC_MIN_BPR + 1);
+    int regno = preemption_level / 32;
+    int bitno = preemption_level % 32;
+
+    if (gic_has_groups(s) && GIC_TEST_GROUP(irq, (1 << cpu))) {
+        s->nsapr[regno][cpu] &= (1 << bitno);
     } else {
-        s->running_priority[cpu] = gic_get_group_priority(s, cpu, irq);
+        s->apr[regno][cpu] &= (1 << bitno);
     }
-    gic_update(s);
+
+    s->running_priority[cpu] = prio;
+}
+
+static int gic_get_prio_from_apr_bits(GICState *s, int cpu)
+{
+    /* Recalculate the current running priority for this CPU based
+     * on the set bits in the Active Priority Registers.
+     */
+    int i;
+    for (i = 0; i < GIC_NR_APRS; i++) {
+        uint32_t apr = s->apr[i][cpu] | s->nsapr[i][cpu];
+        if (!apr) {
+            continue;
+        }
+        return (i * 32 + ctz32(apr)) << (GIC_MIN_BPR + 1);
+    }
+    return 0x100;
+}
+
+static void gic_drop_prio(GICState *s, int cpu, int group)
+{
+    /* Drop the priority of the currently active interrupt in the
+     * specified group.
+     *
+     * Note that we can guarantee (because of the requirement to nest
+     * GICC_IAR reads [which activate an interrupt and raise priority]
+     * with GICC_EOIR writes [which drop the priority for the interrupt])
+     * that the interrupt we're being called for is the highest priority
+     * active interrupt, meaning that it has the lowest set bit in the
+     * APR registers.
+     *
+     * If the guest does not honour the ordering constraints then the
+     * behaviour of the GIC is UNPREDICTABLE, which for us means that
+     * the values of the APR registers might become incorrect and the
+     * running priority will be wrong, so interrupts that should preempt
+     * might not do so, and interrupts that should not preempt might do so.
+     */
+    int i;
+
+    for (i = 0; i < GIC_NR_APRS; i++) {
+        uint32_t *papr = group ? &s->nsapr[i][cpu] : &s->apr[i][cpu];
+        if (!*papr) {
+            continue;
+        }
+        /* Clear lowest set bit */
+        *papr &= *papr - 1;
+        break;
+    }
+
+    s->running_priority[cpu] = gic_get_prio_from_apr_bits(s, cpu);
 }
 
 uint32_t gic_acknowledge_irq(GICState *s, int cpu, MemTxAttrs attrs)
@@ -276,7 +333,6 @@ uint32_t gic_acknowledge_irq(GICState *s, int cpu, MemTxAttrs attrs)
         DPRINTF("ACK, pending interrupt (%d) has insufficient priority\n", irq);
         return 1023;
     }
-    s->last_active[irq][cpu] = s->running_irq[cpu];
 
     if (s->revision == REV_11MPCORE || s->revision == REV_NVIC) {
         /* Clear pending flags for both level and edge triggered interrupts.
@@ -307,7 +363,8 @@ uint32_t gic_acknowledge_irq(GICState *s, int cpu, MemTxAttrs attrs)
         }
     }
 
-    gic_set_running_irq(s, cpu, irq);
+    gic_activate_irq(s, cpu, irq);
+    gic_update(s);
     DPRINTF("ACK %d\n", irq);
     return ret;
 }
@@ -437,8 +494,9 @@ static uint8_t gic_get_running_priority(GICState *s, int cpu, MemTxAttrs attrs)
 
 void gic_complete_irq(GICState *s, int cpu, int irq, MemTxAttrs attrs)
 {
-    int update = 0;
     int cm = 1 << cpu;
+    int group;
+
     DPRINTF("EOI %d\n", irq);
     if (irq >= s->num_irq) {
         /* This handles two cases:
@@ -451,8 +509,9 @@ void gic_complete_irq(GICState *s, int cpu, int irq, MemTxAttrs attrs)
          */
         return;
     }
-    if (s->running_irq[cpu] == 1023)
+    if (s->running_priority[cpu] == 0x100) {
         return; /* No active IRQ.  */
+    }
 
     if (s->revision == REV_11MPCORE || s->revision == REV_NVIC) {
         /* Mark level triggered interrupts as pending if they are still
@@ -461,11 +520,12 @@ void gic_complete_irq(GICState *s, int cpu, int irq, MemTxAttrs attrs)
             && GIC_TEST_LEVEL(irq, cm) && (GIC_TARGET(irq) & cm) != 0) {
             DPRINTF("Set %d pending mask %x\n", irq, cm);
             GIC_SET_PENDING(irq, cm);
-            update = 1;
         }
     }
 
-    if (s->security_extn && !attrs.secure && !GIC_TEST_GROUP(irq, cm)) {
+    group = gic_has_groups(s) && GIC_TEST_GROUP(irq, cm);
+
+    if (s->security_extn && !attrs.secure && !group) {
         DPRINTF("Non-secure EOI for Group0 interrupt %d ignored\n", irq);
         return;
     }
@@ -475,23 +535,8 @@ void gic_complete_irq(GICState *s, int cpu, int irq, MemTxAttrs attrs)
      * i.e. go ahead and complete the irq anyway.
      */
 
-    if (irq != s->running_irq[cpu]) {
-        /* Complete an IRQ that is not currently running.  */
-        int tmp = s->running_irq[cpu];
-        while (s->last_active[tmp][cpu] != 1023) {
-            if (s->last_active[tmp][cpu] == irq) {
-                s->last_active[tmp][cpu] = s->last_active[irq][cpu];
-                break;
-            }
-            tmp = s->last_active[tmp][cpu];
-        }
-        if (update) {
-            gic_update(s);
-        }
-    } else {
-        /* Complete the current running IRQ.  */
-        gic_set_running_irq(s, cpu, s->last_active[s->running_irq[cpu]][cpu]);
-    }
+    gic_drop_prio(s, cpu, group);
+    gic_update(s);
 }
 
 static uint32_t gic_dist_readb(void *opaque, hwaddr offset, MemTxAttrs attrs)