451ab91e |
1 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * |
2 | * Mupen64plus - fpu.c * |
3 | * Mupen64Plus homepage: http://code.google.com/p/mupen64plus/ * |
4 | * Copyright (C) 2010 Ari64 * |
5 | * * |
6 | * This program is free software; you can redistribute it and/or modify * |
7 | * it under the terms of the GNU General Public License as published by * |
8 | * the Free Software Foundation; either version 2 of the License, or * |
9 | * (at your option) any later version. * |
10 | * * |
11 | * This program is distributed in the hope that it will be useful, * |
12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
14 | * GNU General Public License for more details. * |
15 | * * |
16 | * You should have received a copy of the GNU General Public License * |
17 | * along with this program; if not, write to the * |
18 | * Free Software Foundation, Inc., * |
19 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. * |
20 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ |
21 | |
22 | #include <math.h> |
23 | |
24 | #include "r4300.h" |
25 | |
26 | #ifdef _MSC_VER |
27 | #define M64P_FPU_INLINE static __inline |
28 | #include <float.h> |
29 | typedef enum { FE_TONEAREST = 0, FE_TOWARDZERO, FE_UPWARD, FE_DOWNWARD } eRoundType; |
30 | static void fesetround(eRoundType RoundType) |
31 | { |
32 | static const unsigned int msRound[4] = { _RC_NEAR, _RC_CHOP, _RC_UP, _RC_DOWN }; |
33 | unsigned int oldX87, oldSSE2; |
34 | __control87_2(msRound[RoundType], _MCW_RC, &oldX87, &oldSSE2); |
35 | } |
36 | static __inline double round(double x) { return floor(x + 0.5); } |
37 | static __inline float roundf(float x) { return (float) floor(x + 0.5); } |
38 | static __inline double trunc(double x) { return (double) (int) x; } |
39 | static __inline float truncf(float x) { return (float) (int) x; } |
40 | #define isnan _isnan |
41 | #else |
42 | #define M64P_FPU_INLINE static inline |
43 | #include <fenv.h> |
44 | #endif |
45 | |
46 | |
47 | M64P_FPU_INLINE void set_rounding(void) |
48 | { |
49 | switch(rounding_mode) { |
50 | case 0x33F: |
51 | fesetround(FE_TONEAREST); |
52 | break; |
53 | case 0xF3F: |
54 | fesetround(FE_TOWARDZERO); |
55 | break; |
56 | case 0xB3F: |
57 | fesetround(FE_UPWARD); |
58 | break; |
59 | case 0x73F: |
60 | fesetround(FE_DOWNWARD); |
61 | break; |
62 | } |
63 | } |
64 | |
65 | M64P_FPU_INLINE void cvt_s_w(int *source,float *dest) |
66 | { |
67 | set_rounding(); |
68 | *dest = (float) *source; |
69 | } |
70 | M64P_FPU_INLINE void cvt_d_w(int *source,double *dest) |
71 | { |
72 | set_rounding(); |
73 | *dest = (double) *source; |
74 | } |
75 | M64P_FPU_INLINE void cvt_s_l(long long *source,float *dest) |
76 | { |
77 | set_rounding(); |
78 | *dest = (float) *source; |
79 | } |
80 | M64P_FPU_INLINE void cvt_d_l(long long *source,double *dest) |
81 | { |
82 | set_rounding(); |
83 | *dest = (double) *source; |
84 | } |
85 | M64P_FPU_INLINE void cvt_d_s(float *source,double *dest) |
86 | { |
87 | set_rounding(); |
88 | *dest = (double) *source; |
89 | } |
90 | M64P_FPU_INLINE void cvt_s_d(double *source,float *dest) |
91 | { |
92 | set_rounding(); |
93 | *dest = (float) *source; |
94 | } |
95 | |
96 | M64P_FPU_INLINE void round_l_s(float *source,long long *dest) |
97 | { |
98 | *dest = (long long) roundf(*source); |
99 | } |
100 | M64P_FPU_INLINE void round_w_s(float *source,int *dest) |
101 | { |
102 | *dest = (int) roundf(*source); |
103 | } |
104 | M64P_FPU_INLINE void trunc_l_s(float *source,long long *dest) |
105 | { |
106 | *dest = (long long) truncf(*source); |
107 | } |
108 | M64P_FPU_INLINE void trunc_w_s(float *source,int *dest) |
109 | { |
110 | *dest = (int) truncf(*source); |
111 | } |
112 | M64P_FPU_INLINE void ceil_l_s(float *source,long long *dest) |
113 | { |
114 | *dest = (long long) ceilf(*source); |
115 | } |
116 | M64P_FPU_INLINE void ceil_w_s(float *source,int *dest) |
117 | { |
118 | *dest = (int) ceilf(*source); |
119 | } |
120 | M64P_FPU_INLINE void floor_l_s(float *source,long long *dest) |
121 | { |
122 | *dest = (long long) floorf(*source); |
123 | } |
124 | M64P_FPU_INLINE void floor_w_s(float *source,int *dest) |
125 | { |
126 | *dest = (int) floorf(*source); |
127 | } |
128 | |
129 | M64P_FPU_INLINE void round_l_d(double *source,long long *dest) |
130 | { |
131 | *dest = (long long) round(*source); |
132 | } |
133 | M64P_FPU_INLINE void round_w_d(double *source,int *dest) |
134 | { |
135 | *dest = (int) round(*source); |
136 | } |
137 | M64P_FPU_INLINE void trunc_l_d(double *source,long long *dest) |
138 | { |
139 | *dest = (long long) trunc(*source); |
140 | } |
141 | M64P_FPU_INLINE void trunc_w_d(double *source,int *dest) |
142 | { |
143 | *dest = (int) trunc(*source); |
144 | } |
145 | M64P_FPU_INLINE void ceil_l_d(double *source,long long *dest) |
146 | { |
147 | *dest = (long long) ceil(*source); |
148 | } |
149 | M64P_FPU_INLINE void ceil_w_d(double *source,int *dest) |
150 | { |
151 | *dest = (int) ceil(*source); |
152 | } |
153 | M64P_FPU_INLINE void floor_l_d(double *source,long long *dest) |
154 | { |
155 | *dest = (long long) floor(*source); |
156 | } |
157 | M64P_FPU_INLINE void floor_w_d(double *source,int *dest) |
158 | { |
159 | *dest = (int) floor(*source); |
160 | } |
161 | |
162 | M64P_FPU_INLINE void cvt_w_s(float *source,int *dest) |
163 | { |
164 | set_rounding(); |
165 | switch(FCR31&3) |
166 | { |
167 | case 0: round_w_s(source,dest);return; |
168 | case 1: trunc_w_s(source,dest);return; |
169 | case 2: ceil_w_s(source,dest);return; |
170 | case 3: floor_w_s(source,dest);return; |
171 | } |
172 | } |
173 | M64P_FPU_INLINE void cvt_w_d(double *source,int *dest) |
174 | { |
175 | set_rounding(); |
176 | switch(FCR31&3) |
177 | { |
178 | case 0: round_w_d(source,dest);return; |
179 | case 1: trunc_w_d(source,dest);return; |
180 | case 2: ceil_w_d(source,dest);return; |
181 | case 3: floor_w_d(source,dest);return; |
182 | } |
183 | } |
184 | M64P_FPU_INLINE void cvt_l_s(float *source,long long *dest) |
185 | { |
186 | set_rounding(); |
187 | switch(FCR31&3) |
188 | { |
189 | case 0: round_l_s(source,dest);return; |
190 | case 1: trunc_l_s(source,dest);return; |
191 | case 2: ceil_l_s(source,dest);return; |
192 | case 3: floor_l_s(source,dest);return; |
193 | } |
194 | } |
195 | M64P_FPU_INLINE void cvt_l_d(double *source,long long *dest) |
196 | { |
197 | set_rounding(); |
198 | switch(FCR31&3) |
199 | { |
200 | case 0: round_l_d(source,dest);return; |
201 | case 1: trunc_l_d(source,dest);return; |
202 | case 2: ceil_l_d(source,dest);return; |
203 | case 3: floor_l_d(source,dest);return; |
204 | } |
205 | } |
206 | |
207 | M64P_FPU_INLINE void c_f_s() |
208 | { |
209 | FCR31 &= ~0x800000; |
210 | } |
211 | M64P_FPU_INLINE void c_un_s(float *source,float *target) |
212 | { |
213 | FCR31=(isnan(*source) || isnan(*target)) ? FCR31|0x800000 : FCR31&~0x800000; |
214 | } |
215 | |
216 | M64P_FPU_INLINE void c_eq_s(float *source,float *target) |
217 | { |
218 | if (isnan(*source) || isnan(*target)) {FCR31&=~0x800000;return;} |
219 | FCR31 = *source==*target ? FCR31|0x800000 : FCR31&~0x800000; |
220 | } |
221 | M64P_FPU_INLINE void c_ueq_s(float *source,float *target) |
222 | { |
223 | if (isnan(*source) || isnan(*target)) {FCR31|=0x800000;return;} |
224 | FCR31 = *source==*target ? FCR31|0x800000 : FCR31&~0x800000; |
225 | } |
226 | |
227 | M64P_FPU_INLINE void c_olt_s(float *source,float *target) |
228 | { |
229 | if (isnan(*source) || isnan(*target)) {FCR31&=~0x800000;return;} |
230 | FCR31 = *source<*target ? FCR31|0x800000 : FCR31&~0x800000; |
231 | } |
232 | M64P_FPU_INLINE void c_ult_s(float *source,float *target) |
233 | { |
234 | if (isnan(*source) || isnan(*target)) {FCR31|=0x800000;return;} |
235 | FCR31 = *source<*target ? FCR31|0x800000 : FCR31&~0x800000; |
236 | } |
237 | |
238 | M64P_FPU_INLINE void c_ole_s(float *source,float *target) |
239 | { |
240 | if (isnan(*source) || isnan(*target)) {FCR31&=~0x800000;return;} |
241 | FCR31 = *source<=*target ? FCR31|0x800000 : FCR31&~0x800000; |
242 | } |
243 | M64P_FPU_INLINE void c_ule_s(float *source,float *target) |
244 | { |
245 | if (isnan(*source) || isnan(*target)) {FCR31|=0x800000;return;} |
246 | FCR31 = *source<=*target ? FCR31|0x800000 : FCR31&~0x800000; |
247 | } |
248 | |
249 | M64P_FPU_INLINE void c_sf_s(float *source,float *target) |
250 | { |
251 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
252 | FCR31&=~0x800000; |
253 | } |
254 | M64P_FPU_INLINE void c_ngle_s(float *source,float *target) |
255 | { |
256 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
257 | FCR31&=~0x800000; |
258 | } |
259 | |
260 | M64P_FPU_INLINE void c_seq_s(float *source,float *target) |
261 | { |
262 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
263 | FCR31 = *source==*target ? FCR31|0x800000 : FCR31&~0x800000; |
264 | } |
265 | M64P_FPU_INLINE void c_ngl_s(float *source,float *target) |
266 | { |
267 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
268 | FCR31 = *source==*target ? FCR31|0x800000 : FCR31&~0x800000; |
269 | } |
270 | |
271 | M64P_FPU_INLINE void c_lt_s(float *source,float *target) |
272 | { |
273 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
274 | FCR31 = *source<*target ? FCR31|0x800000 : FCR31&~0x800000; |
275 | } |
276 | M64P_FPU_INLINE void c_nge_s(float *source,float *target) |
277 | { |
278 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
279 | FCR31 = *source<*target ? FCR31|0x800000 : FCR31&~0x800000; |
280 | } |
281 | |
282 | M64P_FPU_INLINE void c_le_s(float *source,float *target) |
283 | { |
284 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
285 | FCR31 = *source<=*target ? FCR31|0x800000 : FCR31&~0x800000; |
286 | } |
287 | M64P_FPU_INLINE void c_ngt_s(float *source,float *target) |
288 | { |
289 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
290 | FCR31 = *source<=*target ? FCR31|0x800000 : FCR31&~0x800000; |
291 | } |
292 | |
293 | M64P_FPU_INLINE void c_f_d() |
294 | { |
295 | FCR31 &= ~0x800000; |
296 | } |
297 | M64P_FPU_INLINE void c_un_d(double *source,double *target) |
298 | { |
299 | FCR31=(isnan(*source) || isnan(*target)) ? FCR31|0x800000 : FCR31&~0x800000; |
300 | } |
301 | |
302 | M64P_FPU_INLINE void c_eq_d(double *source,double *target) |
303 | { |
304 | if (isnan(*source) || isnan(*target)) {FCR31&=~0x800000;return;} |
305 | FCR31 = *source==*target ? FCR31|0x800000 : FCR31&~0x800000; |
306 | } |
307 | M64P_FPU_INLINE void c_ueq_d(double *source,double *target) |
308 | { |
309 | if (isnan(*source) || isnan(*target)) {FCR31|=0x800000;return;} |
310 | FCR31 = *source==*target ? FCR31|0x800000 : FCR31&~0x800000; |
311 | } |
312 | |
313 | M64P_FPU_INLINE void c_olt_d(double *source,double *target) |
314 | { |
315 | if (isnan(*source) || isnan(*target)) {FCR31&=~0x800000;return;} |
316 | FCR31 = *source<*target ? FCR31|0x800000 : FCR31&~0x800000; |
317 | } |
318 | M64P_FPU_INLINE void c_ult_d(double *source,double *target) |
319 | { |
320 | if (isnan(*source) || isnan(*target)) {FCR31|=0x800000;return;} |
321 | FCR31 = *source<*target ? FCR31|0x800000 : FCR31&~0x800000; |
322 | } |
323 | |
324 | M64P_FPU_INLINE void c_ole_d(double *source,double *target) |
325 | { |
326 | if (isnan(*source) || isnan(*target)) {FCR31&=~0x800000;return;} |
327 | FCR31 = *source<=*target ? FCR31|0x800000 : FCR31&~0x800000; |
328 | } |
329 | M64P_FPU_INLINE void c_ule_d(double *source,double *target) |
330 | { |
331 | if (isnan(*source) || isnan(*target)) {FCR31|=0x800000;return;} |
332 | FCR31 = *source<=*target ? FCR31|0x800000 : FCR31&~0x800000; |
333 | } |
334 | |
335 | M64P_FPU_INLINE void c_sf_d(double *source,double *target) |
336 | { |
337 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
338 | FCR31&=~0x800000; |
339 | } |
340 | M64P_FPU_INLINE void c_ngle_d(double *source,double *target) |
341 | { |
342 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
343 | FCR31&=~0x800000; |
344 | } |
345 | |
346 | M64P_FPU_INLINE void c_seq_d(double *source,double *target) |
347 | { |
348 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
349 | FCR31 = *source==*target ? FCR31|0x800000 : FCR31&~0x800000; |
350 | } |
351 | M64P_FPU_INLINE void c_ngl_d(double *source,double *target) |
352 | { |
353 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
354 | FCR31 = *source==*target ? FCR31|0x800000 : FCR31&~0x800000; |
355 | } |
356 | |
357 | M64P_FPU_INLINE void c_lt_d(double *source,double *target) |
358 | { |
359 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
360 | FCR31 = *source<*target ? FCR31|0x800000 : FCR31&~0x800000; |
361 | } |
362 | M64P_FPU_INLINE void c_nge_d(double *source,double *target) |
363 | { |
364 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
365 | FCR31 = *source<*target ? FCR31|0x800000 : FCR31&~0x800000; |
366 | } |
367 | |
368 | M64P_FPU_INLINE void c_le_d(double *source,double *target) |
369 | { |
370 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
371 | FCR31 = *source<=*target ? FCR31|0x800000 : FCR31&~0x800000; |
372 | } |
373 | M64P_FPU_INLINE void c_ngt_d(double *source,double *target) |
374 | { |
375 | //if (isnan(*source) || isnan(*target)) // FIXME - exception |
376 | FCR31 = *source<=*target ? FCR31|0x800000 : FCR31&~0x800000; |
377 | } |
378 | |
379 | |
380 | M64P_FPU_INLINE void add_s(float *source1,float *source2,float *target) |
381 | { |
382 | set_rounding(); |
383 | *target=(*source1)+(*source2); |
384 | } |
385 | M64P_FPU_INLINE void sub_s(float *source1,float *source2,float *target) |
386 | { |
387 | set_rounding(); |
388 | *target=(*source1)-(*source2); |
389 | } |
390 | M64P_FPU_INLINE void mul_s(float *source1,float *source2,float *target) |
391 | { |
392 | set_rounding(); |
393 | *target=(*source1)*(*source2); |
394 | } |
395 | M64P_FPU_INLINE void div_s(float *source1,float *source2,float *target) |
396 | { |
397 | set_rounding(); |
398 | *target=(*source1)/(*source2); |
399 | } |
400 | M64P_FPU_INLINE void sqrt_s(float *source,float *target) |
401 | { |
402 | set_rounding(); |
403 | *target=sqrtf(*source); |
404 | } |
405 | M64P_FPU_INLINE void abs_s(float *source,float *target) |
406 | { |
407 | set_rounding(); |
408 | *target=fabsf(*source); |
409 | } |
410 | M64P_FPU_INLINE void mov_s(float *source,float *target) |
411 | { |
412 | set_rounding(); |
413 | *target=*source; |
414 | } |
415 | M64P_FPU_INLINE void neg_s(float *source,float *target) |
416 | { |
417 | set_rounding(); |
418 | *target=-(*source); |
419 | } |
420 | M64P_FPU_INLINE void add_d(double *source1,double *source2,double *target) |
421 | { |
422 | set_rounding(); |
423 | *target=(*source1)+(*source2); |
424 | } |
425 | M64P_FPU_INLINE void sub_d(double *source1,double *source2,double *target) |
426 | { |
427 | set_rounding(); |
428 | *target=(*source1)-(*source2); |
429 | } |
430 | M64P_FPU_INLINE void mul_d(double *source1,double *source2,double *target) |
431 | { |
432 | set_rounding(); |
433 | *target=(*source1)*(*source2); |
434 | } |
435 | M64P_FPU_INLINE void div_d(double *source1,double *source2,double *target) |
436 | { |
437 | set_rounding(); |
438 | *target=(*source1)/(*source2); |
439 | } |
440 | M64P_FPU_INLINE void sqrt_d(double *source,double *target) |
441 | { |
442 | set_rounding(); |
443 | *target=sqrt(*source); |
444 | } |
445 | M64P_FPU_INLINE void abs_d(double *source,double *target) |
446 | { |
447 | set_rounding(); |
448 | *target=fabs(*source); |
449 | } |
450 | M64P_FPU_INLINE void mov_d(double *source,double *target) |
451 | { |
452 | set_rounding(); |
453 | *target=*source; |
454 | } |
455 | M64P_FPU_INLINE void neg_d(double *source,double *target) |
456 | { |
457 | set_rounding(); |
458 | *target=-(*source); |
459 | } |