diff --git a/xen/arch/arm/traps.c b/xen/arch/arm/traps.c
index b119dc0..7b296da 100644
--- a/xen/arch/arm/traps.c
+++ b/xen/arch/arm/traps.c
@@ -39,7 +39,6 @@
 #include <asm/event.h>
 #include <asm/regs.h>
 #include <asm/cpregs.h>
-#include <asm/psci.h>
 #include <asm/mmio.h>
 #include <asm/cpufeature.h>
 #include <asm/flushtlb.h>
@@ -1446,119 +1445,6 @@ static void do_debug_trap(struct cpu_user_regs *regs, unsigned int code)
 }
 #endif
 
-#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) & 0x00000000FFFFFFFF)
-#else
-#define PSCI_ARG32(reg,n) PSCI_ARG(reg,n)
-#endif
-
-/* helper function for checking arm mode 32/64 bit */
-static inline int psci_mode_check(struct domain *d, register_t fid)
-{
-        return !( is_64bit_domain(d)^( (fid & PSCI_0_2_64BIT) >> 30 ) );
-}
-
-static void do_trap_psci(struct cpu_user_regs *regs)
-{
-    register_t fid = PSCI_ARG(regs,0);
-
-    /* preloading in case psci_mode_check fails */
-    PSCI_SET_RESULT(regs, PSCI_INVALID_PARAMETERS);
-    switch( fid )
-    {
-    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));
-        }
-        break;
-    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));
-        }
-        break;
-    case PSCI_0_2_FN_PSCI_VERSION:
-        perfc_incr(vpsci_version);
-        PSCI_SET_RESULT(regs, do_psci_0_2_version());
-        break;
-    case PSCI_0_2_FN_CPU_OFF:
-        perfc_incr(vpsci_cpu_off);
-        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_off());
-        break;
-    case PSCI_0_2_FN_MIGRATE_INFO_TYPE:
-        perfc_incr(vpsci_migrate_info_type);
-        PSCI_SET_RESULT(regs, do_psci_0_2_migrate_info_type());
-        break;
-    case PSCI_0_2_FN_MIGRATE_INFO_UP_CPU:
-    case PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU:
-        perfc_incr(vpsci_migrate_info_up_cpu);
-        if ( psci_mode_check(current->domain, fid) )
-            PSCI_SET_RESULT(regs, do_psci_0_2_migrate_info_up_cpu());
-        break;
-    case PSCI_0_2_FN_SYSTEM_OFF:
-        perfc_incr(vpsci_system_off);
-        do_psci_0_2_system_off();
-        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
-        break;
-    case PSCI_0_2_FN_SYSTEM_RESET:
-        perfc_incr(vpsci_system_reset);
-        do_psci_0_2_system_reset();
-        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
-        break;
-    case PSCI_0_2_FN_CPU_ON:
-    case PSCI_0_2_FN64_CPU_ON:
-        perfc_incr(vpsci_cpu_on);
-        if ( psci_mode_check(current->domain, fid) )
-        {
-            register_t vcpuid = PSCI_ARG(regs,1);
-            register_t epoint = PSCI_ARG(regs,2);
-            register_t cid = PSCI_ARG(regs,3);
-            PSCI_SET_RESULT(regs, do_psci_0_2_cpu_on(vcpuid, epoint, cid));
-        }
-        break;
-    case PSCI_0_2_FN_CPU_SUSPEND:
-    case PSCI_0_2_FN64_CPU_SUSPEND:
-        perfc_incr(vpsci_cpu_suspend);
-        if ( psci_mode_check(current->domain, fid) )
-        {
-            uint32_t pstate = PSCI_ARG32(regs,1);
-            register_t epoint = PSCI_ARG(regs,2);
-            register_t cid = PSCI_ARG(regs,3);
-            PSCI_SET_RESULT(regs, do_psci_0_2_cpu_suspend(pstate, epoint, cid));
-        }
-        break;
-    case PSCI_0_2_FN_AFFINITY_INFO:
-    case PSCI_0_2_FN64_AFFINITY_INFO:
-        perfc_incr(vpsci_cpu_affinity_info);
-        if ( psci_mode_check(current->domain, fid) )
-        {
-            register_t taff = PSCI_ARG(regs,1);
-            uint32_t laff = PSCI_ARG32(regs,2);
-            PSCI_SET_RESULT(regs, do_psci_0_2_affinity_info(taff, laff));
-        }
-        break;
-    case PSCI_0_2_FN_MIGRATE:
-    case PSCI_0_2_FN64_MIGRATE:
-        perfc_incr(vpsci_cpu_migrate);
-        if ( psci_mode_check(current->domain, fid) )
-        {
-            uint32_t tcpu = PSCI_ARG32(regs,1);
-            PSCI_SET_RESULT(regs, do_psci_0_2_migrate(tcpu));
-        }
-        break;
-    default:
-        domain_crash_synchronous();
-        return;
-    }
-}
-
 #ifdef CONFIG_ARM_64
 #define HYPERCALL_RESULT_REG(r) (r)->x0
 #define HYPERCALL_ARG1(r) (r)->x0
