1// See LICENSE for license details.
2
3#include "mtrap.h"
4#include "mcall.h"
5#include "htif.h"
6#include "atomic.h"
7#include "bits.h"
8#include "vm.h"
9#include "uart.h"
10#include "uart16550.h"
11#include "uart16750.h"
12#include "finisher.h"
13#include "fdt.h"
14#include "unprivileged_memory.h"
15#include "disabled_hart_mask.h"
16#include <errno.h>
17#include <stdarg.h>
18#include <stdio.h>
19
20void __attribute__((noreturn)) bad_trap(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
21{
22  die("machine mode: unhandlable trap %d @ %p", read_csr(mcause), mepc);
23}
24
25static uintptr_t mcall_console_putchar(uint8_t ch)
26{
27  if (uart) {
28    uart_putchar(ch);
29  } else if (uart16550) {
30    uart16550_putchar(ch);
31  } else if (uart16750) {
32    uart16750_putchar(ch);
33  } else if (htif) {
34    htif_console_putchar(ch);
35  }
36  return 0;
37}
38
39void putstring(const char* s)
40{
41  while (*s)
42    mcall_console_putchar(*s++);
43}
44
45void vprintm(const char* s, va_list vl)
46{
47  char buf[256];
48  vsnprintf(buf, sizeof buf, s, vl);
49  putstring(buf);
50}
51
52void printm(const char* s, ...)
53{
54  va_list vl;
55
56  va_start(vl, s);
57  vprintm(s, vl);
58  va_end(vl);
59}
60
61static void send_ipi(uintptr_t recipient, int event)
62{
63  if (((disabled_hart_mask >> recipient) & 1)) return;
64  atomic_or(&OTHER_HLS(recipient)->mipi_pending, event);
65  mb();
66  *OTHER_HLS(recipient)->ipi = 1;
67}
68
69static uintptr_t mcall_console_getchar()
70{
71  if (uart) {
72    return uart_getchar();
73  } else if (uart16550) {
74    return uart16550_getchar();
75  } else if (uart16750) {
76    return uart16750_getchar();
77  } else if (htif) {
78    return htif_console_getchar();
79  } else {
80    return '\0';
81  }
82}
83
84static uintptr_t mcall_clear_ipi()
85{
86  return clear_csr(mip, MIP_SSIP) & MIP_SSIP;
87}
88
89static uintptr_t mcall_shutdown()
90{
91  poweroff(0);
92}
93
94static uintptr_t mcall_set_timer(uint64_t when)
95{
96  *HLS()->timecmp = when;
97  clear_csr(mip, MIP_STIP);
98  set_csr(mie, MIP_MTIP);
99  return 0;
100}
101
102static void send_ipi_many(uintptr_t* pmask, int event)
103{
104  _Static_assert(MAX_HARTS <= 8 * sizeof(*pmask), "# harts > uintptr_t bits");
105  uintptr_t mask = hart_mask;
106  if (pmask)
107    mask &= load_uintptr_t(pmask, read_csr(mepc));
108
109  // send IPIs to everyone
110  for (uintptr_t i = 0, m = mask; m; i++, m >>= 1)
111    if (m & 1)
112      send_ipi(i, event);
113
114  if (event == IPI_SOFT)
115    return;
116
117  // wait until all events have been handled.
118  // prevent deadlock by consuming incoming IPIs.
119  uint32_t incoming_ipi = 0;
120  for (uintptr_t i = 0, m = mask; m; i++, m >>= 1)
121    if (m & 1)
122      while (*OTHER_HLS(i)->ipi)
123        incoming_ipi |= atomic_swap(HLS()->ipi, 0);
124
125  // if we got an IPI, restore it; it will be taken after returning
126  if (incoming_ipi) {
127    *HLS()->ipi = incoming_ipi;
128    mb();
129  }
130}
131
132void mcall_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
133{
134  write_csr(mepc, mepc + 4);
135
136  uintptr_t n = regs[17], arg0 = regs[10], arg1 = regs[11], retval, ipi_type;
137
138  switch (n)
139  {
140    case SBI_CONSOLE_PUTCHAR:
141      retval = mcall_console_putchar(arg0);
142      break;
143    case SBI_CONSOLE_GETCHAR:
144      retval = mcall_console_getchar();
145      break;
146    case SBI_SEND_IPI:
147      ipi_type = IPI_SOFT;
148      goto send_ipi;
149    case SBI_REMOTE_SFENCE_VMA:
150    case SBI_REMOTE_SFENCE_VMA_ASID:
151      ipi_type = IPI_SFENCE_VMA;
152      goto send_ipi;
153    case SBI_REMOTE_FENCE_I:
154      ipi_type = IPI_FENCE_I;
155send_ipi:
156      send_ipi_many((uintptr_t*)arg0, ipi_type);
157      retval = 0;
158      break;
159    case SBI_CLEAR_IPI:
160      retval = mcall_clear_ipi();
161      break;
162    case SBI_SHUTDOWN:
163      retval = mcall_shutdown();
164      break;
165    case SBI_SET_TIMER:
166#if __riscv_xlen == 32
167      retval = mcall_set_timer(arg0 + ((uint64_t)arg1 << 32));
168#else
169      retval = mcall_set_timer(arg0);
170#endif
171      break;
172    default:
173      retval = -ENOSYS;
174      break;
175  }
176  regs[10] = retval;
177}
178
179void redirect_trap(uintptr_t epc, uintptr_t mstatus, uintptr_t badaddr)
180{
181  /* redirect to HS mode, based on OpenSBI */
182  if (supports_extension('H')) {
183    uintptr_t hstatus = read_csr(0xa00);
184    /* V = 1 */
185    if (mstatus & MSTATUS_MPV) {
186      hstatus &= ~HSTATUS_SP2P;
187      hstatus |= (mstatus & MSTATUS_SPP) ? HSTATUS_SP2P : 0;
188      hstatus &= ~HSTATUS_SP2V;
189      hstatus |= (hstatus & HSTATUS_SPV) ? HSTATUS_SP2V : 0;
190      hstatus &= ~HSTATUS_SPV;
191      hstatus |= (mstatus & MSTATUS_MPV) ? HSTATUS_SPV : 0;
192      hstatus &= ~HSTATUS_STL;
193      hstatus |= (mstatus & MSTATUS_MTL) ? HSTATUS_STL : 0;
194      write_csr(0xa00, hstatus);
195
196      mstatus &= ~MSTATUS_MPV;
197      mstatus &= ~MSTATUS_MTL;
198    }
199  }
200
201  write_csr(sbadaddr, badaddr);
202  write_csr(sepc, epc);
203  write_csr(scause, read_csr(mcause));
204  write_csr(mepc, read_csr(stvec));
205
206  uintptr_t new_mstatus = mstatus & ~(MSTATUS_SPP | MSTATUS_SPIE | MSTATUS_SIE);
207  uintptr_t mpp_s = MSTATUS_MPP & (MSTATUS_MPP >> 1);
208  new_mstatus |= (mstatus * (MSTATUS_SPIE / MSTATUS_SIE)) & MSTATUS_SPIE;
209  new_mstatus |= (mstatus / (mpp_s / MSTATUS_SPP)) & MSTATUS_SPP;
210  new_mstatus |= mpp_s;
211  write_csr(mstatus, new_mstatus);
212
213  extern void __redirect_trap();
214  return __redirect_trap();
215}
216
217void pmp_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
218{
219  redirect_trap(mepc, read_csr(mstatus), read_csr(mbadaddr));
220}
221
222static void machine_page_fault(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
223{
224  // MPRV=1 iff this trap occurred while emulating an instruction on behalf
225  // of a lower privilege level. In that case, a2=epc and a3=mstatus.
226  if (read_csr(mstatus) & MSTATUS_MPRV) {
227    return redirect_trap(regs[12], regs[13], read_csr(mbadaddr));
228  }
229  bad_trap(regs, dummy, mepc);
230}
231
232void trap_from_machine_mode(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
233{
234  uintptr_t mcause = read_csr(mcause);
235
236  switch (mcause)
237  {
238    case CAUSE_LOAD_PAGE_FAULT:
239    case CAUSE_STORE_PAGE_FAULT:
240    case CAUSE_FETCH_ACCESS:
241    case CAUSE_LOAD_ACCESS:
242    case CAUSE_STORE_ACCESS:
243      return machine_page_fault(regs, dummy, mepc);
244    default:
245      bad_trap(regs, dummy, mepc);
246  }
247}
248
249void poweroff(uint16_t code)
250{
251  printm("Power off\r\n");
252  finisher_exit(code);
253  if (htif) {
254    htif_poweroff();
255  } else {
256    send_ipi_many(0, IPI_HALT);
257    while (1) { asm volatile ("wfi\n"); }
258  }
259}
260