armv7-m exit, exception fixes and add MPU

index 3ec8408..a671d84 100644
@@ -15,6 +15,7 @@
 #include "hw/arm/arm.h"
 #include "exec/address-spaces.h"
 #include "gic_internal.h"
+#include "sysemu/sysemu.h"
 
 typedef struct {
     GICState gic;

@@ -348,10 +349,13 @@ static void nvic_writel(nvic_state *s, uint32_t offset, uint32_t value)
         break;
     case 0xd0c: /* Application Interrupt/Reset Control.  */
         if ((value >> 16) == 0x05fa) {
+            if (value & 4) {
+                qemu_system_reset_request();
+            }
             if (value & 2) {
                 qemu_log_mask(LOG_UNIMP, "VECTCLRACTIVE unimplemented\n");
             }
-            if (value & 5) {
+            if (value & 1) {
                 qemu_log_mask(LOG_UNIMP, "AIRCR system reset unimplemented\n");
             }
             if (value & 0x700) {

index eb214db..0fc95de 100644
@@ -178,7 +178,6 @@ qemu_irq *armv7m_init(MemoryRegion *system_memory, int mem_size, int num_irq,
     uint64_t lowaddr;
     int i;
     int big_endian;
-    MemoryRegion *hack = g_new(MemoryRegion, 1);
 
     if (cpu_model == NULL) {
 	cpu_model = "cortex-m3";

@@ -226,13 +225,6 @@ qemu_irq *armv7m_init(MemoryRegion *system_memory, int mem_size, int num_irq,
         }
     }
 
-    /* Hack to map an additional page of ram at the top of the address
-       space.  This stops qemu complaining about executing code outside RAM
-       when returning from an exception.  */
-    memory_region_init_ram(hack, NULL, "armv7m.hack", 0x1000, &error_fatal);
-    vmstate_register_ram_global(hack);
-    memory_region_add_subregion(system_memory, 0xfffff000, hack);
-
     qemu_register_reset(armv7m_reset, cpu);
     return pic;
 }

index 8367997..56b238f 100644
@@ -5346,18 +5346,23 @@ void arm_v7m_cpu_do_interrupt(CPUState *cs)
     switch (cs->exception_index) {
     case EXCP_UDEF:
         armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_USAGE);
-        return;
+        env->v7m.exception = ARMV7M_EXCP_USAGE;
+        break;
     case EXCP_SWI:
         /* The PC already points to the next instruction.  */
         armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_SVC);
-        return;
+        env->v7m.exception = ARMV7M_EXCP_SVC;
+        break;
     case EXCP_PREFETCH_ABORT:
     case EXCP_DATA_ABORT:
-        /* TODO: if we implemented the MPU registers, this is where we
-         * should set the MMFAR, etc from exception.fsr and exception.vaddress.
-         */
+        if(env->v7m.exception!=0 && env->exception.vaddress>=0xfffffff0) {
+            /* this isn't a real fault, but rather a result of return from interrupt */
+            do_v7m_exception_exit(env);
+            return;
+        }
         armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_MEM);
-        return;
+        env->v7m.exception = ARMV7M_EXCP_MEM;
+        break;
     case EXCP_BKPT:
         if (semihosting_enabled()) {
             int nr;

@@ -5407,6 +5412,12 @@ void arm_v7m_cpu_do_interrupt(CPUState *cs)
     addr = ldl_phys(cs->as, env->v7m.vecbase + env->v7m.exception * 4);
     env->regs[15] = addr & 0xfffffffe;
     env->thumb = addr & 1;
+    if(!env->thumb) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "M profile interrupt handler with misaligned "
+                      "PC is UNPREDICTABLE\n");
+        env->thumb = 1;
+    }
 }
 
 /* Function used to synchronize QEMU's AArch64 register set with AArch32

@@ -6682,6 +6693,10 @@ static bool get_phys_addr_pmsav7(CPUARMState *env, uint32_t address,
     *phys_ptr = address;
     *prot = 0;
 
+    /* ensure exception returns take precidence */
+    if(env->v7m.exception!=0 && env->exception.vaddress>=0xfffffff0)
+        return true;
+
     if (regime_translation_disabled(env, mmu_idx)) { /* MPU disabled */
         get_phys_addr_pmsav7_default(env, mmu_idx, address, prot);
     } else { /* MPU enabled */

index a671d84..94011cf 100644
@@ -245,12 +245,11 @@ static uint32_t nvic_readl(nvic_state *s, uint32_t offset)
         if (s->gic.irq_state[ARMV7M_EXCP_USAGE].enabled) val |= (1 << 18);
         return val;
     case 0xd28: /* Configurable Fault Status.  */
-        /* TODO: Implement Fault Status.  */
-        qemu_log_mask(LOG_UNIMP, "Configurable Fault Status unimplemented\n");
-        return 0;
+        return ARM_CPU(current_cpu)->pmsav7_cfsr;
+    case 0xd34: /* MMFAR MemManage Fault Address */
+        return ARM_CPU(current_cpu)->pmsav7_mmfar;
     case 0xd2c: /* Hard Fault Status.  */
     case 0xd30: /* Debug Fault Status.  */
-    case 0xd34: /* Mem Manage Address.  */
     case 0xd38: /* Bus Fault Address.  */
     case 0xd3c: /* Aux Fault Status.  */
         /* TODO: Implement fault status registers.  */

@@ -283,6 +282,55 @@ static uint32_t nvic_readl(nvic_state *s, uint32_t offset)
     case 0xd70: /* ISAR4.  */
         return 0x01310102;
     /* TODO: Implement debug registers.  */
+    case 0xd90: /* MPU_TYPE */
+        cpu = ARM_CPU(current_cpu);
+        return cpu->has_mpu ? (cpu->pmsav7_dregion<<8) : 0;
+        break;
+    case 0xd94: /* MPU_CTRL */
+        val = 0;
+        cpu = ARM_CPU(current_cpu);
+        if(cpu->env.cp15.sctlr_el[0] & SCTLR_M)
+            val |= 1; /* ENABLE */
+        /* HFNMIENA not implemented, see nvic_writel() */
+        if(cpu->env.cp15.sctlr_el[0] & SCTLR_BR)
+            val |= 4; /* PRIVDEFENA */
+        return val;
+    case 0xd98: /* MPU_RNR */
+        return ARM_CPU(current_cpu)->env.cp15.c6_rgnr;
+    case 0xd9c: /* MPU_RBAR */
+    case 0xda4: /* MPU_RBAR_A1 */
+    case 0xdaC: /* MPU_RBAR_A2 */
+    case 0xdb4: /* MPU_RBAR_A3 */
+    {
+        uint32_t range;
+        cpu = ARM_CPU(current_cpu);
+        if(offset==0xd9c)
+            range = cpu->env.cp15.c6_rgnr;
+        else
+            range = (offset-0xda4)/8;
+
+        if(range>=cpu->pmsav7_dregion) return 0;
+
+        return (cpu->env.pmsav7.drbar[range]&(0x1f)) | (range&0xf);
+    }
+    case 0xda0: /* MPU_RASR */
+    case 0xda8: /* MPU_RASR_A1 */
+    case 0xdb0: /* MPU_RASR_A2 */
+    case 0xdb8: /* MPU_RASR_A3 */
+    {
+        uint32_t range;
+        cpu = ARM_CPU(current_cpu);
+
+        if(offset==0xda0)
+            range = cpu->env.cp15.c6_rgnr;
+        else
+            range = (offset-0xda8)/8;
+
+        if(range>=cpu->pmsav7_dregion) return 0;
+
+        return ((cpu->env.pmsav7.dracr[range]&0xffff)<<16)
+                | (cpu->env.pmsav7.drsr[range]&0xffff);
+    }
     default:
         qemu_log_mask(LOG_GUEST_ERROR, "NVIC: Bad read offset 0x%x\n", offset);
         return 0;

@@ -376,14 +424,110 @@ static void nvic_writel(nvic_state *s, uint32_t offset, uint32_t value)
         s->gic.irq_state[ARMV7M_EXCP_USAGE].enabled = (value & (1 << 18)) != 0;
         break;
     case 0xd28: /* Configurable Fault Status.  */
+    case 0xd34: /* Mem Manage Address.  */
+        return;
     case 0xd2c: /* Hard Fault Status.  */
     case 0xd30: /* Debug Fault Status.  */
-    case 0xd34: /* Mem Manage Address.  */
     case 0xd38: /* Bus Fault Address.  */
     case 0xd3c: /* Aux Fault Status.  */
         qemu_log_mask(LOG_UNIMP,
                       "NVIC: fault status registers unimplemented\n");
         break;
+    case 0xd90: /* MPU_TYPE (0xe000ed90) */
+        return; /* RO */
+    case 0xd94: /* MPU_CTRL */
+    {
+        unsigned q;
+        uint32_t tset=0, tclear=0;
+        if(value&1) {
+            tset |= SCTLR_M;
+        } else {
+            tclear |= SCTLR_M;
+        }
+        if(value&2) {
+            /* The M series MPU is almost functionally the same
+             * as the R series MPU, so we translate to the R series
+             * register format.
+             *
+             * One difference is that the R series MPU doesn't implement
+             * HFNMIENA (bypass MPU in some exception handlers).
+             */
+            qemu_log_mask(LOG_UNIMP, "M profile does not implement HFNMIENA bit in MPU_CTRL\n");
+        }
+        if(value&4) {
+            tset |= SCTLR_BR;
+        } else {
+            tclear |= SCTLR_BR;
+        }
+        cpu = ARM_CPU(current_cpu);
+        /* TODO, which sctlr(s) really need to be set? */
+        for(q=0; q<4; q++) {
+            cpu->env.cp15.sctlr_el[q] |= tset;
+            cpu->env.cp15.sctlr_el[q] &= ~tclear;
+        }
+        tlb_flush(CPU(cpu), 1);
+    }
+        break;
+    case 0xd98: /* MPU_RNR */
+        cpu = ARM_CPU(current_cpu);
+        if(value>=cpu->pmsav7_dregion) {
+            qemu_log_mask(LOG_GUEST_ERROR, "MPU region out of range %u/%u\n",
+                          (unsigned)value, (unsigned)cpu->pmsav7_dregion);
+        } else {
+            cpu->env.cp15.c6_rgnr = value;
+        }
+        tlb_flush(CPU(cpu), 1); /* necessary? */
+        break;
+    case 0xd9c: /* MPU_RBAR */
+    case 0xda4: /* MPU_RBAR_A1 */
+    case 0xdac: /* MPU_RBAR_A2 */
+    case 0xdb4: /* MPU_RBAR_A3 */
+    {
+        uint32_t range;
+        uint32_t base = value;
+        cpu = ARM_CPU(current_cpu);
+
+        if(offset==0xd9c)
+            range = cpu->env.cp15.c6_rgnr;
+        else
+            range = (offset-0xda4)/8;
+
+        if(value&(1<<4)) {
+            range = value&0xf;
+
+            if(range>=cpu->pmsav7_dregion) {
+                qemu_log_mask(LOG_GUEST_ERROR, "MPU region out of range %u/%u\n",
+                              (unsigned)range, (unsigned)cpu->pmsav7_dregion);
+                return;
+            }
+            cpu->env.cp15.c6_rgnr = range;
+            base &= ~0x1f;
+
+        } else if(range>=cpu->pmsav7_dregion)
+            return;
+
+        cpu->env.pmsav7.drbar[range] = base&~0x3;
+    }
+        tlb_flush(CPU(cpu), 1);
+        break;
+    case 0xda0: /* MPU_RASR */
+    case 0xda8: /* MPU_RASR_A1 */
+    case 0xdb0: /* MPU_RASR_A2 */
+    case 0xdb8: /* MPU_RASR_A3 */
+    {
+        uint32_t range;
+        cpu = ARM_CPU(current_cpu);
+
+        if(offset==0xda0)
+            range = cpu->env.cp15.c6_rgnr;
+        else
+            range = (offset-0xda8)/8;
+
+        cpu->env.pmsav7.drsr[range] = value&0xff3f;
+        cpu->env.pmsav7.dracr[range] = (value>>16)&0x173f;
+    }
+        tlb_flush(CPU(cpu), 1);
+        break;
     case 0xf00: /* Software Triggered Interrupt Register */
         if ((value & 0x1ff) < s->num_irq) {
             gic_set_pending_private(&s->gic, 0, value & 0x1ff);

index 25fb1ce..d75a496 100644
@@ -107,6 +107,10 @@ typedef struct ARMCPU {
     bool has_mpu;
     /* PMSAv7 MPU number of supported regions */
     uint32_t pmsav7_dregion;
+    /* PMASv7 Fault status */
+    uint32_t pmsav7_cfsr;
+    /* PMASv7 Address of fault */
+    uint32_t pmsav7_mmfar;
 
     /* PSCI conduit used to invoke PSCI methods
      * 0 - disabled, 1 - smc, 2 - hvc

index d7b4445..deb5878 100644
@@ -516,6 +516,12 @@ static Property arm_cpu_has_mpu_property =
 static Property arm_cpu_pmsav7_dregion_property =
             DEFINE_PROP_UINT32("pmsav7-dregion", ARMCPU, pmsav7_dregion, 16);
 
+static Property arm_cpu_pmsav7_cfsr_property =
+            DEFINE_PROP_UINT32("pmsav7-cfsr", ARMCPU, pmsav7_cfsr, 0);
+
+static Property arm_cpu_pmsav7_mmfar_property =
+            DEFINE_PROP_UINT32("pmsav7-mmfar", ARMCPU, pmsav7_mmfar, 0);
+
 static void arm_cpu_post_init(Object *obj)
 {
     ARMCPU *cpu = ARM_CPU(obj);

@@ -551,6 +557,12 @@ static void arm_cpu_post_init(Object *obj)
             qdev_property_add_static(DEVICE(obj),
                                      &arm_cpu_pmsav7_dregion_property,
                                      &error_abort);
+            qdev_property_add_static(DEVICE(obj),
+                                     &arm_cpu_pmsav7_cfsr_property,
+                                     &error_abort);
+            qdev_property_add_static(DEVICE(obj),
+                                     &arm_cpu_pmsav7_mmfar_property,
+                                     &error_abort);
         }
     }
 

@@ -889,6 +901,7 @@ static void cortex_m3_initfn(Object *obj)
     ARMCPU *cpu = ARM_CPU(obj);
     set_feature(&cpu->env, ARM_FEATURE_V7);
     set_feature(&cpu->env, ARM_FEATURE_M);
+    set_feature(&cpu->env, ARM_FEATURE_MPU);
     cpu->midr = 0x410fc231;
 }
 

@@ -898,6 +911,7 @@ static void cortex_m4_initfn(Object *obj)
 
     set_feature(&cpu->env, ARM_FEATURE_V7);
     set_feature(&cpu->env, ARM_FEATURE_M);
+    set_feature(&cpu->env, ARM_FEATURE_MPU);
     set_feature(&cpu->env, ARM_FEATURE_THUMB_DSP);
     cpu->midr = 0x410fc240; /* r0p0 */
 }

index 56b238f..368cbc6 100644
@@ -5357,11 +5357,15 @@ void arm_v7m_cpu_do_interrupt(CPUState *cs)
     case EXCP_DATA_ABORT:
         if(env->v7m.exception!=0 && env->exception.vaddress>=0xfffffff0) {
             /* this isn't a real fault, but rather a result of return from interrupt */
+            cpu->pmsav7_mmfar = 0;
+            cpu->pmsav7_cfsr = 0;
             do_v7m_exception_exit(env);
             return;
         }
         armv7m_nvic_set_pending(env->nvic, ARMV7M_EXCP_MEM);
         env->v7m.exception = ARMV7M_EXCP_MEM;
+        cpu->pmsav7_mmfar = env->exception.vaddress;
+        cpu->pmsav7_cfsr = (1<<1)|(1<<7);
         break;
     case EXCP_BKPT:
         if (semihosting_enabled()) {

@@ -5382,6 +5386,9 @@ void arm_v7m_cpu_do_interrupt(CPUState *cs)
         env->v7m.exception = armv7m_nvic_acknowledge_irq(env->nvic);
         break;
     case EXCP_EXCEPTION_EXIT:
+
+        cpu->pmsav7_mmfar = 0;
+        cpu->pmsav7_cfsr = 0;
         do_v7m_exception_exit(env);
         return;
     default: