[Qemu-devel] [PACTH v4 5/6] target-arm: add emulation of PSCI calls for system emulation

[Ard Biesheuvel]

From: Rob Herring <address@hidden>

Add support for handling PSCI calls in system emulation. Both version
0.1 and 0.2 of the PSCI spec are supported. Platforms can enable support
by setting the "psci-conduit" QOM property on the cpus to SMC or HVC
emulation and having a PSCI binding in their dtb.

Signed-off-by: Rob Herring <address@hidden>
Signed-off-by: Ard Biesheuvel <address@hidden>
---
 target-arm/Makefile.objs   |   1 +
 target-arm/cpu-qom.h       |   6 ++
 target-arm/cpu.c           |  10 ++-
 target-arm/cpu.h           |   6 ++
 target-arm/helper.c        |  12 +++
 target-arm/psci.c          | 183 +++++++++++++++++++++++++++++++++++++++++++++
 target-arm/translate-a64.c |   7 +-
 target-arm/translate.h     |   2 +
 8 files changed, 222 insertions(+), 5 deletions(-)
 create mode 100644 target-arm/psci.c

diff --git a/target-arm/Makefile.objs b/target-arm/Makefile.objs
index dcd167e0d880..9460b409a5a1 100644
--- a/target-arm/Makefile.objs
+++ b/target-arm/Makefile.objs
@@ -7,5 +7,6 @@ obj-$(call lnot,$(CONFIG_KVM)) += kvm-stub.o
 obj-y += translate.o op_helper.o helper.o cpu.o
 obj-y += neon_helper.o iwmmxt_helper.o
 obj-y += gdbstub.o
+obj-$(CONFIG_SOFTMMU) += psci.o
 obj-$(TARGET_AARCH64) += cpu64.o translate-a64.o helper-a64.o gdbstub64.o
 obj-y += crypto_helper.o
diff --git a/target-arm/cpu-qom.h b/target-arm/cpu-qom.h
index 104cc67e82d2..bed7190bae57 100644
--- a/target-arm/cpu-qom.h
+++ b/target-arm/cpu-qom.h
@@ -101,6 +101,11 @@ typedef struct ARMCPU {
     /* CPU currently in PSCI powered-off state */
     bool powered_off;
 
+    /* PSCI conduit used to invoke PSCI methods
+     * 0 - disabled, 1 - smc, 2 - hvc
+     */
+    uint32_t psci_conduit;
+
     /* [QEMU_]KVM_ARM_TARGET_* constant for this CPU, or
      * QEMU_KVM_ARM_TARGET_NONE if the kernel doesn't support this CPU type.
      */
@@ -192,6 +197,7 @@ extern const struct VMStateDescription vmstate_arm_cpu;
 void register_cp_regs_for_features(ARMCPU *cpu);
 void init_cpreg_list(ARMCPU *cpu);
 
+bool arm_handle_psci(CPUState *cs);
 bool arm_cpu_do_hvc(CPUState *cs);
 bool arm_cpu_do_smc(CPUState *cs);
 
diff --git a/target-arm/cpu.c b/target-arm/cpu.c
index 55479ec8b226..eba0271a852e 100644
--- a/target-arm/cpu.c
+++ b/target-arm/cpu.c
@@ -268,9 +268,12 @@ static void arm_cpu_initfn(Object *obj)
     cpu->psci_version = 1; /* By default assume PSCI v0.1 */
     cpu->kvm_target = QEMU_KVM_ARM_TARGET_NONE;
 
-    if (tcg_enabled() && !inited) {
-        inited = true;
-        arm_translate_init();
+    if (tcg_enabled()) {
+        cpu->psci_version = 2; /* TCG implements PSCI 0.2 */
+        if (!inited) {
+            inited = true;
+            arm_translate_init();
+        }
     }
 }
 
@@ -1024,6 +1027,7 @@ static const ARMCPUInfo arm_cpus[] = {
 
 static Property arm_cpu_properties[] = {
     DEFINE_PROP_BOOL("start-powered-off", ARMCPU, start_powered_off, false),
+    DEFINE_PROP_UINT32("psci-conduit", ARMCPU, psci_conduit, 0),
     DEFINE_PROP_UINT32("midr", ARMCPU, midr, 0),
     DEFINE_PROP_END_OF_LIST()
 };
diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index d235929f4c12..a69d69254af4 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -1350,4 +1350,10 @@ static inline void cpu_pc_from_tb(CPUARMState *env, 
TranslationBlock *tb)
     }
 }
 
+enum {
+    QEMU_PSCI_CONDUIT_DISABLED = 0,
+    QEMU_PSCI_CONDUIT_SMC = 1,
+    QEMU_PSCI_CONDUIT_HVC = 2,
+};
+
 #endif
