[Qemu-devel] [PATCH v2 21/23] target-arm: implement SMC instruction

[Fabian Aggeler <aggelerf@ethz.ch>]

From: Sergey Fedorov <s.fedorov@samsung.com>

SMC instruction is implemented similar to SVC instruction. When
executing SMC instruction from monitor CPU mode SCR.NS bit is reset.

Signed-off-by: Sergey Fedorov <s.fedorov@samsung.com>
Signed-off-by: Fabian Aggeler <aggelerf@ethz.ch>
---
 target-arm/cpu.h       |  1 +
 target-arm/helper.c    | 11 +++++++++++
 target-arm/translate.c | 38 ++++++++++++++++++++++++++++----------
 target-arm/translate.h |  2 ++
 4 files changed, 42 insertions(+), 10 deletions(-)

diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index 9b6f8bd..f6261c2 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -51,6 +51,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
diff --git a/target-arm/helper.c b/target-arm/helper.c
index 808b822..deff3de 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -3605,6 +3605,12 @@ void arm_cpu_do_interrupt(CPUState *cs)
         mask = CPSR_A | CPSR_I | CPSR_F;
         offset = 4;
         break;
+    case EXCP_SMC:
+        new_mode = ARM_CPU_MODE_MON;
+        addr = 0x08;
+        mask = CPSR_A | CPSR_I | CPSR_F;
+        offset = 0;
+        break;
     default:
         cpu_abort(cs, "Unhandled exception 0x%x\n", cs->exception_index);
         return; /* Never happens.  Keep compiler happy.  */
@@ -3622,6 +3628,11 @@ void arm_cpu_do_interrupt(CPUState *cs)
          */
         addr += A32_MAPPED_EL3_CURRENT_REG_GET(env, c12_vbar);
     }
+
+    if ((env->uncached_cpsr & CPSR_M) == ARM_CPU_MODE_MON) {
+        env->cp15.c1_scr &= ~1 /* NS */;
+    }
+
     switch_mode (env, new_mode);
     env->spsr = cpsr_read(env);
     /* Clear IT bits.  */
diff --git a/target-arm/translate.c b/target-arm/translate.c
index 3a429ac..e7a6cfa 100644
--- a/target-arm/translate.c
+++ b/target-arm/translate.c
@@ -7767,15 +7767,23 @@ static void disas_arm_insn(CPUARMState * env, DisasContext *s)
         case 7:
         {
             int imm16 = extract32(insn, 0, 4) | (extract32(insn, 8, 12) << 4);
-            /* SMC instruction (op1 == 3)
-               and undefined instructions (op1 == 0 || op1 == 2)
-               will trap */
-            if (op1 != 1) {
+            if (op1 == 1) {
+                /* bkpt */
+                ARCH(5);
+                gen_exception_insn(s, 4, EXCP_BKPT,
+                        syn_aa32_bkpt(imm16, false));
+            } else if (op1 == 3) {
+                /* smi/smc */
+                if (!arm_feature(env, ARM_FEATURE_SECURITY_EXTENSIONS) ||
+                        IS_USER(s)) {
+                    goto illegal_op;
+                }
+                gen_set_pc_im(s, s->pc);
+                s->is_jmp = DISAS_SMC;
+                break;
+            } else {
                 goto illegal_op;
             }
-            /* bkpt */
-            ARCH(5);
-            gen_exception_insn(s, 4, EXCP_BKPT, syn_aa32_bkpt(imm16, false));
             break;
         }
         case 0x8: /* signed multiply */
@@ -9597,9 +9605,12 @@ static int disas_thumb2_insn(CPUARMState *env, DisasContext *s, uint16_t insn_hw
 
                 if (insn & (1 << 26)) {
                     /* Secure monitor call (v6Z) */
-                    qemu_log_mask(LOG_UNIMP,
-                                  "arm: unimplemented secure monitor call\n");
-                    goto illegal_op; /* not implemented.  */
+                    if (!arm_feature(env, ARM_FEATURE_SECURITY_EXTENSIONS) ||
+                            IS_USER(s)) {
+                        goto illegal_op;
+                    }
+                    gen_set_pc_im(s, s->pc);
+                    s->is_jmp = DISAS_SMC;
                 } else {
                     op = (insn >> 20) & 7;
                     switch (op) {
@@ -10998,6 +11009,8 @@ static inline void gen_intermediate_code_internal(ARMCPU *cpu,
             gen_set_condexec(dc);
             if (dc->is_jmp == DISAS_SWI) {
                 gen_exception(EXCP_SWI, syn_aa32_svc(dc->svc_imm, dc->thumb));
+            } else if (dc->is_jmp == DISAS_SMC) {
+                gen_exception(EXCP_SMC, syn_uncategorized());
             } else {
                 gen_exception_internal(EXCP_DEBUG);
             }
@@ -11010,6 +11023,8 @@ static inline void gen_intermediate_code_internal(ARMCPU *cpu,
         gen_set_condexec(dc);
         if (dc->is_jmp == DISAS_SWI && !dc->condjmp) {
             gen_exception(EXCP_SWI, syn_aa32_svc(dc->svc_imm, dc->thumb));
+        } else if (dc->is_jmp == DISAS_SMC && !dc->condjmp) {
+            gen_exception(EXCP_SMC, syn_uncategorized());
         } else {
             /* FIXME: Single stepping a WFI insn will not halt
                the CPU.  */
@@ -11047,6 +11062,9 @@ static inline void gen_intermediate_code_internal(ARMCPU *cpu,
         case DISAS_SWI:
             gen_exception(EXCP_SWI, syn_aa32_svc(dc->svc_imm, dc->thumb));
             break;
+        case DISAS_SMC:
+            gen_exception(EXCP_SMC, syn_uncategorized());
+            break;
         }
         if (dc->condjmp) {
             gen_set_label(dc->condlabel);
diff --git a/target-arm/translate.h b/target-arm/translate.h
index eeb77fb..bd9a416 100644
--- a/target-arm/translate.h
+++ b/target-arm/translate.h
@@ -67,6 +67,8 @@ static inline int arm_dc_feature(DisasContext *dc, int feature)
 #define DISAS_EXC 6
 /* WFE */
 #define DISAS_WFE 7
+/* Secure Monitor Call (SMC) */
+#define DISAS_SMC 8
 
 #ifdef TARGET_AARCH64
 void a64_translate_init(void);
-- 
1.8.3.2