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
|
/* 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 <arch/smp.h>
#include <kmi/tcb.h>
#include <libfdt.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 %d status: %ld\n",
hartid,
r.error);
continue;
}
if (r.value == SBI_HART_STARTED) {
/* there should ever only be one started hart */
catastrophic_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;
r = sbi_hart_start(hartid,
(unsigned long)__pa(riscv_bringup), satp);
if (r.error) {
warn("failed bringing up hart %d: %ld\n",
hartid,
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.
*/
void core_bringup(long hartid)
{
/* assume smp_bringup assigned our cpuid correctly */
id_t cpuid = hartid_to_cpuid(hartid);
info("core %d online\n", cpuid);
/* add us as a thread to init program that cpu 0 is hopefully running by
* now */
struct tcb *t = create_thread(cpu_tcb(0));
t->cpu_id = cpuid;
tcb_assign(t);
use_tcb(t);
/* eventually we should jump to init and start running stuff, but for
* now take it easy */
while (1);
}
|
