diff mbox

[v2,2/4] arm: smccc: handle SMCs/HVCs according to SMCCC

Message ID 1498148700-10758-3-git-send-email-volodymyr_babchuk@epam.com (mailing list archive)
State New, archived
Headers show

Commit Message

Volodymyr Babchuk June 22, 2017, 4:24 p.m. UTC
SMCCC (SMC Call Convention) describes how to handle both HVCs and SMCs.
SMCCC states that both HVC and SMC are valid conduits to call to a different
firmware functions. Thus, for example PSCI calls can be made both by
SMC or HVC. Also SMCCC defines function number coding for such calls.
Besides functional calls there are query calls, which allows underling
OS determine version, UID and number of functions provided by service
provider.

This patch adds new file `vsmc.c`, which handles both generic SMCs
and HVC according to SMC. At this moment it implements only one
service: Standard Hypervisor Service.

Standard Hypervisor Service only supports query calls, so caller can
ask about hypervisor UID and determine that it is XEN running.

This change allows more generic handling for SMCs and HVCs and it can
be easily extended to support new services and functions.

But, before SMC is forwarded to standard SMCCC handler, it can be routed
to a domain monitor, if one is installed.

Signed-off-by: Volodymyr Babchuk <volodymyr_babchuk@epam.com>
Reviewed-by: Oleksandr Andrushchenko <oleksandr_andrushchenko@epam.com>
Reviewed-by: Oleksandr Tyshchenko <oleksandr_tyshchenko@epam.com>
---
 - Moved UID definition to xen/include/public/arch-arm/smc.h
 - Renamed smccc.c to vsmc.c and smccc.h to vsmc.h
 - Reformated vsmc.h and commented definitions there
 - Added immediate value check for SMC64, HVC32 and HVC64
 - Added conditional flags check for SMC calls (HVC will be handled
   and checked in the next patch).
 - Added check for 64 bit calls from 32 bit guests
 - Removed HSR value passing as separate argument
 - Various changes in comments
---
 xen/arch/arm/Makefile             |   1 +
 xen/arch/arm/traps.c              |  16 ++++-
 xen/arch/arm/vsmc.c               | 128 ++++++++++++++++++++++++++++++++++++++
 xen/include/asm-arm/vsmc.h        |  94 ++++++++++++++++++++++++++++
 xen/include/public/arch-arm/smc.h |  45 ++++++++++++++
 5 files changed, 283 insertions(+), 1 deletion(-)
 create mode 100644 xen/arch/arm/vsmc.c
 create mode 100644 xen/include/asm-arm/vsmc.h
 create mode 100644 xen/include/public/arch-arm/smc.h

Comments

Julien Grall June 30, 2017, 3:15 p.m. UTC | #1
Hi Volodymyr,

On 22/06/17 17:24, Volodymyr Babchuk wrote:
> SMCCC (SMC Call Convention) describes how to handle both HVCs and SMCs.
> SMCCC states that both HVC and SMC are valid conduits to call to a different
> firmware functions. Thus, for example PSCI calls can be made both by
> SMC or HVC. Also SMCCC defines function number coding for such calls.
> Besides functional calls there are query calls, which allows underling
> OS determine version, UID and number of functions provided by service
> provider.
>
> This patch adds new file `vsmc.c`, which handles both generic SMCs
> and HVC according to SMC. At this moment it implements only one
> service: Standard Hypervisor Service.
>
> Standard Hypervisor Service only supports query calls, so caller can
> ask about hypervisor UID and determine that it is XEN running.
>
> This change allows more generic handling for SMCs and HVCs and it can
> be easily extended to support new services and functions.
>
> But, before SMC is forwarded to standard SMCCC handler, it can be routed
> to a domain monitor, if one is installed.

Please address the comment I made on v1 after you sent it this version :).

