[U-Boot] [PATCH v1 1/2] ARMv8: Allow SiP service extensions on top of PSCI code

[chee.hong.ang at intel.com <chee.hong.ang@intel.com>]

From: Chee Hong Ang <chee.hong.ang@intel.com>

Allow PSCI layer to handle any SiP service functions added by
platform vendors. PSCI layer will look for SiP service function
in the SiP function table located in '._secure_svc_tbl_entries'
section if the SMC function identifier is not found in the PSCI
standard functions table. Use DECLARE_SECURE_SVC macro to declare
and add platform specific SiP service function.
This new section '._secure_svc_tbl_entries' is located next to
'._secure.text' section. Refer to arch/arm/cpu/armv8/u-boot.lds.

Signed-off-by: Chee Hong Ang <chee.hong.ang@intel.com>
---
 arch/arm/cpu/armv8/psci.S     | 33 +++++++++++++++++++++++++++------
 arch/arm/cpu/armv8/u-boot.lds |  4 ++++
 arch/arm/include/asm/secure.h | 31 +++++++++++++++++++++++++++++++
 3 files changed, 62 insertions(+), 6 deletions(-)

diff --git a/arch/arm/cpu/armv8/psci.S b/arch/arm/cpu/armv8/psci.S
index 358df8f..fc42d80 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) \
@@ -147,18 +148,38 @@ handle_psci:
 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 +192,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 53de80f..2554980 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))
diff --git a/arch/arm/include/asm/secure.h b/arch/arm/include/asm/secure.h
index d23044a..50582c9 100644
--- a/arch/arm/include/asm/secure.h
+++ b/arch/arm/include/asm/secure.h
@@ -6,6 +6,37 @@
 #define __secure __attribute__ ((section ("._secure.text")))
 #define __secure_data __attribute__ ((section ("._secure.data")))
 
+#ifndef __ASSEMBLY__
+
+typedef struct secure_svc_tbl {
+	u32	id;
+#ifdef CONFIG_ARMV8_PSCI
+	u8	pad[4];
+#endif
+	void	*func;
+} secure_svc_tbl_t;
+
+/*
+ * Macro to declare a SiP function service in '_secure_svc_tbl_entries' section
+ */
+#define DECLARE_SECURE_SVC(_name, _id, _fn) \
+	static const secure_svc_tbl_t __secure_svc_ ## _name \
+		__attribute__((used, section("._secure_svc_tbl_entries"))) \
+			 = { \
+				.id = _id, \
+				.func = _fn }
+
+#else
+
+#ifdef CONFIG_ARMV8_PSCI
+#define SECURE_SVC_TBL_OFFSET		16
+#else
+#define SECURE_SVC_TBL_OFFSET		8
+
+#endif
+
+#endif /* __ASSEMBLY__ */
+
 #if defined(CONFIG_ARMV7_SECURE_BASE) || defined(CONFIG_ARMV8_SECURE_BASE)
 /*
  * Warning, horror ahead.
-- 
2.7.4