1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
|
/*
* Copyright (c) 2006-2023, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Change Logs:
* Date Author Notes
* 2011-09-15 Bernard first version
* 2019-07-28 zdzn add smp support
* 2023-02-21 GuEe-GUI mov cpu ofw init to setup
* 2024-04-29 Shell Add generic ticket spinlock using C11 atomic
*/
#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>
#include <cpu.h>
#define DBG_TAG "libcpu.aarch64.cpu"
#define DBG_LVL DBG_INFO
#include <rtdbg.h>
#ifdef RT_USING_SMP
#define REPORT_ERR(retval) LOG_E("got error code %d in %s(), %s:%d", (retval), __func__, __FILE__, __LINE__)
#define CHECK_RETVAL(retval) if (retval) {REPORT_ERR(retval);}
#define cpuid_to_hwid(cpuid) \
((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? rt_cpu_mpidr_early[cpuid] : ID_ERROR)
#define set_hwid(cpuid, hwid) \
((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? (rt_cpu_mpidr_early[cpuid] = (hwid)) : ID_ERROR)
#define get_cpu_node(cpuid) \
((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? _cpu_node[cpuid] : NULL)
#define set_cpu_node(cpuid, node) \
((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? (_cpu_node[cpuid] = node) : NULL)
typedef rt_hw_spinlock_t arch_spinlock_t;
struct cpu_ops_t *cpu_ops_tbl[RT_CPUS_NR];
#ifdef RT_USING_SMART
// _id_to_mpidr is a table translate logical id to mpid, which is a 64-bit value
rt_uint64_t rt_cpu_mpidr_early[RT_CPUS_NR] rt_weak = {[0 ... RT_CPUS_NR - 1] = ID_ERROR};
#else
/* The more common mpidr_el1 table, redefine it in BSP if it is in other cases */
rt_weak rt_uint64_t rt_cpu_mpidr_early[] =
{
[0] = 0x80000000,
[1] = 0x80000001,
[2] = 0x80000002,
[3] = 0x80000003,
[4] = 0x80000004,
[5] = 0x80000005,
[6] = 0x80000006,
[7] = 0x80000007,
[RT_CPUS_NR] = 0
};
#endif /* RT_USING_SMART */
/* in support of C11 atomic */
#if __STDC_VERSION__ >= 201112L
#include <stdatomic.h>
union _spinlock
{
_Atomic(rt_uint32_t) _value;
struct
{
_Atomic(rt_uint16_t) owner;
_Atomic(rt_uint16_t) next;
} ticket;
};
void rt_hw_spin_lock_init(rt_hw_spinlock_t *_lock)
{
union _spinlock *lock = (void *)_lock;
/**
* just a dummy note that this is an atomic operation, though it alway is
* even without usage of atomic API in arm64
*/
atomic_store_explicit(&lock->_value, 0, memory_order_relaxed);
}
rt_bool_t rt_hw_spin_trylock(rt_hw_spinlock_t *_lock)
{
rt_bool_t rc;
rt_uint32_t readonce;
union _spinlock temp;
union _spinlock *lock = (void *)_lock;
readonce = atomic_load_explicit(&lock->_value, memory_order_acquire);
temp._value = readonce;
if (temp.ticket.owner != temp.ticket.next)
{
rc = RT_FALSE;
}
else
{
temp.ticket.next += 1;
rc = atomic_compare_exchange_strong_explicit(
&lock->_value, &readonce, temp._value,
memory_order_acquire, memory_order_relaxed);
}
return rc;
}
rt_inline rt_base_t _load_acq_exclusive(_Atomic(rt_uint16_t) *halfword)
{
rt_uint32_t old;
__asm__ volatile("ldaxrh %w0, [%1]"
: "=&r"(old)
: "r"(halfword)
: "memory");
return old;
}
rt_inline void _send_event_local(void)
{
__asm__ volatile("sevl");
}
rt_inline void _wait_for_event(void)
{
__asm__ volatile("wfe" ::: "memory");
}
void rt_hw_spin_lock(rt_hw_spinlock_t *_lock)
{
union _spinlock *lock = (void *)_lock;
rt_uint16_t ticket =
atomic_fetch_add_explicit(&lock->ticket.next, 1, memory_order_relaxed);
if (atomic_load_explicit(&lock->ticket.owner, memory_order_acquire) !=
ticket)
{
_send_event_local();
do
{
_wait_for_event();
}
while (_load_acq_exclusive(&lock->ticket.owner) != ticket);
}
}
void rt_hw_spin_unlock(rt_hw_spinlock_t *_lock)
{
union _spinlock *lock = (void *)_lock;
atomic_fetch_add_explicit(&lock->ticket.owner, 1, memory_order_release);
}
#endif
static int _cpus_init_data_hardcoded(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[])
{
// load in cpu_hw_ids in cpuid_to_hwid,
// cpu_ops to cpu_ops_tbl
if (num_cpus > RT_CPUS_NR)
{
LOG_W("num_cpus (%d) greater than RT_CPUS_NR (%d)\n", num_cpus, RT_CPUS_NR);
num_cpus = RT_CPUS_NR;
}
for (int i = 0; i < num_cpus; i++)
{
set_hwid(i, cpu_hw_ids[i]);
cpu_ops_tbl[i] = cpu_ops[i];
}
return 0;
}
/** init cpu with hardcoded infomation or parsing from FDT */
static int _cpus_init(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[])
{
int retval;
// first setup cpu_ops_tbl and cpuid_to_hwid
if (num_cpus > 0)
retval = _cpus_init_data_hardcoded(num_cpus, cpu_hw_ids, cpu_ops);
else
{
retval = -1;
}
if (retval)
return retval;
// using cpuid_to_hwid and cpu_ops_tbl to call method_init and cpu_init
// assuming that cpuid 0 has already init
for (int i = 1; i < RT_CPUS_NR; i++)
{
if (rt_cpu_mpidr_early[i] == ID_ERROR)
{
LOG_E("Failed to find hardware id of CPU %d", i);
continue;
}
if (cpu_ops_tbl[i] && cpu_ops_tbl[i]->cpu_init)
{
retval = cpu_ops_tbl[i]->cpu_init(i, RT_NULL);
CHECK_RETVAL(retval);
}
else
{
LOG_E("Failed to find cpu_init for cpu %d with cpu_ops[%p], cpu_ops->cpu_init[%p]"
, rt_cpu_mpidr_early[i], cpu_ops_tbl[i], cpu_ops_tbl[i] ? cpu_ops_tbl[i]->cpu_init : NULL);
}
}
return 0;
}
/**
* @brief boot cpu with hardcoded data
*
* @param num_cpus number of cpus
* @param cpu_hw_ids each element represents a hwid of cpu[i]
* @param cpu_ops each element represents a pointer to cpu_ops of cpu[i]
* @return int 0 on success,
*/
int rt_hw_cpu_boot_secondary(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[])
{
int retval = 0;
if (num_cpus < 1 || !cpu_hw_ids || !cpu_ops)
return -1;
retval = _cpus_init(num_cpus, cpu_hw_ids, cpu_ops);
CHECK_RETVAL(retval);
return retval;
}
#endif /*RT_USING_SMP*/
/**
* @addtogroup ARM CPU
*/
/*@{*/
const char *rt_hw_cpu_arch(void)
{
return "aarch64";
}
/*@}*/
|
