diff mbox

[v5,1/6] ARM: Factor out ARM on/off PSCI control functions

Message ID 994c0b4e133d2608675264dd1e4a85960a9cdeb6.1458989776.git.jcd@tribudubois.net
State New
Headers show

Commit Message

Jean-Christophe DUBOIS March 26, 2016, 11:06 a.m. UTC
Split ARM on/off function from PSCI support code.

This will allow to reuse these functions in other code.

Signed-off-by: Jean-Christophe Dubois <jcd@tribudubois.net>
---

Changes since V1:
 * Not present on V1

Changes since V2:
 * Not present on V2

Changes since V3:
 * Move to a more generic/usefull API
 * Manage CPU level/mode change at startup
 * Allow PSCI to cope with EL2/HYP level.
 * Keep distinct errors for different causes.

Changes since V4:
 * Rework documentation in header file.
 * Rework exception level handling.
 * Added a TODO for mode handling.

 target-arm/Makefile.objs  |   1 +
 target-arm/arm-powerctl.c | 239 ++++++++++++++++++++++++++++++++++++++++++++++
 target-arm/arm-powerctl.h |  75 +++++++++++++++
 target-arm/psci.c         |  68 +------------
 4 files changed, 320 insertions(+), 63 deletions(-)
 create mode 100644 target-arm/arm-powerctl.c
 create mode 100644 target-arm/arm-powerctl.h

Comments