>
> Signed-off-by: Volodymyr Babchuk <volodymyr_babchuk@epam.com>
> Reviewed-by: Oleksandr Andrushchenko <oleksandr_andrushchenko@epam.com>
> Reviewed-by: Oleksandr Tyshchenko <oleksandr_tyshchenko@epam.com>
> ---
>  - Moved UID definition to xen/include/public/arch-arm/smc.h
>  - Renamed smccc.c to vsmc.c and smccc.h to vsmc.h
>  - Reformated vsmc.h and commented definitions there
>  - Added immediate value check for SMC64, HVC32 and HVC64
>  - Added conditional flags check for SMC calls (HVC will be handled
>    and checked in the next patch).
>  - Added check for 64 bit calls from 32 bit guests
>  - Removed HSR value passing as separate argument
>  - Various changes in comments
> ---
>  xen/arch/arm/Makefile             |   1 +
>  xen/arch/arm/traps.c              |  16 ++++-
>  xen/arch/arm/vsmc.c               | 128 ++++++++++++++++++++++++++++++++++++++
>  xen/include/asm-arm/vsmc.h        |  94 ++++++++++++++++++++++++++++
>  xen/include/public/arch-arm/smc.h |  45 ++++++++++++++
>  5 files changed, 283 insertions(+), 1 deletion(-)
>  create mode 100644 xen/arch/arm/vsmc.c
>  create mode 100644 xen/include/asm-arm/vsmc.h
>  create mode 100644 xen/include/public/arch-arm/smc.h
>
> diff --git a/xen/arch/arm/Makefile b/xen/arch/arm/Makefile
> index 49e1fb2..4efd01c 100644
> --- a/xen/arch/arm/Makefile
> +++ b/xen/arch/arm/Makefile
> @@ -50,6 +50,7 @@ obj-$(CONFIG_HAS_GICV3) += vgic-v3.o
>  obj-$(CONFIG_HAS_ITS) += vgic-v3-its.o
>  obj-y += vm_event.o
>  obj-y += vtimer.o
> +obj-y += vsmc.o
>  obj-y += vpsci.o
>  obj-y += vuart.o
>
> diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
> index 2054c69..66242e5 100644
> --- a/xen/arch/arm/traps.c
> +++ b/xen/arch/arm/traps.c
> @@ -44,6 +44,7 @@
>  #include <asm/cpufeature.h>
>  #include <asm/flushtlb.h>
>  #include <asm/monitor.h>
> +#include <asm/vsmc.h>
>
>  #include "decode.h"
>  #include "vtimer.h"
> @@ -2771,10 +2772,23 @@ static void do_trap_smc(struct cpu_user_regs *regs, const union hsr hsr)
>  {
>      int rc = 0;
>
> +    if ( !check_conditional_instr(regs, hsr) )

This change is not related to this patch and not explain in the commit 
message. To help reviewing, each patch should do one logical thing and 
not more. In this case, it should go in a separate patch.

Furthermore, if you look at the check_condition_instr implementation, it 
has a check

/* Unconditional Exception classes */
if ( hsr.ec >= 0x10 )
   return 1;

The EC for SMC32 is 0x13. So this is not going to work.

For SMC64, they are always unconditional and those bit is RES0. But you 
cannot assume, they will not be used for other purpose in the future.

Now, looking at the documentation for ISS for SMC32 trap (D7-2271 and 
G6-4957 in ARM DDI 0487B.a), compare to other conditional instruction 
the ISS has an extra field CCKNOWNPASS (bit 19) to tell you whether CV 
and COND are valid.

But on ARMv7, the ISS is UNK/SBZP. Meaning the software cannot rely on 
reading bits as all 0s. I have raised a question internally on how to 
write software compatible ARMv7 and ARMv8 AArch32. I will let you know 
the update.

Meanwhile, I think you can prepare a patch to support CCKNOWNPASS for 
AArch32 and AArch64 (please mention the ARMv7 problem in it so we don't 
merge it until it is been figured out).

Lastly, looking at the check in itself, I think it is wrong as EC 0 is 
always unconditional (see B3-25 in ARM DDI 0406C.c and D7-2259 in ARM 
DDI 0487B.a). It would be nice to have this fixed (in a separate patch) 
but not highly critical as not called for unknown exception at the moment.

> +    {
> +        advance_pc(regs, hsr);
> +        return;
> +    }
> +
> +    /* If monitor is enabled, let it handle the call */
>      if ( current->domain->arch.monitor.privileged_call_enabled )
>          rc = monitor_smc();
>
> -    if ( rc != 1 )
> +    if ( rc == 1 )
> +        return;
> +
> +    /* Use standard routines to handle the call */
> +    if ( vsmc_handle_call(regs) )
> +        advance_pc(regs, hsr);
> +    else
>          inject_undef_exception(regs, hsr);
>  }
>
> diff --git a/xen/arch/arm/vsmc.c b/xen/arch/arm/vsmc.c
> new file mode 100644
> index 0000000..10c4acd
> --- /dev/null
> +++ b/xen/arch/arm/vsmc.c
> @@ -0,0 +1,128 @@
> +/*
> + * xen/arch/arm/vsmc.c
> + *
> + * Generic handler for SMC and HVC calls according to
> + * ARM SMC calling convention
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * 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.
> + */
> +
> +
> +#include <xen/config.h>

