diff mbox series

[Xen-devel,v3,3/3] xen/arm: vpsci: Move PSCI function dispatching from vsmc.c to vpsci.c

Message ID 20180206155325.11703-4-julien.grall@linaro.org
State Accepted
Commit c9d46c6fba9496478fa9f42c4bbebce8a191527d
Headers show
Series xen/arm: SMCCC fixes and PSCI clean-up | expand

Commit Message

Julien Grall Feb. 6, 2018, 3:53 p.m. UTC 
At the moment PSCI function dispatching is done in vsmc.c and the
function implementation in vpsci.c. Some bits of the implementation is
even done in vsmc.c (see PSCI_SYSTEM_RESET).

This means that it is difficult to follow the implementation and also
requires to export functions for each PSCI functions.

Therefore move PSCI dispatching in two new functions do_vpsci_0_1_call
and do_vpsci_0_2_call. The former will handle PSCI 0.1 call while the latter
0.2 or later call.

At the same time, a new header vpsci.h was created to contain all
definitions for virtual PSCI and avoid confusion with the host PSCI.

Signed-off-by: Julien Grall <julien.grall@linaro.org>

---
    Changes in v3:
        - Add copyright and emacs magic in vpsci.h
        - Add a comment to update SCCC_SMCCC_*_REVISION once per
        release

    Changes in v2:
        - Add a 'v' in the function names to help distinguish virtual vs
        physical PSCI
        - Introduce vpsci.h and VSCPI_NR_FUNCS
---
 xen/arch/arm/vpsci.c        | 148 +++++++++++++++++++++++++++++++++++++++-----
 xen/arch/arm/vsmc.c         |  99 ++---------------------------
 xen/include/asm-arm/psci.h  |  19 ------
 xen/include/asm-arm/vpsci.h |  42 +++++++++++++
 4 files changed, 182 insertions(+), 126 deletions(-)
 create mode 100644 xen/include/asm-arm/vpsci.h

Comments

Volodymyr Babchuk Feb. 6, 2018, 4:39 p.m. UTC | #1 
Hi,

On 06.02.18 17:53, Julien Grall wrote:
> At the moment PSCI function dispatching is done in vsmc.c and the
> function implementation in vpsci.c. Some bits of the implementation is
> even done in vsmc.c (see PSCI_SYSTEM_RESET).
> 
> This means that it is difficult to follow the implementation and also
> requires to export functions for each PSCI functions.
> 
> Therefore move PSCI dispatching in two new functions do_vpsci_0_1_call
> and do_vpsci_0_2_call. The former will handle PSCI 0.1 call while the latter
> 0.2 or later call.
> 
> At the same time, a new header vpsci.h was created to contain all
> definitions for virtual PSCI and avoid confusion with the host PSCI.
> 
> Signed-off-by: Julien Grall <julien.grall@linaro.org>
Reviewed-by: Volodymyr Babchuk <volodymyr_babchuk@epam.com>

