diff --git a/sys/conf/files.arm b/sys/conf/files.arm
index 6134b20e1177..18ca0692c67f 100644
--- a/sys/conf/files.arm
+++ b/sys/conf/files.arm
@@ -114,7 +114,7 @@ dev/ofw/ofwpci.c		optional	fdt pci
 dev/pci/pci_host_generic.c	optional	pci_host_generic pci
 dev/pci/pci_host_generic_fdt.c	optional	pci_host_generic pci fdt
 dev/psci/psci.c			optional	psci
-dev/psci/psci_arm.S		optional	psci
+dev/psci/smccc_arm.S		optional	psci
 dev/syscons/scgfbrndr.c		optional	sc
 dev/uart/uart_cpu_fdt.c		optional	uart fdt
 
diff --git a/sys/conf/files.arm64 b/sys/conf/files.arm64
index 7babadb91f74..3e241238abae 100644
--- a/sys/conf/files.arm64
+++ b/sys/conf/files.arm64
@@ -233,7 +233,7 @@ dev/pci/pci_host_generic.c	optional	pci
 dev/pci/pci_host_generic_acpi.c	optional	pci acpi
 dev/pci/pci_host_generic_fdt.c	optional	pci fdt
 dev/psci/psci.c			standard
-dev/psci/psci_arm64.S		standard
+dev/psci/smccc_arm64.S		standard
 dev/psci/smccc.c		standard
 dev/sdhci/sdhci_xenon.c		optional	sdhci_xenon sdhci fdt
 dev/uart/uart_cpu_arm64.c	optional	uart
diff --git a/sys/dev/psci/psci.c b/sys/dev/psci/psci.c
index c69399f361ad..52ef12b09938 100644
--- a/sys/dev/psci/psci.c
+++ b/sys/dev/psci/psci.c
@@ -128,7 +128,9 @@ static int psci_attach(device_t, psci_initfn_t, int);
 static void psci_shutdown(void *, int);
 
 static int psci_find_callfn(psci_callfn_t *);
-static int psci_def_callfn(register_t, register_t, register_t, register_t);
+static int psci_def_callfn(register_t, register_t, register_t, register_t,
+	register_t, register_t, register_t, register_t,
+	struct arm_smccc_res *res);
 
 psci_callfn_t psci_callfn = psci_def_callfn;
 