xen/config.h is automatically included by the compiler. So please don't 
include it.

> +#include <xen/lib.h>
> +/* Need to include xen/sched.h before asm/domain.h or it breaks build*/

I don't see any inclusion of asm/domain.h in this patch.

> +#include <xen/sched.h>
> +#include <xen/stdbool.h>

Please don't include stdbool.h directly. This is already included by 
lib.h anyway and ...

> +#include <xen/types.h>

lib.h already include xen/types.h.

> +#include <public/arch-arm/smc.h>
> +#include <asm/vsmc.h>
> +#include <asm/regs.h>
> +
> +/*
> + * Hypervisor Service version
> + *
> + * We can't use XEN version here, because of SMCCC requirements:
> + * Major revision should change every time SMC/HVC function is removed.
> + * Minor revision should change every time SMC/HVC function is added.
> + * So, it is SMCCC protocol revision code, not XEN version.
> + *
> + * Those values are subjected to change, when interface will be extended.
> + * They should not be stored in public/asm-arm/smc.h because they should

s/asm-arm/arch-arm/

> + * be queried by guest using SMC/HVC interface.

I am not sure if it is what we really want. From what you describe about 
the use of the SMCCC version. I would have expected the in-guest code to 
handle OP-TEE tie to a specific version of Xen. Very similar to other 
interface we version (see vm-event).

So I would make those versions public.

> + */
> +#define XEN_SMCCC_MAJOR_REVISION 0
> +#define XEN_SMCCC_MINOR_REVISION 1
> +
> +/* Number of functions currently supported by Hypervisor Service. */
> +#define XEN_SMCCC_FUNCTION_COUNT 3
> +
> +/* SMCCC interface for hypervisor. Tell about itself. */
> +static bool handle_hypervisor(struct cpu_user_regs *regs)
> +{
> +    switch ( ARM_SMCCC_FUNC_NUM(get_user_reg(regs, 0)) )
> +    {
> +    case ARM_SMCCC_FUNC_CALL_COUNT:
> +        set_user_reg(regs, 0, XEN_SMCCC_FUNCTION_COUNT);
> +        return true;
> +    case ARM_SMCCC_FUNC_CALL_UID:
> +        set_user_reg(regs, 0, XEN_SMCCC_UID.a[0]);
> +        set_user_reg(regs, 1, XEN_SMCCC_UID.a[1]);
> +        set_user_reg(regs, 2, XEN_SMCCC_UID.a[2]);
> +        set_user_reg(regs, 3, XEN_SMCCC_UID.a[3]);
> +        return true;
> +    case ARM_SMCCC_FUNC_CALL_REVISION:
> +        set_user_reg(regs, 0, XEN_SMCCC_MAJOR_REVISION);
> +        set_user_reg(regs, 1, XEN_SMCCC_MINOR_REVISION);
> +        return true;
> +    }
> +    return false;
> +}
> +
> +/**
> + * vsmc_handle_call() - handle SMC/HVC call according to ARM SMCCC
> + */
> +int vsmc_handle_call(struct cpu_user_regs *regs)

This function return either 0 or 1. So I think you can turned the return 
value into a boolean.

