1// SPDX-License-Identifier: GPL-2.0
2// Copyright (C) 2005-2018 Andes Technology Corporation
3
4#include <asm/bitfield.h>
5#include <asm/uaccess.h>
6#include <asm/sfp-machine.h>
7#include <asm/fpuemu.h>
8#include <asm/nds32_fpu_inst.h>
9
10#define DPFROMREG(dp, x) (dp = (void *)((unsigned long *)fpu_reg + 2*x))
11#ifdef __NDS32_EL__
12#define SPFROMREG(sp, x)\
13	((sp) = (void *)((unsigned long *)fpu_reg + (x^1)))
14#else
15#define SPFROMREG(sp, x) ((sp) = (void *)((unsigned long *)fpu_reg + x))
16#endif
17
18#define DEF3OP(name, p, f1, f2) \
19void fpemu_##name##p(void *ft, void *fa, void *fb) \
20{ \
21	f1(fa, fa, fb); \
22	f2(ft, ft, fa); \
23}
24
25#define DEF3OPNEG(name, p, f1, f2, f3) \
26void fpemu_##name##p(void *ft, void *fa, void *fb) \
27{ \
28	f1(fa, fa, fb); \
29	f2(ft, ft, fa); \
30	f3(ft, ft); \
31}
32DEF3OP(fmadd, s, fmuls, fadds);
33DEF3OP(fmsub, s, fmuls, fsubs);
34DEF3OP(fmadd, d, fmuld, faddd);
35DEF3OP(fmsub, d, fmuld, fsubd);
36DEF3OPNEG(fnmadd, s, fmuls, fadds, fnegs);
37DEF3OPNEG(fnmsub, s, fmuls, fsubs, fnegs);
38DEF3OPNEG(fnmadd, d, fmuld, faddd, fnegd);
39DEF3OPNEG(fnmsub, d, fmuld, fsubd, fnegd);
40
41static const unsigned char cmptab[8] = {
42	SF_CEQ,
43	SF_CEQ,
44	SF_CLT,
45	SF_CLT,
46	SF_CLT | SF_CEQ,
47	SF_CLT | SF_CEQ,
48	SF_CUN,
49	SF_CUN
50};
51
52enum ARGTYPE {
53	S1S = 1,
54	S2S,
55	S1D,
56	CS,
57	D1D,
58	D2D,
59	D1S,
60	CD
61};
62union func_t {
63	void (*t)(void *ft, void *fa, void *fb);
64	void (*b)(void *ft, void *fa);
65};
66/*
67 * Emulate a single FPU arithmetic instruction.
68 */
69static int fpu_emu(struct fpu_struct *fpu_reg, unsigned long insn)
70{
71	int rfmt;		/* resulting format */
72	union func_t func;
73	int ftype = 0;
74
75	switch (rfmt = NDS32Insn_OPCODE_COP0(insn)) {
76	case fs1_op:{
77			switch (NDS32Insn_OPCODE_BIT69(insn)) {
78			case fadds_op:
79				func.t = fadds;
80				ftype = S2S;
81				break;
82			case fsubs_op:
83				func.t = fsubs;
84				ftype = S2S;
85				break;
86			case fmadds_op:
87				func.t = fpemu_fmadds;
88				ftype = S2S;
89				break;
90			case fmsubs_op:
91				func.t = fpemu_fmsubs;
92				ftype = S2S;
93				break;
94			case fnmadds_op:
95				func.t = fpemu_fnmadds;
96				ftype = S2S;
97				break;
98			case fnmsubs_op:
99				func.t = fpemu_fnmsubs;
100				ftype = S2S;
101				break;
102			case fmuls_op:
103				func.t = fmuls;
104				ftype = S2S;
105				break;
106			case fdivs_op:
107				func.t = fdivs;
108				ftype = S2S;
109				break;
110			case fs1_f2op_op:
111				switch (NDS32Insn_OPCODE_BIT1014(insn)) {
112				case fs2d_op:
113					func.b = fs2d;
114					ftype = S1D;
115					break;
116				case fs2si_op:
117					func.b = fs2si;
118					ftype = S1S;
119					break;
120				case fs2si_z_op:
121					func.b = fs2si_z;
122					ftype = S1S;
123					break;
124				case fs2ui_op:
125					func.b = fs2ui;
126					ftype = S1S;
127					break;
128				case fs2ui_z_op:
129					func.b = fs2ui_z;
130					ftype = S1S;
131					break;
132				case fsi2s_op:
133					func.b = fsi2s;
134					ftype = S1S;
135					break;
136				case fui2s_op:
137					func.b = fui2s;
138					ftype = S1S;
139					break;
140				case fsqrts_op:
141					func.b = fsqrts;
142					ftype = S1S;
143					break;
144				default:
145					return SIGILL;
146				}
147				break;
148			default:
149				return SIGILL;
150			}
151			break;
152		}
153	case fs2_op:
154		switch (NDS32Insn_OPCODE_BIT69(insn)) {
155		case fcmpeqs_op:
156		case fcmpeqs_e_op:
157		case fcmplts_op:
158		case fcmplts_e_op:
159		case fcmples_op:
160		case fcmples_e_op:
161		case fcmpuns_op:
162		case fcmpuns_e_op:
163			ftype = CS;
164			break;
165		default:
166			return SIGILL;
167		}
168		break;
169	case fd1_op:{
170			switch (NDS32Insn_OPCODE_BIT69(insn)) {
171			case faddd_op:
172				func.t = faddd;
173				ftype = D2D;
174				break;
175			case fsubd_op:
176				func.t = fsubd;
177				ftype = D2D;
178				break;
179			case fmaddd_op:
180				func.t = fpemu_fmaddd;
181				ftype = D2D;
182				break;
183			case fmsubd_op:
184				func.t = fpemu_fmsubd;
185				ftype = D2D;
186				break;
187			case fnmaddd_op:
188				func.t = fpemu_fnmaddd;
189				ftype = D2D;
190				break;
191			case fnmsubd_op:
192				func.t = fpemu_fnmsubd;
193				ftype = D2D;
194				break;
195			case fmuld_op:
196				func.t = fmuld;
197				ftype = D2D;
198				break;
199			case fdivd_op:
200				func.t = fdivd;
201				ftype = D2D;
202				break;
203			case fd1_f2op_op:
204				switch (NDS32Insn_OPCODE_BIT1014(insn)) {
205				case fd2s_op:
206					func.b = fd2s;
207					ftype = D1S;
208					break;
209				case fd2si_op:
210					func.b = fd2si;
211					ftype = D1S;
212					break;
213				case fd2si_z_op:
214					func.b = fd2si_z;
215					ftype = D1S;
216					break;
217				case fd2ui_op:
218					func.b = fd2ui;
219					ftype = D1S;
220					break;
221				case fd2ui_z_op:
222					func.b = fd2ui_z;
223					ftype = D1S;
224					break;
225				case fsi2d_op:
226					func.b = fsi2d;
227					ftype = D1S;
228					break;
229				case fui2d_op:
230					func.b = fui2d;
231					ftype = D1S;
232					break;
233				case fsqrtd_op:
234					func.b = fsqrtd;
235					ftype = D1D;
236					break;
237				default:
238					return SIGILL;
239				}
240				break;
241			default:
242				return SIGILL;
243
244			}
245			break;
246		}
247
248	case fd2_op:
249		switch (NDS32Insn_OPCODE_BIT69(insn)) {
250		case fcmpeqd_op:
251		case fcmpeqd_e_op:
252		case fcmpltd_op:
253		case fcmpltd_e_op:
254		case fcmpled_op:
255		case fcmpled_e_op:
256		case fcmpund_op:
257		case fcmpund_e_op:
258			ftype = CD;
259			break;
260		default:
261			return SIGILL;
262		}
263		break;
264
265	default:
266		return SIGILL;
267	}
268
269	switch (ftype) {
270	case S1S:{
271			void *ft, *fa;
272
273			SPFROMREG(ft, NDS32Insn_OPCODE_Rt(insn));
274			SPFROMREG(fa, NDS32Insn_OPCODE_Ra(insn));
275			func.b(ft, fa);
276			break;
277		}
278	case S2S:{
279			void *ft, *fa, *fb;
280
281			SPFROMREG(ft, NDS32Insn_OPCODE_Rt(insn));
282			SPFROMREG(fa, NDS32Insn_OPCODE_Ra(insn));
283			SPFROMREG(fb, NDS32Insn_OPCODE_Rb(insn));
284			func.t(ft, fa, fb);
285			break;
286		}
287	case S1D:{
288			void *ft, *fa;
289
290			DPFROMREG(ft, NDS32Insn_OPCODE_Rt(insn));
291			SPFROMREG(fa, NDS32Insn_OPCODE_Ra(insn));
292			func.b(ft, fa);
293			break;
294		}
295	case CS:{
296			unsigned int cmpop = NDS32Insn_OPCODE_BIT69(insn);
297			void *ft, *fa, *fb;
298
299			SPFROMREG(ft, NDS32Insn_OPCODE_Rt(insn));
300			SPFROMREG(fa, NDS32Insn_OPCODE_Ra(insn));
301			SPFROMREG(fb, NDS32Insn_OPCODE_Rb(insn));
302			if (cmpop < 0x8) {
303				cmpop = cmptab[cmpop];
304				fcmps(ft, fa, fb, cmpop);
305			} else
306				return SIGILL;
307			break;
308		}
309	case D1D:{
310			void *ft, *fa;
311
312			DPFROMREG(ft, NDS32Insn_OPCODE_Rt(insn));
313			DPFROMREG(fa, NDS32Insn_OPCODE_Ra(insn));
314			func.b(ft, fa);
315			break;
316		}
317	case D2D:{
318			void *ft, *fa, *fb;
319
320			DPFROMREG(ft, NDS32Insn_OPCODE_Rt(insn));
321			DPFROMREG(fa, NDS32Insn_OPCODE_Ra(insn));
322			DPFROMREG(fb, NDS32Insn_OPCODE_Rb(insn));
323			func.t(ft, fa, fb);
324			break;
325		}
326	case D1S:{
327			void *ft, *fa;
328
329			SPFROMREG(ft, NDS32Insn_OPCODE_Rt(insn));
330			DPFROMREG(fa, NDS32Insn_OPCODE_Ra(insn));
331			func.b(ft, fa);
332			break;
333		}
334	case CD:{
335			unsigned int cmpop = NDS32Insn_OPCODE_BIT69(insn);
336			void *ft, *fa, *fb;
337
338			SPFROMREG(ft, NDS32Insn_OPCODE_Rt(insn));
339			DPFROMREG(fa, NDS32Insn_OPCODE_Ra(insn));
340			DPFROMREG(fb, NDS32Insn_OPCODE_Rb(insn));
341			if (cmpop < 0x8) {
342				cmpop = cmptab[cmpop];
343				fcmpd(ft, fa, fb, cmpop);
344			} else
345				return SIGILL;
346			break;
347		}
348	default:
349		return SIGILL;
350	}
351
352	/*
353	 * If an exception is required, generate a tidy SIGFPE exception.
354	 */
355#if IS_ENABLED(CONFIG_SUPPORT_DENORMAL_ARITHMETIC)
356	if (((fpu_reg->fpcsr << 5) & fpu_reg->fpcsr & FPCSR_mskALLE_NO_UDF_IEXE)
357	    || ((fpu_reg->fpcsr << 5) & (fpu_reg->UDF_IEX_trap))) {
358#else
359	if ((fpu_reg->fpcsr << 5) & fpu_reg->fpcsr & FPCSR_mskALLE) {
360#endif
361		return SIGFPE;
362	}
363	return 0;
364}
365
366int do_fpuemu(struct pt_regs *regs, struct fpu_struct *fpu)
367{
368	unsigned long insn = 0, addr = regs->ipc;
369	unsigned long emulpc, contpc;
370	unsigned char *pc = (void *)&insn;
371	char c;
372	int i = 0, ret;
373
374	for (i = 0; i < 4; i++) {
375		if (__get_user(c, (unsigned char *)addr++))
376			return SIGBUS;
377		*pc++ = c;
378	}
379
380	insn = be32_to_cpu(insn);
381
382	emulpc = regs->ipc;
383	contpc = regs->ipc + 4;
384
385	if (NDS32Insn_OPCODE(insn) != cop0_op)
386		return SIGILL;
387
388	switch (NDS32Insn_OPCODE_COP0(insn)) {
389	case fs1_op:
390	case fs2_op:
391	case fd1_op:
392	case fd2_op:
393		{
394			/* a real fpu computation instruction */
395			ret = fpu_emu(fpu, insn);
396			if (!ret)
397				regs->ipc = contpc;
398		}
399		break;
400
401	default:
402		return SIGILL;
403	}
404
405	return ret;
406}
407