@@ -2865,8 +2751,9 @@ asmlinkage void do_trap_guest_sync(struct cpu_user_regs *regs)
             return do_debug_trap(regs, hsr.iss & 0x00ff);
 #endif
         if ( hsr.iss == 0 )
-            return do_trap_psci(regs);
-        do_trap_hypercall(regs, (register_t *)&regs->r12, hsr.iss);
+            do_trap_hvc_smccc(regs);
+        else
+            do_trap_hypercall(regs, (register_t *)&regs->r12, hsr.iss);
         break;
 #ifdef CONFIG_ARM_64
     case HSR_EC_HVC64:
@@ -2877,8 +2764,9 @@ asmlinkage void do_trap_guest_sync(struct cpu_user_regs *regs)
             return do_debug_trap(regs, hsr.iss & 0x00ff);
 #endif
         if ( hsr.iss == 0 )
-            return do_trap_psci(regs);
-        do_trap_hypercall(regs, &regs->x16, hsr.iss);
+            do_trap_hvc_smccc(regs);
+        else
+            do_trap_hypercall(regs, &regs->x16, hsr.iss);
         break;
     case HSR_EC_SMC64:
         /*
diff --git a/xen/arch/arm/vsmc.c b/xen/arch/arm/vsmc.c
index 5ef6167..718b30f 100644
--- a/xen/arch/arm/vsmc.c
+++ b/xen/arch/arm/vsmc.c
@@ -19,12 +19,16 @@
 #include <xen/types.h>
 #include <public/arch-arm/smc.h>
 #include <asm/monitor.h>
+#include <asm/psci.h>
 #include <asm/vsmc.h>
 #include <asm/regs.h>
 
 /* Number of functions currently supported by Hypervisor Service. */
 #define XEN_SMCCC_FUNCTION_COUNT 3
 
+/* Number of functions currently supported by Standard Service Service. */
+#define SSC_SMCCC_FUNCTION_COUNT 13
+
 /* SMCCC interface for hypervisor. Tell about itself. */
 static bool handle_hypervisor(struct cpu_user_regs *regs)
 {
@@ -47,6 +51,133 @@ static bool handle_hypervisor(struct cpu_user_regs *regs)
     return false;
 }
 