> +{
> +    bool handled = false;
> +    const union hsr hsr = { .bits = regs->hsr };
> +
> +    /*
> +     * Check immediate value for HVC32, HVC64 and SMC64.
> +     * It is not so easy to check immediate value for SMC32,

Please detail a bit more why it is not so easy.

> +     * so we will assume that it is 0x0

Missing full stop.

> +     */
> +    switch ( hsr.ec )
> +    {
> +    case HSR_EC_HVC32:
> +    case HSR_EC_HVC64:
> +    case HSR_EC_SMC64:
> +        if ( hsr.iss != 0)

I know the current HVC code is check hsr.iss != 0. But this is actually 
wrong. You cannot rely on the bits [24:16] to be 0 in the future.

> +            return 0;
> +        break;
> +    case HSR_EC_SMC32:
> +        break;
> +    default:
> +        return 0;
> +    }
> +
> +    /* 64 bit calls are allowed only from 64 bit domains */
> +    if ( ARM_SMCCC_IS_64(get_user_reg(regs, 0)) &&
> +         is_32bit_domain(current->domain) )
> +    {
> +        set_user_reg(regs, 0, ARM_SMCCC_ERR_UNKNOWN_FUNCTION);
> +        return 1;
> +    }
> +
> +    switch ( ARM_SMCCC_OWNER_NUM(get_user_reg(regs, 0)) )
> +    {
> +    case ARM_SMCCC_OWNER_HYPERVISOR:
> +        handled = handle_hypervisor(regs);
> +        break;
> +    }
> +
> +    if ( !handled )
> +    {
> +        gprintk(XENLOG_INFO, "Unhandled SMC/HVC: %08"PRIregister"\n",
> +                get_user_reg(regs, 0));
> +        /* Inform caller that function is not supported */
> +        set_user_reg(regs, 0, ARM_SMCCC_ERR_UNKNOWN_FUNCTION);
> +    }
> +
> +    return 1;
> +}
> +
> +/*
> + * Local variables:
> + * mode: C
> + * c-file-style: "BSD"
> + * c-basic-offset: 4
> + * indent-tabs-mode: nil
> + * End:
> + */
> diff --git a/xen/include/asm-arm/vsmc.h b/xen/include/asm-arm/vsmc.h
> new file mode 100644
> index 0000000..d2401c7
> --- /dev/null
> +++ b/xen/include/asm-arm/vsmc.h
> @@ -0,0 +1,94 @@

On v1, I've asked to use the Xen coding style in this file. This has not 
been done.

Please switch *all* the hard tab to soft tab.

> +/*
> + * Copyright (c) 2017, EPAM Systems
> + *
> + * This software is licensed under the terms of the GNU General Public
> + * License version 2, as published by the Free Software Foundation, and
> + * may be copied, distributed, and modified under those terms.
> + *
> + * 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.
> + *
> + */
> +#ifndef __ASM_ARM_VSMC_H__
> +#define __ASM_ARM_VSMC_H__
> +
> +#include <xen/types.h>
> +
> +/*
> + * This file provides common defines for ARM SMC Calling Convention as
> + * specified in
> + * http://infocenter.arm.com/help/topic/com.arm.doc.den0028a/index.html
> + */
> +
> +#define ARM_SMCCC_STD_CALL		0
> +#define ARM_SMCCC_FAST_CALL		1

By default, any numeric is type int (i.e signed). However 1 << 31 is 
undefined for signed number.

So please use 0U and 1U. A good habit is to always append U or UL 
depending on the type wanted.

I know that we have some places where we don't respect that. I have a 
series in plan to clean-up that.

> +#define ARM_SMCCC_TYPE_SHIFT		31
> +
> +#define ARM_SMCCC_SMC_32		0
> +#define ARM_SMCCC_SMC_64		1

See above.

> +#define ARM_SMCCC_CALL_CONV_SHIFT	30
> +
> +#define ARM_SMCCC_OWNER_MASK		0x3F
> +#define ARM_SMCCC_OWNER_SHIFT		24
> +
> +#define ARM_SMCCC_FUNC_MASK		0xFFFF
> +
> +/* Check if this is fast call */
> +#define ARM_SMCCC_IS_FAST_CALL(smc_val)                         \
> +    ((smc_val) & (ARM_SMCCC_FAST_CALL << ARM_SMCCC_TYPE_SHIFT))
> +
> +/* Check if this is 64 bit call  */
> +#define ARM_SMCCC_IS_64(smc_val)                                        \
> +    ((smc_val) & (ARM_SMCCC_SMC_64 << ARM_SMCCC_CALL_CONV_SHIFT))
> +
> +/* Get function number from function identifier */
> +#define ARM_SMCCC_FUNC_NUM(smc_val)	((smc_val) & ARM_SMCCC_FUNC_MASK)
> +
> +/* Get service owner number from function identifier */
> +#define ARM_SMCCC_OWNER_NUM(smc_val)                                    \
> +    (((smc_val) >> ARM_SMCCC_OWNER_SHIFT) & ARM_SMCCC_OWNER_MASK)
> +
> +/*
> + * Construct function identifier from call type (fast or standard),
> + * calling convention (32 or 64 bit), service owner and function number
> + */
> +#define ARM_SMCCC_CALL_VAL(type, calling_convention, owner, func_num)   \
> +    (((type) << ARM_SMCCC_TYPE_SHIFT) |                                 \
> +     ((calling_convention) << ARM_SMCCC_CALL_CONV_SHIFT) |              \
> +     (((owner) & ARM_SMCCC_OWNER_MASK) << ARM_SMCCC_OWNER_SHIFT) |      \
> +     ((func_num) & ARM_SMCCC_FUNC_MASK))
> +
> +/* List of know service owners */

