1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) 1999  Eddie C. Dost  (ecd@atecom.com)
4  */
5 
6 #include <linux/types.h>
7 #include <linux/sched.h>
8 
9 #include <linux/uaccess.h>
10 #include <asm/reg.h>
11 #include <asm/switch_to.h>
12 
13 #include <asm/sfp-machine.h>
14 #include <math-emu/double.h>
15 
16 #define FLOATFUNC(x)	extern int x(void *, void *, void *, void *)
17 
18 /* The instructions list which may be not implemented by a hardware FPU */
19 FLOATFUNC(fre);
20 FLOATFUNC(frsqrtes);
21 FLOATFUNC(fsqrt);
22 FLOATFUNC(fsqrts);
23 FLOATFUNC(mtfsf);
24 FLOATFUNC(mtfsfi);
25 
26 #ifdef CONFIG_MATH_EMULATION_HW_UNIMPLEMENTED
27 #undef FLOATFUNC(x)
28 #define FLOATFUNC(x)	static inline int x(void *op1, void *op2, void *op3, \
29 						 void *op4) { }
30 #endif
31 
32 FLOATFUNC(fadd);
33 FLOATFUNC(fadds);
34 FLOATFUNC(fdiv);
35 FLOATFUNC(fdivs);
36 FLOATFUNC(fmul);
37 FLOATFUNC(fmuls);
38 FLOATFUNC(fsub);
39 FLOATFUNC(fsubs);
40 
41 FLOATFUNC(fmadd);
42 FLOATFUNC(fmadds);
43 FLOATFUNC(fmsub);
44 FLOATFUNC(fmsubs);
45 FLOATFUNC(fnmadd);
46 FLOATFUNC(fnmadds);
47 FLOATFUNC(fnmsub);
48 FLOATFUNC(fnmsubs);
49 
50 FLOATFUNC(fctiw);
51 FLOATFUNC(fctiwz);
52 FLOATFUNC(frsp);
53 
54 FLOATFUNC(fcmpo);
55 FLOATFUNC(fcmpu);
56 
57 FLOATFUNC(mcrfs);
58 FLOATFUNC(mffs);
59 FLOATFUNC(mtfsb0);
60 FLOATFUNC(mtfsb1);
61 
62 FLOATFUNC(lfd);
63 FLOATFUNC(lfs);
64 
65 FLOATFUNC(stfd);
66 FLOATFUNC(stfs);
67 FLOATFUNC(stfiwx);
68 
69 FLOATFUNC(fabs);
70 FLOATFUNC(fmr);
71 FLOATFUNC(fnabs);
72 FLOATFUNC(fneg);
73 
74 /* Optional */
75 FLOATFUNC(fres);
76 FLOATFUNC(frsqrte);
77 FLOATFUNC(fsel);
78 
79 
80 #define OP31		0x1f		/*   31 */
81 #define LFS		0x30		/*   48 */
82 #define LFSU		0x31		/*   49 */
83 #define LFD		0x32		/*   50 */
84 #define LFDU		0x33		/*   51 */
85 #define STFS		0x34		/*   52 */
86 #define STFSU		0x35		/*   53 */
87 #define STFD		0x36		/*   54 */
88 #define STFDU		0x37		/*   55 */
89 #define OP59		0x3b		/*   59 */
90 #define OP63		0x3f		/*   63 */
91 
92 /* Opcode 31: */
93 /* X-Form: */
94 #define LFSX		0x217		/*  535 */
95 #define LFSUX		0x237		/*  567 */
96 #define LFDX		0x257		/*  599 */
97 #define LFDUX		0x277		/*  631 */
98 #define STFSX		0x297		/*  663 */
99 #define STFSUX		0x2b7		/*  695 */
100 #define STFDX		0x2d7		/*  727 */
101 #define STFDUX		0x2f7		/*  759 */
102 #define STFIWX		0x3d7		/*  983 */
103 
104 /* Opcode 59: */
105 /* A-Form: */
106 #define FDIVS		0x012		/*   18 */
107 #define FSUBS		0x014		/*   20 */
108 #define FADDS		0x015		/*   21 */
109 #define FSQRTS		0x016		/*   22 */
110 #define FRES		0x018		/*   24 */
111 #define FMULS		0x019		/*   25 */
112 #define FRSQRTES	0x01a		/*   26 */
113 #define FMSUBS		0x01c		/*   28 */
114 #define FMADDS		0x01d		/*   29 */
115 #define FNMSUBS		0x01e		/*   30 */
116 #define FNMADDS		0x01f		/*   31 */
117 
118 /* Opcode 63: */
119 /* A-Form: */
120 #define FDIV		0x012		/*   18 */
121 #define FSUB		0x014		/*   20 */
122 #define FADD		0x015		/*   21 */
123 #define FSQRT		0x016		/*   22 */
124 #define FSEL		0x017		/*   23 */
125 #define FRE		0x018		/*   24 */
126 #define FMUL		0x019		/*   25 */
127 #define FRSQRTE		0x01a		/*   26 */
128 #define FMSUB		0x01c		/*   28 */
129 #define FMADD		0x01d		/*   29 */
130 #define FNMSUB		0x01e		/*   30 */
131 #define FNMADD		0x01f		/*   31 */
132 
133 /* X-Form: */
134 #define FCMPU		0x000		/*    0	*/
135 #define FRSP		0x00c		/*   12 */
136 #define FCTIW		0x00e		/*   14 */
137 #define FCTIWZ		0x00f		/*   15 */
138 #define FCMPO		0x020		/*   32 */
139 #define MTFSB1		0x026		/*   38 */
140 #define FNEG		0x028		/*   40 */
141 #define MCRFS		0x040		/*   64 */
142 #define MTFSB0		0x046		/*   70 */
143 #define FMR		0x048		/*   72 */
144 #define MTFSFI		0x086		/*  134 */
145 #define FNABS		0x088		/*  136 */
146 #define FABS		0x108		/*  264 */
147 #define MFFS		0x247		/*  583 */
148 #define MTFSF		0x2c7		/*  711 */
149 
150 
151 #define AB	2
152 #define AC	3
153 #define ABC	4
154 #define D	5
155 #define DU	6
156 #define X	7
157 #define XA	8
158 #define XB	9
159 #define XCR	11
160 #define XCRB	12
161 #define XCRI	13
162 #define XCRL	16
163 #define XE	14
164 #define XEU	15
165 #define XFLB	10
166 
167 static int
record_exception(struct pt_regs * regs,int eflag)168 record_exception(struct pt_regs *regs, int eflag)
169 {
170 	u32 fpscr;
171 
172 	fpscr = __FPU_FPSCR;
173 
174 	if (eflag) {
175 		fpscr |= FPSCR_FX;
176 		if (eflag & EFLAG_OVERFLOW)
177 			fpscr |= FPSCR_OX;
178 		if (eflag & EFLAG_UNDERFLOW)
179 			fpscr |= FPSCR_UX;
180 		if (eflag & EFLAG_DIVZERO)
181 			fpscr |= FPSCR_ZX;
182 		if (eflag & EFLAG_INEXACT)
183 			fpscr |= FPSCR_XX;
184 		if (eflag & EFLAG_INVALID)
185 			fpscr |= FPSCR_VX;
186 		if (eflag & EFLAG_VXSNAN)
187 			fpscr |= FPSCR_VXSNAN;
188 		if (eflag & EFLAG_VXISI)
189 			fpscr |= FPSCR_VXISI;
190 		if (eflag & EFLAG_VXIDI)
191 			fpscr |= FPSCR_VXIDI;
192 		if (eflag & EFLAG_VXZDZ)
193 			fpscr |= FPSCR_VXZDZ;
194 		if (eflag & EFLAG_VXIMZ)
195 			fpscr |= FPSCR_VXIMZ;
196 		if (eflag & EFLAG_VXVC)
197 			fpscr |= FPSCR_VXVC;
198 		if (eflag & EFLAG_VXSOFT)
199 			fpscr |= FPSCR_VXSOFT;
200 		if (eflag & EFLAG_VXSQRT)
201 			fpscr |= FPSCR_VXSQRT;
202 		if (eflag & EFLAG_VXCVI)
203 			fpscr |= FPSCR_VXCVI;
204 	}
205 
206 //	fpscr &= ~(FPSCR_VX);
207 	if (fpscr & (FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI |
208 		     FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC |
209 		     FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI))
210 		fpscr |= FPSCR_VX;
211 
212 	fpscr &= ~(FPSCR_FEX);
213 	if (((fpscr & FPSCR_VX) && (fpscr & FPSCR_VE)) ||
214 	    ((fpscr & FPSCR_OX) && (fpscr & FPSCR_OE)) ||
215 	    ((fpscr & FPSCR_UX) && (fpscr & FPSCR_UE)) ||
216 	    ((fpscr & FPSCR_ZX) && (fpscr & FPSCR_ZE)) ||
217 	    ((fpscr & FPSCR_XX) && (fpscr & FPSCR_XE)))
218 		fpscr |= FPSCR_FEX;
219 
220 	__FPU_FPSCR = fpscr;
221 
222 	return (fpscr & FPSCR_FEX) ? 1 : 0;
223 }
224 
225 int
do_mathemu(struct pt_regs * regs)226 do_mathemu(struct pt_regs *regs)
227 {
228 	void *op0 = 0, *op1 = 0, *op2 = 0, *op3 = 0;
229 	unsigned long pc = regs->nip;
230 	signed short sdisp;
231 	u32 insn = 0;
232 	int idx = 0;
233 	int (*func)(void *, void *, void *, void *);
234 	int type = 0;
235 	int eflag, trap;
236 
237 	if (get_user(insn, (u32 *)pc))
238 		return -EFAULT;
239 
240 	switch (insn >> 26) {
241 	case LFS:	func = lfs;	type = D;	break;
242 	case LFSU:	func = lfs;	type = DU;	break;
243 	case LFD:	func = lfd;	type = D;	break;
244 	case LFDU:	func = lfd;	type = DU;	break;
245 	case STFS:	func = stfs;	type = D;	break;
246 	case STFSU:	func = stfs;	type = DU;	break;
247 	case STFD:	func = stfd;	type = D;	break;
248 	case STFDU:	func = stfd;	type = DU;	break;
249 
250 	case OP31:
251 		switch ((insn >> 1) & 0x3ff) {
252 		case LFSX:	func = lfs;	type = XE;	break;
253 		case LFSUX:	func = lfs;	type = XEU;	break;
254 		case LFDX:	func = lfd;	type = XE;	break;
255 		case LFDUX:	func = lfd;	type = XEU;	break;
256 		case STFSX:	func = stfs;	type = XE;	break;
257 		case STFSUX:	func = stfs;	type = XEU;	break;
258 		case STFDX:	func = stfd;	type = XE;	break;
259 		case STFDUX:	func = stfd;	type = XEU;	break;
260 		case STFIWX:	func = stfiwx;	type = XE;	break;
261 		default:
262 			goto illegal;
263 		}
264 		break;
265 
266 	case OP59:
267 		switch ((insn >> 1) & 0x1f) {
268 		case FDIVS:	func = fdivs;	type = AB;	break;
269 		case FSUBS:	func = fsubs;	type = AB;	break;
270 		case FADDS:	func = fadds;	type = AB;	break;
271 		case FSQRTS:	func = fsqrts;	type = XB;	break;
272 		case FRES:	func = fres;	type = XB;	break;
273 		case FMULS:	func = fmuls;	type = AC;	break;
274 		case FRSQRTES:	func = frsqrtes;type = XB;	break;
275 		case FMSUBS:	func = fmsubs;	type = ABC;	break;
276 		case FMADDS:	func = fmadds;	type = ABC;	break;
277 		case FNMSUBS:	func = fnmsubs;	type = ABC;	break;
278 		case FNMADDS:	func = fnmadds;	type = ABC;	break;
279 		default:
280 			goto illegal;
281 		}
282 		break;
283 
284 	case OP63:
285 		if (insn & 0x20) {
286 			switch ((insn >> 1) & 0x1f) {
287 			case FDIV:	func = fdiv;	type = AB;	break;
288 			case FSUB:	func = fsub;	type = AB;	break;
289 			case FADD:	func = fadd;	type = AB;	break;
290 			case FSQRT:	func = fsqrt;	type = XB;	break;
291 			case FRE:	func = fre;	type = XB;	break;
292 			case FSEL:	func = fsel;	type = ABC;	break;
293 			case FMUL:	func = fmul;	type = AC;	break;
294 			case FRSQRTE:	func = frsqrte;	type = XB;	break;
295 			case FMSUB:	func = fmsub;	type = ABC;	break;
296 			case FMADD:	func = fmadd;	type = ABC;	break;
297 			case FNMSUB:	func = fnmsub;	type = ABC;	break;
298 			case FNMADD:	func = fnmadd;	type = ABC;	break;
299 			default:
300 				goto illegal;
301 			}
302 			break;
303 		}
304 
305 		switch ((insn >> 1) & 0x3ff) {
306 		case FCMPU:	func = fcmpu;	type = XCR;	break;
307 		case FRSP:	func = frsp;	type = XB;	break;
308 		case FCTIW:	func = fctiw;	type = XB;	break;
309 		case FCTIWZ:	func = fctiwz;	type = XB;	break;
310 		case FCMPO:	func = fcmpo;	type = XCR;	break;
311 		case MTFSB1:	func = mtfsb1;	type = XCRB;	break;
312 		case FNEG:	func = fneg;	type = XB;	break;
313 		case MCRFS:	func = mcrfs;	type = XCRL;	break;
314 		case MTFSB0:	func = mtfsb0;	type = XCRB;	break;
315 		case FMR:	func = fmr;	type = XB;	break;
316 		case MTFSFI:	func = mtfsfi;	type = XCRI;	break;
317 		case FNABS:	func = fnabs;	type = XB;	break;
318 		case FABS:	func = fabs;	type = XB;	break;
319 		case MFFS:	func = mffs;	type = X;	break;
320 		case MTFSF:	func = mtfsf;	type = XFLB;	break;
321 		default:
322 			goto illegal;
323 		}
324 		break;
325 
326 	default:
327 		goto illegal;
328 	}
329 
330 	switch (type) {
331 	case AB:
332 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
333 		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
334 		op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
335 		break;
336 
337 	case AC:
338 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
339 		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
340 		op2 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
341 		break;
342 
343 	case ABC:
344 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
345 		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
346 		op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
347 		op3 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
348 		break;
349 
350 	case D:
351 		idx = (insn >> 16) & 0x1f;
352 		sdisp = (insn & 0xffff);
353 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
354 		op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp);
355 		break;
356 
357 	case DU:
358 		idx = (insn >> 16) & 0x1f;
359 		if (!idx)
360 			goto illegal;
361 
362 		sdisp = (insn & 0xffff);
363 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
364 		op1 = (void *)(regs->gpr[idx] + sdisp);
365 		break;
366 
367 	case X:
368 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
369 		break;
370 
371 	case XA:
372 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
373 		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
374 		break;
375 
376 	case XB:
377 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
378 		op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
379 		break;
380 
381 	case XE:
382 		idx = (insn >> 16) & 0x1f;
383 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
384 		op1 = (void *)((idx ? regs->gpr[idx] : 0)
385 				+ regs->gpr[(insn >> 11) & 0x1f]);
386 		break;
387 
388 	case XEU:
389 		idx = (insn >> 16) & 0x1f;
390 		if (!idx)
391 			goto illegal;
392 		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
393 		op1 = (void *)(regs->gpr[idx]
394 				+ regs->gpr[(insn >> 11) & 0x1f]);
395 		break;
396 
397 	case XCR:
398 		op0 = (void *)&regs->ccr;
399 		op1 = (void *)((insn >> 23) & 0x7);
400 		op2 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
401 		op3 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
402 		break;
403 
404 	case XCRL:
405 		op0 = (void *)&regs->ccr;
406 		op1 = (void *)((insn >> 23) & 0x7);
407 		op2 = (void *)((insn >> 18) & 0x7);
408 		break;
409 
410 	case XCRB:
411 		op0 = (void *)((insn >> 21) & 0x1f);
412 		break;
413 
414 	case XCRI:
415 		op0 = (void *)((insn >> 23) & 0x7);
416 		op1 = (void *)((insn >> 12) & 0xf);
417 		break;
418 
419 	case XFLB:
420 		op0 = (void *)((insn >> 17) & 0xff);
421 		op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
422 		break;
423 
424 	default:
425 		goto illegal;
426 	}
427 
428 	/*
429 	 * If we support a HW FPU, we need to ensure the FP state
430 	 * is flushed into the thread_struct before attempting
431 	 * emulation
432 	 */
433 	flush_fp_to_thread(current);
434 
435 	eflag = func(op0, op1, op2, op3);
436 
437 	if (insn & 1) {
438 		regs->ccr &= ~(0x0f000000);
439 		regs->ccr |= (__FPU_FPSCR >> 4) & 0x0f000000;
440 	}
441 
442 	trap = record_exception(regs, eflag);
443 	if (trap)
444 		return 1;
445 
446 	switch (type) {
447 	case DU:
448 	case XEU:
449 		regs->gpr[idx] = (unsigned long)op1;
450 		break;
451 
452 	default:
453 		break;
454 	}
455 
456 	regs->nip += 4;
457 	return 0;
458 
459 illegal:
460 	return -ENOSYS;
461 }
462