diff options
Diffstat (limited to 'arch/arm/cpu/armv8/psci.S')
| -rw-r--r-- | arch/arm/cpu/armv8/psci.S | 33 |
1 files changed, 27 insertions, 6 deletions
diff --git a/arch/arm/cpu/armv8/psci.S b/arch/arm/cpu/armv8/psci.S index 358df8fee9c..fc42d807b52 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 |