xref: /openbmc/linux/arch/sh/kernel/cpu/sh4/fpu.c (revision 82e6fdd6)
1 /*
2  * Save/restore floating point context for signal handlers.
3  *
4  * This file is subject to the terms and conditions of the GNU General Public
5  * License.  See the file "COPYING" in the main directory of this archive
6  * for more details.
7  *
8  * Copyright (C) 1999, 2000  Kaz Kojima & Niibe Yutaka
9  * Copyright (C) 2006  ST Microelectronics Ltd. (denorm support)
10  *
11  * FIXME! These routines have not been tested for big endian case.
12  */
13 #include <linux/sched/signal.h>
14 #include <linux/io.h>
15 #include <cpu/fpu.h>
16 #include <asm/processor.h>
17 #include <asm/fpu.h>
18 #include <asm/traps.h>
19 
20 /* The PR (precision) bit in the FP Status Register must be clear when
21  * an frchg instruction is executed, otherwise the instruction is undefined.
22  * Executing frchg with PR set causes a trap on some SH4 implementations.
23  */
24 
25 #define FPSCR_RCHG 0x00000000
26 extern unsigned long long float64_div(unsigned long long a,
27 				      unsigned long long b);
28 extern unsigned long int float32_div(unsigned long int a, unsigned long int b);
29 extern unsigned long long float64_mul(unsigned long long a,
30 				      unsigned long long b);
31 extern unsigned long int float32_mul(unsigned long int a, unsigned long int b);
32 extern unsigned long long float64_add(unsigned long long a,
33 				      unsigned long long b);
34 extern unsigned long int float32_add(unsigned long int a, unsigned long int b);
35 extern unsigned long long float64_sub(unsigned long long a,
36 				      unsigned long long b);
37 extern unsigned long int float32_sub(unsigned long int a, unsigned long int b);
38 extern unsigned long int float64_to_float32(unsigned long long a);
39 static unsigned int fpu_exception_flags;
40 
41 /*
42  * Save FPU registers onto task structure.
43  */
44 void save_fpu(struct task_struct *tsk)
45 {
46 	unsigned long dummy;
47 
48 	enable_fpu();
49 	asm volatile ("sts.l	fpul, @-%0\n\t"
50 		      "sts.l	fpscr, @-%0\n\t"
51 		      "lds	%2, fpscr\n\t"
52 		      "frchg\n\t"
53 		      "fmov.s	fr15, @-%0\n\t"
54 		      "fmov.s	fr14, @-%0\n\t"
55 		      "fmov.s	fr13, @-%0\n\t"
56 		      "fmov.s	fr12, @-%0\n\t"
57 		      "fmov.s	fr11, @-%0\n\t"
58 		      "fmov.s	fr10, @-%0\n\t"
59 		      "fmov.s	fr9, @-%0\n\t"
60 		      "fmov.s	fr8, @-%0\n\t"
61 		      "fmov.s	fr7, @-%0\n\t"
62 		      "fmov.s	fr6, @-%0\n\t"
63 		      "fmov.s	fr5, @-%0\n\t"
64 		      "fmov.s	fr4, @-%0\n\t"
65 		      "fmov.s	fr3, @-%0\n\t"
66 		      "fmov.s	fr2, @-%0\n\t"
67 		      "fmov.s	fr1, @-%0\n\t"
68 		      "fmov.s	fr0, @-%0\n\t"
69 		      "frchg\n\t"
70 		      "fmov.s	fr15, @-%0\n\t"
71 		      "fmov.s	fr14, @-%0\n\t"
72 		      "fmov.s	fr13, @-%0\n\t"
73 		      "fmov.s	fr12, @-%0\n\t"
74 		      "fmov.s	fr11, @-%0\n\t"
75 		      "fmov.s	fr10, @-%0\n\t"
76 		      "fmov.s	fr9, @-%0\n\t"
77 		      "fmov.s	fr8, @-%0\n\t"
78 		      "fmov.s	fr7, @-%0\n\t"
79 		      "fmov.s	fr6, @-%0\n\t"
80 		      "fmov.s	fr5, @-%0\n\t"
81 		      "fmov.s	fr4, @-%0\n\t"
82 		      "fmov.s	fr3, @-%0\n\t"
83 		      "fmov.s	fr2, @-%0\n\t"
84 		      "fmov.s	fr1, @-%0\n\t"
85 		      "fmov.s	fr0, @-%0\n\t"
86 		      "lds	%3, fpscr\n\t":"=r" (dummy)
87 		      :"0"((char *)(&tsk->thread.xstate->hardfpu.status)),
88 		      "r"(FPSCR_RCHG), "r"(FPSCR_INIT)
89 		      :"memory");
90 
91 	disable_fpu();
92 }
93 
94 void restore_fpu(struct task_struct *tsk)
95 {
96 	unsigned long dummy;
97 
98 	enable_fpu();
99 	asm volatile ("lds	%2, fpscr\n\t"
100 		      "fmov.s	@%0+, fr0\n\t"
101 		      "fmov.s	@%0+, fr1\n\t"
102 		      "fmov.s	@%0+, fr2\n\t"
103 		      "fmov.s	@%0+, fr3\n\t"
104 		      "fmov.s	@%0+, fr4\n\t"
105 		      "fmov.s	@%0+, fr5\n\t"
106 		      "fmov.s	@%0+, fr6\n\t"
107 		      "fmov.s	@%0+, fr7\n\t"
108 		      "fmov.s	@%0+, fr8\n\t"
109 		      "fmov.s	@%0+, fr9\n\t"
110 		      "fmov.s	@%0+, fr10\n\t"
111 		      "fmov.s	@%0+, fr11\n\t"
112 		      "fmov.s	@%0+, fr12\n\t"
113 		      "fmov.s	@%0+, fr13\n\t"
114 		      "fmov.s	@%0+, fr14\n\t"
115 		      "fmov.s	@%0+, fr15\n\t"
116 		      "frchg\n\t"
117 		      "fmov.s	@%0+, fr0\n\t"
118 		      "fmov.s	@%0+, fr1\n\t"
119 		      "fmov.s	@%0+, fr2\n\t"
120 		      "fmov.s	@%0+, fr3\n\t"
121 		      "fmov.s	@%0+, fr4\n\t"
122 		      "fmov.s	@%0+, fr5\n\t"
123 		      "fmov.s	@%0+, fr6\n\t"
124 		      "fmov.s	@%0+, fr7\n\t"
125 		      "fmov.s	@%0+, fr8\n\t"
126 		      "fmov.s	@%0+, fr9\n\t"
127 		      "fmov.s	@%0+, fr10\n\t"
128 		      "fmov.s	@%0+, fr11\n\t"
129 		      "fmov.s	@%0+, fr12\n\t"
130 		      "fmov.s	@%0+, fr13\n\t"
131 		      "fmov.s	@%0+, fr14\n\t"
132 		      "fmov.s	@%0+, fr15\n\t"
133 		      "frchg\n\t"
134 		      "lds.l	@%0+, fpscr\n\t"
135 		      "lds.l	@%0+, fpul\n\t"
136 		      :"=r" (dummy)
137 		      :"0" (tsk->thread.xstate), "r" (FPSCR_RCHG)
138 		      :"memory");
139 	disable_fpu();
140 }
141 
142 /**
143  *      denormal_to_double - Given denormalized float number,
144  *                           store double float
145  *
146  *      @fpu: Pointer to sh_fpu_hard structure
147  *      @n: Index to FP register
148  */
149 static void denormal_to_double(struct sh_fpu_hard_struct *fpu, int n)
150 {
151 	unsigned long du, dl;
152 	unsigned long x = fpu->fpul;
153 	int exp = 1023 - 126;
154 
155 	if (x != 0 && (x & 0x7f800000) == 0) {
156 		du = (x & 0x80000000);
157 		while ((x & 0x00800000) == 0) {
158 			x <<= 1;
159 			exp--;
160 		}
161 		x &= 0x007fffff;
162 		du |= (exp << 20) | (x >> 3);
163 		dl = x << 29;
164 
165 		fpu->fp_regs[n] = du;
166 		fpu->fp_regs[n + 1] = dl;
167 	}
168 }
169 
170 /**
171  *	ieee_fpe_handler - Handle denormalized number exception
172  *
173  *	@regs: Pointer to register structure
174  *
175  *	Returns 1 when it's handled (should not cause exception).
176  */
177 static int ieee_fpe_handler(struct pt_regs *regs)
178 {
179 	unsigned short insn = *(unsigned short *)regs->pc;
180 	unsigned short finsn;
181 	unsigned long nextpc;
182 	int nib[4] = {
183 		(insn >> 12) & 0xf,
184 		(insn >> 8) & 0xf,
185 		(insn >> 4) & 0xf,
186 		insn & 0xf
187 	};
188 
189 	if (nib[0] == 0xb || (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb))
190 		regs->pr = regs->pc + 4;  /* bsr & jsr */
191 
192 	if (nib[0] == 0xa || nib[0] == 0xb) {
193 		/* bra & bsr */
194 		nextpc = regs->pc + 4 + ((short)((insn & 0xfff) << 4) >> 3);
195 		finsn = *(unsigned short *)(regs->pc + 2);
196 	} else if (nib[0] == 0x8 && nib[1] == 0xd) {
197 		/* bt/s */
198 		if (regs->sr & 1)
199 			nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1);
200 		else
201 			nextpc = regs->pc + 4;
202 		finsn = *(unsigned short *)(regs->pc + 2);
203 	} else if (nib[0] == 0x8 && nib[1] == 0xf) {
204 		/* bf/s */
205 		if (regs->sr & 1)
206 			nextpc = regs->pc + 4;
207 		else
208 			nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1);
209 		finsn = *(unsigned short *)(regs->pc + 2);
210 	} else if (nib[0] == 0x4 && nib[3] == 0xb &&
211 		   (nib[2] == 0x0 || nib[2] == 0x2)) {
212 		/* jmp & jsr */
213 		nextpc = regs->regs[nib[1]];
214 		finsn = *(unsigned short *)(regs->pc + 2);
215 	} else if (nib[0] == 0x0 && nib[3] == 0x3 &&
216 		   (nib[2] == 0x0 || nib[2] == 0x2)) {
217 		/* braf & bsrf */
218 		nextpc = regs->pc + 4 + regs->regs[nib[1]];
219 		finsn = *(unsigned short *)(regs->pc + 2);
220 	} else if (insn == 0x000b) {
221 		/* rts */
222 		nextpc = regs->pr;
223 		finsn = *(unsigned short *)(regs->pc + 2);
224 	} else {
225 		nextpc = regs->pc + instruction_size(insn);
226 		finsn = insn;
227 	}
228 
229 	if ((finsn & 0xf1ff) == 0xf0ad) {
230 		/* fcnvsd */
231 		struct task_struct *tsk = current;
232 
233 		if ((tsk->thread.xstate->hardfpu.fpscr & FPSCR_CAUSE_ERROR))
234 			/* FPU error */
235 			denormal_to_double(&tsk->thread.xstate->hardfpu,
236 					   (finsn >> 8) & 0xf);
237 		else
238 			return 0;
239 
240 		regs->pc = nextpc;
241 		return 1;
242 	} else if ((finsn & 0xf00f) == 0xf002) {
243 		/* fmul */
244 		struct task_struct *tsk = current;
245 		int fpscr;
246 		int n, m, prec;
247 		unsigned int hx, hy;
248 
249 		n = (finsn >> 8) & 0xf;
250 		m = (finsn >> 4) & 0xf;
251 		hx = tsk->thread.xstate->hardfpu.fp_regs[n];
252 		hy = tsk->thread.xstate->hardfpu.fp_regs[m];
253 		fpscr = tsk->thread.xstate->hardfpu.fpscr;
254 		prec = fpscr & FPSCR_DBL_PRECISION;
255 
256 		if ((fpscr & FPSCR_CAUSE_ERROR)
257 		    && (prec && ((hx & 0x7fffffff) < 0x00100000
258 				 || (hy & 0x7fffffff) < 0x00100000))) {
259 			long long llx, lly;
260 
261 			/* FPU error because of denormal (doubles) */
262 			llx = ((long long)hx << 32)
263 			    | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
264 			lly = ((long long)hy << 32)
265 			    | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
266 			llx = float64_mul(llx, lly);
267 			tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
268 			tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
269 		} else if ((fpscr & FPSCR_CAUSE_ERROR)
270 			   && (!prec && ((hx & 0x7fffffff) < 0x00800000
271 					 || (hy & 0x7fffffff) < 0x00800000))) {
272 			/* FPU error because of denormal (floats) */
273 			hx = float32_mul(hx, hy);
274 			tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
275 		} else
276 			return 0;
277 
278 		regs->pc = nextpc;
279 		return 1;
280 	} else if ((finsn & 0xf00e) == 0xf000) {
281 		/* fadd, fsub */
282 		struct task_struct *tsk = current;
283 		int fpscr;
284 		int n, m, prec;
285 		unsigned int hx, hy;
286 
287 		n = (finsn >> 8) & 0xf;
288 		m = (finsn >> 4) & 0xf;
289 		hx = tsk->thread.xstate->hardfpu.fp_regs[n];
290 		hy = tsk->thread.xstate->hardfpu.fp_regs[m];
291 		fpscr = tsk->thread.xstate->hardfpu.fpscr;
292 		prec = fpscr & FPSCR_DBL_PRECISION;
293 
294 		if ((fpscr & FPSCR_CAUSE_ERROR)
295 		    && (prec && ((hx & 0x7fffffff) < 0x00100000
296 				 || (hy & 0x7fffffff) < 0x00100000))) {
297 			long long llx, lly;
298 
299 			/* FPU error because of denormal (doubles) */
300 			llx = ((long long)hx << 32)
301 			    | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
302 			lly = ((long long)hy << 32)
303 			    | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
304 			if ((finsn & 0xf00f) == 0xf000)
305 				llx = float64_add(llx, lly);
306 			else
307 				llx = float64_sub(llx, lly);
308 			tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
309 			tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
310 		} else if ((fpscr & FPSCR_CAUSE_ERROR)
311 			   && (!prec && ((hx & 0x7fffffff) < 0x00800000
312 					 || (hy & 0x7fffffff) < 0x00800000))) {
313 			/* FPU error because of denormal (floats) */
314 			if ((finsn & 0xf00f) == 0xf000)
315 				hx = float32_add(hx, hy);
316 			else
317 				hx = float32_sub(hx, hy);
318 			tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
319 		} else
320 			return 0;
321 
322 		regs->pc = nextpc;
323 		return 1;
324 	} else if ((finsn & 0xf003) == 0xf003) {
325 		/* fdiv */
326 		struct task_struct *tsk = current;
327 		int fpscr;
328 		int n, m, prec;
329 		unsigned int hx, hy;
330 
331 		n = (finsn >> 8) & 0xf;
332 		m = (finsn >> 4) & 0xf;
333 		hx = tsk->thread.xstate->hardfpu.fp_regs[n];
334 		hy = tsk->thread.xstate->hardfpu.fp_regs[m];
335 		fpscr = tsk->thread.xstate->hardfpu.fpscr;
336 		prec = fpscr & FPSCR_DBL_PRECISION;
337 
338 		if ((fpscr & FPSCR_CAUSE_ERROR)
339 		    && (prec && ((hx & 0x7fffffff) < 0x00100000
340 				 || (hy & 0x7fffffff) < 0x00100000))) {
341 			long long llx, lly;
342 
343 			/* FPU error because of denormal (doubles) */
344 			llx = ((long long)hx << 32)
345 			    | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
346 			lly = ((long long)hy << 32)
347 			    | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
348 
349 			llx = float64_div(llx, lly);
350 
351 			tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
352 			tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
353 		} else if ((fpscr & FPSCR_CAUSE_ERROR)
354 			   && (!prec && ((hx & 0x7fffffff) < 0x00800000
355 					 || (hy & 0x7fffffff) < 0x00800000))) {
356 			/* FPU error because of denormal (floats) */
357 			hx = float32_div(hx, hy);
358 			tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
359 		} else
360 			return 0;
361 
362 		regs->pc = nextpc;
363 		return 1;
364 	} else if ((finsn & 0xf0bd) == 0xf0bd) {
365 		/* fcnvds - double to single precision convert */
366 		struct task_struct *tsk = current;
367 		int m;
368 		unsigned int hx;
369 
370 		m = (finsn >> 8) & 0x7;
371 		hx = tsk->thread.xstate->hardfpu.fp_regs[m];
372 
373 		if ((tsk->thread.xstate->hardfpu.fpscr & FPSCR_CAUSE_ERROR)
374 			&& ((hx & 0x7fffffff) < 0x00100000)) {
375 			/* subnormal double to float conversion */
376 			long long llx;
377 
378 			llx = ((long long)tsk->thread.xstate->hardfpu.fp_regs[m] << 32)
379 			    | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
380 
381 			tsk->thread.xstate->hardfpu.fpul = float64_to_float32(llx);
382 		} else
383 			return 0;
384 
385 		regs->pc = nextpc;
386 		return 1;
387 	}
388 
389 	return 0;
390 }
391 
392 void float_raise(unsigned int flags)
393 {
394 	fpu_exception_flags |= flags;
395 }
396 
397 int float_rounding_mode(void)
398 {
399 	struct task_struct *tsk = current;
400 	int roundingMode = FPSCR_ROUNDING_MODE(tsk->thread.xstate->hardfpu.fpscr);
401 	return roundingMode;
402 }
403 
404 BUILD_TRAP_HANDLER(fpu_error)
405 {
406 	struct task_struct *tsk = current;
407 	TRAP_HANDLER_DECL;
408 
409 	__unlazy_fpu(tsk, regs);
410 	fpu_exception_flags = 0;
411 	if (ieee_fpe_handler(regs)) {
412 		tsk->thread.xstate->hardfpu.fpscr &=
413 		    ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK);
414 		tsk->thread.xstate->hardfpu.fpscr |= fpu_exception_flags;
415 		/* Set the FPSCR flag as well as cause bits - simply
416 		 * replicate the cause */
417 		tsk->thread.xstate->hardfpu.fpscr |= (fpu_exception_flags >> 10);
418 		grab_fpu(regs);
419 		restore_fpu(tsk);
420 		task_thread_info(tsk)->status |= TS_USEDFPU;
421 		if ((((tsk->thread.xstate->hardfpu.fpscr & FPSCR_ENABLE_MASK) >> 7) &
422 		     (fpu_exception_flags >> 2)) == 0) {
423 			return;
424 		}
425 	}
426 
427 	force_sig(SIGFPE, tsk);
428 }
429