[PATCH 5/5] xen: arm: fix guest register access.

[Ian Campbell <ian.campbell@citrix.com>]

We weren't taking the guest mode (CPSR) into account and would always
access the user version of the registers.

Signed-off-by: Ian Campbell <ian.campbell@citrix.com>
---
 xen/arch/arm/traps.c       |   62 ++++++++++++++++++++++++++++++++++++++++++-
 xen/arch/arm/vgic.c        |    4 +-
 xen/arch/arm/vpl011.c      |    4 +-
 xen/arch/arm/vtimer.c      |    8 +++---
 xen/include/asm-arm/regs.h |    6 ++++
 5 files changed, 74 insertions(+), 10 deletions(-)

diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index 096dc0b..e3c0290 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -73,6 +73,64 @@ static void print_xen_info(void)
            debug, print_tainted(taint_str));
 }
 
+uint32_t *select_user_reg(struct cpu_user_regs *regs, int reg)
+{
+    BUG_ON( guest_mode(regs) );
+
+    /*
+     * We rely heavily on the layout of cpu_user_regs to avoid having
+     * to handle all of the registers individually. Use BUILD_BUG_ON to
+     * ensure that things which expect are contiguous actually are.
+     */
+#define REGOFFS(R) offsetof(struct cpu_user_regs, R)
+
+    switch ( reg ) {
+    case 0 ... 7: /* Unbanked registers */
+        BUILD_BUG_ON(REGOFFS(r0) + 7*sizeof(uint32_t) != REGOFFS(r7));
+        return &regs->r0 + reg;
+    case 8 ... 12: /* Register banked in FIQ mode */
+        BUILD_BUG_ON(REGOFFS(r8_fiq) + 4*sizeof(uint32_t) != REGOFFS(r12_fiq));
+        if ( fiq_mode(regs) )
+            return &regs->r8_fiq + reg - 8;
+        else
+            return &regs->r8_fiq + reg - 8;
+    case 13 ... 14: /* Banked SP + LR registers */
+        BUILD_BUG_ON(REGOFFS(sp_fiq) + 1*sizeof(uint32_t) != REGOFFS(lr_fiq));
+        BUILD_BUG_ON(REGOFFS(sp_irq) + 1*sizeof(uint32_t) != REGOFFS(lr_irq));
+        BUILD_BUG_ON(REGOFFS(sp_svc) + 1*sizeof(uint32_t) != REGOFFS(lr_svc));
+        BUILD_BUG_ON(REGOFFS(sp_abt) + 1*sizeof(uint32_t) != REGOFFS(lr_abt));
+        BUILD_BUG_ON(REGOFFS(sp_und) + 1*sizeof(uint32_t) != REGOFFS(lr_und));
+        switch ( regs->cpsr & PSR_MODE_MASK )
+        {
+        case PSR_MODE_USR:
+        case PSR_MODE_SYS: /* Sys regs are the usr regs */
+            if ( reg == 13 )
+                return &regs->sp_usr;
+            else /* lr_usr == lr in a user frame */
+                return &regs->lr;
+        case PSR_MODE_FIQ:
+            return &regs->sp_fiq + reg - 13;
+        case PSR_MODE_IRQ:
+            return &regs->sp_irq + reg - 13;
+        case PSR_MODE_SVC:
+            return &regs->sp_svc + reg - 13;
+        case PSR_MODE_ABT:
+            return &regs->sp_abt + reg - 13;
+        case PSR_MODE_UND:
+            return &regs->sp_und + reg - 13;
+        case PSR_MODE_MON:
+        case PSR_MODE_HYP:
+        default:
+            BUG();
+        }
+    case 15: /* PC */
+        return &regs->pc;
+    default:
+        BUG();
+    }
+#undef REGOFFS
+}
+
 static const char *decode_fsc(uint32_t fsc, int *level)
 {
     const char *msg = NULL;
@@ -448,7 +506,7 @@ static void do_debug_trap(struct cpu_user_regs *regs, unsigned int code)
     switch ( code ) {
     case 0xe0 ... 0xef:
         reg = code - 0xe0;
-        r = &regs->r0 + reg;
+        r = select_user_reg(regs, reg);
         printk("DOM%d: R%d = %#010"PRIx32" at %#010"PRIx32"\n",
                domid, reg, *r, regs->pc);
         break;
@@ -518,7 +576,7 @@ static void do_cp15_32(struct cpu_user_regs *regs,
                        union hsr hsr)
 {
     struct hsr_cp32 cp32 = hsr.cp32;
-    uint32_t *r = &regs->r0 + cp32.reg;
+    uint32_t *r = select_user_reg(regs, cp32.reg);
 
     if ( !cp32.ccvalid ) {
         dprintk(XENLOG_ERR, "cp_15(32): need to handle invalid condition codes\n");
diff --git a/xen/arch/arm/vgic.c b/xen/arch/arm/vgic.c
index 3f7e757..59780d2 100644
--- a/xen/arch/arm/vgic.c
+++ b/xen/arch/arm/vgic.c
@@ -160,7 +160,7 @@ static int vgic_distr_mmio_read(struct vcpu *v, mmio_info_t *info)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
-    uint32_t *r = &regs->r0 + dabt.reg;
+    uint32_t *r = select_user_reg(regs, dabt.reg);
     struct vgic_irq_rank *rank;
     int offset = (int)(info->gpa - VGIC_DISTR_BASE_ADDRESS);
     int gicd_reg = REG(offset);
@@ -372,7 +372,7 @@ static int vgic_distr_mmio_write(struct vcpu *v, mmio_info_t *info)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
-    uint32_t *r = &regs->r0 + dabt.reg;
+    uint32_t *r = select_user_reg(regs, dabt.reg);
     struct vgic_irq_rank *rank;
     int offset = (int)(info->gpa - VGIC_DISTR_BASE_ADDRESS);
     int gicd_reg = REG(offset);
diff --git a/xen/arch/arm/vpl011.c b/xen/arch/arm/vpl011.c
index 1522667..7dcee90 100644
--- a/xen/arch/arm/vpl011.c
+++ b/xen/arch/arm/vpl011.c
@@ -92,7 +92,7 @@ static int uart0_mmio_read(struct vcpu *v, mmio_info_t *info)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
-    uint32_t *r = &regs->r0 + dabt.reg;
+    uint32_t *r = select_user_reg(regs, dabt.reg);
     int offset = (int)(info->gpa - UART0_START);
 
     switch ( offset )
@@ -114,7 +114,7 @@ static int uart0_mmio_write(struct vcpu *v, mmio_info_t *info)
 {
     struct hsr_dabt dabt = info->dabt;
     struct cpu_user_regs *regs = guest_cpu_user_regs();
-    uint32_t *r = &regs->r0 + dabt.reg;
+    uint32_t *r = select_user_reg(regs, dabt.reg);
     int offset = (int)(info->gpa - UART0_START);
 
     switch ( offset )
diff --git a/xen/arch/arm/vtimer.c b/xen/arch/arm/vtimer.c
index 490b021..07994b2 100644
--- a/xen/arch/arm/vtimer.c
+++ b/xen/arch/arm/vtimer.c
@@ -21,7 +21,7 @@
 #include <xen/lib.h>
 #include <xen/timer.h>
 #include <xen/sched.h>
-#include "gic.h"
+#include <asm/regs.h>
 
 extern s_time_t ticks_to_ns(uint64_t ticks);
 extern uint64_t ns_to_ticks(s_time_t ns);
@@ -49,7 +49,7 @@ static int vtimer_emulate_32(struct cpu_user_regs *regs, union hsr hsr)
 {
     struct vcpu *v = current;
     struct hsr_cp32 cp32 = hsr.cp32;
-    uint32_t *r = &regs->r0 + cp32.reg;
+    uint32_t *r = select_user_reg(regs, cp32.reg);
     s_time_t now;
 
     switch ( hsr.bits & HSR_CP32_REGS_MASK )
@@ -101,8 +101,8 @@ static int vtimer_emulate_64(struct cpu_user_regs *regs, union hsr hsr)
 {
     struct vcpu *v = current;
     struct hsr_cp64 cp64 = hsr.cp64;
-    uint32_t *r1 = &regs->r0 + cp64.reg1;
-    uint32_t *r2 = &regs->r0 + cp64.reg2;
+    uint32_t *r1 = select_user_reg(regs, cp64.reg1);
+    uint32_t *r2 = select_user_reg(regs, cp64.reg2);
     uint64_t ticks;
     s_time_t now;
 
diff --git a/xen/include/asm-arm/regs.h b/xen/include/asm-arm/regs.h
index 54f6ed8..7486944 100644
--- a/xen/include/asm-arm/regs.h
+++ b/xen/include/asm-arm/regs.h
@@ -30,6 +30,12 @@
 
 #define return_reg(v) ((v)->arch.cpu_info->guest_cpu_user_regs.r0)
 
+/*
+ * Returns a pointer to the given register value in regs, taking the
+ * processor mode (CPSR) into account.
+ */
+extern uint32_t *select_user_reg(struct cpu_user_regs *regs, int reg);
+
 #endif /* __ARM_REGS_H__ */
 /*
  * Local variables:
-- 
1.7.2.5