arch/arm/vfp/vfpmodule.c

   1 /*
   2  *  linux/arch/arm/vfp/vfpmodule.c
   3  *
   4  *  Copyright (C) 2004 ARM Limited.
   5  *  Written by Deep Blue Solutions Limited.
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License version 2 as
   9  * published by the Free Software Foundation.
  10  */
  11 #include <linux/module.h>
  12 #include <linux/config.h>
  13 #include <linux/types.h>
  14 #include <linux/kernel.h>
  15 #include <linux/signal.h>
  16 #include <linux/sched.h>
  17 #include <linux/init.h>
  18
  19 #include <asm/thread_notify.h>
  20 #include <asm/vfp.h>
  21
  22 #include "vfpinstr.h"
  23 #include "vfp.h"
  24
  25 /*
  26  * Our undef handlers (in entry.S)
  27  */
  28 void vfp_testing_entry(void);
  29 void vfp_support_entry(void);
  30
  31 void (*vfp_vector)(void) = vfp_testing_entry;
  32 union vfp_state *last_VFP_context;
  33
  34 /*
  35  * Dual-use variable.
  36  * Used in startup: set to non-zero if VFP checks fail
  37  * After startup, holds VFP architecture
  38  */
  39 unsigned int VFP_arch;
  40
  41 static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
  42 {
  43         struct thread_info *thread = v;
  44         union vfp_state *vfp = &thread->vfpstate;
  45
  46         switch (cmd) {
  47         case THREAD_NOTIFY_FLUSH:
  48                 /*
  49                  * Per-thread VFP initialisation.
  50                  */
  51                 memset(vfp, 0, sizeof(union vfp_state));
  52
  53                 vfp->hard.fpexc = FPEXC_ENABLE;
  54                 vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
  55
  56                 /*
  57                  * Disable VFP to ensure we initialise it first.
  58                  */
  59                 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_ENABLE);
  60
  61                 /*
  62                  * FALLTHROUGH: Ensure we don't try to overwrite our newly
  63                  * initialised state information on the first fault.
  64                  */
  65
  66         case THREAD_NOTIFY_RELEASE:
  67                 /*
  68                  * Per-thread VFP cleanup.
  69                  */
  70                 if (last_VFP_context == vfp)
  71                         last_VFP_context = NULL;
  72                 break;
  73
  74         case THREAD_NOTIFY_SWITCH:
  75                 /*
  76                  * Always disable VFP so we can lazily save/restore the
  77                  * old state.
  78                  */
  79                 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_ENABLE);
  80                 break;
  81         }
  82
  83         return NOTIFY_DONE;
  84 }
  85
  86 static struct notifier_block vfp_notifier_block = {
  87         .notifier_call  = vfp_notifier,
  88 };
  89
  90 /*
  91  * Raise a SIGFPE for the current process.
  92  * sicode describes the signal being raised.
  93  */
  94 void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs)
  95 {
  96         siginfo_t info;
  97
  98         memset(&info, 0, sizeof(info));
  99
 100         info.si_signo = SIGFPE;
 101         info.si_code = sicode;
 102         info.si_addr = (void *)(instruction_pointer(regs) - 4);
 103
 104         /*
 105          * This is the same as NWFPE, because it's not clear what
 106          * this is used for
 107          */
 108         current->thread.error_code = 0;
 109         current->thread.trap_no = 6;
 110
 111         send_sig_info(SIGFPE, &info, current);
 112 }
 113
 114 static void vfp_panic(char *reason)
 115 {
 116         int i;
 117
 118         printk(KERN_ERR "VFP: Error: %s\n", reason);
 119         printk(KERN_ERR "VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n",
 120                 fmrx(FPEXC), fmrx(FPSCR), fmrx(FPINST));
 121         for (i = 0; i < 32; i += 2)
 122                 printk(KERN_ERR "VFP: s%2u: 0x%08x s%2u: 0x%08x\n",
 123                        i, vfp_get_float(i), i+1, vfp_get_float(i+1));
 124 }
 125
 126 /*
 127  * Process bitmask of exception conditions.
 128  */
 129 static void vfp_raise_exceptions(u32 exceptions, u32 inst, u32 fpscr, struct pt_regs *regs)
 130 {
 131         int si_code = 0;
 132
 133         pr_debug("VFP: raising exceptions %08x\n", exceptions);
 134
 135         if (exceptions == (u32)-1) {
 136                 vfp_panic("unhandled bounce");
 137                 vfp_raise_sigfpe(0, regs);
 138                 return;
 139         }
 140
 141         /*
 142          * If any of the status flags are set, update the FPSCR.
 143          * Comparison instructions always return at least one of
 144          * these flags set.
 145          */
 146         if (exceptions & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V))
 147                 fpscr &= ~(FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V);
 148
 149         fpscr |= exceptions;
 150
 151         fmxr(FPSCR, fpscr);
 152
 153 #define RAISE(stat,en,sig)                              \
 154         if (exceptions & stat && fpscr & en)            \
 155                 si_code = sig;
 156
 157         /*
 158          * These are arranged in priority order, least to highest.
 159          */
 160         RAISE(FPSCR_IXC, FPSCR_IXE, FPE_FLTRES);
 161         RAISE(FPSCR_UFC, FPSCR_UFE, FPE_FLTUND);
 162         RAISE(FPSCR_OFC, FPSCR_OFE, FPE_FLTOVF);
 163         RAISE(FPSCR_IOC, FPSCR_IOE, FPE_FLTINV);
 164
 165         if (si_code)
 166                 vfp_raise_sigfpe(si_code, regs);
 167 }
 168
 169 /*
 170  * Emulate a VFP instruction.
 171  */
 172 static u32 vfp_emulate_instruction(u32 inst, u32 fpscr, struct pt_regs *regs)
 173 {
 174         u32 exceptions = (u32)-1;
 175
 176         pr_debug("VFP: emulate: INST=0x%08x SCR=0x%08x\n", inst, fpscr);
 177
 178         if (INST_CPRTDO(inst)) {
 179                 if (!INST_CPRT(inst)) {
 180                         /*
 181                          * CPDO
 182                          */
 183                         if (vfp_single(inst)) {
 184                                 exceptions = vfp_single_cpdo(inst, fpscr);
 185                         } else {
 186                                 exceptions = vfp_double_cpdo(inst, fpscr);
 187                         }
 188                 } else {
 189                         /*
 190                          * A CPRT instruction can not appear in FPINST2, nor
 191                          * can it cause an exception.  Therefore, we do not
 192                          * have to emulate it.
 193                          */
 194                 }
 195         } else {
 196                 /*
 197                  * A CPDT instruction can not appear in FPINST2, nor can
 198                  * it cause an exception.  Therefore, we do not have to
 199                  * emulate it.
 200                  */
 201         }
 202         return exceptions & ~VFP_NAN_FLAG;
 203 }
 204
 205 /*
 206  * Package up a bounce condition.
 207  */
 208 void VFP9_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs)
 209 {
 210         u32 fpscr, orig_fpscr, exceptions, inst;
 211
 212         pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc);
 213
 214         /*
 215          * Enable access to the VFP so we can handle the bounce.
 216          */
 217         fmxr(FPEXC, fpexc & ~(FPEXC_EXCEPTION|FPEXC_INV|FPEXC_UFC|FPEXC_IOC));
 218
 219         orig_fpscr = fpscr = fmrx(FPSCR);
 220
 221         /*
 222          * If we are running with inexact exceptions enabled, we need to
 223          * emulate the trigger instruction.  Note that as we're emulating
 224          * the trigger instruction, we need to increment PC.
 225          */
 226         if (fpscr & FPSCR_IXE) {
 227                 regs->ARM_pc += 4;
 228                 goto emulate;
 229         }
 230
 231         barrier();
 232
 233         /*
 234          * Modify fpscr to indicate the number of iterations remaining
 235          */
 236         if (fpexc & FPEXC_EXCEPTION) {
 237                 u32 len;
 238
 239                 len = fpexc + (1 << FPEXC_LENGTH_BIT);
 240
 241                 fpscr &= ~FPSCR_LENGTH_MASK;
 242                 fpscr |= (len & FPEXC_LENGTH_MASK) << (FPSCR_LENGTH_BIT - FPEXC_LENGTH_BIT);
 243         }
 244
 245         /*
 246          * Handle the first FP instruction.  We used to take note of the
 247          * FPEXC bounce reason, but this appears to be unreliable.
 248          * Emulate the bounced instruction instead.
 249          */
 250         inst = fmrx(FPINST);
 251         exceptions = vfp_emulate_instruction(inst, fpscr, regs);
 252         if (exceptions)
 253                 vfp_raise_exceptions(exceptions, inst, orig_fpscr, regs);
 254
 255         /*
 256          * If there isn't a second FP instruction, exit now.
 257          */
 258         if (!(fpexc & FPEXC_FPV2))
 259                 return;
 260
 261         /*
 262          * The barrier() here prevents fpinst2 being read
 263          * before the condition above.
 264          */
 265         barrier();
 266         trigger = fmrx(FPINST2);
 267         orig_fpscr = fpscr = fmrx(FPSCR);
 268
 269  emulate:
 270         exceptions = vfp_emulate_instruction(trigger, fpscr, regs);
 271         if (exceptions)
 272                 vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
 273 }
 274
 275 /*
 276  * VFP support code initialisation.
 277  */
 278 static int __init vfp_init(void)
 279 {
 280         unsigned int vfpsid;
 281
 282         /*
 283          * First check that there is a VFP that we can use.
 284          * The handler is already setup to just log calls, so
 285          * we just need to read the VFPSID register.
 286          */
 287         vfpsid = fmrx(FPSID);
 288
 289         printk(KERN_INFO "VFP support v0.3: ");
 290         if (VFP_arch) {
 291                 printk("not present\n");
 292         } else if (vfpsid & FPSID_NODOUBLE) {
 293                 printk("no double precision support\n");
 294         } else {
 295                 VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;  /* Extract the architecture version */
 296                 printk("implementor %02x architecture %d part %02x variant %x rev %x\n",
 297                         (vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
 298                         (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT,
 299                         (vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
 300                         (vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
 301                         (vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);
 302                 vfp_vector = vfp_support_entry;
 303
 304                 thread_register_notifier(&vfp_notifier_block);
 305         }
 306         return 0;
 307 }
 308
 309 late_initcall(vfp_init);