upstream/linux-5.10/arch/arm/kernel/svc_s.S

#define _USE_VEHICLE_DC
#include "ram_base_config_7520v3.h"

#define	GIC_DIST_BASE			(0xF2000000)
#define	GIC_RDIST_BASE			(0xF2040000)

#define	REAL_TXT_ADDR			(CONFIG_PHYS_OFFSET + 0x8000)
#define	DTB_ADDR			(DDR_BASE_CAP_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)