> ---
>      Changes in v3:
>          - Add copyright and emacs magic in vpsci.h
>          - Add a comment to update SCCC_SMCCC_*_REVISION once per
>          release
> 
>      Changes in v2:
>          - Add a 'v' in the function names to help distinguish virtual vs
>          physical PSCI
>          - Introduce vpsci.h and VSCPI_NR_FUNCS
> ---
>   xen/arch/arm/vpsci.c        | 148 +++++++++++++++++++++++++++++++++++++++-----
>   xen/arch/arm/vsmc.c         |  99 ++---------------------------
>   xen/include/asm-arm/psci.h  |  19 ------
>   xen/include/asm-arm/vpsci.h |  42 +++++++++++++
>   4 files changed, 182 insertions(+), 126 deletions(-)
>   create mode 100644 xen/include/asm-arm/vpsci.h
> 
> diff --git a/xen/arch/arm/vpsci.c b/xen/arch/arm/vpsci.c
> index 979d32ed6d..03fd4eb5b5 100644
> --- a/xen/arch/arm/vpsci.c
> +++ b/xen/arch/arm/vpsci.c
> @@ -16,7 +16,7 @@
>   
>   #include <asm/current.h>
>   #include <asm/vgic.h>
> -#include <asm/psci.h>
> +#include <asm/vpsci.h>
>   #include <asm/event.h>
>   
>   #include <public/sched.h>
> @@ -91,12 +91,12 @@ static int do_common_cpu_on(register_t target_cpu, register_t entry_point,
>       return PSCI_SUCCESS;
>   }
>   
> -int32_t do_psci_cpu_on(uint32_t vcpuid, register_t entry_point)
> +static int32_t do_psci_cpu_on(uint32_t vcpuid, register_t entry_point)
>   {
>       return do_common_cpu_on(vcpuid, entry_point, 0 , PSCI_VERSION(0, 1));
>   }
>   
> -int32_t do_psci_cpu_off(uint32_t power_state)
> +static int32_t do_psci_cpu_off(uint32_t power_state)
>   {
>       struct vcpu *v = current;
>       if ( !test_and_set_bit(_VPF_down, &v->pause_flags) )
> @@ -104,13 +104,14 @@ int32_t do_psci_cpu_off(uint32_t power_state)
>       return PSCI_SUCCESS;
>   }
>   
> -uint32_t do_psci_0_2_version(void)
> +static uint32_t do_psci_0_2_version(void)
>   {
>       return PSCI_VERSION(0, 2);
>   }
>   
> -register_t do_psci_0_2_cpu_suspend(uint32_t power_state, register_t entry_point,
> -                            register_t context_id)
> +static register_t do_psci_0_2_cpu_suspend(uint32_t power_state,
> +                                          register_t entry_point,
> +                                          register_t context_id)
>   {
>       struct vcpu *v = current;
>   
> @@ -123,13 +124,14 @@ register_t do_psci_0_2_cpu_suspend(uint32_t power_state, register_t entry_point,
>       return PSCI_SUCCESS;
>   }
>   
> -int32_t do_psci_0_2_cpu_off(void)
> +static int32_t do_psci_0_2_cpu_off(void)
>   {
>       return do_psci_cpu_off(0);
>   }
>   
> -int32_t do_psci_0_2_cpu_on(register_t target_cpu, register_t entry_point,
> -                       register_t context_id)
> +static int32_t do_psci_0_2_cpu_on(register_t target_cpu,
> +                                  register_t entry_point,
> +                                  register_t context_id)
>   {
>       return do_common_cpu_on(target_cpu, entry_point, context_id,
>                               PSCI_VERSION(0, 2));
> @@ -144,8 +146,8 @@ static const unsigned long target_affinity_mask[] = {
>   #endif
>   };
>   
> -int32_t do_psci_0_2_affinity_info(register_t target_affinity,
> -                              uint32_t lowest_affinity_level)
> +static int32_t do_psci_0_2_affinity_info(register_t target_affinity,
> +                                         uint32_t lowest_affinity_level)
>   {
>       struct domain *d = current->domain;
>       struct vcpu *v;
> @@ -172,23 +174,141 @@ int32_t do_psci_0_2_affinity_info(register_t target_affinity,
>       return PSCI_0_2_AFFINITY_LEVEL_OFF;
>   }
>   
> -uint32_t do_psci_0_2_migrate_info_type(void)
> +static uint32_t do_psci_0_2_migrate_info_type(void)
>   {
>       return PSCI_0_2_TOS_MP_OR_NOT_PRESENT;
>   }
>   
> -void do_psci_0_2_system_off( void )
> +static void do_psci_0_2_system_off( void )
>   {
>       struct domain *d = current->domain;
>       domain_shutdown(d,SHUTDOWN_poweroff);
>   }
>   
> -void do_psci_0_2_system_reset(void)
> +static void do_psci_0_2_system_reset(void)
>   {
>       struct domain *d = current->domain;
>       domain_shutdown(d,SHUTDOWN_reboot);
>   }
>   
> +#define PSCI_SET_RESULT(reg, val) set_user_reg(reg, 0, val)
> +#define PSCI_ARG(reg, n) get_user_reg(reg, n)
> +
> +#ifdef CONFIG_ARM_64
> +#define PSCI_ARG32(reg, n) (uint32_t)(get_user_reg(reg, n))
> +#else
> +#define PSCI_ARG32(reg, n) PSCI_ARG(reg, n)
> +#endif
> +
> +/*
> + * PSCI 0.1 calls. It will return false if the function ID is not
> + * handled.
> + */
> +bool do_vpsci_0_1_call(struct cpu_user_regs *regs, uint32_t fid)
> +{
> +    switch ( (uint32_t)get_user_reg(regs, 0) )
> +    {
> +    case PSCI_cpu_off:
> +    {
> +        uint32_t pstate = PSCI_ARG32(regs, 1);
> +
> +        perfc_incr(vpsci_cpu_off);
> +        PSCI_SET_RESULT(regs, do_psci_cpu_off(pstate));
> +        return true;
> +    }
> +    case PSCI_cpu_on:
> +    {
> +        uint32_t vcpuid = PSCI_ARG32(regs, 1);
> +        register_t epoint = PSCI_ARG(regs, 2);
> +
> +        perfc_incr(vpsci_cpu_on);
> +        PSCI_SET_RESULT(regs, do_psci_cpu_on(vcpuid, epoint));
> +        return true;
> +    }
> +    default:
> +        return false;
> +    }
> +}
> +
> +/*
> + * PSCI 0.2 or later calls. It will return false if the function ID is
> + * not handled.
> + */
> +bool do_vpsci_0_2_call(struct cpu_user_regs *regs, uint32_t fid)
> +{
> +    /*
> +     * /!\ VPSCI_NR_FUNCS (in asm-arm/vpsci.h) should be updated when
> +     * adding/removing a function. SCCC_SMCCC_*_REVISION should be
> +     * updated once per release.
> +     */
> +    switch ( fid )
> +    {
> +    case PSCI_0_2_FN32(PSCI_VERSION):
> +        perfc_incr(vpsci_version);
> +        PSCI_SET_RESULT(regs, do_psci_0_2_version());
> +        return true;
> +
> +    case PSCI_0_2_FN32(CPU_OFF):
> +        perfc_incr(vpsci_cpu_off);
> +        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_off());
> +        return true;
> +
> +    case PSCI_0_2_FN32(MIGRATE_INFO_TYPE):
> +        perfc_incr(vpsci_migrate_info_type);
> +        PSCI_SET_RESULT(regs, do_psci_0_2_migrate_info_type());
> +        return true;
> +
> +    case PSCI_0_2_FN32(SYSTEM_OFF):
> +        perfc_incr(vpsci_system_off);
> +        do_psci_0_2_system_off();
> +        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
> +        return true;
> +
> +    case PSCI_0_2_FN32(SYSTEM_RESET):
> +        perfc_incr(vpsci_system_reset);
> +        do_psci_0_2_system_reset();
> +        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
> +        return true;
> +
> +    case PSCI_0_2_FN32(CPU_ON):
> +    case PSCI_0_2_FN64(CPU_ON):
> +    {
> +        register_t vcpuid = PSCI_ARG(regs, 1);
> +        register_t epoint = PSCI_ARG(regs, 2);
> +        register_t cid = PSCI_ARG(regs, 3);
> +
> +        perfc_incr(vpsci_cpu_on);
> +        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_on(vcpuid, epoint, cid));
> +        return true;
> +    }
> +
> +    case PSCI_0_2_FN32(CPU_SUSPEND):
> +    case PSCI_0_2_FN64(CPU_SUSPEND):
> +    {
> +        uint32_t pstate = PSCI_ARG32(regs, 1);
> +        register_t epoint = PSCI_ARG(regs, 2);
> +        register_t cid = PSCI_ARG(regs, 3);
> +
> +        perfc_incr(vpsci_cpu_suspend);
> +        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_suspend(pstate, epoint, cid));
> +        return true;
> +    }
> +
> +    case PSCI_0_2_FN32(AFFINITY_INFO):
> +    case PSCI_0_2_FN64(AFFINITY_INFO):
> +    {
> +        register_t taff = PSCI_ARG(regs, 1);
> +        uint32_t laff = PSCI_ARG32(regs, 2);
> +
> +        perfc_incr(vpsci_cpu_affinity_info);
> +        PSCI_SET_RESULT(regs, do_psci_0_2_affinity_info(taff, laff));
> +        return true;
> +    }
> +    default:
> +        return false;
> +    }
> +}
> +
>   /*
>    * Local variables:
>    * mode: C
> diff --git a/xen/arch/arm/vsmc.c b/xen/arch/arm/vsmc.c
> index 3d8cbcc808..3d3bd95fee 100644
> --- a/xen/arch/arm/vsmc.c
> +++ b/xen/arch/arm/vsmc.c
> @@ -19,16 +19,16 @@
>   #include <xen/types.h>
>   #include <public/arch-arm/smccc.h>
>   #include <asm/monitor.h>
> -#include <asm/psci.h>
>   #include <asm/regs.h>
>   #include <asm/smccc.h>
>   #include <asm/traps.h>
> +#include <asm/vpsci.h>
>   
>   /* Number of functions currently supported by Hypervisor Service. */
>   #define XEN_SMCCC_FUNCTION_COUNT 3
>   
>   /* Number of functions currently supported by Standard Service Service Calls. */
> -#define SSSC_SMCCC_FUNCTION_COUNT 14
> +#define SSSC_SMCCC_FUNCTION_COUNT (3 + VPSCI_NR_FUNCS)
>   
>   static bool fill_uid(struct cpu_user_regs *regs, xen_uuid_t uuid)
>   {
> @@ -100,41 +100,13 @@ static bool handle_hypervisor(struct cpu_user_regs *regs)
>       }
>   }
>   
> -#define PSCI_SET_RESULT(reg, val) set_user_reg(reg, 0, val)
> -#define PSCI_ARG(reg, n) get_user_reg(reg, n)
> -
> -#ifdef CONFIG_ARM_64
> -#define PSCI_ARG32(reg, n) (uint32_t)(get_user_reg(reg, n))
> -#else
> -#define PSCI_ARG32(reg, n) PSCI_ARG(reg, n)
> -#endif
> -
>   /* Existing (pre SMCCC) APIs. This includes PSCI 0.1 interface */
>   static bool handle_existing_apis(struct cpu_user_regs *regs)
>   {
>       /* Only least 32 bits are significant (ARM DEN 0028B, page 12) */
> -    switch ( (uint32_t)get_user_reg(regs, 0) )
> -    {
> -    case PSCI_cpu_off:
> -    {
> -        uint32_t pstate = PSCI_ARG32(regs, 1);
> -
> -        perfc_incr(vpsci_cpu_off);
> -        PSCI_SET_RESULT(regs, do_psci_cpu_off(pstate));
> -        return true;
> -    }
> -    case PSCI_cpu_on:
> -    {
> -        uint32_t vcpuid = PSCI_ARG32(regs, 1);
> -        register_t epoint = PSCI_ARG(regs, 2);
> +    uint32_t fid = (uint32_t)get_user_reg(regs, 0);
>   
> -        perfc_incr(vpsci_cpu_on);
> -        PSCI_SET_RESULT(regs, do_psci_cpu_on(vcpuid, epoint));
> -        return true;
> -    }
> -    default:
> -        return false;
> -    }
> +    return do_vpsci_0_1_call(regs, fid);
>   }
>   
>   /* PSCI 0.2 interface and other Standard Secure Calls */
> @@ -142,70 +114,11 @@ static bool handle_sssc(struct cpu_user_regs *regs)
>   {
>       uint32_t fid = (uint32_t)get_user_reg(regs, 0);
>   
> -    switch ( fid )
> -    {
> -    case PSCI_0_2_FN32(PSCI_VERSION):
> -        perfc_incr(vpsci_version);
> -        PSCI_SET_RESULT(regs, do_psci_0_2_version());
> +    if ( do_vpsci_0_2_call(regs, fid) )
>           return true;
>   
> -    case PSCI_0_2_FN32(CPU_OFF):
> -        perfc_incr(vpsci_cpu_off);
> -        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_off());
> -        return true;
> -
> -    case PSCI_0_2_FN32(MIGRATE_INFO_TYPE):
> -        perfc_incr(vpsci_migrate_info_type);
> -        PSCI_SET_RESULT(regs, do_psci_0_2_migrate_info_type());
> -        return true;
> -
> -    case PSCI_0_2_FN32(SYSTEM_OFF):
> -        perfc_incr(vpsci_system_off);
> -        do_psci_0_2_system_off();
> -        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
> -        return true;
> -
> -    case PSCI_0_2_FN32(SYSTEM_RESET):
> -        perfc_incr(vpsci_system_reset);
> -        do_psci_0_2_system_reset();
> -        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
> -        return true;
> -
> -    case PSCI_0_2_FN32(CPU_ON):
> -    case PSCI_0_2_FN64(CPU_ON):
> -    {
> -        register_t vcpuid = PSCI_ARG(regs, 1);
> -        register_t epoint = PSCI_ARG(regs, 2);
> -        register_t cid = PSCI_ARG(regs, 3);
> -
> -        perfc_incr(vpsci_cpu_on);
> -        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_on(vcpuid, epoint, cid));
> -        return true;
> -    }
> -
> -    case PSCI_0_2_FN32(CPU_SUSPEND):
> -    case PSCI_0_2_FN64(CPU_SUSPEND):
> -    {
> -        uint32_t pstate = PSCI_ARG32(regs, 1);
> -        register_t epoint = PSCI_ARG(regs, 2);
> -        register_t cid = PSCI_ARG(regs, 3);
> -
> -        perfc_incr(vpsci_cpu_suspend);
> -        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_suspend(pstate, epoint, cid));
> -        return true;
> -    }
> -
> -    case PSCI_0_2_FN32(AFFINITY_INFO):
> -    case PSCI_0_2_FN64(AFFINITY_INFO):
> +    switch ( fid )
>       {
> -        register_t taff = PSCI_ARG(regs, 1);
> -        uint32_t laff = PSCI_ARG32(regs, 2);
> -
> -        perfc_incr(vpsci_cpu_affinity_info);
> -        PSCI_SET_RESULT(regs, do_psci_0_2_affinity_info(taff, laff));
> -        return true;
> -    }
> -
>       case ARM_SMCCC_CALL_COUNT_FID(STANDARD):
>           return fill_function_call_count(regs, SSSC_SMCCC_FUNCTION_COUNT);
>   
> diff --git a/xen/include/asm-arm/psci.h b/xen/include/asm-arm/psci.h
> index 32c1f81f21..3c44468e72 100644
> --- a/xen/include/asm-arm/psci.h
> +++ b/xen/include/asm-arm/psci.h
> @@ -22,25 +22,6 @@ int call_psci_cpu_on(int cpu);
>   void call_psci_system_off(void);
>   void call_psci_system_reset(void);
>   
> -/* functions to handle guest PSCI requests */
> -int32_t do_psci_cpu_on(uint32_t vcpuid, register_t entry_point);
> -int32_t do_psci_cpu_off(uint32_t power_state);
> -int32_t do_psci_cpu_suspend(uint32_t power_state, register_t entry_point);
> -int32_t do_psci_migrate(uint32_t vcpuid);
> -
> -/* PSCI 0.2 functions to handle guest PSCI requests */
> -uint32_t do_psci_0_2_version(void);
> -register_t do_psci_0_2_cpu_suspend(uint32_t power_state, register_t entry_point,
> -                            register_t context_id);
> -int32_t do_psci_0_2_cpu_off(void);
> -int32_t do_psci_0_2_cpu_on(register_t target_cpu, register_t entry_point,
> -                       register_t context_id);
> -int32_t do_psci_0_2_affinity_info(register_t target_affinity,
> -                              uint32_t lowest_affinity_level);
> -uint32_t do_psci_0_2_migrate_info_type(void);
> -void do_psci_0_2_system_off(void);
> -void do_psci_0_2_system_reset(void);
> -
>   /* PSCI v0.2 interface */
>   #define PSCI_0_2_FN32(name) ARM_SMCCC_CALL_VAL(ARM_SMCCC_FAST_CALL,             \
>                                                  ARM_SMCCC_CONV_32,               \
> diff --git a/xen/include/asm-arm/vpsci.h b/xen/include/asm-arm/vpsci.h
> new file mode 100644
> index 0000000000..035a41e812
> --- /dev/null
> +++ b/xen/include/asm-arm/vpsci.h
> @@ -0,0 +1,42 @@
> +/*
> + * xen/include/asm-arm/vpsci.h
> + *
> + * Julien Grall <julien.gral@linaro.org>
> + * Copyright (c) 2018 Linaro Limited.
> + *
> + * 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; under version 2 of the License.
> + *
> + * 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/>.
> + */
> +
> +#ifndef __ASM_VPSCI_H__
> +#define __ASM_VPSCI_H__
> +
> +#include <asm/psci.h>
> +
> +/* Number of function implemented by virtual PSCI (only 0.2 or later) */
> +#define VPSCI_NR_FUNCS  11
> +
> +/* Functions handle PSCI calls from the guests */
> +bool do_vpsci_0_1_call(struct cpu_user_regs *regs, uint32_t fid);
> +bool do_vpsci_0_2_call(struct cpu_user_regs *regs, uint32_t fid);
> +
> +#endif /* __ASM_VPSCI_H__ */
> +
> +/*
> + * Local variables:
> + * mode: C
> + * c-file-style: "BSD"
> + * c-basic-offset: 4
> + * tab-width: 4
> + * indent-tabs-mode: nil
> + * End:
> + */
>
diff mbox series

