[Qemu-devel] [PATCH 34/48] target-arm: add support for smc

[Riku Voipio]

From: Juha Riihimäki <address@hidden>

Signed-Off-By: Riku Voipio <address@hidden>
Signed-off-by: Juha Riihimäki <address@hidden>
---
 hw/omap3_boot.c        |   32 +++++++++++++++--------
 target-arm/cpu.h       |   10 +++++--
 target-arm/helper.c    |   67 +++++++++++++++++++++++++++++++++++++++++++-----
 target-arm/translate.c |   31 ++++++++++++++++++---
 4 files changed, 114 insertions(+), 26 deletions(-)

diff --git a/hw/omap3_boot.c b/hw/omap3_boot.c
index c079c1d..6f3389f 100644
--- a/hw/omap3_boot.c
+++ b/hw/omap3_boot.c
@@ -98,12 +98,17 @@ static const uint8_t omap3_boot_rom[] = { /* 
0x40014000-0x4001bfff */
     0xff, 0xff, 0xff, 0xff, /* 0x40014048: booting parameter structure 4-7 */
     0xff, 0xff, 0xff, 0xff, /* 0x4001404c: booting parameter structure 8-11 */
     0x0e, 0xf0, 0xb0, 0xe1, /* 0x40014050: "movs pc, lr" */
-    0xff, 0xff, 0xff, 0xff,
-    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+    0xff, 0xff, 0xff, 0xff, /* 0x40014054 */
+    0xff, 0xff, 0xff, 0xff, /* 0x40014058 */
+    0xff, 0xff, 0xff, 0xff, /* 0x4001405c */
+    0xfe, 0xff, 0xff, 0xea, /* 0x40014060: monitor vector 0 (unused) */
+    0xfe, 0xff, 0xff, 0xea, /* 0x40014064: monitor vector 1 (unused) */
+    0x15, 0x00, 0x00, 0xea, /* 0x40014068: monitor vector 2 (smc) */
+    0xfe, 0xff, 0xff, 0xea, /* 0x4001406c: monitor vector 3 (pabt) */
+    0xfe, 0xff, 0xff, 0xea, /* 0x40014070: monitor vector 4 (dabt) */
+    0xfe, 0xff, 0xff, 0xea, /* 0x40014074: monitor vector 5 (unused) */
+    0xfe, 0xff, 0xff, 0xea, /* 0x40014078: monitor vector 6 (irq) */
+    0xfe, 0xff, 0xff, 0xea, /* 0x4001407c: monitor vector 7 (fiq) */
     /* 0x40014080: Dead loops */
     0xfe, 0xff, 0xff, 0xea, /* b 0x40014080 @ undefined exception */
     0xfe, 0xff, 0xff, 0xea, /* b 0x40014084 @ swi exception */
@@ -123,9 +128,12 @@ static const uint8_t omap3_boot_rom[] = { /* 
0x40014000-0x4001bfff */
     0xfe, 0xff, 0xff, 0xea, /* b 0x400140bc @ reserved */
     /* 0x400140c0: should perform a software reset & jump to r0 */
     0x00, 0xf0, 0xa0, 0xe1, /* mov pc, r0 */
-    0xff, 0xff, 0xff, 0xff,
-    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+    /* 0x400140c4: monitor mode smc vector handler */
+    0x02, 0x00, 0x5c, 0xe3, /* cmp r12, #2 */
+    0x50, 0x0f, 0x29, 0xee, /* mcreq p15, 1, r0, c9, c0, 2 @ l2c aux ctrl */
+    0x03, 0x00, 0x5c, 0xe3, /* cmp r12, #3 */
+    0x30, 0x0f, 0x01, 0xee, /* mcreq p15, 0, r0, c1, c0, 1 @ aux ctrl*/
+    0x0e, 0xf0, 0xb0, 0xe1, /* movs pc, r14 */
     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
@@ -153,8 +161,10 @@ static const uint8_t omap3_boot_rom[] = { /* 
0x40014000-0x4001bfff */
     0x08, 0x0c, 0x40, 0xe2, /* sub r0, r0, #2048  @ 2kB UND stack */
     0xd3, 0xf0, 0x21, 0xe3, /* msr cpsr_c, #0xd3  @ enter SVC mode */
     0x00, 0xd0, 0xa0, 0xe1, /* mov sp, r0         @ 23kB left for SVC stack */
-    0x44, 0x00, 0x04, 0xe3, /* movw r0, #0x4044 */
-    0x01, 0x00, 0x44, 0xe3, /* movt r0, #0x4001   @ r0 -> booting parameter 
struct */
+    0x60, 0x00, 0x04, 0xe3, /* movw r0, #0x4060   @ r0 -> monitor vba */
+    0x01, 0x00, 0x44, 0xe3, /* movt r0, #0x4001 */
+    0x30, 0x0f, 0x0c, 0xfe, /* mcr2 p15, 0, r0, c12, c0, 1 */
+    0x1c, 0x00, 0x40, 0xe2, /* sub r0, r0, #1c    @ r0 -> booting parameter 
struct */
     0x01, 0xf0, 0xa0, 0xe1, /* mov pc, r1 */
 };
 
diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index 3c5e181..7440163 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -41,6 +41,7 @@
 #define EXCP_EXCEPTION_EXIT  8   /* Return from v7M exception.  */
 #define EXCP_KERNEL_TRAP     9   /* Jumped to kernel code page.  */
 #define EXCP_STREX          10
+#define EXCP_SMC            11   /* secure monitor call */
 
 #define ARMV7M_EXCP_RESET   1
 #define ARMV7M_EXCP_NMI     2
@@ -80,9 +81,9 @@ typedef struct CPUARMState {
     uint32_t spsr;
 
     /* Banked registers.  */
-    uint32_t banked_spsr[6];
-    uint32_t banked_r13[6];
-    uint32_t banked_r14[6];
+    uint32_t banked_spsr[7];
+    uint32_t banked_r13[7];
+    uint32_t banked_r14[7];
 
     /* These hold r8-r12.  */
     uint32_t usr_regs[5];
@@ -129,6 +130,8 @@ typedef struct CPUARMState {
         uint32_t c6_data;
         uint32_t c9_insn; /* Cache lockdown registers.  */
         uint32_t c9_data;
+        uint32_t c12_vbar; /* secure/nonsecure vector base address register. */
+        uint32_t c12_mvbar; /* monitor vector base address register. */
         uint32_t c13_fcse; /* FCSE PID.  */
         uint32_t c13_context; /* Context ID.  */
         uint32_t c13_tls1; /* User RW Thread register.  */
@@ -308,6 +311,7 @@ enum arm_cpu_mode {
   ARM_CPU_MODE_FIQ = 0x11,
   ARM_CPU_MODE_IRQ = 0x12,
   ARM_CPU_MODE_SVC = 0x13,
+  ARM_CPU_MODE_SMC = 0x16,
   ARM_CPU_MODE_ABT = 0x17,
   ARM_CPU_MODE_UND = 0x1b,
   ARM_CPU_MODE_SYS = 0x1f
diff --git a/target-arm/helper.c b/target-arm/helper.c
index a726947..798e9f9 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -586,6 +586,8 @@ static inline int bank_number (int mode)
         return 4;
     case ARM_CPU_MODE_FIQ:
         return 5;
+    case ARM_CPU_MODE_SMC:
+        return 6;
     }
     cpu_abort(cpu_single_env, "Bad mode %x\n", mode);
     return -1;
@@ -835,13 +837,39 @@ void do_interrupt(CPUARMState *env)
         mask = CPSR_A | CPSR_I | CPSR_F;
         offset = 4;
         break;
+    case EXCP_SMC:
+        if (semihosting_enabled) {
+            cpu_abort(env, "SMC handling under semihosting not implemented\n");
+            return;
+        }
+        if ((env->uncached_cpsr & CPSR_M) == ARM_CPU_MODE_SMC) {
+            env->cp15.c1_secfg &= ~1;
+        }
+        offset = env->thumb ? 2 : 0;
+        new_mode = ARM_CPU_MODE_SMC;
+        addr = 0x08;
+        mask = CPSR_A | CPSR_I | CPSR_F;
+        break;
     default:
         cpu_abort(env, "Unhandled exception 0x%x\n", env->exception_index);
         return; /* Never happens.  Keep compiler happy.  */
     }
-    /* High vectors.  */
-    if (env->cp15.c1_sys & (1 << 13)) {
-        addr += 0xffff0000;
+    if (arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
+        if (new_mode == ARM_CPU_MODE_SMC ||
+            (env->uncached_cpsr & CPSR_M) == ARM_CPU_MODE_SMC) {
+            addr += env->cp15.c12_mvbar;
+        } else {
+            if (env->cp15.c1_sys & (1 << 13)) {
+                addr += 0xffff0000;
+            } else {
+                addr += env->cp15.c12_vbar;
+            }
+        }
+    } else {
+        /* High vectors.  */
+        if (env->cp15.c1_sys & (1 << 13)) {
+            addr += 0xffff0000;
+        }
     }
     switch_mode (env, new_mode);
     env->spsr = cpsr_read(env);
@@ -1538,6 +1566,27 @@ void HELPER(set_cp15)(CPUState *env, uint32_t insn, 
uint32_t val)
         /* ??? TLB lockdown not implemented.  */
         break;
     case 12: /* Reserved.  */
+        if (!op1 && !crm) {
+            switch (op2) {
+            case 0:
+                if (!arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
+                    goto bad_reg;
+                }
+                env->cp15.c12_vbar = val & ~0x1f;
+                break;
+            case 1: 
+                if (!arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
+                    goto bad_reg;
+                }
+                if (!(env->cp15.c1_secfg & 1)) {
+                    env->cp15.c12_mvbar = val & ~0x1f;
+                }
+                break;
+            default:
+                goto bad_reg;
+            }
+            break;
+        }
         goto bad_reg;
     case 13: /* Process ID.  */
         switch (op2) {
@@ -1856,12 +1905,16 @@ uint32_t HELPER(get_cp15)(CPUState *env, uint32_t insn)
         return 0;
     case 11: /* TCM DMA control.  */
     case 12: /* Reserved.  */
-        if (!op1) {
-            switch (crm) {
+        if (!op1 && !crm) {
+            switch (op2) {
             case 0: /* secure or nonsecure vector base address */
                 if (arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
-                    /* FIXME: implement true vector base addressing */
-                    return 0; /* reset value according to ARM Cortex-A8 TRM */
+                    return env->cp15.c12_vbar;
+                }
+                break;
+            case 1: /* monitor vector base address */
+                if (arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
+                    return env->cp15.c12_mvbar;
                 }
                 break;
             default:
diff --git a/target-arm/translate.c b/target-arm/translate.c
index b214bff..e5f5cfb 100644
--- a/target-arm/translate.c
+++ b/target-arm/translate.c
@@ -73,6 +73,7 @@ typedef struct DisasContext {
    conditional executions state has been updated.  */
 #define DISAS_WFI 4
 #define DISAS_SWI 5
+#define DISAS_SMC 6
 
 static TCGv_ptr cpu_env;
 /* We reuse the same 64-bit temporaries for efficiency.  */
@@ -837,6 +838,12 @@ static inline void store_reg_bx(CPUState *env, 
DisasContext *s,
     }
 }
 
+static inline void gen_smc(CPUState *env, DisasContext *s)
+{
+    tcg_gen_movi_i32(cpu_R[15], s->pc);
+    s->is_jmp = DISAS_SMC;
+}
+
 static inline TCGv gen_ld8s(TCGv addr, int index)
 {
     TCGv tmp = new_tmp();
@@ -6245,8 +6252,12 @@ static void disas_arm_insn(CPUState * env, DisasContext 
*s)
             }
         } else if ((insn & 0x0fe00000) == 0x0c400000) {
             /* Coprocessor double register transfer.  */
+            cpu_abort(env, "unsupported coprocessor double register 
transfer\n");
         } else if ((insn & 0x0f000010) == 0x0e000010) {
             /* Additional coprocessor register transfer.  */
+            if (!disas_coproc_insn(env, s, insn)) {
+                return;
+            }
         } else if ((insn & 0x0ff10020) == 0x01000000) {
             uint32_t mask;
             uint32_t val;
@@ -6401,10 +6412,10 @@ static void disas_arm_insn(CPUState * env, DisasContext 
*s)
                 s->is_jmp = DISAS_JUMP;
             } else if (op1 == 3) {
                 /* smi/smc */
-                if (!(env->cp15.c0_c2[4] & 0xf000) || IS_USER(s))
+                if (!(env->cp15.c0_c2[4] & 0xf000) || IS_USER(s)) {
                     goto illegal_op;
-                /* TODO: real implementation; execute as NOP for now */
-                /*fprintf(stderr, "smc [0x%08x] pc=0x%08x\n", insn, s->pc);*/
+                }
+                gen_smc(env, s);
             } else {
                 goto illegal_op;
             }
@@ -7991,8 +8002,11 @@ static int disas_thumb2_insn(CPUState *env, DisasContext 
*s, uint16_t insn_hw1)
                     goto illegal_op;
 
                 if (insn & (1 << 26)) {
-                    /* Secure monitor call (v6Z) */
-                    goto illegal_op; /* not implemented.  */
+                    /* Secure monitor call / smc (v6Z) */
+                    if (!(env->cp15.c0_c2[4] & 0xf000) || IS_USER(s)) {
+                        goto illegal_op;
+                    }
+                    gen_smc(env, s);
                 } else {
                     op = (insn >> 20) & 7;
                     switch (op) {
@@ -9245,6 +9259,8 @@ static inline void 
gen_intermediate_code_internal(CPUState *env,
             gen_set_condexec(dc);
             if (dc->is_jmp == DISAS_SWI) {
                 gen_exception(EXCP_SWI);
+            } else if (dc->is_jmp == DISAS_SMC) {
+                gen_exception(EXCP_SMC);
             } else {
                 gen_exception(EXCP_DEBUG);
             }
@@ -9257,6 +9273,8 @@ static inline void 
gen_intermediate_code_internal(CPUState *env,
         gen_set_condexec(dc);
         if (dc->is_jmp == DISAS_SWI && !dc->condjmp) {
             gen_exception(EXCP_SWI);
+        } else if (dc->is_jmp == DISAS_SMC && !dc->condjmp) {
+            gen_exception(EXCP_SMC);
         } else {
             /* FIXME: Single stepping a WFI insn will not halt
                the CPU.  */
@@ -9291,6 +9309,9 @@ static inline void 
gen_intermediate_code_internal(CPUState *env,
         case DISAS_SWI:
             gen_exception(EXCP_SWI);
             break;
+        case DISAS_SMC:
+            gen_exception(EXCP_SMC);
+            break;
         }
         if (dc->condjmp) {
             gen_set_label(dc->condlabel);
-- 
1.6.5