@@ -149,7 +151,10 @@ SYSINIT(psci_start, SI_SUB_CPU, SI_ORDER_FIRST, psci_init, NULL);
 
 static int
 psci_def_callfn(register_t a __unused, register_t b __unused,
-    register_t c __unused, register_t d __unused)
+    register_t c __unused, register_t d __unused,
+    register_t e __unused, register_t f __unused,
+    register_t g __unused, register_t h __unused,
+    struct arm_smccc_res *res __unused)
 {
 
 	panic("No PSCI/SMCCC call function set");
@@ -186,9 +191,9 @@ psci_fdt_get_callfn(phandle_t node)
 
 	if ((OF_getprop(node, "method", method, sizeof(method))) > 0) {
 		if (strcmp(method, "hvc") == 0)
-			return (psci_hvc_despatch);
+			return (arm_smccc_hvc);
 		else if (strcmp(method, "smc") == 0)
-			return (psci_smc_despatch);
+			return (arm_smccc_smc);
 		else
 			printf("psci: PSCI conduit \"%s\" invalid\n", method);
 	} else
@@ -282,9 +287,9 @@ psci_acpi_get_callfn(int flags)
 
 	if ((flags & ACPI_FADT_PSCI_COMPLIANT) != 0) {
 		if ((flags & ACPI_FADT_PSCI_USE_HVC) != 0)
-			return (psci_hvc_despatch);
+			return (arm_smccc_hvc);
 		else
-			return (psci_smc_despatch);
+			return (arm_smccc_smc);
 	} else {
 		printf("psci: PSCI conduit not supplied in the device tree\n");
 	}
diff --git a/sys/dev/psci/psci.h b/sys/dev/psci/psci.h
index 5d481a937b1f..f2c8a43c81ea 100644
--- a/sys/dev/psci/psci.h
+++ b/sys/dev/psci/psci.h
@@ -30,9 +30,12 @@
 #define	_MACHINE_PSCI_H_
 
 #include <sys/types.h>
+#include <dev/psci/smccc.h>
 
 typedef int (*psci_initfn_t)(device_t dev, int default_version);
-typedef int (*psci_callfn_t)(register_t, register_t, register_t, register_t);
+typedef int (*psci_callfn_t)(register_t, register_t, register_t, register_t,
+	register_t, register_t, register_t, register_t,
+	struct arm_smccc_res *res);
 
 extern int psci_present;
 
@@ -47,12 +50,8 @@ static inline int
 psci_call(register_t a, register_t b, register_t c, register_t d)
 {
 
-	return (psci_callfn(a, b, c, d));
+	return (psci_callfn(a, b, c, d, 0, 0, 0, 0, NULL));
 }
-/* One of these handlers will be selected during the boot */
-int	psci_hvc_despatch(register_t, register_t, register_t, register_t);
-int	psci_smc_despatch(register_t, register_t, register_t, register_t);
-
 
 /*
  * PSCI return codes.
diff --git a/sys/dev/psci/smccc.h b/sys/dev/psci/smccc.h
index 5ff78655552d..6bb4dbcf7076 100644
--- a/sys/dev/psci/smccc.h
+++ b/sys/dev/psci/smccc.h
@@ -49,6 +49,21 @@
 #define	SMCCC_32BIT_CALL	0
 #define	SMCCC_64BIT_CALL	1
 
+#define	SMCCC_ARM_ARCH_CALLS		0
+#define	SMCCC_CPU_SERVICE_CALLS		1
+#define	SMCCC_SIP_SERVICE_CALLS		2
+#define	SMCCC_OEM_SERVICE_CALLS		3
+#define	SMCCC_STD_SECURE_SERVICE_CALLS	4
+#define	SMCCC_STD_HYP_SERVICE_CALLS	5
+#define	SMCCC_VENDOR_HYP_SERVICE_CALLS	6
+
+struct arm_smccc_res {
+	register_t a0;
+	register_t a1;
+	register_t a2;
+	register_t a3;
+};
+
 /*
  * Arm Architecture Calls.
  * These are documented in the document ARM DEN 0070A.
@@ -71,5 +86,9 @@ int32_t smccc_arch_features(uint32_t);
 int smccc_arch_workaround_1(void);
 int smccc_arch_workaround_2(int);
 
+int arm_smccc_smc(register_t, register_t, register_t, register_t, register_t,
+    register_t, register_t, register_t, struct arm_smccc_res *res);
+int arm_smccc_hvc(register_t, register_t, register_t, register_t, register_t,
+    register_t, register_t, register_t, struct arm_smccc_res *res);
 
 #endif /* _PSCI_SMCCC_H_ */
diff --git a/sys/dev/psci/psci_arm.S b/sys/dev/psci/smccc_arm.S
similarity index 59%
rename from sys/dev/psci/psci_arm.S
rename to sys/dev/psci/smccc_arm.S
index 42769cfed5a4..ae5a94197718 100644
--- a/sys/dev/psci/psci_arm.S
+++ b/sys/dev/psci/smccc_arm.S
@@ -1,6 +1,13 @@
 /*-
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Ruslan Bukin <br@bsdpad.com>
  * Copyright (c) 2015 Andrew Turner
- * All rights reserved.
+ *
+ * This software was developed by SRI International and the University of
+ * Cambridge Computer Laboratory (Department of Computer Science and
+ * Technology) under DARPA contract HR0011-18-C-0016 ("ECATS"), as part of the
+ * DARPA SSITH research programme.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -22,7 +29,6 @@
  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
- *
  */
 
 #include <machine/asm.h>
@@ -32,17 +38,37 @@ __FBSDID("$FreeBSD$");
 .arch_extension virt	/* For hvc */
 
 /*
- * int psci_hvc_despatch(register_t psci_fnid, register_t...)
+ * int arm_smccc_hvc(register_t, register_t, register_t, register_t,
+ *     register_t, register_t, register_t, register_t,
+ *     struct arm_smccc_res *res)
  */
-ENTRY(psci_hvc_despatch)
+ENTRY(arm_smccc_hvc)
+	mov	r12, sp
+	push	{r4-r7}
+	ldm	r12, {r4-r7}
 	hvc	#0
-	RET
-END(psci_hvc_despatch)
+	pop	{r4-r7}
+	ldr	r12, [sp, #(4 * 4)]
+	cmp     r12, #0
+	beq	1f
+	stm	r12, {r0-r3}
+1:	bx	lr
+END(arm_smccc_hvc)
 
 /*
- * int psci_smc_despatch(register_t psci_fnid, register_t...)
+ * int arm_smccc_smc(register_t, register_t, register_t, register_t,
+ *     register_t, register_t, register_t, register_t,
+ *     struct arm_smccc_res *res)
  */
-ENTRY(psci_smc_despatch)
+ENTRY(arm_smccc_smc)
+	mov	r12, sp
+	push	{r4-r7}
+	ldm	r12, {r4-r7}
 	smc	#0
-	RET
-END(psci_smc_despatch)
+	pop	{r4-r7}
+	ldr	r12, [sp, #(4 * 4)]
+	cmp     r12, #0
+	beq	1f
+	stm	r12, {r0-r3}
+1:	bx	lr
+END(arm_smccc_smc)
diff --git a/sys/dev/psci/psci_arm64.S b/sys/dev/psci/smccc_arm64.S
similarity index 60%
rename from sys/dev/psci/psci_arm64.S
rename to sys/dev/psci/smccc_arm64.S
index 39564dd2c3c4..0b94a11fdf04 100644
--- a/sys/dev/psci/psci_arm64.S
+++ b/sys/dev/psci/smccc_arm64.S
@@ -1,10 +1,12 @@
 /*-
- * Copyright (c) 2013, 2014 Robin Randhawa
- * Copyright (c) 2015 The FreeBSD Foundation
- * All rights reserved.
+ * SPDX-License-Identifier: BSD-2-Clause
  *
- * This software was developed by Andrew Turner under
- * sponsorship from the FreeBSD Foundation.
+ * Copyright (c) 2019 Ruslan Bukin <br@bsdpad.com>
+ *
+ * This software was developed by SRI International and the University of
+ * Cambridge Computer Laboratory (Department of Computer Science and
+ * Technology) under DARPA contract HR0011-18-C-0016 ("ECATS"), as part of the
+ * DARPA SSITH research programme.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -26,24 +28,35 @@
  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
- *
  */
 
 #include <machine/asm.h>
 __FBSDID("$FreeBSD$");
 
 /*
- * uint64_t psci_hvc_despatch(uint64_t psci_fnid, uint64_t, uint64_t, uint64_t)
+ * int arm_smccc_hvc(register_t, register_t, register_t, register_t,
+ *     register_t, register_t, register_t, register_t,
+ *     struct arm_smccc_res *res)
  */
-ENTRY(psci_hvc_despatch)
+ENTRY(arm_smccc_hvc)
 	hvc	#0
-	ret
-END(psci_hvc_despatch)
+	ldr	x4, [sp]
+	cbz	x4, 1f
+	stp	x0, x1, [x4, #16 * 0]
+	stp	x2, x3, [x4, #16 * 1]
+1:	ret
+END(arm_smccc_hvc)
 
 /*
- * uint64_t psci_smc_despatch(uint64_t psci_fnid, uint64_t, uint64_t, uint64_t)
+ * int arm_smccc_smc(register_t, register_t, register_t, register_t,
+ *     register_t, register_t, register_t, register_t,
+ *     struct arm_smccc_res *res)
  */
-ENTRY(psci_smc_despatch)
+ENTRY(arm_smccc_smc)
 	smc	#0
-	ret
-END(psci_smc_despatch)
+	ldr	x4, [sp]
+	cbz	x4, 1f
+	stp	x0, x1, [x4, #16 * 0]
+	stp	x2, x3, [x4, #16 * 1]
+1:	ret
+END(arm_smccc_smc)