Patch

diff --git a/xen/arch/arm/vpsci.c b/xen/arch/arm/vpsci.c
index 979d32ed6d..03fd4eb5b5 100644
--- a/xen/arch/arm/vpsci.c
+++ b/xen/arch/arm/vpsci.c
@@ -16,7 +16,7 @@ 
 
 #include <asm/current.h>
 #include <asm/vgic.h>
-#include <asm/psci.h>
+#include <asm/vpsci.h>
 #include <asm/event.h>
 
 #include <public/sched.h>
@@ -91,12 +91,12 @@  static int do_common_cpu_on(register_t target_cpu, register_t entry_point,
     return PSCI_SUCCESS;
 }
 
-int32_t do_psci_cpu_on(uint32_t vcpuid, register_t entry_point)
+static int32_t do_psci_cpu_on(uint32_t vcpuid, register_t entry_point)
 {
     return do_common_cpu_on(vcpuid, entry_point, 0 , PSCI_VERSION(0, 1));
 }
 
-int32_t do_psci_cpu_off(uint32_t power_state)
+static int32_t do_psci_cpu_off(uint32_t power_state)
 {
     struct vcpu *v = current;
     if ( !test_and_set_bit(_VPF_down, &v->pause_flags) )
@@ -104,13 +104,14 @@  int32_t do_psci_cpu_off(uint32_t power_state)
     return PSCI_SUCCESS;
 }
 