+#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) & 0x00000000FFFFFFFF)
+#else
+#define PSCI_ARG32(reg,n) PSCI_ARG(reg,n)
+#endif
+
+/* old (arvm7) PSCI interface */
+static bool handle_arch(struct cpu_user_regs *regs)
+{
+    switch ( get_user_reg(regs,0) & 0xFFFFFFFF )
+    {
+    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;
+    }
+    return false;
+}
+
+/* helper function for checking arm mode 32/64 bit */
+static inline int psci_mode_check(struct domain *d, register_t fid)
+{
+    return !( is_64bit_domain(d)^( (fid & PSCI_0_2_64BIT) >> 30 ) );
+}
+
+/* PSCI 2.0 interface */
+static bool handle_ssc(struct cpu_user_regs *regs)
+{
+    register_t fid = PSCI_ARG(regs, 0);
+
+    switch ( ARM_SMCCC_FUNC_NUM(fid) )
+    {
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_PSCI_VERSION):
+        perfc_incr(vpsci_version);
+        PSCI_SET_RESULT(regs, do_psci_0_2_version());
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_CPU_OFF):
+        perfc_incr(vpsci_cpu_off);
+        PSCI_SET_RESULT(regs, do_psci_0_2_cpu_off());
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_MIGRATE_INFO_TYPE):
+        perfc_incr(vpsci_migrate_info_type);
+        PSCI_SET_RESULT(regs, do_psci_0_2_migrate_info_type());
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_MIGRATE_INFO_UP_CPU):
+        perfc_incr(vpsci_migrate_info_up_cpu);
+        if ( psci_mode_check(current->domain, fid) )
+            PSCI_SET_RESULT(regs, do_psci_0_2_migrate_info_up_cpu());
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_SYSTEM_OFF):
+        perfc_incr(vpsci_system_off);
+        do_psci_0_2_system_off();
+        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_SYSTEM_RESET):
+        perfc_incr(vpsci_system_reset);
+        do_psci_0_2_system_reset();
+        PSCI_SET_RESULT(regs, PSCI_INTERNAL_FAILURE);
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_CPU_ON):
+        perfc_incr(vpsci_cpu_on);
+        if ( psci_mode_check(current->domain, fid) )
+        {
+            register_t vcpuid = PSCI_ARG(regs,1);
+            register_t epoint = PSCI_ARG(regs,2);
+            register_t cid = PSCI_ARG(regs,3);
+            PSCI_SET_RESULT(regs, do_psci_0_2_cpu_on(vcpuid, epoint, cid));
+        }
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_CPU_SUSPEND):
+        perfc_incr(vpsci_cpu_suspend);
+        if ( psci_mode_check(current->domain, fid) )
+        {
+            uint32_t pstate = PSCI_ARG32(regs,1);
+            register_t epoint = PSCI_ARG(regs,2);
+            register_t cid = PSCI_ARG(regs,3);
+            PSCI_SET_RESULT(regs, do_psci_0_2_cpu_suspend(pstate, epoint, cid));
+        }
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_AFFINITY_INFO):
+        perfc_incr(vpsci_cpu_affinity_info);
+        if ( psci_mode_check(current->domain, fid) )
+        {
+            register_t taff = PSCI_ARG(regs,1);
+            uint32_t laff = PSCI_ARG32(regs,2);
+            PSCI_SET_RESULT(regs, do_psci_0_2_affinity_info(taff, laff));
+        }
+        return true;
+    case ARM_SMCCC_FUNC_NUM(PSCI_0_2_FN_MIGRATE):
+        perfc_incr(vpsci_cpu_migrate);
+        if ( psci_mode_check(current->domain, fid) )
+        {
+            uint32_t tcpu = PSCI_ARG32(regs,1);
+            PSCI_SET_RESULT(regs, do_psci_0_2_migrate(tcpu));
+        }
+        return true;
+    case ARM_SMCCC_FUNC_CALL_COUNT:
+        set_user_reg(regs, 0, SSC_SMCCC_FUNCTION_COUNT);
+        return true;
+    case ARM_SMCCC_FUNC_CALL_UID:
+        set_user_reg(regs, 0, SSC_SMCCC_UID.a[0]);
+        set_user_reg(regs, 1, SSC_SMCCC_UID.a[1]);
+        set_user_reg(regs, 2, SSC_SMCCC_UID.a[2]);
+        set_user_reg(regs, 3, SSC_SMCCC_UID.a[3]);
+        return true;
+    case ARM_SMCCC_FUNC_CALL_REVISION:
+        set_user_reg(regs, 0, SSC_SMCCC_MAJOR_REVISION);
+        set_user_reg(regs, 1, SSC_SMCCC_MINOR_REVISION);
+        return true;
+    }
+    return false;
+}
+
 /*
  * vsmc_handle_call() - handle SMC/HVC call according to ARM SMCCC.
  * returns true if that was valid SMCCC call (even if function number
@@ -91,6 +222,12 @@ static bool vsmc_handle_call(struct cpu_user_regs *regs)
     case ARM_SMCCC_OWNER_HYPERVISOR:
         handled = handle_hypervisor(regs);
         break;
+    case ARM_SMCCC_OWNER_ARCH:
+        handled = handle_arch(regs);
+        break;
+    case ARM_SMCCC_OWNER_STANDARD:
+        handled = handle_ssc(regs);
+        break;
     }
 
     if ( !handled )
@@ -129,6 +266,13 @@ void do_trap_smc(struct cpu_user_regs *regs, const union hsr hsr)
         inject_undef_exception(regs, hsr);
 }
 
+/* This function will be called from traps.c */
+void do_trap_hvc_smccc(struct cpu_user_regs *regs)
+{
+    if ( !vsmc_handle_call(regs) )
+        domain_crash_synchronous();
+}
+
 /*
  * Local variables:
  * mode: C
diff --git a/xen/include/asm-arm/vsmc.h b/xen/include/asm-arm/vsmc.h
index 7baabef..d1e7b01 100644
--- a/xen/include/asm-arm/vsmc.h
+++ b/xen/include/asm-arm/vsmc.h
@@ -81,6 +81,7 @@
 #define ARM_SMCCC_ERR_UNKNOWN_FUNCTION  (-1)
 
 void do_trap_smc(struct cpu_user_regs *regs, const union hsr hsr);
+void do_trap_hvc_smccc(struct cpu_user_regs *regs);
 
 #endif  /* __ASM_ARM_VSMC_H__ */
 
diff --git a/xen/include/public/arch-arm/smc.h b/xen/include/public/arch-arm/smc.h
index 42f3165..11b5ef7 100644
--- a/xen/include/public/arch-arm/smc.h
+++ b/xen/include/public/arch-arm/smc.h
@@ -56,8 +56,16 @@ typedef struct {
                                         0x9a, 0xcf, 0x79, 0xd1, \
                                         0x8d, 0xde, 0xe6, 0x67)
 
-#endif  /* __XEN_PUBLIC_ARCH_ARM_SMC_H__ */
+/* Standard Service version. Check comment for Hypervisor Service for rules. */
+#define SSC_SMCCC_MAJOR_REVISION 0
+#define SSC_SMCCC_MINOR_REVISION 1
+
+/* Standard Service Call UID. Randomly generated with 3rd party tool. */
+#define SSC_SMCCC_UID XEN_ARM_SMCCC_UID(0xf863386f, 0x4b39, 0x4cbd, \
+                                        0x92, 0x20, 0xce, 0x16, \
+                                        0x41, 0xe5, 0x9f, 0x6f)
 
+#endif  /* __XEN_PUBLIC_ARCH_ARM_SMC_H__ */
 /*
  * Local variables:
  * mode: C