s/know/known/

> +#define ARM_SMCCC_OWNER_ARCH		0
> +#define ARM_SMCCC_OWNER_CPU		1
> +#define ARM_SMCCC_OWNER_SIP		2
> +#define ARM_SMCCC_OWNER_OEM		3
> +#define ARM_SMCCC_OWNER_STANDARD	4
> +#define ARM_SMCCC_OWNER_HYPERVISOR	5
> +#define ARM_SMCCC_OWNER_TRUSTED_APP	48
> +#define ARM_SMCCC_OWNER_TRUSTED_APP_END	49
> +#define ARM_SMCCC_OWNER_TRUSTED_OS	50
> +#define ARM_SMCCC_OWNER_TRUSTED_OS_END	63
> +
> +/* List of generic function numbers */
> +#define ARM_SMCCC_FUNC_CALL_COUNT	0xFF00
> +#define ARM_SMCCC_FUNC_CALL_UID		0xFF01
> +#define ARM_SMCCC_FUNC_CALL_REVISION	0xFF03
> +
> +/* Only one error code defined in SMCCC */
> +#define ARM_SMCCC_ERR_UNKNOWN_FUNCTION	(-1)
> +
> +int vsmc_handle_call(struct cpu_user_regs *regs);
> +
> +#endif  /* __ASM_ARM_VSMC_H__ */
> +
> +/*
> + * Local variables:
> + * mode: C
> + * c-file-style: "BSD"
> + * c-basic-offset: 4
> + * indent-tabs-mode: nil
> + * End:b
> + */
> diff --git a/xen/include/public/arch-arm/smc.h b/xen/include/public/arch-arm/smc.h
> new file mode 100644
> index 0000000..aac292a
> --- /dev/null
> +++ b/xen/include/public/arch-arm/smc.h

See above for the coding style.

> @@ -0,0 +1,45 @@
> +/*
> + * smc.h
> + *
> + * SMC/HVC interface in accordance with SMC Calling Convention.
> + *
> + * Permission is hereby granted, free of charge, to any person obtaining a copy
> + * of this software and associated documentation files (the "Software"), to
> + * deal in the Software without restriction, including without limitation the
> + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
> + * sell copies of the Software, and to permit persons to whom the Software is
> + * furnished to do so, subject to the following conditions:
> + *
> + * The above copyright notice and this permission notice shall be included in
> + * all copies or substantial portions of the Software.
> + *
> + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
> + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
> + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
> + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
> + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
> + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
> + * DEALINGS IN THE SOFTWARE.
> + *
> + * Copyright 2017 (C) EPAM Systems
> + */
> +
> +#ifndef __XEN_PUBLIC_ARCH_ARM_SMC_H__
> +#define __XEN_PUBLIC_ARCH_ARM_SMC_H__
> +
> +typedef struct {
> +    uint32_t a[4];
> +} arm_smccc_uid;
> +
> +#define ARM_SMCCC_UID(a, b, c, d0, d1, d2, d3, d4, d5, d6, d7)		\
> +    ((arm_smccc_uid) {{(a), ((b) << 16 | (c) ),                         \
> +                ((d0) << 24 | (d1) << 16 | (d2) << 8 | (d3) << 0),      \
> +                ((d4) << 24 | (d5) << 16 | (d6) << 8 | (d7) << 0)}})

This is public headers, meaning they will get exported to userspace, 
kernel... I am a concerned that name with ARM_* will clash with 
something else.

I think we should at least prefix with XEN_ to prevent that. Same for 
arm_smccc_uid.

> +
> +
> +/* Hypervisor Service UID. Randomly generated with 3rd party tool  */
> +#define XEN_SMCCC_UID ARM_SMCCC_UID(0xa71812dc, 0xc698, 0x4369, \
> +                                    0x9a, 0xcf, 0x79, 0xd1, \
> +                                    0x8d, 0xde, 0xe6, 0x67)
> +
> +#endif	/* __XEN_PUBLIC_ARCH_ARM_SMC_H__ */
>

Missing emacs magic here.