-uint32_t do_psci_0_2_version(void)
+static uint32_t do_psci_0_2_version(void)
 {
     return PSCI_VERSION(0, 2);
 }
 
-register_t do_psci_0_2_cpu_suspend(uint32_t power_state, register_t entry_point,
-                            register_t context_id)
+static register_t do_psci_0_2_cpu_suspend(uint32_t power_state,
+                                          register_t entry_point,
+                                          register_t context_id)
 {
     struct vcpu *v = current;
 
@@ -123,13 +124,14 @@  register_t do_psci_0_2_cpu_suspend(uint32_t power_state, register_t entry_point,
     return PSCI_SUCCESS;
 }
 
-int32_t do_psci_0_2_cpu_off(void)
+static int32_t do_psci_0_2_cpu_off(void)
 {
     return do_psci_cpu_off(0);
 }
 
-int32_t do_psci_0_2_cpu_on(register_t target_cpu, register_t entry_point,
-                       register_t context_id)
+static int32_t do_psci_0_2_cpu_on(register_t target_cpu,
+                                  register_t entry_point,
+                                  register_t context_id)
 {
     return do_common_cpu_on(target_cpu, entry_point, context_id,
                             PSCI_VERSION(0, 2));
@@ -144,8 +146,8 @@  static const unsigned long target_affinity_mask[] = {
 #endif
 };
 
-int32_t do_psci_0_2_affinity_info(register_t target_affinity,
-                              uint32_t lowest_affinity_level)
+static int32_t do_psci_0_2_affinity_info(register_t target_affinity,
+                                         uint32_t lowest_affinity_level)
 {
     struct domain *d = current->domain;
     struct vcpu *v;
@@ -172,23 +174,141 @@  int32_t do_psci_0_2_affinity_info(register_t target_affinity,
     return PSCI_0_2_AFFINITY_LEVEL_OFF;
 }
 
-uint32_t do_psci_0_2_migrate_info_type(void)
+static uint32_t do_psci_0_2_migrate_info_type(void)
 {
     return PSCI_0_2_TOS_MP_OR_NOT_PRESENT;
 }
 
-void do_psci_0_2_system_off( void )
+static void do_psci_0_2_system_off( void )
 {
     struct domain *d = current->domain;
     domain_shutdown(d,SHUTDOWN_poweroff);
 }
 
-void do_psci_0_2_system_reset(void)
+static void do_psci_0_2_system_reset(void)
 {
     struct domain *d = current->domain;
     domain_shutdown(d,SHUTDOWN_reboot);
 }
 
+#define PSCI_SET_RESULT(reg, val) set_user_reg(reg, 0, val)
+#define PSCI_ARG(reg, n) get_user_reg(reg, n)
+
+#ifdef CONFIG_ARM_64
+#define PSCI_ARG32(reg, n) (uint32_t)(get_user_reg(reg, n))
+#else
+#define PSCI_ARG32(reg, n) PSCI_ARG(reg, n)
+#endif
+
+/*
+ * PSCI 0.1 calls. It will return false if the function ID is not
+ * handled.
+ */
+bool do_vpsci_0_1_call(struct cpu_user_regs *regs, uint32_t fid)
+{
+    switch ( (uint32_t)get_user_reg(regs, 0) )
+    {
+    case PSCI_cpu_off:
+    {
+        uint32_t pstate = PSCI_ARG32(regs, 1);
+
+        perfc_incr(vpsci_cpu_off);
+        PSCI_SET_RESULT(regs, do_psci_cpu_off(pstate));
+        return true;
+    }
+    case PSCI_cpu_on:
+    {
+        uint32_t vcpuid = PSCI_ARG32(regs, 1);
+        register_t epoint = PSCI_ARG(regs, 2);
+
+        perfc_incr(vpsci_cpu_on);
+        PSCI_SET_RESULT(regs, do_psci_cpu_on(vcpuid, epoint));
+        return true;
+    }
+    default:
+        return false;
+    }
+}
+
+/*
+ * PSCI 0.2 or later calls. It will return false if the function ID is
+ * not handled.
+ */
+bool do_vpsci_0_2_call(struct cpu_user_regs *regs, uint32_t fid)
+{
+    /*
+     * /!\ VPSCI_NR_FUNCS (in asm-arm/vpsci.h) should be updated when
+     * adding/removing a function. SCCC_SMCCC_*_REVISION should be
+     * updated once per release.
+     */
+    switch ( fid )
+    {
+    case PSCI_0_2_FN32(PSCI_VERSION):
+        perfc_incr(vpsci_version);
+        PSCI_SET_RESULT(regs, do_psci_0_2_version());
+        return true;
+
+    case PSCI_0_2_FN32(CPU_OFF):
+        perfc_incr(vpsci_cpu_off);
+        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_off());
+        return true;
+
+    case PSCI_0_2_FN32(MIGRATE_INFO_TYPE):
+        perfc_incr(vpsci_migrate_info_type);
+        PSCI_SET_RESULT(regs, do_psci_0_2_migrate_info_type());
+        return true;
+
+    case PSCI_0_2_FN32(SYSTEM_OFF):
+        perfc_incr(vpsci_system_off);
+        do_psci_0_2_system_off();
+        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
+        return true;
+
+    case PSCI_0_2_FN32(SYSTEM_RESET):
+        perfc_incr(vpsci_system_reset);
+        do_psci_0_2_system_reset();
+        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
+        return true;
+
+    case PSCI_0_2_FN32(CPU_ON):
+    case PSCI_0_2_FN64(CPU_ON):
+    {
+        register_t vcpuid = PSCI_ARG(regs, 1);
+        register_t epoint = PSCI_ARG(regs, 2);
+        register_t cid = PSCI_ARG(regs, 3);
+
+        perfc_incr(vpsci_cpu_on);
+        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_on(vcpuid, epoint, cid));
+        return true;
+    }
+
+    case PSCI_0_2_FN32(CPU_SUSPEND):
+    case PSCI_0_2_FN64(CPU_SUSPEND):
+    {
+        uint32_t pstate = PSCI_ARG32(regs, 1);
+        register_t epoint = PSCI_ARG(regs, 2);
+        register_t cid = PSCI_ARG(regs, 3);
+
+        perfc_incr(vpsci_cpu_suspend);
+        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_suspend(pstate, epoint, cid));
+        return true;
+    }
+
+    case PSCI_0_2_FN32(AFFINITY_INFO):
+    case PSCI_0_2_FN64(AFFINITY_INFO):
+    {
+        register_t taff = PSCI_ARG(regs, 1);
+        uint32_t laff = PSCI_ARG32(regs, 2);
+
+        perfc_incr(vpsci_cpu_affinity_info);
+        PSCI_SET_RESULT(regs, do_psci_0_2_affinity_info(taff, laff));
+        return true;
+    }
+    default:
+        return false;
+    }
+}
+
 /*
  * Local variables:
  * mode: C
diff --git a/xen/arch/arm/vsmc.c b/xen/arch/arm/vsmc.c
index 3d8cbcc808..3d3bd95fee 100644
--- a/xen/arch/arm/vsmc.c
+++ b/xen/arch/arm/vsmc.c
@@ -19,16 +19,16 @@ 
 #include <xen/types.h>
 #include <public/arch-arm/smccc.h>
 #include <asm/monitor.h>
-#include <asm/psci.h>
 #include <asm/regs.h>
 #include <asm/smccc.h>
 #include <asm/traps.h>
+#include <asm/vpsci.h>
 
 /* Number of functions currently supported by Hypervisor Service. */
 #define XEN_SMCCC_FUNCTION_COUNT 3
 
 /* Number of functions currently supported by Standard Service Service Calls. */
