arch/nds32/math-emu/fpuemu.c

   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) \
  19 void 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) \
  26 void 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 }
  32 DEF3OP(fmadd, s, fmuls, fadds);
  33 DEF3OP(fmsub, s, fmuls, fsubs);
  34 DEF3OP(fmadd, d, fmuld, faddd);
  35 DEF3OP(fmsub, d, fmuld, fsubd);
  36 DEF3OPNEG(fnmadd, s, fmuls, fadds, fnegs);
  37 DEF3OPNEG(fnmsub, s, fmuls, fsubs, fnegs);
  38 DEF3OPNEG(fnmadd, d, fmuld, faddd, fnegd);
  39 DEF3OPNEG(fnmsub, d, fmuld, fsubd, fnegd);
  40
  41 static 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
  52 enum ARGTYPE {
  53         S1S = 1,
  54         S2S,
  55         S1D,
  56         CS,
  57         D1D,
  58         D2D,
  59         D1S,
  60         CD
  61 };
  62 union 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  */
  69 static 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
 366 int 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 }