Kernel properly handles floating point (XCC)
[akaros.git] / kern / arch / riscv / env.c
1 #include <trap.h>
2 #include <env.h>
3 #include <assert.h>
4 #include <arch/arch.h>
5 #include <pmap.h>
6
7 void
8 env_push_ancillary_state(env_t* e)
9 {
10         if(e->scp_ctx.hw_tf.sr & SR_EF)
11                 save_fp_state(&e->env_ancillary_state);
12 }
13
14 void
15 save_fp_state(ancillary_state_t* silly)
16 {
17         /* TODO: save FP state! */
18         return; // don't save FP state for now
19         uintptr_t sr = mfpcr(PCR_SR);
20         mtpcr(PCR_SR, sr | SR_EF);
21
22         asm("mffsr %0" : "=r"(silly->fsr));
23
24         asm("fsd f0,%0" : "=m"(silly->fpr[0]));
25         asm("fsd f1,%0" : "=m"(silly->fpr[1]));
26         asm("fsd f2,%0" : "=m"(silly->fpr[2]));
27         asm("fsd f3,%0" : "=m"(silly->fpr[3]));
28         asm("fsd f4,%0" : "=m"(silly->fpr[4]));
29         asm("fsd f5,%0" : "=m"(silly->fpr[5]));
30         asm("fsd f6,%0" : "=m"(silly->fpr[6]));
31         asm("fsd f7,%0" : "=m"(silly->fpr[7]));
32         asm("fsd f8,%0" : "=m"(silly->fpr[8]));
33         asm("fsd f9,%0" : "=m"(silly->fpr[9]));
34         asm("fsd f10,%0" : "=m"(silly->fpr[10]));
35         asm("fsd f11,%0" : "=m"(silly->fpr[11]));
36         asm("fsd f12,%0" : "=m"(silly->fpr[12]));
37         asm("fsd f13,%0" : "=m"(silly->fpr[13]));
38         asm("fsd f14,%0" : "=m"(silly->fpr[14]));
39         asm("fsd f15,%0" : "=m"(silly->fpr[15]));
40         asm("fsd f16,%0" : "=m"(silly->fpr[16]));
41         asm("fsd f17,%0" : "=m"(silly->fpr[17]));
42         asm("fsd f18,%0" : "=m"(silly->fpr[18]));
43         asm("fsd f19,%0" : "=m"(silly->fpr[19]));
44         asm("fsd f20,%0" : "=m"(silly->fpr[20]));
45         asm("fsd f21,%0" : "=m"(silly->fpr[21]));
46         asm("fsd f22,%0" : "=m"(silly->fpr[22]));
47         asm("fsd f23,%0" : "=m"(silly->fpr[23]));
48         asm("fsd f24,%0" : "=m"(silly->fpr[24]));
49         asm("fsd f25,%0" : "=m"(silly->fpr[25]));
50         asm("fsd f26,%0" : "=m"(silly->fpr[26]));
51         asm("fsd f27,%0" : "=m"(silly->fpr[27]));
52         asm("fsd f28,%0" : "=m"(silly->fpr[28]));
53         asm("fsd f29,%0" : "=m"(silly->fpr[29]));
54         asm("fsd f30,%0" : "=m"(silly->fpr[30]));
55         asm("fsd f31,%0" : "=m"(silly->fpr[31]));
56
57         mtpcr(PCR_SR, sr);
58 }
59
60 void
61 env_pop_ancillary_state(env_t* e)
62
63         if(e->scp_ctx.hw_tf.sr & SR_EF)
64                 restore_fp_state(&e->env_ancillary_state);
65 }
66
67 void
68 restore_fp_state(ancillary_state_t* silly)
69 {
70         return; // don't restore FP state for now
71         uintptr_t sr = mfpcr(PCR_SR);
72         mtpcr(PCR_SR, sr | SR_EF);
73
74         asm("mtfsr %0" : : "r"(silly->fsr));
75
76         asm("fld f0,%0" : : "m"(silly->fpr[0]));
77         asm("fld f1,%0" : : "m"(silly->fpr[1]));
78         asm("fld f2,%0" : : "m"(silly->fpr[2]));
79         asm("fld f3,%0" : : "m"(silly->fpr[3]));
80         asm("fld f4,%0" : : "m"(silly->fpr[4]));
81         asm("fld f5,%0" : : "m"(silly->fpr[5]));
82         asm("fld f6,%0" : : "m"(silly->fpr[6]));
83         asm("fld f7,%0" : : "m"(silly->fpr[7]));
84         asm("fld f8,%0" : : "m"(silly->fpr[8]));
85         asm("fld f9,%0" : : "m"(silly->fpr[9]));
86         asm("fld f10,%0" : : "m"(silly->fpr[10]));
87         asm("fld f11,%0" : : "m"(silly->fpr[11]));
88         asm("fld f12,%0" : : "m"(silly->fpr[12]));
89         asm("fld f13,%0" : : "m"(silly->fpr[13]));
90         asm("fld f14,%0" : : "m"(silly->fpr[14]));
91         asm("fld f15,%0" : : "m"(silly->fpr[15]));
92         asm("fld f16,%0" : : "m"(silly->fpr[16]));
93         asm("fld f17,%0" : : "m"(silly->fpr[17]));
94         asm("fld f18,%0" : : "m"(silly->fpr[18]));
95         asm("fld f19,%0" : : "m"(silly->fpr[19]));
96         asm("fld f20,%0" : : "m"(silly->fpr[20]));
97         asm("fld f21,%0" : : "m"(silly->fpr[21]));
98         asm("fld f22,%0" : : "m"(silly->fpr[22]));
99         asm("fld f23,%0" : : "m"(silly->fpr[23]));
100         asm("fld f24,%0" : : "m"(silly->fpr[24]));
101         asm("fld f25,%0" : : "m"(silly->fpr[25]));
102         asm("fld f26,%0" : : "m"(silly->fpr[26]));
103         asm("fld f27,%0" : : "m"(silly->fpr[27]));
104         asm("fld f28,%0" : : "m"(silly->fpr[28]));
105         asm("fld f29,%0" : : "m"(silly->fpr[29]));
106         asm("fld f30,%0" : : "m"(silly->fpr[30]));
107         asm("fld f31,%0" : : "m"(silly->fpr[31]));
108
109         mtpcr(PCR_SR, sr);
110 }
111
112 void init_fp_state(void)
113 {
114         /* TODO: implement me! */
115 }
116
117 static int
118 user_mem_walk_recursive(env_t* e, uintptr_t start, size_t len,
119                         mem_walk_callback_t callback, void* arg,
120                         mem_walk_callback_t pt_callback, void* pt_arg,
121                         pte_t* pt, int level)
122 {
123         int ret = 0;
124         int pgshift = L1PGSHIFT - level*(L1PGSHIFT-L2PGSHIFT);
125         uintptr_t pgsize = 1UL << pgshift;
126
127         uintptr_t start_idx = (start >> pgshift) & (NPTENTRIES-1);
128         uintptr_t end_idx = ((start+len-1) >> pgshift) & (NPTENTRIES-1);
129
130         for(uintptr_t idx = start_idx; idx <= end_idx; idx++)
131         {
132                 uintptr_t pgaddr = ROUNDDOWN(start, pgsize) + (idx-start_idx)*pgsize;
133                 pte_t* pte = &pt[idx];
134
135                 if(*pte & PTE_T)
136                 {
137                         assert(level < NPTLEVELS-1);
138                         uintptr_t st = MAX(pgaddr, start);
139                         size_t ln = MIN(start + len, pgaddr + pgsize) - st;
140                         if((ret = user_mem_walk_recursive(e, st, ln, callback, arg,
141                                                           pt_callback, pt_arg,
142                                                           KADDR(PTD_ADDR(*pte)), level+1)))
143                                 goto out;
144                         if(pt_callback != NULL && (ret = pt_callback(e, pte, (void*)pgaddr, arg)))
145                                 goto out;
146                 }
147                 else if(callback != NULL && !PAGE_UNMAPPED(*pte))
148                         if((ret = callback(e, pte, (void*)pgaddr, arg)))
149                                 goto out;
150         }
151 out:
152         return ret;
153 }
154
155 int
156 env_user_mem_walk(env_t* e, void* start, size_t len,
157                   mem_walk_callback_t callback, void* arg)
158 {
159         assert(PGOFF(start) == 0 && PGOFF(len) == 0);
160         return user_mem_walk_recursive(e, (uintptr_t)start, len, callback, arg,
161                                        NULL, NULL, e->env_pgdir, 0);
162 }
163
164 void
165 env_pagetable_free(env_t* e)
166 {
167         int pt_free(env_t* e, pte_t* pte, void* va, void* arg)
168         {
169                 page_decref(pa2page(PTD_ADDR(*pte)));
170                 return 0;
171         }
172
173         assert(user_mem_walk_recursive(e, 0, KERNBASE, NULL, NULL,
174                                        pt_free, NULL, e->env_pgdir, 0) == 0);
175 }