-#define SSSC_SMCCC_FUNCTION_COUNT 14
+#define SSSC_SMCCC_FUNCTION_COUNT (3 + VPSCI_NR_FUNCS)
 
 static bool fill_uid(struct cpu_user_regs *regs, xen_uuid_t uuid)
 {
@@ -100,41 +100,13 @@  static bool handle_hypervisor(struct cpu_user_regs *regs)
     }
 }
 
-#define PSCI_SET_RESULT(reg, val) set_user_reg(reg, 0, val)
-#define PSCI_ARG(reg, n) get_user_reg(reg, n)
-
-#ifdef CONFIG_ARM_64
-#define PSCI_ARG32(reg, n) (uint32_t)(get_user_reg(reg, n))
-#else
-#define PSCI_ARG32(reg, n) PSCI_ARG(reg, n)
-#endif
-
 /* Existing (pre SMCCC) APIs. This includes PSCI 0.1 interface */
 static bool handle_existing_apis(struct cpu_user_regs *regs)
 {
     /* Only least 32 bits are significant (ARM DEN 0028B, page 12) */
-    switch ( (uint32_t)get_user_reg(regs, 0) )
-    {
-    case PSCI_cpu_off:
-    {
-        uint32_t pstate = PSCI_ARG32(regs, 1);
-
-        perfc_incr(vpsci_cpu_off);
-        PSCI_SET_RESULT(regs, do_psci_cpu_off(pstate));
-        return true;
-    }
-    case PSCI_cpu_on:
-    {
-        uint32_t vcpuid = PSCI_ARG32(regs, 1);
-        register_t epoint = PSCI_ARG(regs, 2);
+    uint32_t fid = (uint32_t)get_user_reg(regs, 0);
 
-        perfc_incr(vpsci_cpu_on);
-        PSCI_SET_RESULT(regs, do_psci_cpu_on(vcpuid, epoint));
-        return true;
-    }
-    default:
-        return false;
-    }
+    return do_vpsci_0_1_call(regs, fid);
 }
 
 /* PSCI 0.2 interface and other Standard Secure Calls */
