• Home
  • History
  • Annotate
  • Line#
  • Navigate
  • Raw
  • Download
  • only in /netgear-R7000-V1.0.7.12_1.2.5/components/opensource/linux/linux-2.6.36/arch/powerpc/math-emu/
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
16FLOATFUNC(fadd);
17FLOATFUNC(fadds);
18FLOATFUNC(fdiv);
19FLOATFUNC(fdivs);
20FLOATFUNC(fmul);
21FLOATFUNC(fmuls);
22FLOATFUNC(fsub);
23FLOATFUNC(fsubs);
24
25FLOATFUNC(fmadd);
26FLOATFUNC(fmadds);
27FLOATFUNC(fmsub);
28FLOATFUNC(fmsubs);
29FLOATFUNC(fnmadd);
30FLOATFUNC(fnmadds);
31FLOATFUNC(fnmsub);
32FLOATFUNC(fnmsubs);
33
34FLOATFUNC(fctiw);
35FLOATFUNC(fctiwz);
36FLOATFUNC(frsp);
37
38FLOATFUNC(fcmpo);
39FLOATFUNC(fcmpu);
40
41FLOATFUNC(mcrfs);
42FLOATFUNC(mffs);
43FLOATFUNC(mtfsb0);
44FLOATFUNC(mtfsb1);
45FLOATFUNC(mtfsf);
46FLOATFUNC(mtfsfi);
47
48FLOATFUNC(lfd);
49FLOATFUNC(lfs);
50
51FLOATFUNC(stfd);
52FLOATFUNC(stfs);
53FLOATFUNC(stfiwx);
54
55FLOATFUNC(fabs);
56FLOATFUNC(fmr);
57FLOATFUNC(fnabs);
58FLOATFUNC(fneg);
59
60/* Optional */
61FLOATFUNC(fres);
62FLOATFUNC(frsqrte);
63FLOATFUNC(fsel);
64FLOATFUNC(fsqrt);
65FLOATFUNC(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
154static int
155record_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
213int
214do_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
487illegal:
488	return -ENOSYS;
489}
490