ARMv8: Allow SiP service extensions on top of PSCI code

[platform/kernel/u-boot.git] / arch / arm / cpu / armv8 / psci.S

diff --git a/arch/arm/cpu/armv8/psci.S b/arch/arm/cpu/armv8/psci.S
index 358df8f..fc42d80 100644 (file)
--- 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