@@ -142,70 +114,11 @@  static bool handle_sssc(struct cpu_user_regs *regs)
 {
     uint32_t fid = (uint32_t)get_user_reg(regs, 0);
 
-    switch ( fid )
-    {
-    case PSCI_0_2_FN32(PSCI_VERSION):
-        perfc_incr(vpsci_version);
-        PSCI_SET_RESULT(regs, do_psci_0_2_version());
+    if ( do_vpsci_0_2_call(regs, fid) )
         return true;
 
-    case PSCI_0_2_FN32(CPU_OFF):
-        perfc_incr(vpsci_cpu_off);
-        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_off());
-        return true;
-
-    case PSCI_0_2_FN32(MIGRATE_INFO_TYPE):
-        perfc_incr(vpsci_migrate_info_type);
-        PSCI_SET_RESULT(regs, do_psci_0_2_migrate_info_type());
-        return true;
-
-    case PSCI_0_2_FN32(SYSTEM_OFF):
-        perfc_incr(vpsci_system_off);
-        do_psci_0_2_system_off();
-        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
-        return true;
-
-    case PSCI_0_2_FN32(SYSTEM_RESET):
-        perfc_incr(vpsci_system_reset);
-        do_psci_0_2_system_reset();
-        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
-        return true;
-
-    case PSCI_0_2_FN32(CPU_ON):
-    case PSCI_0_2_FN64(CPU_ON):
-    {
-        register_t vcpuid = PSCI_ARG(regs, 1);
-        register_t epoint = PSCI_ARG(regs, 2);
-        register_t cid = PSCI_ARG(regs, 3);
-
-        perfc_incr(vpsci_cpu_on);
-        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_on(vcpuid, epoint, cid));
-        return true;
-    }
-
-    case PSCI_0_2_FN32(CPU_SUSPEND):
-    case PSCI_0_2_FN64(CPU_SUSPEND):
-    {
-        uint32_t pstate = PSCI_ARG32(regs, 1);
-        register_t epoint = PSCI_ARG(regs, 2);
-        register_t cid = PSCI_ARG(regs, 3);
-
-        perfc_incr(vpsci_cpu_suspend);
-        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_suspend(pstate, epoint, cid));
-        return true;
-    }
-
-    case PSCI_0_2_FN32(AFFINITY_INFO):
-    case PSCI_0_2_FN64(AFFINITY_INFO):
+    switch ( fid )
     {
-        register_t taff = PSCI_ARG(regs, 1);
-        uint32_t laff = PSCI_ARG32(regs, 2);
-
-        perfc_incr(vpsci_cpu_affinity_info);
-        PSCI_SET_RESULT(regs, do_psci_0_2_affinity_info(taff, laff));
-        return true;
-    }
-
     case ARM_SMCCC_CALL_COUNT_FID(STANDARD):
         return fill_function_call_count(regs, SSSC_SMCCC_FUNCTION_COUNT);
 
