You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
172 lines
2.5 KiB
172 lines
2.5 KiB
/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
|
|
/*
|
|
* Copyright (C) 2023 Rockchip Electronics Co., Ltd.
|
|
*/
|
|
|
|
.global __sp
|
|
|
|
.global secondary_main
|
|
.global secondary_init
|
|
.global get_cpu_id
|
|
.global lock_byte_mutex
|
|
.global unlock_byte_mutex
|
|
|
|
#ifndef CONFIG_ARM64
|
|
.align 7
|
|
vectors_2:
|
|
ldr pc, =die_loop /* reset */
|
|
ldr pc, =die_loop /* undefine */
|
|
ldr pc, =die_loop /* swi */
|
|
ldr pc, =die_loop /* iabort */
|
|
ldr pc, =die_loop /* dabort */
|
|
ldr pc, =die_loop /* reserved */
|
|
ldr pc, =die_loop /* irq */
|
|
ldr pc, =die_loop /* fiq */
|
|
|
|
.align 7
|
|
die_loop:
|
|
b die_loop
|
|
|
|
.align 7
|
|
.type secondary_init, %function
|
|
secondary_init:
|
|
bl irq_disable
|
|
bl icache_invalid
|
|
|
|
/* set sp */
|
|
ldr r2, =__sp
|
|
ldr r1, [r2]
|
|
bic r1, r1, #0xf
|
|
mov sp, r1
|
|
bl icache_invalid
|
|
|
|
mrc p15, 0, r0, c1, c0, 0 /* CP15 C1 System Control Register */
|
|
bic r0, r0, #0x2000 /* clear V (bit[13], VBAR) */
|
|
mcr p15, 0, r0, c1, c0, 0 /* for remap VBAR */
|
|
ldr r0, =vectors_2
|
|
mcr p15, 0, r0, c12, c0, 0
|
|
bl icache_invalid
|
|
|
|
b secondary_main
|
|
|
|
irq_disable:
|
|
mrs r0, cpsr
|
|
orr r0, r0, #0xc0
|
|
msr cpsr, r0
|
|
bx lr
|
|
|
|
icache_invalid:
|
|
mov r0, #0
|
|
mcr p15, 0, r0, c7, c5, 0
|
|
bx lr
|
|
|
|
.type get_cpu_id, %function
|
|
get_cpu_id:
|
|
mrc p15, 0, r0, c0, c0, 5
|
|
and r0, r0, #0x3
|
|
bx lr
|
|
|
|
.align 7
|
|
.type lock_byte_mutex, %function
|
|
lock_byte_mutex:
|
|
mov r2, #0x1
|
|
try:
|
|
ldrex r1, [r0]
|
|
cmp r1, #0
|
|
strexeq r1, r2, [r0]
|
|
cmpeq r1, #0
|
|
bne try
|
|
dmb
|
|
bx lr
|
|
|
|
.align 7
|
|
.type unlock_byte_mutex, %function
|
|
unlock_byte_mutex:
|
|
dmb
|
|
mov r1, #0
|
|
str r1, [r0]
|
|
dsb
|
|
bx lr
|
|
#else /* CONFIG_ARM64 */
|
|
.align 7
|
|
el2_vectors:
|
|
synchronous_sp0:
|
|
b secondary_init
|
|
.align 7
|
|
irq_sp0:
|
|
b irq_sp0
|
|
.align 7
|
|
fiq_sp0:
|
|
b fiq_sp0
|
|
.align 7
|
|
serror_sp0:
|
|
b serror_sp0
|
|
.align 7
|
|
synchronous_spx:
|
|
b synchronous_spx
|
|
.align 7
|
|
irq_spx:
|
|
b irq_spx
|
|
.align 7
|
|
fiq_spx:
|
|
b fiq_spx
|
|
.align 7
|
|
serror_spx:
|
|
b serror_spx
|
|
|
|
.align 7
|
|
.type secondary_init, %function
|
|
secondary_init:
|
|
bl irq_disable
|
|
|
|
/* set sp */
|
|
ldr x2, =__sp
|
|
ldr x1, [x2]
|
|
bic x1, x1, #0xf
|
|
mov sp, x1
|
|
bl icache_invalid
|
|
|
|
ldr w0, =el2_vectors
|
|
msr vbar_el2, x0
|
|
bl icache_invalid
|
|
|
|
bl secondary_main
|
|
|
|
.type irq_disable, %function
|
|
irq_disable:
|
|
msr daifset, #0x3
|
|
ic iallu
|
|
ret
|
|
|
|
.type icache_invalid, %function
|
|
icache_invalid:
|
|
ic iallu
|
|
ret
|
|
|
|
.align 7
|
|
.type lock_byte_mutex, %function
|
|
lock_byte_mutex:
|
|
ldxrb w1, [x0]
|
|
cmp w1, #1
|
|
bne 1f
|
|
wfe
|
|
b lock_byte_mutex
|
|
1:
|
|
mov x1, #1
|
|
stxrb w2, w1, [x0]
|
|
cmp w2, #0
|
|
bne lock_byte_mutex
|
|
dmb sy
|
|
ret
|
|
|
|
.align 7
|
|
.type unlock_byte_mutex, %function
|
|
unlock_byte_mutex:
|
|
dmb sy
|
|
mov x1, #0
|
|
strb w1, [x0]
|
|
dsb sy
|
|
sev
|
|
ret
|
|
#endif /* #ifdef CONFIG_ARM */
|