1 /* $OpenBSD: vfp.c,v 1.5 2022/08/29 02:01:18 jsg Exp $ */
2
3 /*
4 * Copyright (c) 2011 Dale Rahn <drahn@openbsd.org>
5 *
6 * Permission to use, copy, modify, and distribute this software for any
7 * purpose with or without fee is hereby granted, provided that the above
8 * copyright notice and this permission notice appear in all copies.
9 *
10 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
11 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
12 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
13 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
14 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17 */
18
19 #include <sys/param.h>
20 #include <sys/systm.h>
21 #include <sys/proc.h>
22 #include <sys/user.h>
23
24 #include <arm/include/cpufunc.h>
25 #include <arm/include/vfp.h>
26 #include <arm/include/undefined.h>
27
28 static inline void
set_vfp_fpexc(uint32_t val)29 set_vfp_fpexc(uint32_t val)
30 {
31 __asm volatile(
32 ".fpu vfpv3\n"
33 "vmsr fpexc, %0" :: "r" (val));
34 }
35
36 static inline uint32_t
get_vfp_fpexc(void)37 get_vfp_fpexc(void)
38 {
39 uint32_t val;
40 __asm volatile(
41 ".fpu vfpv3\n"
42 "vmrs %0, fpexc" : "=r" (val));
43 return val;
44 }
45
46 int vfp_fault(unsigned int, unsigned int, trapframe_t *, int, uint32_t);
47 void vfp_load(struct proc *p);
48 void vfp_store(struct fpreg *vfpsave);
49
50 void
vfp_init(void)51 vfp_init(void)
52 {
53 uint32_t val;
54
55 install_coproc_handler(10, vfp_fault);
56 install_coproc_handler(11, vfp_fault);
57
58 __asm volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val));
59 val |= COPROC10 | COPROC11;
60 __asm volatile("mcr p15, 0, %0, c1, c0, 2" :: "r" (val));
61 __asm volatile("isb");
62 }
63
64 void
vfp_store(struct fpreg * vfpsave)65 vfp_store(struct fpreg *vfpsave)
66 {
67 uint32_t scratch;
68
69 if (get_vfp_fpexc() & VFPEXC_EN) {
70 __asm volatile(
71 ".fpu vfpv3\n"
72 "vstmia %1!, {d0-d15}\n" /* d0-d15 */
73 "vstmia %1!, {d16-d31}\n" /* d16-d31 */
74 "vmrs %0, fpscr\n"
75 "str %0, [%1]\n" /* save vfpscr */
76 : "=&r" (scratch) : "r" (vfpsave));
77 }
78
79 /* disable FPU */
80 set_vfp_fpexc(0);
81 }
82
83 uint32_t
vfp_save(void)84 vfp_save(void)
85 {
86 struct cpu_info *ci = curcpu();
87 struct pcb *pcb = curpcb;
88 struct proc *p = curproc;
89 uint32_t fpexc;
90
91 if (ci->ci_fpuproc == 0)
92 return 0;
93
94 fpexc = get_vfp_fpexc();
95
96 if ((fpexc & VFPEXC_EN) == 0)
97 return fpexc; /* not enabled, nothing to do */
98
99 if (fpexc & VFPEXC_EX)
100 panic("vfp exceptional data fault, time to write more code");
101
102 if (pcb->pcb_fpcpu == NULL || ci->ci_fpuproc == NULL ||
103 !(pcb->pcb_fpcpu == ci && ci->ci_fpuproc == p)) {
104 /* disable fpu before panic, otherwise recurse */
105 set_vfp_fpexc(0);
106
107 panic("FPU unit enabled when curproc and curcpu dont agree %p %p %p %p", pcb->pcb_fpcpu, ci, ci->ci_fpuproc, p);
108 }
109
110 vfp_store(&p->p_addr->u_pcb.pcb_fpstate);
111
112 /*
113 * NOTE: fpu state is saved but remains 'valid', as long as
114 * curpcb()->pcb_fpucpu == ci && ci->ci_fpuproc == curproc()
115 * is true FPU state is valid and can just be enabled without reload.
116 */
117 return fpexc;
118 }
119
120 void
vfp_enable(void)121 vfp_enable(void)
122 {
123 struct cpu_info *ci = curcpu();
124
125 if (curproc->p_addr->u_pcb.pcb_fpcpu == ci &&
126 ci->ci_fpuproc == curproc) {
127 disable_interrupts(PSR_I|PSR_F);
128
129 /* FPU state is still valid, just enable and go */
130 set_vfp_fpexc(VFPEXC_EN);
131 }
132 }
133
134 void
vfp_load(struct proc * p)135 vfp_load(struct proc *p)
136 {
137 struct cpu_info *ci = curcpu();
138 struct pcb *pcb = &p->p_addr->u_pcb;
139 uint32_t scratch = 0;
140 int psw;
141
142 /* do not allow a partially synced state here */
143 psw = disable_interrupts(PSR_I|PSR_F);
144
145 /*
146 * p->p_pcb->pcb_fpucpu _may_ not be NULL here, but the FPU state
147 * was synced on kernel entry, so we can steal the FPU state
148 * instead of signalling and waiting for it to save
149 */
150
151 /* enable to be able to load ctx */
152 set_vfp_fpexc(VFPEXC_EN);
153
154 __asm volatile(
155 ".fpu vfpv3\n"
156 "vldmia %1!, {d0-d15}\n" /* d0-d15 */
157 "vldmia %1!, {d16-d31}\n" /* d16-d31 */
158 "ldr %0, [%1]\n" /* set old vfpscr */
159 "vmsr fpscr, %0\n"
160 : "=&r" (scratch) : "r" (&pcb->pcb_fpstate));
161
162 ci->ci_fpuproc = p;
163 pcb->pcb_fpcpu = ci;
164
165 /* disable until return to userland */
166 set_vfp_fpexc(0);
167
168 restore_interrupts(psw);
169 }
170
171 int
vfp_fault(unsigned int pc,unsigned int insn,trapframe_t * tf,int fault_code,uint32_t fpexc)172 vfp_fault(unsigned int pc, unsigned int insn, trapframe_t *tf, int fault_code,
173 uint32_t fpexc)
174 {
175 struct proc *p = curproc;
176 struct pcb *pcb = &p->p_addr->u_pcb;
177
178 if ((fpexc & VFPEXC_EN) != 0) {
179 /*
180 * We probably ran into an unsupported instruction,
181 * like NEON on a non-NEON system. Let the process know.
182 */
183 return 1;
184 }
185
186 /* we should be able to ignore old state of pcb_fpcpu ci_fpuproc */
187 if ((pcb->pcb_flags & PCB_FPU) == 0) {
188 pcb->pcb_flags |= PCB_FPU;
189 memset(&pcb->pcb_fpstate, 0, sizeof(pcb->pcb_fpstate));
190 }
191 vfp_load(p);
192
193 return 0;
194 }
195
196 void
vfp_discard(struct proc * p)197 vfp_discard(struct proc *p)
198 {
199 struct cpu_info *ci = curcpu();
200
201 if (curpcb->pcb_fpcpu == ci && ci->ci_fpuproc == p) {
202 ci->ci_fpuproc = NULL;
203 curpcb->pcb_fpcpu = NULL;
204 }
205 }
206