diff --git a/xen/include/asm-arm/psci.h b/xen/include/asm-arm/psci.h
index 32c1f81f21..3c44468e72 100644
--- a/xen/include/asm-arm/psci.h
+++ b/xen/include/asm-arm/psci.h
@@ -22,25 +22,6 @@  int call_psci_cpu_on(int cpu);
 void call_psci_system_off(void);
 void call_psci_system_reset(void);
 
-/* functions to handle guest PSCI requests */
-int32_t do_psci_cpu_on(uint32_t vcpuid, register_t entry_point);
-int32_t do_psci_cpu_off(uint32_t power_state);
-int32_t do_psci_cpu_suspend(uint32_t power_state, register_t entry_point);
-int32_t do_psci_migrate(uint32_t vcpuid);
-
-/* PSCI 0.2 functions to handle guest PSCI requests */
-uint32_t do_psci_0_2_version(void);
-register_t do_psci_0_2_cpu_suspend(uint32_t power_state, register_t entry_point,
-                            register_t context_id);
-int32_t do_psci_0_2_cpu_off(void);
-int32_t do_psci_0_2_cpu_on(register_t target_cpu, register_t entry_point,
-                       register_t context_id);
-int32_t do_psci_0_2_affinity_info(register_t target_affinity,
-                              uint32_t lowest_affinity_level);
-uint32_t do_psci_0_2_migrate_info_type(void);
-void do_psci_0_2_system_off(void);
-void do_psci_0_2_system_reset(void);
-
 /* PSCI v0.2 interface */
 #define PSCI_0_2_FN32(name) ARM_SMCCC_CALL_VAL(ARM_SMCCC_FAST_CALL,             \
                                                ARM_SMCCC_CONV_32,               \
