1 /*-
2 * Copyright (c) 2024 Ruslan Bukin <br@bsdpad.com>
3 *
4 * This software was developed by the University of Cambridge Computer
5 * Laboratory (Department of Computer Science and Technology) under Innovate
6 * UK project 105694, "Digital Security by Design (DSbD) Technology Platform
7 * Prototype".
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 * 1. Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * 2. Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
17 *
18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
28 * SUCH DAMAGE.
29 */
30
31 #include <sys/param.h>
32 #include <sys/systm.h>
33 #include <sys/kernel.h>
34
35 #include <vm/uma.h>
36
37 #include <machine/fpe.h>
38 #include <machine/reg.h>
39
40 static uma_zone_t fpu_save_area_zone;
41 static struct fpreg *fpu_initialstate;
42
43 void
fpe_enable(void)44 fpe_enable(void)
45 {
46 uint64_t reg;
47
48 reg = SSTATUS_FS_INITIAL;
49
50 csr_set(sstatus, reg);
51 }
52
53 void
fpe_disable(void)54 fpe_disable(void)
55 {
56 uint64_t mask;
57
58 mask = SSTATUS_FS_MASK;
59
60 csr_clear(sstatus, mask);
61 }
62
63 void
fpe_store(struct fpreg * regs)64 fpe_store(struct fpreg *regs)
65 {
66 uint64_t fcsr, (*fp_x)[32][2];
67
68 fp_x = ®s->fp_x;
69
70 __asm __volatile(
71 "frcsr %0 \n"
72 "fsd f0, (16 * 0)(%1)\n"
73 "fsd f1, (16 * 1)(%1)\n"
74 "fsd f2, (16 * 2)(%1)\n"
75 "fsd f3, (16 * 3)(%1)\n"
76 "fsd f4, (16 * 4)(%1)\n"
77 "fsd f5, (16 * 5)(%1)\n"
78 "fsd f6, (16 * 6)(%1)\n"
79 "fsd f7, (16 * 7)(%1)\n"
80 "fsd f8, (16 * 8)(%1)\n"
81 "fsd f9, (16 * 9)(%1)\n"
82 "fsd f10, (16 * 10)(%1)\n"
83 "fsd f11, (16 * 11)(%1)\n"
84 "fsd f12, (16 * 12)(%1)\n"
85 "fsd f13, (16 * 13)(%1)\n"
86 "fsd f14, (16 * 14)(%1)\n"
87 "fsd f15, (16 * 15)(%1)\n"
88 "fsd f16, (16 * 16)(%1)\n"
89 "fsd f17, (16 * 17)(%1)\n"
90 "fsd f18, (16 * 18)(%1)\n"
91 "fsd f19, (16 * 19)(%1)\n"
92 "fsd f20, (16 * 20)(%1)\n"
93 "fsd f21, (16 * 21)(%1)\n"
94 "fsd f22, (16 * 22)(%1)\n"
95 "fsd f23, (16 * 23)(%1)\n"
96 "fsd f24, (16 * 24)(%1)\n"
97 "fsd f25, (16 * 25)(%1)\n"
98 "fsd f26, (16 * 26)(%1)\n"
99 "fsd f27, (16 * 27)(%1)\n"
100 "fsd f28, (16 * 28)(%1)\n"
101 "fsd f29, (16 * 29)(%1)\n"
102 "fsd f30, (16 * 30)(%1)\n"
103 "fsd f31, (16 * 31)(%1)\n"
104 : "=&r"(fcsr), "=r"(fp_x), "=m"(*fp_x));
105
106 regs->fp_fcsr = fcsr;
107 }
108
109 void
fpe_restore(struct fpreg * regs)110 fpe_restore(struct fpreg *regs)
111 {
112 uint64_t fcsr, (*fp_x)[32][2];
113
114 fp_x = ®s->fp_x;
115 fcsr = regs->fp_fcsr;
116
117 __asm __volatile(
118 "fscsr %0 \n"
119 "fld f0, (16 * 0)(%1)\n"
120 "fld f1, (16 * 1)(%1)\n"
121 "fld f2, (16 * 2)(%1)\n"
122 "fld f3, (16 * 3)(%1)\n"
123 "fld f4, (16 * 4)(%1)\n"
124 "fld f5, (16 * 5)(%1)\n"
125 "fld f6, (16 * 6)(%1)\n"
126 "fld f7, (16 * 7)(%1)\n"
127 "fld f8, (16 * 8)(%1)\n"
128 "fld f9, (16 * 9)(%1)\n"
129 "fld f10, (16 * 10)(%1)\n"
130 "fld f11, (16 * 11)(%1)\n"
131 "fld f12, (16 * 12)(%1)\n"
132 "fld f13, (16 * 13)(%1)\n"
133 "fld f14, (16 * 14)(%1)\n"
134 "fld f15, (16 * 15)(%1)\n"
135 "fld f16, (16 * 16)(%1)\n"
136 "fld f17, (16 * 17)(%1)\n"
137 "fld f18, (16 * 18)(%1)\n"
138 "fld f19, (16 * 19)(%1)\n"
139 "fld f20, (16 * 20)(%1)\n"
140 "fld f21, (16 * 21)(%1)\n"
141 "fld f22, (16 * 22)(%1)\n"
142 "fld f23, (16 * 23)(%1)\n"
143 "fld f24, (16 * 24)(%1)\n"
144 "fld f25, (16 * 25)(%1)\n"
145 "fld f26, (16 * 26)(%1)\n"
146 "fld f27, (16 * 27)(%1)\n"
147 "fld f28, (16 * 28)(%1)\n"
148 "fld f29, (16 * 29)(%1)\n"
149 "fld f30, (16 * 30)(%1)\n"
150 "fld f31, (16 * 31)(%1)\n"
151 :: "r"(fcsr), "r"(fp_x), "m"(*fp_x));
152 }
153
154 struct fpreg *
fpu_save_area_alloc(void)155 fpu_save_area_alloc(void)
156 {
157
158 return (uma_zalloc(fpu_save_area_zone, M_WAITOK));
159 }
160
161 void
fpu_save_area_free(struct fpreg * fsa)162 fpu_save_area_free(struct fpreg *fsa)
163 {
164
165 uma_zfree(fpu_save_area_zone, fsa);
166 }
167
168 void
fpu_save_area_reset(struct fpreg * fsa)169 fpu_save_area_reset(struct fpreg *fsa)
170 {
171
172 memcpy(fsa, fpu_initialstate, sizeof(*fsa));
173 }
174
175 static void
fpe_init(const void * dummy __unused)176 fpe_init(const void *dummy __unused)
177 {
178
179 fpu_save_area_zone = uma_zcreate("FPE save area", sizeof(struct fpreg),
180 NULL, NULL, NULL, NULL, _Alignof(struct fpreg) - 1, 0);
181 fpu_initialstate = uma_zalloc(fpu_save_area_zone, M_WAITOK | M_ZERO);
182
183 fpe_enable();
184 fpe_store(fpu_initialstate);
185 fpe_disable();
186
187 bzero(fpu_initialstate->fp_x, sizeof(fpu_initialstate->fp_x));
188 }
189
190 SYSINIT(fpe, SI_SUB_CPU, SI_ORDER_ANY, fpe_init, NULL);
191