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
|
/* SPDX-License-Identifier: copyleft-next-0.3.1 */
/* Copyright 2023, Kim Kuparinen < kimi.h.kuparinen@gmail.com > */
/**
* @file smp.c
* riscv multicore bringup implementation.
*/
#include <kmi/debug.h>
#include <kmi/bkl.h>
#include <kmi/tcb.h>
#include <libfdt.h>
#include <arch/proc.h>
#include <arch/arch.h>
#include <arch/smp.h>
#include "arch.h"
#include "sbi.h"
/**
* Array of stacks for bringing up harts.
* Note not static since we want to access it from core_bringup.S.
*/
void *smp_init_stacks[MAX_CPUS];
/**
* Check if FDT node is a cpu and that its status is 'okay'.
* Helper for smp_bringup(), cpu nodes that are marked 'okay' in the FDT should
* be brought up.
*
* @param fdt Flattened device tree.
* @param node Node to check.
* @return true if node is a cpu node and it should be brought up, false
* otherwise.
*/
static bool riscv_cpu_okay(void *fdt, int node)
{
const char *s = fdt_getprop(fdt, node, "device_type", NULL);
if (!s || strcmp(s, "cpu") != 0)
return false;
s = fdt_getprop(fdt, node, "status", NULL);
if (!s)
return false;
if (strcmp(s, "okay") == 0)
return true;
return false;
}
/** Counter for how many cores system has. */
static size_t cpus = 1;
/* called from main to start other cores */
void smp_bringup(struct vmem *b, void *fdt)
{
/* defined in core_bringup.S */
extern void riscv_bringup(void);
/* mark first hart available */
cpuid_to_hartid(0) = -1;
/* assume we're in default Sv mode, in the future this will have to be
* fixed if we start implementing Sv48 etc. */
pm_t satp = branch_to_satp(b, DEFAULT_Sv_MODE);
int cpu_offset = fdt_path_offset(fdt, "/cpus");
struct cell_info ci = get_cellinfo(fdt, cpu_offset);
/* almost directly lifted from netbsd */
int node;
fdt_for_each_subnode(node, fdt, cpu_offset) {
if (!riscv_cpu_okay(fdt, node))
continue;
const void *reg = fdt_getprop(fdt, node, "reg", NULL);
id_t hartid = fdt_load_reg_addr(ci, reg, 0);
struct sbiret r = sbi_hart_status(hartid);
if (r.error) {
warn("failed getting hart %ld status: %ld\n",
(long)hartid,
(long)r.error);
continue;
}
if (r.value == SBI_HART_STARTED) {
/* there should ever only be one started hart */
assert(cpuid_to_hartid(0) == -1);
cpuid_to_hartid(0) = hartid;
continue;
}
/** @todo should check that cpus doesn't go over MAX_CPUS */
cpuid_to_hartid(cpus++) = hartid;
/** @todo try to remember to free these as well */
smp_init_stacks[hartid] = (void *)alloc_page(BASE_PAGE) +
BASE_PAGE_SIZE;
/* fixup physical address of bringup */
pm_t bringup = (pm_t)riscv_bringup;
bringup = bringup - VM_KERNEL + get_load_addr();
r = sbi_hart_start(hartid, bringup, satp);
if (r.error) {
warn("failed bringing up hart %ld: %ld\n",
(long)hartid,
(long)r.error);
continue;
}
}
}
/**
* Called from secondary_bringup.S to finish bringing up core we're running on.
*
* @param hartid Hart that's being brought up.
*/
__noreturn void core_bringup(long hartid)
{
bkl_lock();
/* assume smp_bringup assigned our cpuid correctly */
id_t cpuid = hartid_to_cpuid(hartid);
/* output is somewhat messed up due to no synchronisation but I guess
* that's fine for now */
info("core %ld online\n", (long)cpuid);
/* realistically stuff after this point could probably be placed
* somewhere in common/ */
/* add us as a thread to init program that cpu 0 is hopefully running by
* now */
struct tcb *init = get_tcb(1);
assert(init);
struct tcb *t = create_thread(init);
assert(t);
alloc_stack(t);
/* init is special in that all threads jump to the entrypoint of the
* program */
t->callback = init->callback;
t->exec = init->exec;
t->cpu_id = cpuid;
/** @todo this is pretty hacky, should really be a separate function? */
setup_irq(NULL);
setup_arch(NULL);
tcb_assign(t);
use_tcb(t);
info("core %ld releasing BKL\n", (long)cpuid);
run_init(t, NULL, NULL);
unreachable();
}
|
