source/mupen64plus-core/src/r4300/fpu.h

   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 }