diff --git a/xen/include/asm-arm/vpsci.h b/xen/include/asm-arm/vpsci.h
new file mode 100644
index 0000000000..035a41e812
--- /dev/null
+++ b/xen/include/asm-arm/vpsci.h
@@ -0,0 +1,42 @@ 
+/*
+ * xen/include/asm-arm/vpsci.h
+ *
+ * Julien Grall <julien.gral@linaro.org>
+ * Copyright (c) 2018 Linaro Limited.
+ *
+ * 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; under version 2 of the License.
+ *
+ * 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/>.
+ */
+
+#ifndef __ASM_VPSCI_H__
+#define __ASM_VPSCI_H__
+
+#include <asm/psci.h>
+
+/* Number of function implemented by virtual PSCI (only 0.2 or later) */
+#define VPSCI_NR_FUNCS  11
+
+/* Functions handle PSCI calls from the guests */
+bool do_vpsci_0_1_call(struct cpu_user_regs *regs, uint32_t fid);
+bool do_vpsci_0_2_call(struct cpu_user_regs *regs, uint32_t fid);
+
+#endif /* __ASM_VPSCI_H__ */
+
+/*
+ * Local variables:
+ * mode: C
+ * c-file-style: "BSD"
+ * c-basic-offset: 4
+ * tab-width: 4
+ * indent-tabs-mode: nil
+ * End:
+ */