1/*
2 * Copyright (c) 2006-2021, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Email: opensource_embedded@phytium.com.cn
7 *
8 * Change Logs:
9 * Date        Author       Notes
10 * 2023-07-26  huanghe      first commit
11 * 2024-07-02  zhangyan     modify
12 *
13 */
14
15#include "fparameters.h"
16#include "rtconfig.h"
17
18#ifndef __aarch64__
19.globl cpu_id_mapping
20cpu_id_mapping:
21#if defined(TARGET_PE2204)
22cmp r0, #0 // compare cpu_id with 0
23beq map_cpu_id_0
24cmp r0, #1 // compare cpu_id with 1
25beq map_cpu_id_1
26cmp r0, #2 // compare cpu_id with 2
27beq map_cpu_id_2
28cmp r0, #3 // compare cpu_id with 3
29beq map_cpu_id_3
30mov pc, lr // no mapping needed
31#endif
32mov pc, lr // no mapping needed
33
34// Mapping for PE2204
35map_cpu_id_0:
36mov r0, #2
37mov pc, lr
38
39map_cpu_id_1:
40mov r0, #3
41mov pc, lr
42
43map_cpu_id_2:
44mov r0, #0
45mov pc, lr
46
47map_cpu_id_3:
48mov r0, #1
49mov pc, lr
50
51.globl rt_hw_cpu_id_early
52rt_hw_cpu_id_early:
53// read MPIDR
54    mov r9, lr
55    mrc p15, 0, r0, c0, c0, 5
56    ubfx r0, r0, #0, #12
57    ldr r1,= CORE0_AFF
58    cmp r0, r1
59    beq core0
60
61#if defined(CORE1_AFF)
62    ldr r1,= CORE1_AFF
63    cmp r0, r1
64    beq core1
65#endif
66
67#if defined(CORE2_AFF)
68    ldr r1,= CORE2_AFF
69    cmp r0, r1
70    beq core2
71#endif
72
73#if defined(CORE3_AFF)
74    ldr r1,= CORE3_AFF
75    cmp r0, r1
76    beq core3
77#endif
78
79    b default
80
81core0:
82    mov r0, #0
83    b return
84
85core1:
86    mov r0, #1
87    b return
88
89core2:
90    mov r0, #2
91    b return
92
93core3:
94    mov r0, #3
95    b return
96
97default:
98    and r0, r0, #15
99
100return:
101    bl cpu_id_mapping
102    mov pc, r9
103
104#else
105
106.globl cpu_id_mapping
107cpu_id_mapping:
108#if defined(TARGET_PE2204)
109cmp x0, #0 // compare cpu_id with 0
110beq map_cpu_id_0
111cmp x0, #1 // compare cpu_id with 1
112beq map_cpu_id_1
113cmp x0, #2 // compare cpu_id with 2
114beq map_cpu_id_2
115cmp x0, #3 // compare cpu_id with 3
116beq map_cpu_id_3
117RET // no mapping needed
118#endif
119RET // no mapping needed
120
121// Mapping for PE2204
122map_cpu_id_0:
123mov x0, #2
124RET
125
126map_cpu_id_1:
127mov x0, #3
128RET
129
130map_cpu_id_2:
131mov x0, #0
132RET
133
134map_cpu_id_3:
135mov x0, #1
136RET
137
138.globl rt_hw_cpu_id_set
139rt_hw_cpu_id_set:
140    mov x9, lr
141    mrs x0,MPIDR_EL1
142    and x1, x0, #15
143    msr tpidr_el1, x1
144    mov lr, x9
145    RET
146
147.globl rt_hw_cpu_id
148rt_hw_cpu_id:
149    mrs x0,MPIDR_EL1
150    ubfx x0, x0, #0, #20
151    ldr x1,= CORE0_AFF
152    cmp x0, x1
153    beq core0
154
155#if defined(CORE1_AFF)
156    ldr x1,= CORE1_AFF
157    cmp x0, x1
158    beq core1
159#endif
160
161#if defined(CORE2_AFF)
162    ldr x1,= CORE2_AFF
163    cmp x0, x1
164    beq core2
165#endif
166
167#if defined(CORE3_AFF)
168    ldr x1,= CORE3_AFF
169    cmp x0, x1
170    beq core3
171#endif
172
173#if defined(CORE4_AFF)
174    ldr x1,= CORE4_AFF
175    cmp x0, x1
176    beq core4
177#endif
178
179#if defined(CORE5_AFF)
180    ldr x1,= CORE5_AFF
181    cmp x0, x1
182    beq core5
183#endif
184
185#if defined(CORE6_AFF)
186    ldr x1,= CORE6_AFF
187    cmp x0, x1
188    beq core6
189#endif
190
191#if defined(CORE7_AFF)
192    ldr x1,= CORE7_AFF
193    cmp x0, x1
194    beq core7
195#endif
196
197core0:
198    mov x0, #0
199    b return
200
201core1:
202    mov x0, #1
203    b return
204
205core2:
206    mov x0, #2
207    b return
208
209core3:
210    mov x0, #3
211    b return
212
213core4:
214    mov x0, #4
215    b return
216
217core5:
218    mov x0, #5
219    b return
220
221core6:
222    mov x0, #6
223    b return
224
225core7:
226    mov x0, #7
227    b return
228
229return:
230    b cpu_id_mapping
231
232    RET
233#endif
234
235