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