[Feature][T106_eSDK]T106-V2.01.01.02P56U06.AP.15.11_CAP.15.11(SDK4.6)diff_17.02(SDK4.7)
Only Configure: No
Affected branch: master
Affected module: unknow
Is it affected on both ZXIC and MTK: only ZXIC
Self-test: Yes
Doc Update: No
Change-Id: I9dc02273b59a074828ab3eeaa84306415b153138
diff --git a/upstream/linux-5.10/arch/arm/kernel/svc_s.S b/upstream/linux-5.10/arch/arm/kernel/svc_s.S
new file mode 100755
index 0000000..a52abb7
--- /dev/null
+++ b/upstream/linux-5.10/arch/arm/kernel/svc_s.S
@@ -0,0 +1,101 @@
+
+#define GIC_DIST_BASE (0xF2000000)
+#define GIC_RDIST_BASE (0xF2040000)
+
+#define REAL_TXT_ADDR (CONFIG_PHYS_OFFSET + 0x8000)
+#define DTB_ADDR (CONFIG_DTB_ADDR)
+
+secure_init:
+
+#if 0
+ mov r5, r0
+ mov r6, r1
+ mov r7, r2
+#endif
+ mov r4, lr
+
+#if 1
+ /* use r0--r4 only */
+ bl get_core_id
+ mov r1, r0
+ bl get_cluster_id
+ mov r2, r0
+
+ ldr r3, =GIC_DIST_BASE
+ ldr r0, =0x50
+ str r0, [r3]
+
+ ldr r3, =GIC_RDIST_BASE
+ lsl r2, r2, #2
+ add r1, r1, r2
+ lsl r1, r1, #17
+
+ add r1, r1, r3
+ add r1, r1, #0x14
+
+ LDR R0, [R1]
+ LDR R2, =0xfffffffd
+ AND R0, R0, R2
+ STR R0, [R1]
+
+ LDR R2, = 0xFFFFFFFB
+wait:
+ LDR R0, [R1]
+ AND R0, R0, R2
+ CMP R0, #0
+ BNE wait
+
+ SUB R1, R1, #0x14
+ LDR R2, =0x10080
+ ADD R1, R1, R2
+ LDR R2, =0xFFFFFFFF
+ STR R2, [R1]
+#endif
+
+ MRS R0, CPSR
+ BIC R0, #0x1F
+ ORR R0, #0xD6
+ MSR CPSR_c, R0
+
+ MOV r3, #0xD
+ MCR p15,#0x6,r3,c12,c12,#5
+ MCR p15,0,r3,c12,c12,#5
+
+ MRC p15,0,r1,c1,c1,0
+ MOV r2, r1
+ ORR r2, #0x1
+ MCR p15,0,r2,c1,c1,0
+
+ MCR p15,#0x4,r3,c12,c9,#5
+
+ MRS R0, CPSR
+ BIC R0, #0x1F
+ ORR R0, #0xD3
+ MSR CPSR_c, R0
+
+#if 0
+ mov r0, r5
+ mov r1, r6
+ mov r2, r7
+#else
+ ldr r0, =0
+ ldr r1, =REAL_TXT_ADDR
+ ldr r2, =DTB_ADDR
+#endif
+ mov lr, r4
+
+ ret lr
+ENDPROC(secure_init)
+
+get_core_id:
+ MRC p15, 0, R0, c0, c0, 5
+ AND R0, R0, #0xFF
+ BX R14
+ENDPROC(get_core_id)
+
+get_cluster_id:
+ MRC p15, 0, r0, c0, c0, 5
+ AND r0, r0, #0xFF00
+ LSR r0, r0, #0x8
+ BX lr
+ENDPROC(get_cluster_id)