xref: /openbmc/linux/arch/sh/kernel/cpu/sh2a/fpu.c (revision 3cf5d076)
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Save/restore floating point context for signal handlers.
4  *
5  * Copyright (C) 1999, 2000  Kaz Kojima & Niibe Yutaka
6  *
7  * FIXME! These routines can be optimized in big endian case.
8  */
9 #include <linux/sched/signal.h>
10 #include <linux/signal.h>
11 #include <asm/processor.h>
12 #include <asm/io.h>
13 #include <asm/fpu.h>
14 #include <asm/traps.h>
15 
16 /* The PR (precision) bit in the FP Status Register must be clear when
17  * an frchg instruction is executed, otherwise the instruction is undefined.
18  * Executing frchg with PR set causes a trap on some SH4 implementations.
19  */
20 
21 #define FPSCR_RCHG 0x00000000
22 
23 
24 /*
25  * Save FPU registers onto task structure.
26  */
save_fpu(struct task_struct * tsk)27 void save_fpu(struct task_struct *tsk)
28 {
29 	unsigned long dummy;
30 
31 	enable_fpu();
32 	asm volatile("sts.l	fpul, @-%0\n\t"
33 		     "sts.l	fpscr, @-%0\n\t"
34 		     "fmov.s	fr15, @-%0\n\t"
35 		     "fmov.s	fr14, @-%0\n\t"
36 		     "fmov.s	fr13, @-%0\n\t"
37 		     "fmov.s	fr12, @-%0\n\t"
38 		     "fmov.s	fr11, @-%0\n\t"
39 		     "fmov.s	fr10, @-%0\n\t"
40 		     "fmov.s	fr9, @-%0\n\t"
41 		     "fmov.s	fr8, @-%0\n\t"
42 		     "fmov.s	fr7, @-%0\n\t"
43 		     "fmov.s	fr6, @-%0\n\t"
44 		     "fmov.s	fr5, @-%0\n\t"
45 		     "fmov.s	fr4, @-%0\n\t"
46 		     "fmov.s	fr3, @-%0\n\t"
47 		     "fmov.s	fr2, @-%0\n\t"
48 		     "fmov.s	fr1, @-%0\n\t"
49 		     "fmov.s	fr0, @-%0\n\t"
50 		     "lds	%3, fpscr\n\t"
51 		     : "=r" (dummy)
52 		     : "0" ((char *)(&tsk->thread.xstate->hardfpu.status)),
53 		       "r" (FPSCR_RCHG),
54 		       "r" (FPSCR_INIT)
55 		     : "memory");
56 
57 	disable_fpu();
58 }
59 
restore_fpu(struct task_struct * tsk)60 void restore_fpu(struct task_struct *tsk)
61 {
62 	unsigned long dummy;
63 
64 	enable_fpu();
65 	asm volatile("fmov.s	@%0+, fr0\n\t"
66 		     "fmov.s	@%0+, fr1\n\t"
67 		     "fmov.s	@%0+, fr2\n\t"
68 		     "fmov.s	@%0+, fr3\n\t"
69 		     "fmov.s	@%0+, fr4\n\t"
70 		     "fmov.s	@%0+, fr5\n\t"
71 		     "fmov.s	@%0+, fr6\n\t"
72 		     "fmov.s	@%0+, fr7\n\t"
73 		     "fmov.s	@%0+, fr8\n\t"
74 		     "fmov.s	@%0+, fr9\n\t"
75 		     "fmov.s	@%0+, fr10\n\t"
76 		     "fmov.s	@%0+, fr11\n\t"
77 		     "fmov.s	@%0+, fr12\n\t"
78 		     "fmov.s	@%0+, fr13\n\t"
79 		     "fmov.s	@%0+, fr14\n\t"
80 		     "fmov.s	@%0+, fr15\n\t"
81 		     "lds.l	@%0+, fpscr\n\t"
82 		     "lds.l	@%0+, fpul\n\t"
83 		     : "=r" (dummy)
84 		     : "0" (tsk->thread.xstate), "r" (FPSCR_RCHG)
85 		     : "memory");
86 	disable_fpu();
87 }
88 
89 /*
90  *	Emulate arithmetic ops on denormalized number for some FPU insns.
91  */
92 
93 /* denormalized float * float */
denormal_mulf(int hx,int hy)94 static int denormal_mulf(int hx, int hy)
95 {
96 	unsigned int ix, iy;
97 	unsigned long long m, n;
98 	int exp, w;
99 
100 	ix = hx & 0x7fffffff;
101 	iy = hy & 0x7fffffff;
102 	if (iy < 0x00800000 || ix == 0)
103 		return ((hx ^ hy) & 0x80000000);
104 
105 	exp = (iy & 0x7f800000) >> 23;
106 	ix &= 0x007fffff;
107 	iy = (iy & 0x007fffff) | 0x00800000;
108 	m = (unsigned long long)ix * iy;
109 	n = m;
110 	w = -1;
111 	while (n) { n >>= 1; w++; }
112 
113 	/* FIXME: use guard bits */
114 	exp += w - 126 - 46;
115 	if (exp > 0)
116 		ix = ((int) (m >> (w - 23)) & 0x007fffff) | (exp << 23);
117 	else if (exp + 22 >= 0)
118 		ix = (int) (m >> (w - 22 - exp)) & 0x007fffff;
119 	else
120 		ix = 0;
121 
122 	ix |= (hx ^ hy) & 0x80000000;
123 	return ix;
124 }
125 
126 /* denormalized double * double */
mult64(unsigned long long x,unsigned long long y,unsigned long long * highp,unsigned long long * lowp)127 static void mult64(unsigned long long x, unsigned long long y,
128 		unsigned long long *highp, unsigned long long *lowp)
129 {
130 	unsigned long long sub0, sub1, sub2, sub3;
131 	unsigned long long high, low;
132 
133 	sub0 = (x >> 32) * (unsigned long) (y >> 32);
134 	sub1 = (x & 0xffffffffLL) * (unsigned long) (y >> 32);
135 	sub2 = (x >> 32) * (unsigned long) (y & 0xffffffffLL);
136 	sub3 = (x & 0xffffffffLL) * (unsigned long) (y & 0xffffffffLL);
137 	low = sub3;
138 	high = 0LL;
139 	sub3 += (sub1 << 32);
140 	if (low > sub3)
141 		high++;
142 	low = sub3;
143 	sub3 += (sub2 << 32);
144 	if (low > sub3)
145 		high++;
146 	low = sub3;
147 	high += (sub1 >> 32) + (sub2 >> 32);
148 	high += sub0;
149 	*lowp = low;
150 	*highp = high;
151 }
152 
rshift64(unsigned long long mh,unsigned long long ml,int n)153 static inline long long rshift64(unsigned long long mh,
154 		unsigned long long ml, int n)
155 {
156 	if (n >= 64)
157 		return mh >> (n - 64);
158 	return (mh << (64 - n)) | (ml >> n);
159 }
160 
denormal_muld(long long hx,long long hy)161 static long long denormal_muld(long long hx, long long hy)
162 {
163 	unsigned long long ix, iy;
164 	unsigned long long mh, ml, nh, nl;
165 	int exp, w;
166 
167 	ix = hx & 0x7fffffffffffffffLL;
168 	iy = hy & 0x7fffffffffffffffLL;
169 	if (iy < 0x0010000000000000LL || ix == 0)
170 		return ((hx ^ hy) & 0x8000000000000000LL);
171 
172 	exp = (iy & 0x7ff0000000000000LL) >> 52;
173 	ix &= 0x000fffffffffffffLL;
174 	iy = (iy & 0x000fffffffffffffLL) | 0x0010000000000000LL;
175 	mult64(ix, iy, &mh, &ml);
176 	nh = mh;
177 	nl = ml;
178 	w = -1;
179 	if (nh) {
180 		while (nh) { nh >>= 1; w++;}
181 		w += 64;
182 	} else
183 		while (nl) { nl >>= 1; w++;}
184 
185 	/* FIXME: use guard bits */
186 	exp += w - 1022 - 52 * 2;
187 	if (exp > 0)
188 		ix = (rshift64(mh, ml, w - 52) & 0x000fffffffffffffLL)
189 			| ((long long)exp << 52);
190 	else if (exp + 51 >= 0)
191 		ix = rshift64(mh, ml, w - 51 - exp) & 0x000fffffffffffffLL;
192 	else
193 		ix = 0;
194 
195 	ix |= (hx ^ hy) & 0x8000000000000000LL;
196 	return ix;
197 }
198 
199 /* ix - iy where iy: denormal and ix, iy >= 0 */
denormal_subf1(unsigned int ix,unsigned int iy)200 static int denormal_subf1(unsigned int ix, unsigned int iy)
201 {
202 	int frac;
203 	int exp;
204 
205 	if (ix < 0x00800000)
206 		return ix - iy;
207 
208 	exp = (ix & 0x7f800000) >> 23;
209 	if (exp - 1 > 31)
210 		return ix;
211 	iy >>= exp - 1;
212 	if (iy == 0)
213 		return ix;
214 
215 	frac = (ix & 0x007fffff) | 0x00800000;
216 	frac -= iy;
217 	while (frac < 0x00800000) {
218 		if (--exp == 0)
219 			return frac;
220 		frac <<= 1;
221 	}
222 
223 	return (exp << 23) | (frac & 0x007fffff);
224 }
225 
226 /* ix + iy where iy: denormal and ix, iy >= 0 */
denormal_addf1(unsigned int ix,unsigned int iy)227 static int denormal_addf1(unsigned int ix, unsigned int iy)
228 {
229 	int frac;
230 	int exp;
231 
232 	if (ix < 0x00800000)
233 		return ix + iy;
234 
235 	exp = (ix & 0x7f800000) >> 23;
236 	if (exp - 1 > 31)
237 		return ix;
238 	iy >>= exp - 1;
239 	if (iy == 0)
240 	  return ix;
241 
242 	frac = (ix & 0x007fffff) | 0x00800000;
243 	frac += iy;
244 	if (frac >= 0x01000000) {
245 		frac >>= 1;
246 		++exp;
247 	}
248 
249 	return (exp << 23) | (frac & 0x007fffff);
250 }
251 
denormal_addf(int hx,int hy)252 static int denormal_addf(int hx, int hy)
253 {
254 	unsigned int ix, iy;
255 	int sign;
256 
257 	if ((hx ^ hy) & 0x80000000) {
258 		sign = hx & 0x80000000;
259 		ix = hx & 0x7fffffff;
260 		iy = hy & 0x7fffffff;
261 		if (iy < 0x00800000) {
262 			ix = denormal_subf1(ix, iy);
263 			if ((int) ix < 0) {
264 				ix = -ix;
265 				sign ^= 0x80000000;
266 			}
267 		} else {
268 			ix = denormal_subf1(iy, ix);
269 			sign ^= 0x80000000;
270 		}
271 	} else {
272 		sign = hx & 0x80000000;
273 		ix = hx & 0x7fffffff;
274 		iy = hy & 0x7fffffff;
275 		if (iy < 0x00800000)
276 			ix = denormal_addf1(ix, iy);
277 		else
278 			ix = denormal_addf1(iy, ix);
279 	}
280 
281 	return sign | ix;
282 }
283 
284 /* ix - iy where iy: denormal and ix, iy >= 0 */
denormal_subd1(unsigned long long ix,unsigned long long iy)285 static long long denormal_subd1(unsigned long long ix, unsigned long long iy)
286 {
287 	long long frac;
288 	int exp;
289 
290 	if (ix < 0x0010000000000000LL)
291 		return ix - iy;
292 
293 	exp = (ix & 0x7ff0000000000000LL) >> 52;
294 	if (exp - 1 > 63)
295 		return ix;
296 	iy >>= exp - 1;
297 	if (iy == 0)
298 		return ix;
299 
300 	frac = (ix & 0x000fffffffffffffLL) | 0x0010000000000000LL;
301 	frac -= iy;
302 	while (frac < 0x0010000000000000LL) {
303 		if (--exp == 0)
304 			return frac;
305 		frac <<= 1;
306 	}
307 
308 	return ((long long)exp << 52) | (frac & 0x000fffffffffffffLL);
309 }
310 
311 /* ix + iy where iy: denormal and ix, iy >= 0 */
denormal_addd1(unsigned long long ix,unsigned long long iy)312 static long long denormal_addd1(unsigned long long ix, unsigned long long iy)
313 {
314 	long long frac;
315 	long long exp;
316 
317 	if (ix < 0x0010000000000000LL)
318 		return ix + iy;
319 
320 	exp = (ix & 0x7ff0000000000000LL) >> 52;
321 	if (exp - 1 > 63)
322 		return ix;
323 	iy >>= exp - 1;
324 	if (iy == 0)
325 	  return ix;
326 
327 	frac = (ix & 0x000fffffffffffffLL) | 0x0010000000000000LL;
328 	frac += iy;
329 	if (frac >= 0x0020000000000000LL) {
330 		frac >>= 1;
331 		++exp;
332 	}
333 
334 	return (exp << 52) | (frac & 0x000fffffffffffffLL);
335 }
336 
denormal_addd(long long hx,long long hy)337 static long long denormal_addd(long long hx, long long hy)
338 {
339 	unsigned long long ix, iy;
340 	long long sign;
341 
342 	if ((hx ^ hy) & 0x8000000000000000LL) {
343 		sign = hx & 0x8000000000000000LL;
344 		ix = hx & 0x7fffffffffffffffLL;
345 		iy = hy & 0x7fffffffffffffffLL;
346 		if (iy < 0x0010000000000000LL) {
347 			ix = denormal_subd1(ix, iy);
348 			if ((int) ix < 0) {
349 				ix = -ix;
350 				sign ^= 0x8000000000000000LL;
351 			}
352 		} else {
353 			ix = denormal_subd1(iy, ix);
354 			sign ^= 0x8000000000000000LL;
355 		}
356 	} else {
357 		sign = hx & 0x8000000000000000LL;
358 		ix = hx & 0x7fffffffffffffffLL;
359 		iy = hy & 0x7fffffffffffffffLL;
360 		if (iy < 0x0010000000000000LL)
361 			ix = denormal_addd1(ix, iy);
362 		else
363 			ix = denormal_addd1(iy, ix);
364 	}
365 
366 	return sign | ix;
367 }
368 
369 /**
370  *	denormal_to_double - Given denormalized float number,
371  *	                     store double float
372  *
373  *	@fpu: Pointer to sh_fpu_hard structure
374  *	@n: Index to FP register
375  */
376 static void
denormal_to_double(struct sh_fpu_hard_struct * fpu,int n)377 denormal_to_double (struct sh_fpu_hard_struct *fpu, int n)
378 {
379 	unsigned long du, dl;
380 	unsigned long x = fpu->fpul;
381 	int exp = 1023 - 126;
382 
383 	if (x != 0 && (x & 0x7f800000) == 0) {
384 		du = (x & 0x80000000);
385 		while ((x & 0x00800000) == 0) {
386 			x <<= 1;
387 			exp--;
388 		}
389 		x &= 0x007fffff;
390 		du |= (exp << 20) | (x >> 3);
391 		dl = x << 29;
392 
393 		fpu->fp_regs[n] = du;
394 		fpu->fp_regs[n+1] = dl;
395 	}
396 }
397 
398 /**
399  *	ieee_fpe_handler - Handle denormalized number exception
400  *
401  *	@regs: Pointer to register structure
402  *
403  *	Returns 1 when it's handled (should not cause exception).
404  */
405 static int
ieee_fpe_handler(struct pt_regs * regs)406 ieee_fpe_handler (struct pt_regs *regs)
407 {
408 	unsigned short insn = *(unsigned short *) regs->pc;
409 	unsigned short finsn;
410 	unsigned long nextpc;
411 	int nib[4] = {
412 		(insn >> 12) & 0xf,
413 		(insn >> 8) & 0xf,
414 		(insn >> 4) & 0xf,
415 		insn & 0xf};
416 
417 	if (nib[0] == 0xb ||
418 	    (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb)) /* bsr & jsr */
419 		regs->pr = regs->pc + 4;
420 	if (nib[0] == 0xa || nib[0] == 0xb) { /* bra & bsr */
421 		nextpc = regs->pc + 4 + ((short) ((insn & 0xfff) << 4) >> 3);
422 		finsn = *(unsigned short *) (regs->pc + 2);
423 	} else if (nib[0] == 0x8 && nib[1] == 0xd) { /* bt/s */
424 		if (regs->sr & 1)
425 			nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1);
426 		else
427 			nextpc = regs->pc + 4;
428 		finsn = *(unsigned short *) (regs->pc + 2);
429 	} else if (nib[0] == 0x8 && nib[1] == 0xf) { /* bf/s */
430 		if (regs->sr & 1)
431 			nextpc = regs->pc + 4;
432 		else
433 			nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1);
434 		finsn = *(unsigned short *) (regs->pc + 2);
435 	} else if (nib[0] == 0x4 && nib[3] == 0xb &&
436 		 (nib[2] == 0x0 || nib[2] == 0x2)) { /* jmp & jsr */
437 		nextpc = regs->regs[nib[1]];
438 		finsn = *(unsigned short *) (regs->pc + 2);
439 	} else if (nib[0] == 0x0 && nib[3] == 0x3 &&
440 		 (nib[2] == 0x0 || nib[2] == 0x2)) { /* braf & bsrf */
441 		nextpc = regs->pc + 4 + regs->regs[nib[1]];
442 		finsn = *(unsigned short *) (regs->pc + 2);
443 	} else if (insn == 0x000b) { /* rts */
444 		nextpc = regs->pr;
445 		finsn = *(unsigned short *) (regs->pc + 2);
446 	} else {
447 		nextpc = regs->pc + 2;
448 		finsn = insn;
449 	}
450 
451 #define FPSCR_FPU_ERROR (1 << 17)
452 
453 	if ((finsn & 0xf1ff) == 0xf0ad) { /* fcnvsd */
454 		struct task_struct *tsk = current;
455 
456 		if ((tsk->thread.xstate->hardfpu.fpscr & FPSCR_FPU_ERROR)) {
457 			/* FPU error */
458 			denormal_to_double (&tsk->thread.xstate->hardfpu,
459 					    (finsn >> 8) & 0xf);
460 		} else
461 			return 0;
462 
463 		regs->pc = nextpc;
464 		return 1;
465 	} else if ((finsn & 0xf00f) == 0xf002) { /* fmul */
466 		struct task_struct *tsk = current;
467 		int fpscr;
468 		int n, m, prec;
469 		unsigned int hx, hy;
470 
471 		n = (finsn >> 8) & 0xf;
472 		m = (finsn >> 4) & 0xf;
473 		hx = tsk->thread.xstate->hardfpu.fp_regs[n];
474 		hy = tsk->thread.xstate->hardfpu.fp_regs[m];
475 		fpscr = tsk->thread.xstate->hardfpu.fpscr;
476 		prec = fpscr & (1 << 19);
477 
478 		if ((fpscr & FPSCR_FPU_ERROR)
479 		     && (prec && ((hx & 0x7fffffff) < 0x00100000
480 				   || (hy & 0x7fffffff) < 0x00100000))) {
481 			long long llx, lly;
482 
483 			/* FPU error because of denormal */
484 			llx = ((long long) hx << 32)
485 			       | tsk->thread.xstate->hardfpu.fp_regs[n+1];
486 			lly = ((long long) hy << 32)
487 			       | tsk->thread.xstate->hardfpu.fp_regs[m+1];
488 			if ((hx & 0x7fffffff) >= 0x00100000)
489 				llx = denormal_muld(lly, llx);
490 			else
491 				llx = denormal_muld(llx, lly);
492 			tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
493 			tsk->thread.xstate->hardfpu.fp_regs[n+1] = llx & 0xffffffff;
494 		} else if ((fpscr & FPSCR_FPU_ERROR)
495 		     && (!prec && ((hx & 0x7fffffff) < 0x00800000
496 				   || (hy & 0x7fffffff) < 0x00800000))) {
497 			/* FPU error because of denormal */
498 			if ((hx & 0x7fffffff) >= 0x00800000)
499 				hx = denormal_mulf(hy, hx);
500 			else
501 				hx = denormal_mulf(hx, hy);
502 			tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
503 		} else
504 			return 0;
505 
506 		regs->pc = nextpc;
507 		return 1;
508 	} else if ((finsn & 0xf00e) == 0xf000) { /* fadd, fsub */
509 		struct task_struct *tsk = current;
510 		int fpscr;
511 		int n, m, prec;
512 		unsigned int hx, hy;
513 
514 		n = (finsn >> 8) & 0xf;
515 		m = (finsn >> 4) & 0xf;
516 		hx = tsk->thread.xstate->hardfpu.fp_regs[n];
517 		hy = tsk->thread.xstate->hardfpu.fp_regs[m];
518 		fpscr = tsk->thread.xstate->hardfpu.fpscr;
519 		prec = fpscr & (1 << 19);
520 
521 		if ((fpscr & FPSCR_FPU_ERROR)
522 		     && (prec && ((hx & 0x7fffffff) < 0x00100000
523 				   || (hy & 0x7fffffff) < 0x00100000))) {
524 			long long llx, lly;
525 
526 			/* FPU error because of denormal */
527 			llx = ((long long) hx << 32)
528 			       | tsk->thread.xstate->hardfpu.fp_regs[n+1];
529 			lly = ((long long) hy << 32)
530 			       | tsk->thread.xstate->hardfpu.fp_regs[m+1];
531 			if ((finsn & 0xf00f) == 0xf000)
532 				llx = denormal_addd(llx, lly);
533 			else
534 				llx = denormal_addd(llx, lly ^ (1LL << 63));
535 			tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
536 			tsk->thread.xstate->hardfpu.fp_regs[n+1] = llx & 0xffffffff;
537 		} else if ((fpscr & FPSCR_FPU_ERROR)
538 		     && (!prec && ((hx & 0x7fffffff) < 0x00800000
539 				   || (hy & 0x7fffffff) < 0x00800000))) {
540 			/* FPU error because of denormal */
541 			if ((finsn & 0xf00f) == 0xf000)
542 				hx = denormal_addf(hx, hy);
543 			else
544 				hx = denormal_addf(hx, hy ^ 0x80000000);
545 			tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
546 		} else
547 			return 0;
548 
549 		regs->pc = nextpc;
550 		return 1;
551 	}
552 
553 	return 0;
554 }
555 
BUILD_TRAP_HANDLER(fpu_error)556 BUILD_TRAP_HANDLER(fpu_error)
557 {
558 	struct task_struct *tsk = current;
559 	TRAP_HANDLER_DECL;
560 
561 	__unlazy_fpu(tsk, regs);
562 	if (ieee_fpe_handler(regs)) {
563 		tsk->thread.xstate->hardfpu.fpscr &=
564 			~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK);
565 		grab_fpu(regs);
566 		restore_fpu(tsk);
567 		task_thread_info(tsk)->status |= TS_USEDFPU;
568 		return;
569 	}
570 
571 	force_sig(SIGFPE);
572 }
573