1
2
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
68
69static int fpu_emu(struct fpu_struct *fpu_reg, unsigned long insn)
70{
71 int rfmt;
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
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
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
