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
|
#include "mtrap.h"
#include "mcall.h"
#include "htif.h"
#include "atomic.h"
#include "bits.h"
#include "vm.h"
#include "uart.h"
#include "uart16550.h"
#include "finisher.h"
#include "fdt.h"
#include "unprivileged_memory.h"
#include "disabled_hart_mask.h"
#include <errno.h>
#include <stdarg.h>
#include <stdio.h>
void __attribute__((noreturn)) bad_trap(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
{
die("machine mode: unhandlable trap %d @ %p", read_csr(mcause), mepc);
}
static uintptr_t mcall_console_putchar(uint8_t ch)
{
if (uart) {
uart_putchar(ch);
} else if (uart16550) {
uart16550_putchar(ch);
} else if (htif) {
htif_console_putchar(ch);
}
return 0;
}
void poweroff(uint16_t code)
{
printm("Power off\n");
finisher_exit(code);
if (htif) {
htif_poweroff();
} else {
while (1);
}
}
void putstring(const char* s)
{
while (*s)
mcall_console_putchar(*s++);
}
void vprintm(const char* s, va_list vl)
{
char buf[256];
vsnprintf(buf, sizeof buf, s, vl);
putstring(buf);
}
void printm(const char* s, ...)
{
va_list vl;
va_start(vl, s);
vprintm(s, vl);
va_end(vl);
}
static void send_ipi(uintptr_t recipient, int event)
{
if (((disabled_hart_mask >> recipient) & 1)) return;
atomic_or(&OTHER_HLS(recipient)->mipi_pending, event);
mb();
*OTHER_HLS(recipient)->ipi = 1;
}
static uintptr_t mcall_console_getchar()
{
if (uart) {
return uart_getchar();
} else if (uart16550) {
return uart16550_getchar();
} else if (htif) {
return htif_console_getchar();
} else {
return '\0';
}
}
static uintptr_t mcall_clear_ipi()
{
return clear_csr(mip, MIP_SSIP) & MIP_SSIP;
}
static uintptr_t mcall_shutdown()
{
poweroff(0);
}
static uintptr_t mcall_set_timer(uint64_t when)
{
*HLS()->timecmp = when;
clear_csr(mip, MIP_STIP);
set_csr(mie, MIP_MTIP);
return 0;
}
static void send_ipi_many(uintptr_t* pmask, int event)
{
_Static_assert(MAX_HARTS <= 8 * sizeof(*pmask), "# harts > uintptr_t bits");
uintptr_t mask = hart_mask;
if (pmask)
mask &= load_uintptr_t(pmask, read_csr(mepc));
// send IPIs to everyone
for (uintptr_t i = 0, m = mask; m; i++, m >>= 1)
if (m & 1)
send_ipi(i, event);
if (event == IPI_SOFT)
return;
// wait until all events have been handled.
// prevent deadlock by consuming incoming IPIs.
uint32_t incoming_ipi = 0;
for (uintptr_t i = 0, m = mask; m; i++, m >>= 1)
if (m & 1)
while (*OTHER_HLS(i)->ipi)
incoming_ipi |= atomic_swap(HLS()->ipi, 0);
// if we got an IPI, restore it; it will be taken after returning
if (incoming_ipi) {
*HLS()->ipi = incoming_ipi;
mb();
}
}
void mcall_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
{
write_csr(mepc, mepc + 4);
uintptr_t n = regs[17], arg0 = regs[10], arg1 = regs[11], retval, ipi_type;
switch (n)
{
case SBI_CONSOLE_PUTCHAR:
retval = mcall_console_putchar(arg0);
break;
case SBI_CONSOLE_GETCHAR:
retval = mcall_console_getchar();
break;
case SBI_SEND_IPI:
ipi_type = IPI_SOFT;
goto send_ipi;
case SBI_REMOTE_SFENCE_VMA:
case SBI_REMOTE_SFENCE_VMA_ASID:
ipi_type = IPI_SFENCE_VMA;
goto send_ipi;
case SBI_REMOTE_FENCE_I:
ipi_type = IPI_FENCE_I;
send_ipi:
send_ipi_many((uintptr_t*)arg0, ipi_type);
retval = 0;
break;
case SBI_CLEAR_IPI:
retval = mcall_clear_ipi();
break;
case SBI_SHUTDOWN:
retval = mcall_shutdown();
break;
case SBI_SET_TIMER:
#if __riscv_xlen == 32
retval = mcall_set_timer(arg0 + ((uint64_t)arg1 << 32));
#else
retval = mcall_set_timer(arg0);
#endif
break;
default:
retval = -ENOSYS;
break;
}
regs[10] = retval;
}
void redirect_trap(uintptr_t epc, uintptr_t mstatus, uintptr_t badaddr)
{
write_csr(sbadaddr, badaddr);
write_csr(sepc, epc);
write_csr(scause, read_csr(mcause));
write_csr(mepc, read_csr(stvec));
uintptr_t new_mstatus = mstatus & ~(MSTATUS_SPP | MSTATUS_SPIE | MSTATUS_SIE);
uintptr_t mpp_s = MSTATUS_MPP & (MSTATUS_MPP >> 1);
new_mstatus |= (mstatus * (MSTATUS_SPIE / MSTATUS_SIE)) & MSTATUS_SPIE;
new_mstatus |= (mstatus / (mpp_s / MSTATUS_SPP)) & MSTATUS_SPP;
new_mstatus |= mpp_s;
write_csr(mstatus, new_mstatus);
extern void __redirect_trap();
return __redirect_trap();
}
void pmp_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
{
redirect_trap(mepc, read_csr(mstatus), read_csr(mbadaddr));
}
static void machine_page_fault(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
{
// MPRV=1 iff this trap occurred while emulating an instruction on behalf
// of a lower privilege level. In that case, a2=epc and a3=mstatus.
if (read_csr(mstatus) & MSTATUS_MPRV) {
return redirect_trap(regs[12], regs[13], read_csr(mbadaddr));
}
bad_trap(regs, dummy, mepc);
}
void trap_from_machine_mode(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
{
uintptr_t mcause = read_csr(mcause);
switch (mcause)
{
case CAUSE_LOAD_PAGE_FAULT:
case CAUSE_STORE_PAGE_FAULT:
case CAUSE_FETCH_ACCESS:
case CAUSE_LOAD_ACCESS:
case CAUSE_STORE_ACCESS:
return machine_page_fault(regs, dummy, mepc);
default:
bad_trap(regs, dummy, mepc);
}
}
|
