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

   1 /* $Id: fpu.c,v 1.4 2004/01/13 05:52:11 kkojima Exp $
   2  *
   3  * linux/arch/sh/kernel/fpu.c
   4  *
   5  * Save/restore floating point context for signal handlers.
   6  *
   7  * This file is subject to the terms and conditions of the GNU General Public
   8  * License.  See the file "COPYING" in the main directory of this archive
   9  * for more details.
  10  *
  11  * Copyright (C) 1999, 2000  Kaz Kojima & Niibe Yutaka
  12  *
  13  * FIXME! These routines can be optimized in big endian case.
  14  */
  15
  16 #include <linux/sched.h>
  17 #include <linux/signal.h>
  18 #include <asm/processor.h>
  19 #include <asm/system.h>
  20 #include <asm/io.h>
  21
  22 /* The PR (precision) bit in the FP Status Register must be clear when
  23  * an frchg instruction is executed, otherwise the instruction is undefined.
  24  * Executing frchg with PR set causes a trap on some SH4 implementations.
  25  */
  26
  27 #define FPSCR_RCHG 0x00000000
  28
  29
  30 /*
  31  * Save FPU registers onto task structure.
  32  * Assume called with FPU enabled (SR.FD=0).
  33  */
  34 void
  35 save_fpu(struct task_struct *tsk, struct pt_regs *regs)
  36 {
  37         unsigned long dummy;
  38
  39         clear_tsk_thread_flag(tsk, TIF_USEDFPU);
  40         enable_fpu();
  41         asm volatile("sts.l     fpul, @-%0\n\t"
  42                      "sts.l     fpscr, @-%0\n\t"
  43                      "lds       %2, fpscr\n\t"
  44                      "frchg\n\t"
  45                      "fmov.s    fr15, @-%0\n\t"
  46                      "fmov.s    fr14, @-%0\n\t"
  47                      "fmov.s    fr13, @-%0\n\t"
  48                      "fmov.s    fr12, @-%0\n\t"
  49                      "fmov.s    fr11, @-%0\n\t"
  50                      "fmov.s    fr10, @-%0\n\t"
  51                      "fmov.s    fr9, @-%0\n\t"
  52                      "fmov.s    fr8, @-%0\n\t"
  53                      "fmov.s    fr7, @-%0\n\t"
  54                      "fmov.s    fr6, @-%0\n\t"
  55                      "fmov.s    fr5, @-%0\n\t"
  56                      "fmov.s    fr4, @-%0\n\t"
  57                      "fmov.s    fr3, @-%0\n\t"
  58                      "fmov.s    fr2, @-%0\n\t"
  59                      "fmov.s    fr1, @-%0\n\t"
  60                      "fmov.s    fr0, @-%0\n\t"
  61                      "frchg\n\t"
  62                      "fmov.s    fr15, @-%0\n\t"
  63                      "fmov.s    fr14, @-%0\n\t"
  64                      "fmov.s    fr13, @-%0\n\t"
  65                      "fmov.s    fr12, @-%0\n\t"
  66                      "fmov.s    fr11, @-%0\n\t"
  67                      "fmov.s    fr10, @-%0\n\t"
  68                      "fmov.s    fr9, @-%0\n\t"
  69                      "fmov.s    fr8, @-%0\n\t"
  70                      "fmov.s    fr7, @-%0\n\t"
  71                      "fmov.s    fr6, @-%0\n\t"
  72                      "fmov.s    fr5, @-%0\n\t"
  73                      "fmov.s    fr4, @-%0\n\t"
  74                      "fmov.s    fr3, @-%0\n\t"
  75                      "fmov.s    fr2, @-%0\n\t"
  76                      "fmov.s    fr1, @-%0\n\t"
  77                      "fmov.s    fr0, @-%0\n\t"
  78                      "lds       %3, fpscr\n\t"
  79                      : "=r" (dummy)
  80                      : "0" ((char *)(&tsk->thread.fpu.hard.status)),
  81                        "r" (FPSCR_RCHG),
  82                        "r" (FPSCR_INIT)
  83                      : "memory");
  84
  85         disable_fpu();
  86         release_fpu(regs);
  87 }
  88
  89 static void
  90 restore_fpu(struct task_struct *tsk)
  91 {
  92         unsigned long dummy;
  93
  94         enable_fpu();
  95         asm volatile("lds       %2, fpscr\n\t"
  96                      "fmov.s    @%0+, fr0\n\t"
  97                      "fmov.s    @%0+, fr1\n\t"
  98                      "fmov.s    @%0+, fr2\n\t"
  99                      "fmov.s    @%0+, fr3\n\t"
 100                      "fmov.s    @%0+, fr4\n\t"
 101                      "fmov.s    @%0+, fr5\n\t"
 102                      "fmov.s    @%0+, fr6\n\t"
 103                      "fmov.s    @%0+, fr7\n\t"
 104                      "fmov.s    @%0+, fr8\n\t"
 105                      "fmov.s    @%0+, fr9\n\t"
 106                      "fmov.s    @%0+, fr10\n\t"
 107                      "fmov.s    @%0+, fr11\n\t"
 108                      "fmov.s    @%0+, fr12\n\t"
 109                      "fmov.s    @%0+, fr13\n\t"
 110                      "fmov.s    @%0+, fr14\n\t"
 111                      "fmov.s    @%0+, fr15\n\t"
 112                      "frchg\n\t"
 113                      "fmov.s    @%0+, fr0\n\t"
 114                      "fmov.s    @%0+, fr1\n\t"
 115                      "fmov.s    @%0+, fr2\n\t"
 116                      "fmov.s    @%0+, fr3\n\t"
 117                      "fmov.s    @%0+, fr4\n\t"
 118                      "fmov.s    @%0+, fr5\n\t"
 119                      "fmov.s    @%0+, fr6\n\t"
 120                      "fmov.s    @%0+, fr7\n\t"
 121                      "fmov.s    @%0+, fr8\n\t"
 122                      "fmov.s    @%0+, fr9\n\t"
 123                      "fmov.s    @%0+, fr10\n\t"
 124                      "fmov.s    @%0+, fr11\n\t"
 125                      "fmov.s    @%0+, fr12\n\t"
 126                      "fmov.s    @%0+, fr13\n\t"
 127                      "fmov.s    @%0+, fr14\n\t"
 128                      "fmov.s    @%0+, fr15\n\t"
 129                      "frchg\n\t"
 130                      "lds.l     @%0+, fpscr\n\t"
 131                      "lds.l     @%0+, fpul\n\t"
 132                      : "=r" (dummy)
 133                      : "0" (&tsk->thread.fpu), "r" (FPSCR_RCHG)
 134                      : "memory");
 135         disable_fpu();
 136 }
 137
 138 /*
 139  * Load the FPU with signalling NANS.  This bit pattern we're using
 140  * has the property that no matter wether considered as single or as
 141  * double precision represents signaling NANS.
 142  */
 143
 144 static void
 145 fpu_init(void)
 146 {
 147         enable_fpu();
 148         asm volatile("lds       %0, fpul\n\t"
 149                      "lds       %1, fpscr\n\t"
 150                      "fsts      fpul, fr0\n\t"
 151                      "fsts      fpul, fr1\n\t"
 152                      "fsts      fpul, fr2\n\t"
 153                      "fsts      fpul, fr3\n\t"
 154                      "fsts      fpul, fr4\n\t"
 155                      "fsts      fpul, fr5\n\t"
 156                      "fsts      fpul, fr6\n\t"
 157                      "fsts      fpul, fr7\n\t"
 158                      "fsts      fpul, fr8\n\t"
 159                      "fsts      fpul, fr9\n\t"
 160                      "fsts      fpul, fr10\n\t"
 161                      "fsts      fpul, fr11\n\t"
 162                      "fsts      fpul, fr12\n\t"
 163                      "fsts      fpul, fr13\n\t"
 164                      "fsts      fpul, fr14\n\t"
 165                      "fsts      fpul, fr15\n\t"
 166                      "frchg\n\t"
 167                      "fsts      fpul, fr0\n\t"
 168                      "fsts      fpul, fr1\n\t"
 169                      "fsts      fpul, fr2\n\t"
 170                      "fsts      fpul, fr3\n\t"
 171                      "fsts      fpul, fr4\n\t"
 172                      "fsts      fpul, fr5\n\t"
 173                      "fsts      fpul, fr6\n\t"
 174                      "fsts      fpul, fr7\n\t"
 175                      "fsts      fpul, fr8\n\t"
 176                      "fsts      fpul, fr9\n\t"
 177                      "fsts      fpul, fr10\n\t"
 178                      "fsts      fpul, fr11\n\t"
 179                      "fsts      fpul, fr12\n\t"
 180                      "fsts      fpul, fr13\n\t"
 181                      "fsts      fpul, fr14\n\t"
 182                      "fsts      fpul, fr15\n\t"
 183                      "frchg\n\t"
 184                      "lds       %2, fpscr\n\t"
 185                      : /* no output */
 186                      : "r" (0), "r" (FPSCR_RCHG), "r" (FPSCR_INIT));
 187         disable_fpu();
 188 }
 189
 190 /**
 191  *      denormal_to_double - Given denormalized float number,
 192  *                           store double float
 193  *
 194  *      @fpu: Pointer to sh_fpu_hard structure
 195  *      @n: Index to FP register
 196  */
 197 static void
 198 denormal_to_double (struct sh_fpu_hard_struct *fpu, int n)
 199 {
 200         unsigned long du, dl;
 201         unsigned long x = fpu->fpul;
 202         int exp = 1023 - 126;
 203
 204         if (x != 0 && (x & 0x7f800000) == 0) {
 205                 du = (x & 0x80000000);
 206                 while ((x & 0x00800000) == 0) {
 207                         x <<= 1;
 208                         exp--;
 209                 }
 210                 x &= 0x007fffff;
 211                 du |= (exp << 20) | (x >> 3);
 212                 dl = x << 29;
 213
 214                 fpu->fp_regs[n] = du;
 215                 fpu->fp_regs[n+1] = dl;
 216         }
 217 }
 218
 219 /**
 220  *      ieee_fpe_handler - Handle denormalized number exception
 221  *
 222  *      @regs: Pointer to register structure
 223  *
 224  *      Returns 1 when it's handled (should not cause exception).
 225  */
 226 static int
 227 ieee_fpe_handler (struct pt_regs *regs)
 228 {
 229         unsigned short insn = *(unsigned short *) regs->pc;
 230         unsigned short finsn;
 231         unsigned long nextpc;
 232         int nib[4] = {
 233                 (insn >> 12) & 0xf,
 234                 (insn >> 8) & 0xf,
 235                 (insn >> 4) & 0xf,
 236                 insn & 0xf};
 237
 238         if (nib[0] == 0xb ||
 239             (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb)) /* bsr & jsr */
 240                 regs->pr = regs->pc + 4;
 241
 242         if (nib[0] == 0xa || nib[0] == 0xb) { /* bra & bsr */
 243                 nextpc = regs->pc + 4 + ((short) ((insn & 0xfff) << 4) >> 3);
 244                 finsn = *(unsigned short *) (regs->pc + 2);
 245         } else if (nib[0] == 0x8 && nib[1] == 0xd) { /* bt/s */
 246                 if (regs->sr & 1)
 247                         nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1);
 248                 else
 249                         nextpc = regs->pc + 4;
 250                 finsn = *(unsigned short *) (regs->pc + 2);
 251         } else if (nib[0] == 0x8 && nib[1] == 0xf) { /* bf/s */
 252                 if (regs->sr & 1)
 253                         nextpc = regs->pc + 4;
 254                 else
 255                         nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1);
 256                 finsn = *(unsigned short *) (regs->pc + 2);
 257         } else if (nib[0] == 0x4 && nib[3] == 0xb &&
 258                  (nib[2] == 0x0 || nib[2] == 0x2)) { /* jmp & jsr */
 259                 nextpc = regs->regs[nib[1]];
 260                 finsn = *(unsigned short *) (regs->pc + 2);
 261         } else if (nib[0] == 0x0 && nib[3] == 0x3 &&
 262                  (nib[2] == 0x0 || nib[2] == 0x2)) { /* braf & bsrf */
 263                 nextpc = regs->pc + 4 + regs->regs[nib[1]];
 264                 finsn = *(unsigned short *) (regs->pc + 2);
 265         } else if (insn == 0x000b) { /* rts */
 266                 nextpc = regs->pr;
 267                 finsn = *(unsigned short *) (regs->pc + 2);
 268         } else {
 269                 nextpc = regs->pc + instruction_size(insn);
 270                 finsn = insn;
 271         }
 272
 273         if ((finsn & 0xf1ff) == 0xf0ad) { /* fcnvsd */
 274                 struct task_struct *tsk = current;
 275
 276                 save_fpu(tsk, regs);
 277                 if ((tsk->thread.fpu.hard.fpscr & (1 << 17))) {
 278                         /* FPU error */
 279                         denormal_to_double (&tsk->thread.fpu.hard,
 280                                             (finsn >> 8) & 0xf);
 281                         tsk->thread.fpu.hard.fpscr &=
 282                                 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK);
 283                         grab_fpu(regs);
 284                         restore_fpu(tsk);
 285                         set_tsk_thread_flag(tsk, TIF_USEDFPU);
 286                 } else
 287                         force_sig(SIGFPE, tsk);
 288
 289                 regs->pc = nextpc;
 290                 return 1;
 291         }
 292
 293         return 0;
 294 }
 295
 296 asmlinkage void
 297 do_fpu_error(unsigned long r4, unsigned long r5, unsigned long r6,
 298              unsigned long r7, struct pt_regs __regs)
 299 {
 300         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
 301         struct task_struct *tsk = current;
 302
 303         if (ieee_fpe_handler(regs))
 304                 return;
 305
 306         regs->pc += 2;
 307         save_fpu(tsk, regs);
 308         force_sig(SIGFPE, tsk);
 309 }
 310
 311 asmlinkage void
 312 do_fpu_state_restore(unsigned long r4, unsigned long r5, unsigned long r6,
 313                      unsigned long r7, struct pt_regs __regs)
 314 {
 315         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
 316         struct task_struct *tsk = current;
 317
 318         grab_fpu(regs);
 319         if (!user_mode(regs)) {
 320                 printk(KERN_ERR "BUG: FPU is used in kernel mode.\n");
 321                 return;
 322         }
 323
 324         if (used_math()) {
 325                 /* Using the FPU again.  */
 326                 restore_fpu(tsk);
 327         } else  {
 328                 /* First time FPU user.  */
 329                 fpu_init();
 330                 set_used_math();
 331         }
 332         set_tsk_thread_flag(tsk, TIF_USEDFPU);
 333 }