4 files changed, 40 insertions, 15 deletions
diff --git a/arch/arm/cpu/armv8/Makefile b/arch/arm/cpu/armv8/Makefile
index a5f54330e3..b349b13f49 100644
--- a/arch/arm/cpu/armv8/Makefile
+++ b/arch/arm/cpu/armv8/Makefile
@@ -19,7 +19,9 @@ endif
 obj-y	+= cache.o
 obj-y	+= tlb.o
 obj-y	+= transition.o
+ifndef CONFIG_ARMV8_PSCI
 obj-y	+= fwcall.o
+endif
 obj-y	+= cpu-dt.o
 obj-$(CONFIG_ARM_SMCCC)		+= smccc-call.o
 
diff --git a/arch/arm/cpu/armv8/fwcall.c b/arch/arm/cpu/armv8/fwcall.c
index 9957c2974b..b0aca1b72a 100644
--- a/arch/arm/cpu/armv8/fwcall.c
+++ b/arch/arm/cpu/armv8/fwcall.c
@@ -28,7 +28,6 @@ static void hvc_call(struct pt_regs *args)
 		"ldr x4, %4\n"
 		"ldr x5, %5\n"
 		"ldr x6, %6\n"
-		"ldr x7, %7\n"
 		"hvc	#0\n"
 		"str x0, %0\n"
 		"str x1, %1\n"
@@ -37,7 +36,7 @@ static void hvc_call(struct pt_regs *args)
 		: "+m" (args->regs[0]), "+m" (args->regs[1]),
 		  "+m" (args->regs[2]), "+m" (args->regs[3])
 		: "m" (args->regs[4]), "m" (args->regs[5]),
-		  "m" (args->regs[6]), "m" (args->regs[7])
+		  "m" (args->regs[6])
 		: "x0", "x1", "x2", "x3", "x4", "x5", "x6", "x7",
 		  "x8", "x9", "x10", "x11", "x12", "x13", "x14", "x15",
 		  "x16", "x17");
diff --git a/arch/arm/cpu/armv8/psci.S b/arch/arm/cpu/armv8/psci.S
index 358df8fee9..7ffc8dbadb 100644
--- a/arch/arm/cpu/armv8/psci.S
+++ b/arch/arm/cpu/armv8/psci.S
@@ -8,6 +8,7 @@
 #include <config.h>
 #include <linux/linkage.h>
 #include <asm/psci.h>
+#include <asm/secure.h>
 
 /* Default PSCI function, return -1, Not Implemented */
 #define PSCI_DEFAULT(__fn) \
@@ -19,8 +20,8 @@
 
 /* PSCI function and ID table definition*/
 #define PSCI_TABLE(__id, __fn) \
-	.word __id; \
-	.word __fn
+	.quad __id; \
+	.quad __fn
 
 .pushsection ._secure.text, "ax"
 
@@ -132,33 +133,52 @@ PSCI_TABLE(0, 0)
 /* Caller must put PSCI function-ID table base in x9 */
 handle_psci:
 	psci_enter
-1:	ldr x10, [x9]			/* Load PSCI function table */
-	ubfx x11, x10, #32, #32
-	ubfx x10, x10, #0, #32
+1:	ldr	x10, [x9]		/* Load PSCI function table */
 	cbz	x10, 3f			/* If reach the end, bail out */
 	cmp	x10, x0
 	b.eq	2f			/* PSCI function found */
-	add x9, x9, #8			/* If not match, try next entry */
+	add x9, x9, #16			/* If not match, try next entry */
 	b	1b
 
-2:	blr	x11			/* Call PSCI function */
+2:	ldr	x11, [x9, #8]		/* Load PSCI function */
+	blr	x11			/* Call PSCI function */
 	psci_return
 
 3:	mov	x0, #ARM_PSCI_RET_NI
 	psci_return
 
-unknown_smc_id:
-	ldr	x0, =0xFFFFFFFF
+/*
+ * Handle SiP service functions defined in SiP service function table.
+ * Use DECLARE_SECURE_SVC(_name, _id, _fn) to add platform specific SiP
+ * service function into the SiP service function table.
+ * SiP service function table is located in '._secure_svc_tbl_entries' section,
+ * which is next to '._secure.text' section.
+ */
+handle_svc:
+	adr	x9, __secure_svc_tbl_start
+	adr	x10, __secure_svc_tbl_end
+	subs	x12, x10, x9	/* Get number of entries in table */
+	b.eq	2f		/* Make sure SiP function table is not empty */
+	psci_enter
+1:	ldr x10, [x9]		/* Load SiP function table */
+	ldr x11, [x9, #8]
+	cmp	w10, w0
+	b.eq	2b		/* SiP service function found */
+	add x9, x9, #SECURE_SVC_TBL_OFFSET	/* Move to next entry */
+	subs	x12, x12, #SECURE_SVC_TBL_OFFSET
+	b.eq	3b		/* If reach the end, bail out */
+	b	1b
+2:	ldr	x0, =0xFFFFFFFF
 	eret
 
 handle_smc32:
 	/* SMC function ID  0x84000000-0x8400001F: 32 bits PSCI */
 	ldr	w9, =0x8400001F
 	cmp	w0, w9
-	b.gt	unknown_smc_id
+	b.gt	handle_svc
 	ldr	w9, =0x84000000
 	cmp	w0, w9
-	b.lt	unknown_smc_id
+	b.lt	handle_svc
 
 	adr	x9, _psci_32_table
 	b	handle_psci
@@ -171,10 +191,10 @@ handle_smc64:
 	/* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
 	ldr	x9, =0xC400001F
 	cmp	x0, x9
-	b.gt	unknown_smc_id
+	b.gt	handle_svc
 	ldr	x9, =0xC4000000
 	cmp	x0, x9
-	b.lt	unknown_smc_id
+	b.lt	handle_svc
 
 	adr	x9, _psci_64_table
 	b	handle_psci
diff --git a/arch/arm/cpu/armv8/u-boot.lds b/arch/arm/cpu/armv8/u-boot.lds
index 53de80f745..2554980595 100644
--- a/arch/arm/cpu/armv8/u-boot.lds
+++ b/arch/arm/cpu/armv8/u-boot.lds
@@ -58,6 +58,10 @@ SECTIONS
 		AT(ADDR(.__secure_start) + SIZEOF(.__secure_start))
 	{
 		*(._secure.text)
+		. = ALIGN(8);
+		__secure_svc_tbl_start = .;
+		KEEP(*(._secure_svc_tbl_entries))
+		__secure_svc_tbl_end = .;
 	}
 
 	.secure_data : AT(LOADADDR(.secure_text) + SIZEOF(.secure_text))