|
@@ -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
|