1 #include "csi_core.h"
2 #include "rv_hart.h"
3 #include <risc-v/csr.h>
4
5 #define PRV_U 0
6 #define PRV_S 1
7 #define PRV_H 2
8 #define PRV_M 3
9
10 #define INSERT_FIELD(val, which, fieldval) \
11 (((val) & ~(which)) | ((fieldval) * ((which) & ~((which)-1))))
12
rv_hart_hang(void)13 void __attribute__((noreturn)) rv_hart_hang(void)
14 {
15 while (1) {
16 __WFI();
17 }
18
19 __builtin_unreachable();
20 }
21
22 /* determine CPU extension, return non-zero support */
rv_hart_misa_ext_chk(char ext)23 int rv_hart_misa_ext_chk(char ext)
24 {
25 unsigned long misa = READ_CSR(misa);
26
27 if (misa) {
28 if ('A' <= ext && ext <= 'Z') {
29 return misa & (1 << (ext - 'A'));
30 }
31
32 if ('a' <= ext && ext <= 'z') {
33 return misa & (1 << (ext - 'a'));
34 }
35
36 return 0;
37 }
38
39 return 0;
40 }
41
rv_enterSupervisor(void)42 void rv_enterSupervisor(void)
43 {
44 __asm volatile(
45 "fence\n\t"
46 "csrw mepc, ra\n\t"
47 "li t0, ~(3 << 11)\n\t"
48 "li t1, 1 << 11\n\t"
49 "csrr t2, mstatus\n\t"
50 "and t0, t0, t2\n\t"
51 "or t0, t0, t1\n\t"
52 "csrw mstatus, t0\n\t"
53 "mret\n\t"
54 :
55 :
56 : "memory");
57 }
58
rv_enterUser(void)59 void rv_enterUser(void)
60 {
61 __asm volatile(
62 "fence\n\t"
63 "csrw mepc, ra\n\t"
64 "li t0, ~(3 << 11)\n\t"
65 "csrr t2, mstatus\n\t"
66 "and t0, t0, t2\n\t"
67 "csrw mstatus, t0\n\t"
68 "mret\n\t"
69 :
70 :
71 : "memory");
72 }
73
74 void __attribute__((noreturn))
rv_hart_switch_mode_from_M(uintptr_t arg0,uintptr_t arg1,uintptr_t next_addr,uintptr_t next_mode)75 rv_hart_switch_mode_from_M(uintptr_t arg0, uintptr_t arg1,
76 uintptr_t next_addr, uintptr_t next_mode)
77 {
78 #if __riscv_xlen == 32
79 unsigned long val;
80 #else
81 unsigned long val;
82 #endif
83
84 switch (next_mode) {
85 case PRV_M:
86 break;
87
88 case PRV_S:
89 if (!rv_hart_misa_ext_chk('S')) {
90 rv_hart_hang();
91 }
92
93 break;
94
95 case PRV_U:
96 if (!rv_hart_misa_ext_chk('U')) {
97 rv_hart_hang();
98 }
99
100 break;
101
102 default:
103 rv_hart_hang();
104 }
105
106 val = READ_CSR(mstatus);
107 val = INSERT_FIELD(val, MSTATUS_MPPM, next_mode);
108 val = INSERT_FIELD(val, MSTATUS_MPIE, 0);
109
110 WRITE_CSR(mstatus, val);
111 WRITE_CSR(mepc, next_addr);
112
113 if (next_mode == PRV_S) {
114 WRITE_CSR(stvec, next_addr);
115 WRITE_CSR(sscratch, 0);
116 WRITE_CSR(sie, 0);
117 WRITE_CSR(satp, 0);
118 } else if (next_mode == PRV_U) {
119 /* un-implemented
120 WRITE_CSR(utvec, next_addr);
121 WRITE_CSR(uscratch, 0);
122 WRITE_CSR(uie, 0);
123 */
124 }
125
126 register unsigned long a0 __asm("a0") = arg0;
127 register unsigned long a1 __asm("a1") = arg1;
128 __asm__ __volatile__("mret"
129 :
130 : "r"(a0), "r"(a1));
131 __builtin_unreachable();
132 }
133