1/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */ 2/* 3 * Copyright (C) 2023 Rockchip Electronics Co., Ltd. 4 */ 5 6 .global __sp 7 8 .global secondary_main 9 .global secondary_init 10 .global get_cpu_id 11 .global lock_byte_mutex 12 .global unlock_byte_mutex 13 14#ifndef CONFIG_ARM64 15 .align 7 16vectors_2: 17 ldr pc, =die_loop /* reset */ 18 ldr pc, =die_loop /* undefine */ 19 ldr pc, =die_loop /* swi */ 20 ldr pc, =die_loop /* iabort */ 21 ldr pc, =die_loop /* dabort */ 22 ldr pc, =die_loop /* reserved */ 23 ldr pc, =die_loop /* irq */ 24 ldr pc, =die_loop /* fiq */ 25 26 .align 7 27die_loop: 28 b die_loop 29 30 .align 7 31 .type secondary_init, %function 32secondary_init: 33 bl irq_disable 34 bl icache_invalid 35 36 /* set sp */ 37 ldr r2, =__sp 38 ldr r1, [r2] 39 bic r1, r1, #0xf 40 mov sp, r1 41 bl icache_invalid 42 43 mrc p15, 0, r0, c1, c0, 0 /* CP15 C1 System Control Register */ 44 bic r0, r0, #0x2000 /* clear V (bit[13], VBAR) */ 45 mcr p15, 0, r0, c1, c0, 0 /* for remap VBAR */ 46 ldr r0, =vectors_2 47 mcr p15, 0, r0, c12, c0, 0 48 bl icache_invalid 49 50 b secondary_main 51 52irq_disable: 53 mrs r0, cpsr 54 orr r0, r0, #0xc0 55 msr cpsr, r0 56 bx lr 57 58icache_invalid: 59 mov r0, #0 60 mcr p15, 0, r0, c7, c5, 0 61 bx lr 62 63 .type get_cpu_id, %function 64get_cpu_id: 65 mrc p15, 0, r0, c0, c0, 5 66 and r0, r0, #0x3 67 bx lr 68 69 .align 7 70 .type lock_byte_mutex, %function 71lock_byte_mutex: 72 mov r2, #0x1 73try: 74 ldrex r1, [r0] 75 cmp r1, #0 76 strexeq r1, r2, [r0] 77 cmpeq r1, #0 78 bne try 79 dmb 80 bx lr 81 82 .align 7 83 .type unlock_byte_mutex, %function 84unlock_byte_mutex: 85 dmb 86 mov r1, #0 87 str r1, [r0] 88 dsb 89 bx lr 90#else /* CONFIG_ARM64 */ 91 .align 7 92el2_vectors: 93synchronous_sp0: 94 b secondary_init 95 .align 7 96irq_sp0: 97 b irq_sp0 98 .align 7 99fiq_sp0: 100 b fiq_sp0 101 .align 7 102serror_sp0: 103 b serror_sp0 104 .align 7 105synchronous_spx: 106 b synchronous_spx 107 .align 7 108irq_spx: 109 b irq_spx 110 .align 7 111fiq_spx: 112 b fiq_spx 113 .align 7 114serror_spx: 115 b serror_spx 116 117 .align 7 118 .type secondary_init, %function 119secondary_init: 120 bl irq_disable 121 122 /* set sp */ 123 ldr x2, =__sp 124 ldr x1, [x2] 125 bic x1, x1, #0xf 126 mov sp, x1 127 bl icache_invalid 128 129 ldr w0, =el2_vectors 130 msr vbar_el2, x0 131 bl icache_invalid 132 133 bl secondary_main 134 135 .type irq_disable, %function 136irq_disable: 137 msr daifset, #0x3 138 ic iallu 139 ret 140 141 .type icache_invalid, %function 142icache_invalid: 143 ic iallu 144 ret 145 146 .align 7 147 .type lock_byte_mutex, %function 148lock_byte_mutex: 149 ldxrb w1, [x0] 150 cmp w1, #1 151 bne 1f 152 wfe 153 b lock_byte_mutex 1541: 155 mov x1, #1 156 stxrb w2, w1, [x0] 157 cmp w2, #0 158 bne lock_byte_mutex 159 dmb sy 160 ret 161 162 .align 7 163 .type unlock_byte_mutex, %function 164unlock_byte_mutex: 165 dmb sy 166 mov x1, #0 167 strb w1, [x0] 168 dsb sy 169 sev 170 ret 171#endif /* #ifdef CONFIG_ARM */ 172