arch/sh/kernel/cpu/sh4/fpu.c

   1 /*
   2  * Save/restore floating point context for signal handlers.
   3  *
   4  * This file is subject to the terms and conditions of the GNU General Public
   5  * License.  See the file "COPYING" in the main directory of this archive
   6  * for more details.
   7  *
   8  * Copyright (C) 1999, 2000  Kaz Kojima & Niibe Yutaka
   9  * Copyright (C) 2006  ST Microelectronics Ltd. (denorm support)
  10  *
  11  * FIXME! These routines have not been tested for big endian case.
  12  */
  13 #include <linux/sched/signal.h>
  14 #include <linux/io.h>
  15 #include <cpu/fpu.h>
  16 #include <asm/processor.h>
  17 #include <asm/fpu.h>
  18 #include <asm/traps.h>
  19
  20 /* The PR (precision) bit in the FP Status Register must be clear when
  21  * an frchg instruction is executed, otherwise the instruction is undefined.
  22  * Executing frchg with PR set causes a trap on some SH4 implementations.
  23  */
  24
  25 #define FPSCR_RCHG 0x00000000
  26 extern unsigned long long float64_div(unsigned long long a,
  27                                       unsigned long long b);
  28 extern unsigned long int float32_div(unsigned long int a, unsigned long int b);
  29 extern unsigned long long float64_mul(unsigned long long a,
  30                                       unsigned long long b);
  31 extern unsigned long int float32_mul(unsigned long int a, unsigned long int b);
  32 extern unsigned long long float64_add(unsigned long long a,
  33                                       unsigned long long b);
  34 extern unsigned long int float32_add(unsigned long int a, unsigned long int b);
  35 extern unsigned long long float64_sub(unsigned long long a,
  36                                       unsigned long long b);
  37 extern unsigned long int float32_sub(unsigned long int a, unsigned long int b);
  38 extern unsigned long int float64_to_float32(unsigned long long a);
  39 static unsigned int fpu_exception_flags;
  40
  41 /*
  42  * Save FPU registers onto task structure.
  43  */
  44 void save_fpu(struct task_struct *tsk)
  45 {
  46         unsigned long dummy;
  47
  48         enable_fpu();
  49         asm volatile ("sts.l    fpul, @-%0\n\t"
  50                       "sts.l    fpscr, @-%0\n\t"
  51                       "lds      %2, fpscr\n\t"
  52                       "frchg\n\t"
  53                       "fmov.s   fr15, @-%0\n\t"
  54                       "fmov.s   fr14, @-%0\n\t"
  55                       "fmov.s   fr13, @-%0\n\t"
  56                       "fmov.s   fr12, @-%0\n\t"
  57                       "fmov.s   fr11, @-%0\n\t"
  58                       "fmov.s   fr10, @-%0\n\t"
  59                       "fmov.s   fr9, @-%0\n\t"
  60                       "fmov.s   fr8, @-%0\n\t"
  61                       "fmov.s   fr7, @-%0\n\t"
  62                       "fmov.s   fr6, @-%0\n\t"
  63                       "fmov.s   fr5, @-%0\n\t"
  64                       "fmov.s   fr4, @-%0\n\t"
  65                       "fmov.s   fr3, @-%0\n\t"
  66                       "fmov.s   fr2, @-%0\n\t"
  67                       "fmov.s   fr1, @-%0\n\t"
  68                       "fmov.s   fr0, @-%0\n\t"
  69                       "frchg\n\t"
  70                       "fmov.s   fr15, @-%0\n\t"
  71                       "fmov.s   fr14, @-%0\n\t"
  72                       "fmov.s   fr13, @-%0\n\t"
  73                       "fmov.s   fr12, @-%0\n\t"
  74                       "fmov.s   fr11, @-%0\n\t"
  75                       "fmov.s   fr10, @-%0\n\t"
  76                       "fmov.s   fr9, @-%0\n\t"
  77                       "fmov.s   fr8, @-%0\n\t"
  78                       "fmov.s   fr7, @-%0\n\t"
  79                       "fmov.s   fr6, @-%0\n\t"
  80                       "fmov.s   fr5, @-%0\n\t"
  81                       "fmov.s   fr4, @-%0\n\t"
  82                       "fmov.s   fr3, @-%0\n\t"
  83                       "fmov.s   fr2, @-%0\n\t"
  84                       "fmov.s   fr1, @-%0\n\t"
  85                       "fmov.s   fr0, @-%0\n\t"
  86                       "lds      %3, fpscr\n\t":"=r" (dummy)
  87                       :"0"((char *)(&tsk->thread.xstate->hardfpu.status)),
  88                       "r"(FPSCR_RCHG), "r"(FPSCR_INIT)
  89                       :"memory");
  90
  91         disable_fpu();
  92 }
  93
  94 void restore_fpu(struct task_struct *tsk)
  95 {
  96         unsigned long dummy;
  97
  98         enable_fpu();
  99         asm volatile ("lds      %2, fpscr\n\t"
 100                       "fmov.s   @%0+, fr0\n\t"
 101                       "fmov.s   @%0+, fr1\n\t"
 102                       "fmov.s   @%0+, fr2\n\t"
 103                       "fmov.s   @%0+, fr3\n\t"
 104                       "fmov.s   @%0+, fr4\n\t"
 105                       "fmov.s   @%0+, fr5\n\t"
 106                       "fmov.s   @%0+, fr6\n\t"
 107                       "fmov.s   @%0+, fr7\n\t"
 108                       "fmov.s   @%0+, fr8\n\t"
 109                       "fmov.s   @%0+, fr9\n\t"
 110                       "fmov.s   @%0+, fr10\n\t"
 111                       "fmov.s   @%0+, fr11\n\t"
 112                       "fmov.s   @%0+, fr12\n\t"
 113                       "fmov.s   @%0+, fr13\n\t"
 114                       "fmov.s   @%0+, fr14\n\t"
 115                       "fmov.s   @%0+, fr15\n\t"
 116                       "frchg\n\t"
 117                       "fmov.s   @%0+, fr0\n\t"
 118                       "fmov.s   @%0+, fr1\n\t"
 119                       "fmov.s   @%0+, fr2\n\t"
 120                       "fmov.s   @%0+, fr3\n\t"
 121                       "fmov.s   @%0+, fr4\n\t"
 122                       "fmov.s   @%0+, fr5\n\t"
 123                       "fmov.s   @%0+, fr6\n\t"
 124                       "fmov.s   @%0+, fr7\n\t"
 125                       "fmov.s   @%0+, fr8\n\t"
 126                       "fmov.s   @%0+, fr9\n\t"
 127                       "fmov.s   @%0+, fr10\n\t"
 128                       "fmov.s   @%0+, fr11\n\t"
 129                       "fmov.s   @%0+, fr12\n\t"
 130                       "fmov.s   @%0+, fr13\n\t"
 131                       "fmov.s   @%0+, fr14\n\t"
 132                       "fmov.s   @%0+, fr15\n\t"
 133                       "frchg\n\t"
 134                       "lds.l    @%0+, fpscr\n\t"
 135                       "lds.l    @%0+, fpul\n\t"
 136                       :"=r" (dummy)
 137                       :"0" (tsk->thread.xstate), "r" (FPSCR_RCHG)
 138                       :"memory");
 139         disable_fpu();
 140 }
 141
 142 /**
 143  *      denormal_to_double - Given denormalized float number,
 144  *                           store double float
 145  *
 146  *      @fpu: Pointer to sh_fpu_hard structure
 147  *      @n: Index to FP register
 148  */
 149 static void denormal_to_double(struct sh_fpu_hard_struct *fpu, int n)
 150 {
 151         unsigned long du, dl;
 152         unsigned long x = fpu->fpul;
 153         int exp = 1023 - 126;
 154
 155         if (x != 0 && (x & 0x7f800000) == 0) {
 156                 du = (x & 0x80000000);
 157                 while ((x & 0x00800000) == 0) {
 158                         x <<= 1;
 159                         exp--;
 160                 }
 161                 x &= 0x007fffff;
 162                 du |= (exp << 20) | (x >> 3);
 163                 dl = x << 29;
 164
 165                 fpu->fp_regs[n] = du;
 166                 fpu->fp_regs[n + 1] = dl;
 167         }
 168 }
 169
 170 /**
 171  *      ieee_fpe_handler - Handle denormalized number exception
 172  *
 173  *      @regs: Pointer to register structure
 174  *
 175  *      Returns 1 when it's handled (should not cause exception).
 176  */
 177 static int ieee_fpe_handler(struct pt_regs *regs)
 178 {
 179         unsigned short insn = *(unsigned short *)regs->pc;
 180         unsigned short finsn;
 181         unsigned long nextpc;
 182         int nib[4] = {
 183                 (insn >> 12) & 0xf,
 184                 (insn >> 8) & 0xf,
 185                 (insn >> 4) & 0xf,
 186                 insn & 0xf
 187         };
 188
 189         if (nib[0] == 0xb || (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb))
 190                 regs->pr = regs->pc + 4;  /* bsr & jsr */
 191
 192         if (nib[0] == 0xa || nib[0] == 0xb) {
 193                 /* bra & bsr */
 194                 nextpc = regs->pc + 4 + ((short)((insn & 0xfff) << 4) >> 3);
 195                 finsn = *(unsigned short *)(regs->pc + 2);
 196         } else if (nib[0] == 0x8 && nib[1] == 0xd) {
 197                 /* bt/s */
 198                 if (regs->sr & 1)
 199                         nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1);
 200                 else
 201                         nextpc = regs->pc + 4;
 202                 finsn = *(unsigned short *)(regs->pc + 2);
 203         } else if (nib[0] == 0x8 && nib[1] == 0xf) {
 204                 /* bf/s */
 205                 if (regs->sr & 1)
 206                         nextpc = regs->pc + 4;
 207                 else
 208                         nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1);
 209                 finsn = *(unsigned short *)(regs->pc + 2);
 210         } else if (nib[0] == 0x4 && nib[3] == 0xb &&
 211                    (nib[2] == 0x0 || nib[2] == 0x2)) {
 212                 /* jmp & jsr */
 213                 nextpc = regs->regs[nib[1]];
 214                 finsn = *(unsigned short *)(regs->pc + 2);
 215         } else if (nib[0] == 0x0 && nib[3] == 0x3 &&
 216                    (nib[2] == 0x0 || nib[2] == 0x2)) {
 217                 /* braf & bsrf */
 218                 nextpc = regs->pc + 4 + regs->regs[nib[1]];
 219                 finsn = *(unsigned short *)(regs->pc + 2);
 220         } else if (insn == 0x000b) {
 221                 /* rts */
 222                 nextpc = regs->pr;
 223                 finsn = *(unsigned short *)(regs->pc + 2);
 224         } else {
 225                 nextpc = regs->pc + instruction_size(insn);
 226                 finsn = insn;
 227         }
 228
 229         if ((finsn & 0xf1ff) == 0xf0ad) {
 230                 /* fcnvsd */
 231                 struct task_struct *tsk = current;
 232
 233                 if ((tsk->thread.xstate->hardfpu.fpscr & FPSCR_CAUSE_ERROR))
 234                         /* FPU error */
 235                         denormal_to_double(&tsk->thread.xstate->hardfpu,
 236                                            (finsn >> 8) & 0xf);
 237                 else
 238                         return 0;
 239
 240                 regs->pc = nextpc;
 241                 return 1;
 242         } else if ((finsn & 0xf00f) == 0xf002) {
 243                 /* fmul */
 244                 struct task_struct *tsk = current;
 245                 int fpscr;
 246                 int n, m, prec;
 247                 unsigned int hx, hy;
 248
 249                 n = (finsn >> 8) & 0xf;
 250                 m = (finsn >> 4) & 0xf;
 251                 hx = tsk->thread.xstate->hardfpu.fp_regs[n];
 252                 hy = tsk->thread.xstate->hardfpu.fp_regs[m];
 253                 fpscr = tsk->thread.xstate->hardfpu.fpscr;
 254                 prec = fpscr & FPSCR_DBL_PRECISION;
 255
 256                 if ((fpscr & FPSCR_CAUSE_ERROR)
 257                     && (prec && ((hx & 0x7fffffff) < 0x00100000
 258                                  || (hy & 0x7fffffff) < 0x00100000))) {
 259                         long long llx, lly;
 260
 261                         /* FPU error because of denormal (doubles) */
 262                         llx = ((long long)hx << 32)
 263                             | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
 264                         lly = ((long long)hy << 32)
 265                             | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
 266                         llx = float64_mul(llx, lly);
 267                         tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
 268                         tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
 269                 } else if ((fpscr & FPSCR_CAUSE_ERROR)
 270                            && (!prec && ((hx & 0x7fffffff) < 0x00800000
 271                                          || (hy & 0x7fffffff) < 0x00800000))) {
 272                         /* FPU error because of denormal (floats) */
 273                         hx = float32_mul(hx, hy);
 274                         tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
 275                 } else
 276                         return 0;
 277
 278                 regs->pc = nextpc;
 279                 return 1;
 280         } else if ((finsn & 0xf00e) == 0xf000) {
 281                 /* fadd, fsub */
 282                 struct task_struct *tsk = current;
 283                 int fpscr;
 284                 int n, m, prec;
 285                 unsigned int hx, hy;
 286
 287                 n = (finsn >> 8) & 0xf;
 288                 m = (finsn >> 4) & 0xf;
 289                 hx = tsk->thread.xstate->hardfpu.fp_regs[n];
 290                 hy = tsk->thread.xstate->hardfpu.fp_regs[m];
 291                 fpscr = tsk->thread.xstate->hardfpu.fpscr;
 292                 prec = fpscr & FPSCR_DBL_PRECISION;
 293
 294                 if ((fpscr & FPSCR_CAUSE_ERROR)
 295                     && (prec && ((hx & 0x7fffffff) < 0x00100000
 296                                  || (hy & 0x7fffffff) < 0x00100000))) {
 297                         long long llx, lly;
 298
 299                         /* FPU error because of denormal (doubles) */
 300                         llx = ((long long)hx << 32)
 301                             | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
 302                         lly = ((long long)hy << 32)
 303                             | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
 304                         if ((finsn & 0xf00f) == 0xf000)
 305                                 llx = float64_add(llx, lly);
 306                         else
 307                                 llx = float64_sub(llx, lly);
 308                         tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
 309                         tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
 310                 } else if ((fpscr & FPSCR_CAUSE_ERROR)
 311                            && (!prec && ((hx & 0x7fffffff) < 0x00800000
 312                                          || (hy & 0x7fffffff) < 0x00800000))) {
 313                         /* FPU error because of denormal (floats) */
 314                         if ((finsn & 0xf00f) == 0xf000)
 315                                 hx = float32_add(hx, hy);
 316                         else
 317                                 hx = float32_sub(hx, hy);
 318                         tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
 319                 } else
 320                         return 0;
 321
 322                 regs->pc = nextpc;
 323                 return 1;
 324         } else if ((finsn & 0xf003) == 0xf003) {
 325                 /* fdiv */
 326                 struct task_struct *tsk = current;
 327                 int fpscr;
 328                 int n, m, prec;
 329                 unsigned int hx, hy;
 330
 331                 n = (finsn >> 8) & 0xf;
 332                 m = (finsn >> 4) & 0xf;
 333                 hx = tsk->thread.xstate->hardfpu.fp_regs[n];
 334                 hy = tsk->thread.xstate->hardfpu.fp_regs[m];
 335                 fpscr = tsk->thread.xstate->hardfpu.fpscr;
 336                 prec = fpscr & FPSCR_DBL_PRECISION;
 337
 338                 if ((fpscr & FPSCR_CAUSE_ERROR)
 339                     && (prec && ((hx & 0x7fffffff) < 0x00100000
 340                                  || (hy & 0x7fffffff) < 0x00100000))) {
 341                         long long llx, lly;
 342
 343                         /* FPU error because of denormal (doubles) */
 344                         llx = ((long long)hx << 32)
 345                             | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
 346                         lly = ((long long)hy << 32)
 347                             | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
 348
 349                         llx = float64_div(llx, lly);
 350
 351                         tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
 352                         tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
 353                 } else if ((fpscr & FPSCR_CAUSE_ERROR)
 354                            && (!prec && ((hx & 0x7fffffff) < 0x00800000
 355                                          || (hy & 0x7fffffff) < 0x00800000))) {
 356                         /* FPU error because of denormal (floats) */
 357                         hx = float32_div(hx, hy);
 358                         tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
 359                 } else
 360                         return 0;
 361
 362                 regs->pc = nextpc;
 363                 return 1;
 364         } else if ((finsn & 0xf0bd) == 0xf0bd) {
 365                 /* fcnvds - double to single precision convert */
 366                 struct task_struct *tsk = current;
 367                 int m;
 368                 unsigned int hx;
 369
 370                 m = (finsn >> 8) & 0x7;
 371                 hx = tsk->thread.xstate->hardfpu.fp_regs[m];
 372
 373                 if ((tsk->thread.xstate->hardfpu.fpscr & FPSCR_CAUSE_ERROR)
 374                         && ((hx & 0x7fffffff) < 0x00100000)) {
 375                         /* subnormal double to float conversion */
 376                         long long llx;
 377
 378                         llx = ((long long)tsk->thread.xstate->hardfpu.fp_regs[m] << 32)
 379                             | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
 380
 381                         tsk->thread.xstate->hardfpu.fpul = float64_to_float32(llx);
 382                 } else
 383                         return 0;
 384
 385                 regs->pc = nextpc;
 386                 return 1;
 387         }
 388
 389         return 0;
 390 }
 391
 392 void float_raise(unsigned int flags)
 393 {
 394         fpu_exception_flags |= flags;
 395 }
 396
 397 int float_rounding_mode(void)
 398 {
 399         struct task_struct *tsk = current;
 400         int roundingMode = FPSCR_ROUNDING_MODE(tsk->thread.xstate->hardfpu.fpscr);
 401         return roundingMode;
 402 }
 403
 404 BUILD_TRAP_HANDLER(fpu_error)
 405 {
 406         struct task_struct *tsk = current;
 407         TRAP_HANDLER_DECL;
 408
 409         __unlazy_fpu(tsk, regs);
 410         fpu_exception_flags = 0;
 411         if (ieee_fpe_handler(regs)) {
 412                 tsk->thread.xstate->hardfpu.fpscr &=
 413                     ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK);
 414                 tsk->thread.xstate->hardfpu.fpscr |= fpu_exception_flags;
 415                 /* Set the FPSCR flag as well as cause bits - simply
 416                  * replicate the cause */
 417                 tsk->thread.xstate->hardfpu.fpscr |= (fpu_exception_flags >> 10);
 418                 grab_fpu(regs);
 419                 restore_fpu(tsk);
 420                 task_thread_info(tsk)->status |= TS_USEDFPU;
 421                 if ((((tsk->thread.xstate->hardfpu.fpscr & FPSCR_ENABLE_MASK) >> 7) &
 422                      (fpu_exception_flags >> 2)) == 0) {
 423                         return;
 424                 }
 425         }
 426
 427         force_sig(SIGFPE, tsk);
 428 }