| 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 | } |