Peter Maydell March 29, 2016, 7:46 p.m. UTC | #1
On 26 March 2016 at 11:06, Jean-Christophe Dubois <jcd@tribudubois.net> wrote:
> Split ARM on/off function from PSCI support code.
>
> This will allow to reuse these functions in other code.
>
> Signed-off-by: Jean-Christophe Dubois <jcd@tribudubois.net>
> ---
>
> Changes since V1:
>  * Not present on V1
>
> Changes since V2:
>  * Not present on V2
>
> Changes since V3:
>  * Move to a more generic/usefull API
>  * Manage CPU level/mode change at startup
>  * Allow PSCI to cope with EL2/HYP level.
>  * Keep distinct errors for different causes.
>
> Changes since V4:
>  * Rework documentation in header file.
>  * Rework exception level handling.
>  * Added a TODO for mode handling.
>
>  target-arm/Makefile.objs  |   1 +
>  target-arm/arm-powerctl.c | 239 ++++++++++++++++++++++++++++++++++++++++++++++
>  target-arm/arm-powerctl.h |  75 +++++++++++++++
>  target-arm/psci.c         |  68 +------------
>  4 files changed, 320 insertions(+), 63 deletions(-)
>  create mode 100644 target-arm/arm-powerctl.c
>  create mode 100644 target-arm/arm-powerctl.h
>
> diff --git a/target-arm/Makefile.objs b/target-arm/Makefile.objs
> index a80eb39..60fd1dd 100644
> --- a/target-arm/Makefile.objs
> +++ b/target-arm/Makefile.objs
> @@ -9,3 +9,4 @@ obj-y += neon_helper.o iwmmxt_helper.o
>  obj-y += gdbstub.o
>  obj-$(TARGET_AARCH64) += cpu64.o translate-a64.o helper-a64.o gdbstub64.o
>  obj-y += crypto_helper.o
> +obj-y += arm-powerctl.o
> diff --git a/target-arm/arm-powerctl.c b/target-arm/arm-powerctl.c
> new file mode 100644
> index 0000000..67e1579
> --- /dev/null
> +++ b/target-arm/arm-powerctl.c
> @@ -0,0 +1,239 @@
> +/*
> + * QEMU support -- ARM Power Control specific functions.
> + *
> + * Copyright (c) 2016 Jean-Christophe Dubois
> + *
> + * This work is licensed under the terms of the GNU GPL, version 2 or later.
> + * See the COPYING file in the top-level directory.
> + *
> + */
> +
> +#include "qemu/osdep.h"
> +#include <cpu.h>
> +#include <cpu-qom.h>
> +#include "internals.h"
> +#include "arm-powerctl.h"
> +
> +#ifndef DEBUG_ARM_POWERCTL
> +#define DEBUG_ARM_POWERCTL 0
> +#endif
> +
> +#define DPRINTF(fmt, args...) \
> +    do { \
> +        if (DEBUG_ARM_POWERCTL) { \
> +            fprintf(stderr, "[ARM]%s: " fmt , __func__, ##args); \
> +        } \
> +    } while (0)
> +
> +CPUState *arm_get_cpu_by_id(uint64_t id)
> +{
> +    CPUState *cpu;
> +
> +    DPRINTF("cpu %" PRId64 "\n", id);
> +
> +    CPU_FOREACH(cpu) {
> +        ARMCPU *armcpu = ARM_CPU(cpu);
> +
> +        if (armcpu->mp_affinity == id) {
> +            return cpu;
> +        }
> +    }
> +
> +    qemu_log_mask(LOG_GUEST_ERROR,
> +                  "[ARM]%s: Requesting unknown CPU %" PRId64 "\n",
> +                  __func__, id);
> +
> +    return NULL;
> +}
> +
> +int arm_set_cpu_on(uint64_t cpuid, uint64_t entry, uint64_t context_id,
> +                   int target_el, bool target_aa64)
> +{
> +    CPUState *target_cpu_state;
> +    ARMCPU *target_cpu;
> +
> +    DPRINTF("cpu %" PRId64 " (EL %d, %s) @ 0x%" PRIx64 " with R0 = 0x%" PRIx64
> +            "\n", cpuid, target_el, target_aa64 ? "aarch64" : "aarch32", entry,
> +            context_id);
> +
> +    /* requested EL level need to be above 0 */
> +    assert(target_el >= 1 && target_el <= 3);
> +
> +    /* change to the cpu we are powering up */

We're not changing any kind of current CPU state here...

> +    target_cpu_state = arm_get_cpu_by_id(cpuid);
> +    if (!target_cpu_state) {
> +        /* The cpu was not found */
> +        return QEMU_ARM_POWERCTL_INVALID_PARAM;
> +    }
> +    target_cpu = ARM_CPU(target_cpu_state);
> +    if (!target_cpu->powered_off) {
> +        qemu_log_mask(LOG_GUEST_ERROR,
> +                      "[ARM]%s: CPU %" PRId64 " is already on\n",
> +                      __func__, cpuid);
> +        return QEMU_ARM_POWERCTL_ALREADY_ON;
> +    }
> +
> +    /* Initialize the cpu we are turning on */
> +    cpu_reset(target_cpu_state);
> +    target_cpu->powered_off = false;
> +    target_cpu_state->halted = 0;
> +
> +    /*
> +     * The newly brought CPU is requested to enter the exception level
> +     * "target_el" and be in the requested mode (aarch64 ou aarch32).

"or"

> +     */
> +
> +    /*
> +     * Check that the requested mode is a supported feature.
> +     * 64 bits processors do support 32 bits mode but the opposite
> +     * is not true.
> +     */
> +    if (target_aa64 && !arm_feature(&target_cpu->env, ARM_FEATURE_AARCH64)) {
> +        return QEMU_ARM_POWERCTL_INVALID_PARAM;
> +    }
> +
> +    /* We check that the request can be fulfilled */
> +    if (target_aa64 != arm_el_is_aa64(&target_cpu->env, target_el)) {

This is going to return "invalid" for every 64-bit CPU which
supports EL2 or EL3 if target_aa64 is 64, which doesn't seem
right given you have the code below to set the HCR_EL2.RW
and SCR_EL3.RW bits.

If this was supposed to be doing the "don't try to support
setting env->aarch64 differently yet" handling, you need to do
that with if (target_aa64 != is_a64(target_cpu->env)).

> +        /*
> +         * TODO: We need to revisit this as the default config of a 64 bits
> +         * CPU might not allow to boot in 32 bits even if the CPU is
> +         * capable of it. We should implement some code to "force" the CPU in
> +         * the requested mode if it is not its default mode for the considered
> +         * exception level.
> +         */
> +        return QEMU_ARM_POWERCTL_INVALID_PARAM;
> +    }
> +
> +    /*
> +     * Check that the CPU is supporting the requested level
> +     */
> +    switch (target_el) {
> +    case 3:
> +        if (arm_feature(&target_cpu->env, ARM_FEATURE_EL3)) {
> +            if (target_aa64) {
> +                target_cpu->env.pstate = PSTATE_MODE_EL3h;
> +            } else {
> +                cpsr_write(&target_cpu->env, ARM_CPU_MODE_MON, CPSR_M,
> +                           CPSRWriteRaw);

We should probably start in Secure SVC here, not MON (since
that's what the CPU does in reset).

> +            }
> +            /* Processor is in secure mode */
> +            target_cpu->env.cp15.scr_el3 &= ~SCR_NS;
> +        } else {
> +            return QEMU_ARM_POWERCTL_INVALID_PARAM;
> +        }

These conditionals would read more clearly if you did them as
   if (some check) {
      return early;
   }
   normal code;

rather than
   if (!some check) {
      normal code;
   } else {
      return early;
   }

> +        break;
> +    case 2:
> +        if (arm_feature(&target_cpu->env, ARM_FEATURE_EL2)) {
> +            if (target_aa64) {
> +                if (arm_feature(&target_cpu->env, ARM_FEATURE_EL3)) {
> +                    /* Lower EL3 exception is aarch64 */

Not sure what you're trying to say with this comment ?

> +                    target_cpu->env.cp15.scr_el3 |= SCR_RW;
> +                }
> +                target_cpu->env.pstate = PSTATE_MODE_EL2h;
> +            } else {
> +                cpsr_write(&target_cpu->env, ARM_CPU_MODE_HYP, CPSR_M,
> +                           CPSRWriteRaw);
> +            }
> +            /* Processor is not in secure mode */
> +            target_cpu->env.cp15.scr_el3 |= SCR_NS;
> +        } else {
> +            return QEMU_ARM_POWERCTL_INVALID_PARAM;
> +        }
> +        break;
> +    case 1:
> +    default:
> +        if (target_aa64) {
> +            if (arm_feature(&target_cpu->env, ARM_FEATURE_EL3)) {
> +                /* Lower EL3 exception is aarch64 */
> +                target_cpu->env.cp15.scr_el3 |= SCR_RW;
> +            }
> +            if (arm_feature(&target_cpu->env, ARM_FEATURE_EL2)) {
> +                /* Lower EL2 exception is aarch64 */
> +                target_cpu->env.cp15.hcr_el2 |= HCR_RW;
> +            }
> +            target_cpu->env.pstate = PSTATE_MODE_EL1h;
> +        } else {
> +            cpsr_write(&target_cpu->env, ARM_CPU_MODE_SVC, CPSR_M,
> +                       CPSRWriteRaw);
> +        }
> +        /* Processor is not in secure mode */
> +        target_cpu->env.cp15.scr_el3 |= SCR_NS;
> +        break;
> +    }

These switch statement cases still all look quite repetitive.
I'm tempted to suggest this would look better as (slightly
pseudocoded):

   if ((target_el == 2 && !feature EL2) ||
       (target_el == 3 && !feature EL3)) {
       return failure;
   }
   if (target_aa64) {
      if (target_el < 3 && feature EL3) {
          scr_el3 |= SCR_RW;
      }
      if (target_el < 2 && feature EL2) {
          hcr_el2 |= HCR_RW;
      }
      pstate = aarch64_pstate_mode(target_el, true);
   } else {
      /* TODO perhaps we should support callers being able to start
       * us in other modes?
       */
      int mode_for_el = { 0,
                          ARM_CPU_MODE_SVC,
                          ARM_CPU_MODE_HYP,
                          ARM_CPU_MODE_MON /* or SVC ?? */ };

      cpsr_write(..., mode_for_el, ...);
   }
   if (feature EL3 && target_el < 3) {
      /* TODO perhaps we should support being able to start in
       * Secure EL1 ?
       */
      scr_el3 |= SCR_NS;
   }

> +
> +    /* We check if the started CPU is now in the correct level */
> +    assert(target_el == arm_current_el(&target_cpu->env));
> +
> +    if (target_aa64) {
> +        target_cpu->env.xregs[0] = context_id;
> +        target_cpu->env.thumb = false;
> +        /* check that the entry address is 4 bytes aligned */
> +        if (entry & 3) {
> +            return QEMU_ARM_POWERCTL_INVALID_PARAM;

You should do this check before you change the state of the
target CPU, since you can't really back that out.

> +        }
> +    } else {
> +        target_cpu->env.regs[0] = context_id;
> +        target_cpu->env.thumb = entry & 1;
> +        entry &= 0xfffffffe;
> +    }
> +
> +    /* Start the new CPU at the requested address */
> +    cpu_set_pc(target_cpu_state, entry);
> +
> +    /* We are good to go */
> +    return QEMU_ARM_POWERCTL_RET_SUCCESS;
> +}
> +
> +int arm_set_cpu_off(uint64_t cpuid)
> +{
> +    CPUState *target_cpu_state;
> +    ARMCPU *target_cpu;
> +
> +    DPRINTF("cpu %" PRId64 "\n", cpuid);
> +
> +    /* change to the cpu we are powering up */
> +    target_cpu_state = arm_get_cpu_by_id(cpuid);
> +    if (!target_cpu_state) {
> +        return QEMU_ARM_POWERCTL_INVALID_PARAM;
> +    }
> +    target_cpu = ARM_CPU(target_cpu_state);
> +    if (target_cpu->powered_off) {
> +        qemu_log_mask(LOG_GUEST_ERROR,
> +                      "[ARM]%s: CPU %" PRId64 " is already off\n",
> +                      __func__, cpuid);
> +        return QEMU_ARM_POWERCTL_IS_OFF;
> +    }
> +
> +    target_cpu->powered_off = true;
> +    target_cpu_state->halted = 1;
> +    target_cpu_state->exception_index = EXCP_HLT;
> +    cpu_loop_exit(target_cpu_state);
> +
> +    return QEMU_ARM_POWERCTL_RET_SUCCESS;
> +}
> +
> +int arm_reset_cpu(uint64_t cpuid)
> +{
> +    CPUState *target_cpu_state;
> +    ARMCPU *target_cpu;
> +
> +    DPRINTF("cpu %" PRId64 "\n", cpuid);
> +
> +    /* change to the cpu we are resetting */
> +    target_cpu_state = arm_get_cpu_by_id(cpuid);
> +    if (!target_cpu_state) {
> +        return QEMU_ARM_POWERCTL_INVALID_PARAM;
> +    }
> +    target_cpu = ARM_CPU(target_cpu_state);
> +    if (target_cpu->powered_off) {
> +        qemu_log_mask(LOG_GUEST_ERROR,
> +                      "[ARM]%s: CPU %" PRId64 " is off\n",
> +                      __func__, cpuid);
> +        return QEMU_ARM_POWERCTL_IS_OFF;
> +    }
> +
> +    /* Reset the cpu */
> +    cpu_reset(target_cpu_state);
> +
> +    return QEMU_ARM_POWERCTL_RET_SUCCESS;
> +}
> diff --git a/target-arm/arm-powerctl.h b/target-arm/arm-powerctl.h
> new file mode 100644
> index 0000000..e9dd264
> --- /dev/null
> +++ b/target-arm/arm-powerctl.h
> @@ -0,0 +1,75 @@
> +/*
> + * QEMU support -- ARM Power Control specific functions.
> + *
> + * Copyright (c) 2016 Jean-Christophe Dubois
> + *
> + * This work is licensed under the terms of the GNU GPL, version 2 or later.
> + * See the COPYING file in the top-level directory.
> + *
> + */
> +
> +#ifndef QEMU_ARM_POWERCTL_H
> +#define QEMU_ARM_POWERCTL_H
> +
> +#include "kvm-consts.h"
> +
> +#define QEMU_ARM_POWERCTL_RET_SUCCESS QEMU_PSCI_RET_SUCCESS
> +#define QEMU_ARM_POWERCTL_INVALID_PARAM QEMU_PSCI_RET_INVALID_PARAMS
> +#define QEMU_ARM_POWERCTL_ALREADY_ON QEMU_PSCI_RET_ALREADY_ON
> +#define QEMU_ARM_POWERCTL_IS_OFF QEMU_PSCI_RET_DENIED
> +
> +/*
> + * arm_get_cpu_by_id:
> + * @cpuid: the id of the CPU we want to retrieve the state
> + *
> + * Retreive a CPUState object from its CPU ID provided in @cpuid.

"Retrieve"

> + *
> + * Returns: a pointer to the CPUState structure of the requested CPU.
> + */
> +CPUState *arm_get_cpu_by_id(uint64_t cpuid);
> +
> +/*
> + * arm_set_cpu_on:
> + * @cpuid: the id of the CPU we want to start/wake up.
> + * @entry: the address the CPU shall start from.
> + * @context_id: the value to put in r0/xr0.

"r0/x0" (also below)

> + * @target_el: The desired exception level.
> + * @target_aa64: 1 if the requested mode is aarch64. 0 otherwise.

"AArch64" (capitalization, also below, also for AArch32).

> + *
> + * Start the cpu designated by @cpuid in @target_el exception level. The mode
> + * shall be aarch64 if @target_aa64 is set to 1. Otherwise the mode is
> + * aarch32. The CPU shall start at @entry with @context_id in r0/xr0.
> + *
> + * Returns: QEMU_ARM_POWERCTL_RET_SUCCESS on sucess.
> + * QEMU_ARM_POWERCTL_INVALID_PARAM if bad parameters are provided.
> + * QEMU_ARM_POWERCTL_ALREADY_ON if the CPU was already started.
> + */
> +int arm_set_cpu_on(uint64_t cpuid, uint64_t entry, uint64_t context_id,
> +                   int target_el, bool target_aa64);
> +
> +/*
> + * arm_set_cpu_off:
> + * @cpuid: the id of the CPU we want to stop/shut down.
> + *
> + * Stop the cpu designated by @cpuid.
> + *
> + * Returns: QEMU_ARM_POWERCTL_RET_SUCCESS on sucess.

"success" (also below)

> + * QEMU_ARM_POWERCTL_INVALID_PARAM if bad parameters are provided.
> + * QEMU_ARM_POWERCTL_IS_OFF if CPU is already off
> + */
> +
> +int arm_set_cpu_off(uint64_t cpuid);
> +
> +/*
> + * arm_reset_cpu:
> + * @cpuid: the id of the CPU we want to reset.
> + *
> + * Reset the cpu designated by @cpuid.
> + *
> + * Returns: QEMU_ARM_POWERCTL_RET_SUCCESS on sucess.
> + * QEMU_ARM_POWERCTL_INVALID_PARAM if bad parameters are provided.
> + * QEMU_ARM_POWERCTL_IS_OFF if CPU is off
> + */
> +int arm_reset_cpu(uint64_t cpuid);
> +
> +#endif
> diff --git a/target-arm/psci.c b/target-arm/psci.c
> index c55487f..e24123a 100644
> --- a/target-arm/psci.c
> +++ b/target-arm/psci.c
> @@ -22,6 +22,7 @@
>  #include <kvm-consts.h>
>  #include <sysemu/sysemu.h>
>  #include "internals.h"
> +#include "arm-powerctl.h"
>
>  bool arm_is_psci_call(ARMCPU *cpu, int excp_type)
>  {
> @@ -73,21 +74,6 @@ bool arm_is_psci_call(ARMCPU *cpu, int excp_type)
>      }
>  }
>
> -static CPUState *get_cpu_by_id(uint64_t id)
> -{
> -    CPUState *cpu;
> -
> -    CPU_FOREACH(cpu) {
> -        ARMCPU *armcpu = ARM_CPU(cpu);
> -
> -        if (armcpu->mp_affinity == id) {
> -            return cpu;
> -        }
> -    }
> -
> -    return NULL;
> -}
> -
>  void arm_handle_psci_call(ARMCPU *cpu)
>  {
>      /*
> @@ -98,7 +84,6 @@ void arm_handle_psci_call(ARMCPU *cpu)
>       * Additional information about the calling convention used is available in
>       * the document 'SMC Calling Convention' (ARM DEN 0028)
>       */
> -    CPUState *cs = CPU(cpu);
>      CPUARMState *env = &cpu->env;
>      uint64_t param[4];
>      uint64_t context_id, mpidr;
> @@ -123,7 +108,6 @@ void arm_handle_psci_call(ARMCPU *cpu)
>      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;
> @@ -137,7 +121,7 @@ void arm_handle_psci_call(ARMCPU *cpu)
>
>          switch (param[2]) {
>          case 0:
> -            target_cpu_state = get_cpu_by_id(mpidr);
> +            target_cpu_state = arm_get_cpu_by_id(mpidr);
>              if (!target_cpu_state) {
>                  ret = QEMU_PSCI_RET_INVALID_PARAMS;
>                  break;
> @@ -167,52 +151,13 @@ void arm_handle_psci_call(ARMCPU *cpu)
>          mpidr = param[1];
>          entry = param[2];
>          context_id = param[3];
> -
> -        /* change to the cpu we are powering up */
> -        target_cpu_state = get_cpu_by_id(mpidr);
> -        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;
> +        ret = arm_set_cpu_on(mpidr, entry, context_id, arm_current_el(env),
> +                             is_a64(env));
>          break;
>      case QEMU_PSCI_0_1_FN_CPU_OFF:
>      case QEMU_PSCI_0_2_FN_CPU_OFF:
> @@ -250,9 +195,6 @@ err:
>      return;
>
>  cpu_off:
> -    cpu->powered_off = true;
> -    cs->halted = 1;
> -    cs->exception_index = EXCP_HLT;
> -    cpu_loop_exit(cs);
> +    (void)arm_set_cpu_off(cpu->mp_affinity);

This would be better asserting that it can't fail rather than
just discarding the result. (It can't fail because the current
CPU (a) definitely exists and (b) is not already off.)

>      /* notreached */
>  }
> --
> 2.5.0
>

thanks
-- PMM
diff mbox

Patch

diff --git a/target-arm/Makefile.objs b/target-arm/Makefile.objs
index a80eb39..60fd1dd 100644
--- a/target-arm/Makefile.objs
+++ b/target-arm/Makefile.objs
@@ -9,3 +9,4 @@  obj-y += neon_helper.o iwmmxt_helper.o
 obj-y += gdbstub.o
 obj-$(TARGET_AARCH64) += cpu64.o translate-a64.o helper-a64.o gdbstub64.o
 obj-y += crypto_helper.o
+obj-y += arm-powerctl.o
diff --git a/target-arm/arm-powerctl.c b/target-arm/arm-powerctl.c
new file mode 100644
index 0000000..67e1579
--- /dev/null
+++ b/target-arm/arm-powerctl.c
@@ -0,0 +1,239 @@ 
+/*
+ * QEMU support -- ARM Power Control specific functions.
+ *
+ * Copyright (c) 2016 Jean-Christophe Dubois
+ *
+ * This work is licensed under the terms of the GNU GPL, version 2 or later.
+ * See the COPYING file in the top-level directory.
+ *
+ */
+
+#include "qemu/osdep.h"
+#include <cpu.h>
+#include <cpu-qom.h>
+#include "internals.h"
+#include "arm-powerctl.h"
+
+#ifndef DEBUG_ARM_POWERCTL
+#define DEBUG_ARM_POWERCTL 0
+#endif
+
+#define DPRINTF(fmt, args...) \
+    do { \
+        if (DEBUG_ARM_POWERCTL) { \
+            fprintf(stderr, "[ARM]%s: " fmt , __func__, ##args); \
+        } \
+    } while (0)
+
+CPUState *arm_get_cpu_by_id(uint64_t id)
+{
+    CPUState *cpu;
+
+    DPRINTF("cpu %" PRId64 "\n", id);
+
+    CPU_FOREACH(cpu) {
+        ARMCPU *armcpu = ARM_CPU(cpu);
+
+        if (armcpu->mp_affinity == id) {
+            return cpu;
+        }
+    }
+
+    qemu_log_mask(LOG_GUEST_ERROR,
+                  "[ARM]%s: Requesting unknown CPU %" PRId64 "\n",
+                  __func__, id);
+
+    return NULL;
+}
+
+int arm_set_cpu_on(uint64_t cpuid, uint64_t entry, uint64_t context_id,
+                   int target_el, bool target_aa64)
+{
+    CPUState *target_cpu_state;
+    ARMCPU *target_cpu;
+
+    DPRINTF("cpu %" PRId64 " (EL %d, %s) @ 0x%" PRIx64 " with R0 = 0x%" PRIx64
+            "\n", cpuid, target_el, target_aa64 ? "aarch64" : "aarch32", entry,
+            context_id);
+
+    /* requested EL level need to be above 0 */
+    assert(target_el >= 1 && target_el <= 3);
+
+    /* change to the cpu we are powering up */
+    target_cpu_state = arm_get_cpu_by_id(cpuid);
+    if (!target_cpu_state) {
+        /* The cpu was not found */
+        return QEMU_ARM_POWERCTL_INVALID_PARAM;
+    }
+    target_cpu = ARM_CPU(target_cpu_state);
+    if (!target_cpu->powered_off) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "[ARM]%s: CPU %" PRId64 " is already on\n",
+                      __func__, cpuid);
+        return QEMU_ARM_POWERCTL_ALREADY_ON;
+    }
+
+    /* Initialize the cpu we are turning on */
+    cpu_reset(target_cpu_state);
+    target_cpu->powered_off = false;
+    target_cpu_state->halted = 0;
+
+    /*
+     * The newly brought CPU is requested to enter the exception level
+     * "target_el" and be in the requested mode (aarch64 ou aarch32).
+     */
+
+    /*
+     * Check that the requested mode is a supported feature.
+     * 64 bits processors do support 32 bits mode but the opposite
+     * is not true.
+     */
+    if (target_aa64 && !arm_feature(&target_cpu->env, ARM_FEATURE_AARCH64)) {
+        return QEMU_ARM_POWERCTL_INVALID_PARAM;
+    }
+
+    /* We check that the request can be fulfilled */
+    if (target_aa64 != arm_el_is_aa64(&target_cpu->env, target_el)) {
+        /*
+         * TODO: We need to revisit this as the default config of a 64 bits
+         * CPU might not allow to boot in 32 bits even if the CPU is
+         * capable of it. We should implement some code to "force" the CPU in
+         * the requested mode if it is not its default mode for the considered
+         * exception level.
+         */
+        return QEMU_ARM_POWERCTL_INVALID_PARAM;
+    }
+
+    /*
+     * Check that the CPU is supporting the requested level
+     */
+    switch (target_el) {
+    case 3:
+        if (arm_feature(&target_cpu->env, ARM_FEATURE_EL3)) {
+            if (target_aa64) {
+                target_cpu->env.pstate = PSTATE_MODE_EL3h;
+            } else {
+                cpsr_write(&target_cpu->env, ARM_CPU_MODE_MON, CPSR_M,
+                           CPSRWriteRaw);
+            }
+            /* Processor is in secure mode */
+            target_cpu->env.cp15.scr_el3 &= ~SCR_NS;
+        } else {
+            return QEMU_ARM_POWERCTL_INVALID_PARAM;
+        }
+        break;
+    case 2:
+        if (arm_feature(&target_cpu->env, ARM_FEATURE_EL2)) {
+            if (target_aa64) {
+                if (arm_feature(&target_cpu->env, ARM_FEATURE_EL3)) {
+                    /* Lower EL3 exception is aarch64 */
+                    target_cpu->env.cp15.scr_el3 |= SCR_RW;
+                }
+                target_cpu->env.pstate = PSTATE_MODE_EL2h;
+            } else {
+                cpsr_write(&target_cpu->env, ARM_CPU_MODE_HYP, CPSR_M,
+                           CPSRWriteRaw);
+            }
+            /* Processor is not in secure mode */
+            target_cpu->env.cp15.scr_el3 |= SCR_NS;
+        } else {
+            return QEMU_ARM_POWERCTL_INVALID_PARAM;
+        }
+        break;
+    case 1:
+    default:
+        if (target_aa64) {
+            if (arm_feature(&target_cpu->env, ARM_FEATURE_EL3)) {
+                /* Lower EL3 exception is aarch64 */
+                target_cpu->env.cp15.scr_el3 |= SCR_RW;
+            }
+            if (arm_feature(&target_cpu->env, ARM_FEATURE_EL2)) {
+                /* Lower EL2 exception is aarch64 */
+                target_cpu->env.cp15.hcr_el2 |= HCR_RW;
+            }
+            target_cpu->env.pstate = PSTATE_MODE_EL1h;
+        } else {
+            cpsr_write(&target_cpu->env, ARM_CPU_MODE_SVC, CPSR_M,
+                       CPSRWriteRaw);
+        }
+        /* Processor is not in secure mode */
+        target_cpu->env.cp15.scr_el3 |= SCR_NS;
+        break;
+    }
+
+    /* We check if the started CPU is now in the correct level */
+    assert(target_el == arm_current_el(&target_cpu->env));
+
+    if (target_aa64) {
+        target_cpu->env.xregs[0] = context_id;
+        target_cpu->env.thumb = false;
+        /* check that the entry address is 4 bytes aligned */
+        if (entry & 3) {
+            return QEMU_ARM_POWERCTL_INVALID_PARAM;
+        }
+    } else {
+        target_cpu->env.regs[0] = context_id;
+        target_cpu->env.thumb = entry & 1;
+        entry &= 0xfffffffe;
+    }
+
+    /* Start the new CPU at the requested address */
+    cpu_set_pc(target_cpu_state, entry);
+
+    /* We are good to go */
+    return QEMU_ARM_POWERCTL_RET_SUCCESS;
+}
+
+int arm_set_cpu_off(uint64_t cpuid)
+{
+    CPUState *target_cpu_state;
+    ARMCPU *target_cpu;
+
+    DPRINTF("cpu %" PRId64 "\n", cpuid);
+
+    /* change to the cpu we are powering up */
+    target_cpu_state = arm_get_cpu_by_id(cpuid);
+    if (!target_cpu_state) {
+        return QEMU_ARM_POWERCTL_INVALID_PARAM;
+    }
+    target_cpu = ARM_CPU(target_cpu_state);
+    if (target_cpu->powered_off) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "[ARM]%s: CPU %" PRId64 " is already off\n",
+                      __func__, cpuid);
+        return QEMU_ARM_POWERCTL_IS_OFF;
+    }
+
+    target_cpu->powered_off = true;
+    target_cpu_state->halted = 1;
+    target_cpu_state->exception_index = EXCP_HLT;
+    cpu_loop_exit(target_cpu_state);
+
+    return QEMU_ARM_POWERCTL_RET_SUCCESS;
+}
+
+int arm_reset_cpu(uint64_t cpuid)
+{
+    CPUState *target_cpu_state;
+    ARMCPU *target_cpu;
+
+    DPRINTF("cpu %" PRId64 "\n", cpuid);
+
+    /* change to the cpu we are resetting */
+    target_cpu_state = arm_get_cpu_by_id(cpuid);
+    if (!target_cpu_state) {
+        return QEMU_ARM_POWERCTL_INVALID_PARAM;
+    }
+    target_cpu = ARM_CPU(target_cpu_state);
+    if (target_cpu->powered_off) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "[ARM]%s: CPU %" PRId64 " is off\n",
+                      __func__, cpuid);
+        return QEMU_ARM_POWERCTL_IS_OFF;
+    }
+
+    /* Reset the cpu */
+    cpu_reset(target_cpu_state);
+
+    return QEMU_ARM_POWERCTL_RET_SUCCESS;
+}
diff --git a/target-arm/arm-powerctl.h b/target-arm/arm-powerctl.h
new file mode 100644
index 0000000..e9dd264
--- /dev/null
+++ b/target-arm/arm-powerctl.h
@@ -0,0 +1,75 @@ 
+/*
+ * QEMU support -- ARM Power Control specific functions.
+ *
+ * Copyright (c) 2016 Jean-Christophe Dubois
+ *
+ * This work is licensed under the terms of the GNU GPL, version 2 or later.
+ * See the COPYING file in the top-level directory.
+ *
+ */
+
+#ifndef QEMU_ARM_POWERCTL_H
+#define QEMU_ARM_POWERCTL_H
+
+#include "kvm-consts.h"
+
+#define QEMU_ARM_POWERCTL_RET_SUCCESS QEMU_PSCI_RET_SUCCESS
+#define QEMU_ARM_POWERCTL_INVALID_PARAM QEMU_PSCI_RET_INVALID_PARAMS
+#define QEMU_ARM_POWERCTL_ALREADY_ON QEMU_PSCI_RET_ALREADY_ON
+#define QEMU_ARM_POWERCTL_IS_OFF QEMU_PSCI_RET_DENIED
+
+/*
+ * arm_get_cpu_by_id:
+ * @cpuid: the id of the CPU we want to retrieve the state
+ *
+ * Retreive a CPUState object from its CPU ID provided in @cpuid.
+ *
+ * Returns: a pointer to the CPUState structure of the requested CPU.
+ */
+CPUState *arm_get_cpu_by_id(uint64_t cpuid);
+
+/*
+ * arm_set_cpu_on:
+ * @cpuid: the id of the CPU we want to start/wake up.
+ * @entry: the address the CPU shall start from.
+ * @context_id: the value to put in r0/xr0.
+ * @target_el: The desired exception level.
+ * @target_aa64: 1 if the requested mode is aarch64. 0 otherwise.
+ *
+ * Start the cpu designated by @cpuid in @target_el exception level. The mode
+ * shall be aarch64 if @target_aa64 is set to 1. Otherwise the mode is
+ * aarch32. The CPU shall start at @entry with @context_id in r0/xr0.
+ *
+ * Returns: QEMU_ARM_POWERCTL_RET_SUCCESS on sucess.
+ * QEMU_ARM_POWERCTL_INVALID_PARAM if bad parameters are provided.
+ * QEMU_ARM_POWERCTL_ALREADY_ON if the CPU was already started.
+ */
+int arm_set_cpu_on(uint64_t cpuid, uint64_t entry, uint64_t context_id,
+                   int target_el, bool target_aa64);
+
+/*
+ * arm_set_cpu_off:
+ * @cpuid: the id of the CPU we want to stop/shut down.
+ *
+ * Stop the cpu designated by @cpuid.
+ *
+ * Returns: QEMU_ARM_POWERCTL_RET_SUCCESS on sucess.
+ * QEMU_ARM_POWERCTL_INVALID_PARAM if bad parameters are provided.
+ * QEMU_ARM_POWERCTL_IS_OFF if CPU is already off
+ */
+
+int arm_set_cpu_off(uint64_t cpuid);
+
+/*
+ * arm_reset_cpu:
+ * @cpuid: the id of the CPU we want to reset.
+ *
+ * Reset the cpu designated by @cpuid.
+ *
+ * Returns: QEMU_ARM_POWERCTL_RET_SUCCESS on sucess.
+ * QEMU_ARM_POWERCTL_INVALID_PARAM if bad parameters are provided.
+ * QEMU_ARM_POWERCTL_IS_OFF if CPU is off
+ */
+int arm_reset_cpu(uint64_t cpuid);
+
+#endif
diff --git a/target-arm/psci.c b/target-arm/psci.c
index c55487f..e24123a 100644
--- a/target-arm/psci.c
+++ b/target-arm/psci.c
@@ -22,6 +22,7 @@ 
 #include <kvm-consts.h>
 #include <sysemu/sysemu.h>
 #include "internals.h"