Cheers,
Julien Grall July 18, 2017, 1:21 p.m. UTC | #2
On 30/06/17 16:15, Julien Grall wrote:
> Now, looking at the documentation for ISS for SMC32 trap (D7-2271 and
> G6-4957 in ARM DDI 0487B.a), compare to other conditional instruction
> the ISS has an extra field CCKNOWNPASS (bit 19) to tell you whether CV
> and COND are valid.
>
> But on ARMv7, the ISS is UNK/SBZP. Meaning the software cannot rely on
> reading bits as all 0s. I have raised a question internally on how to
> write software compatible ARMv7 and ARMv8 AArch32. I will let you know
> the update.
>
> Meanwhile, I think you can prepare a patch to support CCKNOWNPASS for
> AArch32 and AArch64 (please mention the ARMv7 problem in it so we don't
> merge it until it is been figured out).

I got an answer on this one. The policy is Should-Be-Zero-Preserved, if 
you do read-modify-write you have to preserve the values. For read-only 
operation it will appear as 0 on older revision and 0/1 on new revision.

Cheers,
diff mbox

Patch

diff --git a/xen/arch/arm/Makefile b/xen/arch/arm/Makefile
index 49e1fb2..4efd01c 100644
--- a/xen/arch/arm/Makefile
+++ b/xen/arch/arm/Makefile
@@ -50,6 +50,7 @@  obj-$(CONFIG_HAS_GICV3) += vgic-v3.o
 obj-$(CONFIG_HAS_ITS) += vgic-v3-its.o
 obj-y += vm_event.o
 obj-y += vtimer.o
+obj-y += vsmc.o
 obj-y += vpsci.o
 obj-y += vuart.o
 
diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index 2054c69..66242e5 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -44,6 +44,7 @@ 
 #include <asm/cpufeature.h>
 #include <asm/flushtlb.h>
 #include <asm/monitor.h>
+#include <asm/vsmc.h>
 
 #include "decode.h"
 #include "vtimer.h"
@@ -2771,10 +2772,23 @@  static void do_trap_smc(struct cpu_user_regs *regs, const union hsr hsr)
 {
     int rc = 0;
 
+    if ( !check_conditional_instr(regs, hsr) )
+    {
+        advance_pc(regs, hsr);
+        return;
+    }
+
+    /* If monitor is enabled, let it handle the call */
     if ( current->domain->arch.monitor.privileged_call_enabled )
         rc = monitor_smc();
 
-    if ( rc != 1 )
+    if ( rc == 1 )
+        return;
+
+    /* Use standard routines to handle the call */
+    if ( vsmc_handle_call(regs) )
+        advance_pc(regs, hsr);
+    else
         inject_undef_exception(regs, hsr);
 }
 