diff --git a/target-arm/helper.c b/target-arm/helper.c
index 64bd49ecdaf9..2df34adc0ff7 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -3494,11 +3494,23 @@ void arm_v7m_cpu_do_interrupt(CPUState *cs)
 
 bool arm_cpu_do_hvc(CPUState *cs)
 {
+    ARMCPU *cpu = ARM_CPU(cs);
+
+    if (cpu->psci_conduit == QEMU_PSCI_CONDUIT_HVC) {
+        return arm_handle_psci(cs);
+    }
+
     return false;
 }
 
 bool arm_cpu_do_smc(CPUState *cs)
 {
+    ARMCPU *cpu = ARM_CPU(cs);
+
+    if (cpu->psci_conduit == QEMU_PSCI_CONDUIT_SMC) {
+        return arm_handle_psci(cs);
+    }
+
     return false;
 }
 
diff --git a/target-arm/psci.c b/target-arm/psci.c
new file mode 100644
index 000000000000..7347cbdc17ef
--- /dev/null
+++ b/target-arm/psci.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2014 - Linaro
+ * Author: Rob Herring <address@hidden>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+#include <cpu.h>
+#include <cpu-qom.h>
+#include <exec/helper-proto.h>
+#include <kvm-consts.h>
+#include <sysemu/sysemu.h>
+
+bool arm_handle_psci(CPUState *cs)
+{
+    /*
+     * This function partially implements the logic for dispatching Power State
+     * Coordination Interface (PSCI) calls (as described in ARM DEN 0022B.b),
+     * to the extent required for bringing up and taking down secondary cores,
+     * and for handling reset and poweroff requests.
+     * Additional information about the calling convention used is available in
+     * the document 'SMC Calling Convention' (ARM DEN 0028)
+     */
+    ARMCPU *cpu = ARM_CPU(cs);
+    CPUARMState *env = &cpu->env;
+    uint64_t param[4];
+    uint64_t context_id, mpidr;
+    target_ulong entry;
+    int32_t ret = 0;
+    int i;
+
+    for (i = 0; i < 4; i++) {
+        /*
+         * All PSCI functions take explicit 32-bit or native int sized
+         * arguments so we can simply zero-extend all arguments regardless
+         * of which exact function we are about to call.
+         */
+        param[i] = is_a64(env) ? env->xregs[i] : env->regs[i];
+    }
+
+    if ((param[0] & QEMU_PSCI_0_2_64BIT) && !is_a64(env)) {
+        ret = QEMU_PSCI_RET_INVALID_PARAMS;
+        goto err;
+    }
+
+    switch (param[0]) {
+        CPUState *target_cpu_state;
+        ARMCPU *target_cpu;
+        CPUClass *target_cpu_class;
+
+    case QEMU_PSCI_0_2_FN_PSCI_VERSION:
+        ret = QEMU_PSCI_0_2_RET_VERSION_0_2;
+        break;
+    case QEMU_PSCI_0_2_FN_MIGRATE_INFO_TYPE:
+        ret = QEMU_PSCI_0_2_RET_TOS_MIGRATION_NOT_REQUIRED; /* No trusted OS */
+        break;
+    case QEMU_PSCI_0_2_FN_AFFINITY_INFO:
+    case QEMU_PSCI_0_2_FN64_AFFINITY_INFO:
+        mpidr = param[1];
+
+        switch (param[2]) {
+        case 0:
+            target_cpu_state = qemu_get_cpu(mpidr & 0xff);
+            if (!target_cpu_state) {
+                ret = QEMU_PSCI_RET_INVALID_PARAMS;
+                break;
+            }
+            target_cpu = ARM_CPU(target_cpu_state);
+            ret = target_cpu->powered_off ? 1 : 0;
+            break;
+        default:
+            /* Everything above affinity level 0 is always on. */
+            ret = 0;
+        }
+        break;
+    case QEMU_PSCI_0_2_FN_SYSTEM_RESET:
+        qemu_system_reset_request();
+        break;
+    case QEMU_PSCI_0_2_FN_SYSTEM_OFF:
+        qemu_system_shutdown_request();
+        break;
+    case QEMU_PSCI_0_1_FN_CPU_ON:
+    case QEMU_PSCI_0_2_FN_CPU_ON:
+    case QEMU_PSCI_0_2_FN64_CPU_ON:
+        mpidr = param[1];
+        entry = param[2];
+        context_id = param[3];
+
+        /* change to the cpu we are powering up */
+        target_cpu_state = qemu_get_cpu(mpidr & 0xff);
+        if (!target_cpu_state) {
+            ret = QEMU_PSCI_RET_INVALID_PARAMS;
+            break;
+        }
+        target_cpu = ARM_CPU(target_cpu_state);
+        if (!target_cpu->powered_off) {
+            ret = QEMU_PSCI_RET_ALREADY_ON;
+            break;
+        }
+        target_cpu_class = CPU_GET_CLASS(target_cpu);
+
+        /* Initialize the cpu we are turning on */
+        cpu_reset(target_cpu_state);
+        target_cpu->powered_off = false;
+        target_cpu_state->halted = 0;
+
+        /*
+         * The PSCI spec mandates that newly brought up CPUs enter the
+         * exception level of the caller in the same execution mode as
+         * the caller, with context_id in x0/r0, respectively.
+         *
+         * For now, it is sufficient to assert() that CPUs come out of
+         * reset in the same mode as the calling CPU, since we only
+         * implement EL1, which means that
+         * (a) there is no EL2 for the calling CPU to trap into to change
+         *     its state
+         * (b) the newly brought up CPU enters EL1 immediately after coming
+         *     out of reset in the default state
+         */
+        assert(is_a64(env) == is_a64(&target_cpu->env));
+        if (is_a64(env)) {
+            if (entry & 1) {
+                ret = QEMU_PSCI_RET_INVALID_PARAMS;
+                break;
+            }
+            target_cpu->env.xregs[0] = context_id;
+        } else {
+            target_cpu->env.regs[0] = context_id;
+            target_cpu->env.thumb = entry & 1;
+        }
+        target_cpu_class->set_pc(target_cpu_state, entry);
+
+        ret = 0;
+        break;
+    case QEMU_PSCI_0_1_FN_CPU_OFF:
+    case QEMU_PSCI_0_2_FN_CPU_OFF:
+        cpu->powered_off = true;
+        cs->halted = 1;
+        cs->exception_index = EXCP_HLT;
+        cpu_loop_exit(cs);
+        /* notreached */
+    case QEMU_PSCI_0_1_FN_CPU_SUSPEND:
+    case QEMU_PSCI_0_2_FN_CPU_SUSPEND:
+    case QEMU_PSCI_0_2_FN64_CPU_SUSPEND:
+        /* Affinity levels are not supported in QEMU */
+        if (param[1] & 0xfffe0000) {
+            ret = QEMU_PSCI_RET_INVALID_PARAMS;
+            break;
+        }
+        /* Powerdown is not supported, we always go into WFI */
+        if (is_a64(env)) {
+            env->xregs[0] = 0;
+        } else {
+            env->regs[0] = 0;
+        }
+        helper_wfi(env);
+        break;
+    case QEMU_PSCI_0_1_FN_MIGRATE:
+    case QEMU_PSCI_0_2_FN_MIGRATE:
+        ret = QEMU_PSCI_RET_NOT_SUPPORTED;
+        break;
+    default:
+        return false;
+    }
+
+err:
+    if (is_a64(env)) {
+        env->xregs[0] = ret;
+    } else {
+        env->regs[0] = ret;
+    }
+    return true;
+}
diff --git a/target-arm/translate-a64.c b/target-arm/translate-a64.c
index e5fb775c4a50..fa5c82ee37a8 100644
--- a/target-arm/translate-a64.c
+++ b/target-arm/translate-a64.c
@@ -1485,7 +1485,8 @@ static void disas_exc(DisasContext *s, uint32_t insn)
             break;
         case 2:
             if (s->current_pl != 0
-                && arm_dc_feature(s, ARM_FEATURE_EL2)) {
+                && (arm_dc_feature(s, ARM_FEATURE_EL2)
+                    || s->psci_conduit == QEMU_PSCI_CONDUIT_HVC)) {
                 gen_ss_advance(s);
                 gen_exception_insn(s, 0, EXCP_HVC, syn_aa64_hvc(imm16));
                 break;
@@ -1494,7 +1495,8 @@ static void disas_exc(DisasContext *s, uint32_t insn)
             break;
         case 3:
             if (s->current_pl != 0
-                && arm_dc_feature(s, ARM_FEATURE_EL3)) {
+                && (arm_dc_feature(s, ARM_FEATURE_EL3)
+                    || s->psci_conduit == QEMU_PSCI_CONDUIT_SMC)) {
                 gen_ss_advance(s);
                 gen_exception_insn(s, 0, EXCP_SMC, syn_aa64_smc(imm16));
                 break;
@@ -10943,6 +10945,7 @@ void gen_intermediate_code_internal_a64(ARMCPU *cpu,
     dc->pstate_ss = ARM_TBFLAG_AA64_PSTATE_SS(tb->flags);
     dc->is_ldex = false;
     dc->ss_same_el = (arm_debug_target_el(env) == dc->current_pl);
+    dc->psci_conduit = cpu->psci_conduit;
 
     init_tmp_a64_array(dc);
 
diff --git a/target-arm/translate.h b/target-arm/translate.h
index 50c4d6ef4ab6..7babe79e123d 100644
--- a/target-arm/translate.h
+++ b/target-arm/translate.h
@@ -52,6 +52,8 @@ typedef struct DisasContext {
     bool is_ldex;
     /* True if a single-step exception will be taken to the current EL */
     bool ss_same_el;
+    /* Conduit used for PSCI method calls, either HVC or SMC (or disabled) */
+    uint32_t psci_conduit;
 #define TMP_A64_MAX 16
     int tmp_a64_count;
     TCGv_i64 tmp_a64[TMP_A64_MAX];
-- 
1.8.3.2