+#include "arm-powerctl.h"
 
 bool arm_is_psci_call(ARMCPU *cpu, int excp_type)
 {
@@ -73,21 +74,6 @@  bool arm_is_psci_call(ARMCPU *cpu, int excp_type)
     }
 }
 
-static CPUState *get_cpu_by_id(uint64_t id)
-{
-    CPUState *cpu;
-
-    CPU_FOREACH(cpu) {
-        ARMCPU *armcpu = ARM_CPU(cpu);
-
-        if (armcpu->mp_affinity == id) {
-            return cpu;
-        }
-    }
-
-    return NULL;
-}
-
 void arm_handle_psci_call(ARMCPU *cpu)
 {
     /*
@@ -98,7 +84,6 @@  void arm_handle_psci_call(ARMCPU *cpu)
      * Additional information about the calling convention used is available in
      * the document 'SMC Calling Convention' (ARM DEN 0028)
      */
-    CPUState *cs = CPU(cpu);
     CPUARMState *env = &cpu->env;
     uint64_t param[4];
     uint64_t context_id, mpidr;
@@ -123,7 +108,6 @@  void arm_handle_psci_call(ARMCPU *cpu)
     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;
@@ -137,7 +121,7 @@  void arm_handle_psci_call(ARMCPU *cpu)
 
         switch (param[2]) {
         case 0:
-            target_cpu_state = get_cpu_by_id(mpidr);
+            target_cpu_state = arm_get_cpu_by_id(mpidr);
             if (!target_cpu_state) {
                 ret = QEMU_PSCI_RET_INVALID_PARAMS;
                 break;
@@ -167,52 +151,13 @@  void arm_handle_psci_call(ARMCPU *cpu)
         mpidr = param[1];
         entry = param[2];
         context_id = param[3];
-
-        /* change to the cpu we are powering up */
-        target_cpu_state = get_cpu_by_id(mpidr);
-        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;
+        ret = arm_set_cpu_on(mpidr, entry, context_id, arm_current_el(env),
+                             is_a64(env));
         break;
     case QEMU_PSCI_0_1_FN_CPU_OFF:
     case QEMU_PSCI_0_2_FN_CPU_OFF:
@@ -250,9 +195,6 @@  err:
     return;
 
 cpu_off:
-    cpu->powered_off = true;
-    cs->halted = 1;
-    cs->exception_index = EXCP_HLT;
-    cpu_loop_exit(cs);
+    (void)arm_set_cpu_off(cpu->mp_affinity);
     /* notreached */
 }