diff --git a/xen/arch/arm/vsmc.c b/xen/arch/arm/vsmc.c
new file mode 100644
index 0000000..10c4acd
--- /dev/null
+++ b/xen/arch/arm/vsmc.c
@@ -0,0 +1,128 @@ 
+/*
+ * xen/arch/arm/vsmc.c
+ *
+ * Generic handler for SMC and HVC calls according to
+ * ARM SMC calling convention
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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.
+ */
+
+
+#include <xen/config.h>
+#include <xen/lib.h>
+/* Need to include xen/sched.h before asm/domain.h or it breaks build*/
+#include <xen/sched.h>
+#include <xen/stdbool.h>
+#include <xen/types.h>
+#include <public/arch-arm/smc.h>
+#include <asm/vsmc.h>
+#include <asm/regs.h>
+
+/*
+ * Hypervisor Service version
+ *
+ * We can't use XEN version here, because of SMCCC requirements:
+ * Major revision should change every time SMC/HVC function is removed.
+ * Minor revision should change every time SMC/HVC function is added.
+ * So, it is SMCCC protocol revision code, not XEN version.
+ *
+ * Those values are subjected to change, when interface will be extended.
+ * They should not be stored in public/asm-arm/smc.h because they should
+ * be queried by guest using SMC/HVC interface.
+ */
+#define XEN_SMCCC_MAJOR_REVISION 0
+#define XEN_SMCCC_MINOR_REVISION 1
+
+/* Number of functions currently supported by Hypervisor Service. */
+#define XEN_SMCCC_FUNCTION_COUNT 3
+
+/* SMCCC interface for hypervisor. Tell about itself. */
+static bool handle_hypervisor(struct cpu_user_regs *regs)
+{
+    switch ( ARM_SMCCC_FUNC_NUM(get_user_reg(regs, 0)) )
+    {
+    case ARM_SMCCC_FUNC_CALL_COUNT:
+        set_user_reg(regs, 0, XEN_SMCCC_FUNCTION_COUNT);
+        return true;
+    case ARM_SMCCC_FUNC_CALL_UID:
+        set_user_reg(regs, 0, XEN_SMCCC_UID.a[0]);
+        set_user_reg(regs, 1, XEN_SMCCC_UID.a[1]);
+        set_user_reg(regs, 2, XEN_SMCCC_UID.a[2]);
+        set_user_reg(regs, 3, XEN_SMCCC_UID.a[3]);
+        return true;
+    case ARM_SMCCC_FUNC_CALL_REVISION:
+        set_user_reg(regs, 0, XEN_SMCCC_MAJOR_REVISION);
+        set_user_reg(regs, 1, XEN_SMCCC_MINOR_REVISION);
+        return true;
+    }
+    return false;
+}
+
+/**
+ * vsmc_handle_call() - handle SMC/HVC call according to ARM SMCCC
+ */
+int vsmc_handle_call(struct cpu_user_regs *regs)
+{
+    bool handled = false;
+    const union hsr hsr = { .bits = regs->hsr };
+
+    /*
+     * Check immediate value for HVC32, HVC64 and SMC64.
+     * It is not so easy to check immediate value for SMC32,
+     * so we will assume that it is 0x0
+     */
+    switch ( hsr.ec )
+    {
+    case HSR_EC_HVC32:
+    case HSR_EC_HVC64:
+    case HSR_EC_SMC64:
+        if ( hsr.iss != 0)
+            return 0;
+        break;
+    case HSR_EC_SMC32:
+        break;
+    default:
+        return 0;
+    }
+
+    /* 64 bit calls are allowed only from 64 bit domains */
+    if ( ARM_SMCCC_IS_64(get_user_reg(regs, 0)) &&
+         is_32bit_domain(current->domain) )
+    {
+        set_user_reg(regs, 0, ARM_SMCCC_ERR_UNKNOWN_FUNCTION);
+        return 1;
+    }
+
+    switch ( ARM_SMCCC_OWNER_NUM(get_user_reg(regs, 0)) )
+    {
+    case ARM_SMCCC_OWNER_HYPERVISOR:
+        handled = handle_hypervisor(regs);
+        break;
+    }
+
+    if ( !handled )
+    {
+        gprintk(XENLOG_INFO, "Unhandled SMC/HVC: %08"PRIregister"\n",
+                get_user_reg(regs, 0));
+        /* Inform caller that function is not supported */
+        set_user_reg(regs, 0, ARM_SMCCC_ERR_UNKNOWN_FUNCTION);
+    }
+
+    return 1;
+}
+
+/*
+ * Local variables:
+ * mode: C
+ * c-file-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
diff --git a/xen/include/asm-arm/vsmc.h b/xen/include/asm-arm/vsmc.h
new file mode 100644
index 0000000..d2401c7
--- /dev/null
+++ b/xen/include/asm-arm/vsmc.h
@@ -0,0 +1,94 @@ 
+/*
+ * Copyright (c) 2017, EPAM Systems
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+#ifndef __ASM_ARM_VSMC_H__
+#define __ASM_ARM_VSMC_H__
+
+#include <xen/types.h>
+
+/*
+ * This file provides common defines for ARM SMC Calling Convention as
+ * specified in
+ * http://infocenter.arm.com/help/topic/com.arm.doc.den0028a/index.html
+ */
+
+#define ARM_SMCCC_STD_CALL		0
+#define ARM_SMCCC_FAST_CALL		1
+#define ARM_SMCCC_TYPE_SHIFT		31
+
+#define ARM_SMCCC_SMC_32		0
+#define ARM_SMCCC_SMC_64		1
+#define ARM_SMCCC_CALL_CONV_SHIFT	30
+
+#define ARM_SMCCC_OWNER_MASK		0x3F
+#define ARM_SMCCC_OWNER_SHIFT		24
+
+#define ARM_SMCCC_FUNC_MASK		0xFFFF
+
+/* Check if this is fast call */
+#define ARM_SMCCC_IS_FAST_CALL(smc_val)                         \
+    ((smc_val) & (ARM_SMCCC_FAST_CALL << ARM_SMCCC_TYPE_SHIFT))
+
+/* Check if this is 64 bit call  */
+#define ARM_SMCCC_IS_64(smc_val)                                        \
+    ((smc_val) & (ARM_SMCCC_SMC_64 << ARM_SMCCC_CALL_CONV_SHIFT))
+
+/* Get function number from function identifier */
+#define ARM_SMCCC_FUNC_NUM(smc_val)	((smc_val) & ARM_SMCCC_FUNC_MASK)
+
+/* Get service owner number from function identifier */
+#define ARM_SMCCC_OWNER_NUM(smc_val)                                    \
+    (((smc_val) >> ARM_SMCCC_OWNER_SHIFT) & ARM_SMCCC_OWNER_MASK)
+
+/*
+ * Construct function identifier from call type (fast or standard),
+ * calling convention (32 or 64 bit), service owner and function number
+ */
+#define ARM_SMCCC_CALL_VAL(type, calling_convention, owner, func_num)   \
+    (((type) << ARM_SMCCC_TYPE_SHIFT) |                                 \
+     ((calling_convention) << ARM_SMCCC_CALL_CONV_SHIFT) |              \
+     (((owner) & ARM_SMCCC_OWNER_MASK) << ARM_SMCCC_OWNER_SHIFT) |      \
+     ((func_num) & ARM_SMCCC_FUNC_MASK))
+
+/* List of know service owners */
+#define ARM_SMCCC_OWNER_ARCH		0
+#define ARM_SMCCC_OWNER_CPU		1
+#define ARM_SMCCC_OWNER_SIP		2
+#define ARM_SMCCC_OWNER_OEM		3
+#define ARM_SMCCC_OWNER_STANDARD	4
+#define ARM_SMCCC_OWNER_HYPERVISOR	5
+#define ARM_SMCCC_OWNER_TRUSTED_APP	48
+#define ARM_SMCCC_OWNER_TRUSTED_APP_END	49
+#define ARM_SMCCC_OWNER_TRUSTED_OS	50
+#define ARM_SMCCC_OWNER_TRUSTED_OS_END	63
+
+/* List of generic function numbers */
+#define ARM_SMCCC_FUNC_CALL_COUNT	0xFF00
+#define ARM_SMCCC_FUNC_CALL_UID		0xFF01
+#define ARM_SMCCC_FUNC_CALL_REVISION	0xFF03
+
+/* Only one error code defined in SMCCC */
+#define ARM_SMCCC_ERR_UNKNOWN_FUNCTION	(-1)
+
+int vsmc_handle_call(struct cpu_user_regs *regs);
+
+#endif  /* __ASM_ARM_VSMC_H__ */
+
+/*
+ * Local variables:
+ * mode: C
+ * c-file-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:b
+ */
diff --git a/xen/include/public/arch-arm/smc.h b/xen/include/public/arch-arm/smc.h
new file mode 100644
index 0000000..aac292a
--- /dev/null
+++ b/xen/include/public/arch-arm/smc.h
@@ -0,0 +1,45 @@ 
+/*
+ * smc.h
+ *
+ * SMC/HVC interface in accordance with SMC Calling Convention.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to
+ * deal in the Software without restriction, including without limitation the
+ * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+ * sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Copyright 2017 (C) EPAM Systems
+ */
+
+#ifndef __XEN_PUBLIC_ARCH_ARM_SMC_H__
+#define __XEN_PUBLIC_ARCH_ARM_SMC_H__
+
+typedef struct {
+    uint32_t a[4];
+} arm_smccc_uid;
+
+#define ARM_SMCCC_UID(a, b, c, d0, d1, d2, d3, d4, d5, d6, d7)		\
+    ((arm_smccc_uid) {{(a), ((b) << 16 | (c) ),                         \
+                ((d0) << 24 | (d1) << 16 | (d2) << 8 | (d3) << 0),      \
+                ((d4) << 24 | (d5) << 16 | (d6) << 8 | (d7) << 0)}})
+
+
+/* Hypervisor Service UID. Randomly generated with 3rd party tool  */
+#define XEN_SMCCC_UID ARM_SMCCC_UID(0xa71812dc, 0xc698, 0x4369, \
+                                    0x9a, 0xcf, 0x79, 0xd1, \
+                                    0x8d, 0xde, 0xe6, 0x67)
+
+#endif	/* __XEN_PUBLIC_ARCH_ARM_SMC_H__ */