From 0c8e700376cec0c7b5a70f999b5e286efc321423 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Fri, 16 Dec 2011 19:46:40 +0100
Subject: [PATCH 1/4] target-arm: Fixed ARMv7-M SHPR access

According to "ARMv7-M Architecture Reference Manual" issue D section
"B3.2.10 System Handler Prioriy Register 1, SHPR1", "B3.2.11 System
Handler Prioriy Register 2, SHPR2", and "B3.2.12 System Handler Prioriy
Register 3, SHPR3".

Signed-off-by: Sebastian Huber <sebastian.huber@embedded-brains.de>
---
 hw/arm_gic.c     |   16 ++++++++++++++--
 hw/armv7m_nvic.c |   19 -------------------
 2 files changed, 14 insertions(+), 21 deletions(-)

diff --git a/hw/arm_gic.c b/hw/arm_gic.c
index 9b52119..5139d95 100644
--- a/hw/arm_gic.c
+++ b/hw/arm_gic.c
@@ -356,6 +356,11 @@ static uint32_t gic_dist_readb(void *opaque, target_phys_addr_t offset)
             if (GIC_TEST_TRIGGER(irq + i))
                 res |= (2 << (i * 2));
         }
+#else
+    } else if (0xd18 <= offset && offset < 0xd24) {
+        /* System Handler Priority.  */
+        irq = offset - 0xd14;
+        res = GIC_GET_PRIORITY(irq, cpu);
 #endif
     } else if (offset < 0xfe0) {
         goto bad_reg;
@@ -387,7 +392,8 @@ static uint32_t gic_dist_readl(void *opaque, target_phys_addr_t offset)
     gic_state *s = (gic_state *)opaque;
     uint32_t addr;
     addr = offset;
-    if (addr < 0x100 || addr > 0xd00)
+    if (addr < 0x100 || (addr > 0xd00 && addr != 0xd18 && addr != 0xd1c
+        && addr != 0xd20))
         return nvic_readl(s, addr);
 #endif
     val = gic_dist_readw(opaque, offset);
@@ -528,6 +534,11 @@ static void gic_dist_writeb(void *opaque, target_phys_addr_t offset,
                 GIC_CLEAR_TRIGGER(irq + i);
             }
         }
+#else
+    } else if (0xd18 <= offset && offset < 0xd24) {
+        /* System Handler Priority.  */
+        irq = offset - 0xd14;
+        s->priority1[irq][0] = value & 0xff;
 #endif
     } else {
         /* 0xf00 is only handled for 32-bit writes.  */
@@ -553,7 +564,8 @@ static void gic_dist_writel(void *opaque, target_phys_addr_t offset,
 #ifdef NVIC
     uint32_t addr;
     addr = offset;
-    if (addr < 0x100 || (addr > 0xd00 && addr != 0xf00)) {
+    if (addr < 0x100 || (addr > 0xd00 && addr != 0xd18 && addr != 0xd1c
+        && addr != 0xd20 && addr != 0xf00)) {
         nvic_writel(s, addr, value);
         return;
     }
diff --git a/hw/armv7m_nvic.c b/hw/armv7m_nvic.c
index bf8c3c5..65b575e 100644
--- a/hw/armv7m_nvic.c
+++ b/hw/armv7m_nvic.c
@@ -195,14 +195,6 @@ static uint32_t nvic_readl(void *opaque, uint32_t offset)
     case 0xd14: /* Configuration Control.  */
         /* TODO: Implement Configuration Control bits.  */
         return 0;
-    case 0xd18: case 0xd1c: case 0xd20: /* System Handler Priority.  */
-        irq = offset - 0xd14;
-        val = 0;
-        val |= s->gic.priority1[irq++][0];
-        val |= s->gic.priority1[irq++][0] << 8;
-        val |= s->gic.priority1[irq++][0] << 16;
-        val |= s->gic.priority1[irq][0] << 24;
-        return val;
     case 0xd24: /* System Handler Status.  */
         val = 0;
         if (s->gic.irq_state[ARMV7M_EXCP_MEM].active) val |= (1 << 0);
@@ -335,17 +327,6 @@ static void nvic_writel(void *opaque, uint32_t offset, uint32_t value)
     case 0xd14: /* Configuration Control.  */
         /* TODO: Implement control registers.  */
         goto bad_reg;
-    case 0xd18: case 0xd1c: case 0xd20: /* System Handler Priority.  */
-        {
-            int irq;
-            irq = offset - 0xd14;
-            s->gic.priority1[irq++][0] = value & 0xff;
-            s->gic.priority1[irq++][0] = (value >> 8) & 0xff;
-            s->gic.priority1[irq++][0] = (value >> 16) & 0xff;
-            s->gic.priority1[irq][0] = (value >> 24) & 0xff;
-            gic_update(&s->gic);
-        }
-        break;
     case 0xd24: /* System Handler Control.  */
         /* TODO: Real hardware allows you to set/clear the active bits
            under some circumstances.  We don't implement this.  */
-- 
1.7.1