[BACK]Return to vfp_init.c CVS log [TXT][DIR] Up to [cvs.NetBSD.org] / src / sys / arch / arm / vfp

Annotation of src/sys/arch/arm/vfp/vfp_init.c, Revision 1.53

1.53    ! jmcneill    1: /*      $NetBSD: vfp_init.c,v 1.52 2017/03/22 23:36:02 chs Exp $ */
1.1       rearnsha    2:
                      3: /*
                      4:  * Copyright (c) 2008 ARM Ltd
                      5:  * All rights reserved.
                      6:  *
                      7:  * Redistribution and use in source and binary forms, with or without
                      8:  * modification, are permitted provided that the following conditions
                      9:  * are met:
                     10:  * 1. Redistributions of source code must retain the above copyright
                     11:  *    notice, this list of conditions and the following disclaimer.
                     12:  * 2. Redistributions in binary form must reproduce the above copyright
                     13:  *    notice, this list of conditions and the following disclaimer in the
                     14:  *    documentation and/or other materials provided with the distribution.
                     15:  * 3. The name of the company may not be used to endorse or promote
                     16:  *    products derived from this software without specific prior written
                     17:  *    permission.
                     18:  *
                     19:  * THIS SOFTWARE IS PROVIDED BY ARM LTD ``AS IS'' AND ANY EXPRESS OR
                     20:  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
                     21:  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
                     22:  * ARE DISCLAIMED.  IN NO EVENT SHALL ARM LTD BE LIABLE FOR ANY
                     23:  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
                     24:  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
                     25:  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
                     26:  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
                     27:  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
                     28:  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
                     29:  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
                     30:  */
                     31:
                     32: #include <sys/param.h>
                     33: #include <sys/types.h>
                     34: #include <sys/systm.h>
                     35: #include <sys/device.h>
                     36: #include <sys/proc.h>
1.4       matt       37: #include <sys/cpu.h>
1.1       rearnsha   38:
1.23      matt       39: #include <arm/locore.h>
1.5       matt       40: #include <arm/pcb.h>
1.1       rearnsha   41: #include <arm/undefined.h>
                     42: #include <arm/vfpreg.h>
1.8       matt       43: #include <arm/mcontext.h>
1.1       rearnsha   44:
1.12      matt       45: #include <uvm/uvm_extern.h>            /* for pmap.h */
                     46:
1.11      matt       47: #ifdef FPU_VFP
                     48:
1.29      matt       49: #ifdef CPU_CORTEX
                     50: __asm(".fpu\tvfpv4");
                     51: #else
                     52: __asm(".fpu\tvfp");
                     53: #endif
                     54:
1.1       rearnsha   55: /* FLDMD <X>, {d0-d15} */
1.11      matt       56: static inline void
1.13      matt       57: load_vfpregs_lo(const uint64_t *p)
1.10      matt       58: {
1.29      matt       59:        __asm __volatile("vldmia %0, {d0-d15}" :: "r" (p) : "memory");
1.10      matt       60: }
                     61:
                     62: /* FSTMD <X>, {d0-d15} */
1.11      matt       63: static inline void
1.10      matt       64: save_vfpregs_lo(uint64_t *p)
                     65: {
1.29      matt       66:        __asm __volatile("vstmia %0, {d0-d15}" :: "r" (p) : "memory");
1.10      matt       67: }
                     68:
                     69: #ifdef CPU_CORTEX
                     70: /* FLDMD <X>, {d16-d31} */
1.11      matt       71: static inline void
1.13      matt       72: load_vfpregs_hi(const uint64_t *p)
1.10      matt       73: {
1.29      matt       74:        __asm __volatile("vldmia\t%0, {d16-d31}" :: "r" (&p[16]) : "memory");
1.10      matt       75: }
                     76:
                     77: /* FLDMD <X>, {d16-d31} */
1.11      matt       78: static inline void
1.10      matt       79: save_vfpregs_hi(uint64_t *p)
                     80: {
1.29      matt       81:        __asm __volatile("vstmia\t%0, {d16-d31}" :: "r" (&p[16]) : "memory");
1.10      matt       82: }
                     83: #endif
1.1       rearnsha   84:
1.13      matt       85: static inline void
                     86: load_vfpregs(const struct vfpreg *fregs)
                     87: {
                     88:        load_vfpregs_lo(fregs->vfp_regs);
                     89: #ifdef CPU_CORTEX
                     90: #ifdef CPU_ARM11
                     91:        switch (curcpu()->ci_vfp_id) {
                     92:        case FPU_VFP_CORTEXA5:
                     93:        case FPU_VFP_CORTEXA7:
                     94:        case FPU_VFP_CORTEXA8:
                     95:        case FPU_VFP_CORTEXA9:
1.20      matt       96:        case FPU_VFP_CORTEXA15:
1.42      slp        97:        case FPU_VFP_CORTEXA15_QEMU:
1.50      skrll      98:        case FPU_VFP_CORTEXA53:
1.53    ! jmcneill   99:        case FPU_VFP_CORTEXA57:
1.13      matt      100: #endif
                    101:                load_vfpregs_hi(fregs->vfp_regs);
                    102: #ifdef CPU_ARM11
                    103:                break;
                    104:        }
                    105: #endif
                    106: #endif
                    107: }
                    108:
                    109: static inline void
                    110: save_vfpregs(struct vfpreg *fregs)
                    111: {
                    112:        save_vfpregs_lo(fregs->vfp_regs);
                    113: #ifdef CPU_CORTEX
                    114: #ifdef CPU_ARM11
                    115:        switch (curcpu()->ci_vfp_id) {
                    116:        case FPU_VFP_CORTEXA5:
                    117:        case FPU_VFP_CORTEXA7:
                    118:        case FPU_VFP_CORTEXA8:
                    119:        case FPU_VFP_CORTEXA9:
1.20      matt      120:        case FPU_VFP_CORTEXA15:
1.42      slp       121:        case FPU_VFP_CORTEXA15_QEMU:
1.50      skrll     122:        case FPU_VFP_CORTEXA53:
1.53    ! jmcneill  123:        case FPU_VFP_CORTEXA57:
1.13      matt      124: #endif
                    125:                save_vfpregs_hi(fregs->vfp_regs);
                    126: #ifdef CPU_ARM11
                    127:                break;
                    128:        }
                    129: #endif
                    130: #endif
                    131: }
                    132:
1.1       rearnsha  133: /* The real handler for VFP bounces.  */
                    134: static int vfp_handler(u_int, u_int, trapframe_t *, int);
1.13      matt      135: #ifdef CPU_CORTEX
                    136: static int neon_handler(u_int, u_int, trapframe_t *, int);
                    137: #endif
1.1       rearnsha  138:
1.13      matt      139: static void vfp_state_load(lwp_t *, u_int);
1.39      rmind     140: static void vfp_state_save(lwp_t *);
                    141: static void vfp_state_release(lwp_t *);
1.4       matt      142:
                    143: const pcu_ops_t arm_vfp_ops = {
                    144:        .pcu_id = PCU_FPU,
1.13      matt      145:        .pcu_state_save = vfp_state_save,
1.4       matt      146:        .pcu_state_load = vfp_state_load,
                    147:        .pcu_state_release = vfp_state_release,
                    148: };
1.1       rearnsha  149:
1.34      matt      150: /* determine what bits can be changed */
                    151: uint32_t vfp_fpscr_changable = VFP_FPSCR_CSUM;
                    152: /* default to run fast */
                    153: uint32_t vfp_fpscr_default = (VFP_FPSCR_DN | VFP_FPSCR_FZ | VFP_FPSCR_RN);
                    154:
1.1       rearnsha  155: /*
                    156:  * Used to test for a VFP. The following function is installed as a coproc10
                    157:  * handler on the undefined instruction vector and then we issue a VFP
                    158:  * instruction. If undefined_test is non zero then the VFP did not handle
                    159:  * the instruction so must be absent, or disabled.
                    160:  */
                    161:
                    162: static int undefined_test;
                    163:
                    164: static int
1.4       matt      165: vfp_test(u_int address, u_int insn, trapframe_t *frame, int fault_code)
1.1       rearnsha  166: {
                    167:
                    168:        frame->tf_pc += INSN_SIZE;
                    169:        ++undefined_test;
1.4       matt      170:        return 0;
                    171: }
                    172:
1.35      matt      173: #else
                    174: /* determine what bits can be changed */
                    175: uint32_t vfp_fpscr_changable = VFP_FPSCR_CSUM|VFP_FPSCR_ESUM|VFP_FPSCR_RMODE;
1.4       matt      176: #endif /* FPU_VFP */
                    177:
                    178: static int
                    179: vfp_fpscr_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code)
                    180: {
                    181:        struct lwp * const l = curlwp;
                    182:        const u_int regno = (insn >> 12) & 0xf;
                    183:        /*
                    184:         * Only match move to/from the FPSCR register and we
                    185:         * can't be using the SP,LR,PC as a source.
                    186:         */
                    187:        if ((insn & 0xffef0fff) != 0xeee10a10 || regno > 12)
                    188:                return 1;
                    189:
                    190:        struct pcb * const pcb = lwp_getpcb(l);
                    191:
                    192: #ifdef FPU_VFP
                    193:        /*
                    194:         * If FPU is valid somewhere, let's just reenable VFP and
                    195:         * retry the instruction (only safe thing to do since the
                    196:         * pcb has a stale copy).
                    197:         */
                    198:        if (pcb->pcb_vfp.vfp_fpexc & VFP_FPEXC_EN)
                    199:                return 1;
                    200:
1.51      chs       201:        if (__predict_false(!vfp_used_p(l))) {
1.35      matt      202:                pcb->pcb_vfp.vfp_fpscr = vfp_fpscr_default;
1.4       matt      203:        }
1.26      matt      204: #endif
1.4       matt      205:
                    206:        /*
1.30      skrll     207:         * We now know the pcb has the saved copy.
1.4       matt      208:         */
                    209:        register_t * const regp = &frame->tf_r0 + regno;
                    210:        if (insn & 0x00100000) {
                    211:                *regp = pcb->pcb_vfp.vfp_fpscr;
                    212:        } else {
1.34      matt      213:                pcb->pcb_vfp.vfp_fpscr &= ~vfp_fpscr_changable;
                    214:                pcb->pcb_vfp.vfp_fpscr |= *regp & vfp_fpscr_changable;
1.4       matt      215:        }
                    216:
1.37      matt      217:        curcpu()->ci_vfp_evs[0].ev_count++;
1.4       matt      218:
                    219:        frame->tf_pc += INSN_SIZE;
                    220:        return 0;
1.1       rearnsha  221: }
                    222:
1.4       matt      223: #ifndef FPU_VFP
                    224: /*
                    225:  * If we don't want VFP support, we still need to handle emulating VFP FPSCR
                    226:  * instructions.
                    227:  */
                    228: void
1.37      matt      229: vfp_attach(struct cpu_info *ci)
1.4       matt      230: {
1.37      matt      231:        if (CPU_IS_PRIMARY(ci)) {
                    232:                install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
                    233:        }
                    234:        evcnt_attach_dynamic(&ci->ci_vfp_evs[0], EVCNT_TYPE_TRAP, NULL,
                    235:            ci->ci_cpuname, "vfp fpscr traps");
1.4       matt      236: }
                    237:
                    238: #else
1.1       rearnsha  239: void
1.37      matt      240: vfp_attach(struct cpu_info *ci)
1.1       rearnsha  241: {
1.4       matt      242:        const char *model = NULL;
1.1       rearnsha  243:
1.37      matt      244:        if (CPU_ID_ARM11_P(ci->ci_arm_cpuid)
                    245:            || CPU_ID_MV88SV58XX_P(ci->ci_arm_cpuid)
                    246:            || CPU_ID_CORTEX_P(ci->ci_arm_cpuid)) {
                    247: #if 0
                    248:                const uint32_t nsacr = armreg_nsacr_read();
                    249:                const uint32_t nsacr_vfp = __BITS(VFP_COPROC,VFP_COPROC2);
                    250:                if ((nsacr & nsacr_vfp) != nsacr_vfp) {
1.40      matt      251:                        aprint_normal_dev(ci->ci_dev,
                    252:                            "VFP access denied (NSACR=%#x)\n", nsacr);
1.37      matt      253:                        install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
                    254:                        ci->ci_vfp_id = 0;
                    255:                        evcnt_attach_dynamic(&ci->ci_vfp_evs[0],
                    256:                            EVCNT_TYPE_TRAP, NULL, ci->ci_cpuname,
                    257:                            "vfp fpscr traps");
                    258:                        return;
                    259:                }
                    260: #endif
1.7       matt      261:                const uint32_t cpacr_vfp = CPACR_CPn(VFP_COPROC);
                    262:                const uint32_t cpacr_vfp2 = CPACR_CPn(VFP_COPROC2);
1.1       rearnsha  263:
1.7       matt      264:                /*
                    265:                 * We first need to enable access to the coprocessors.
                    266:                 */
                    267:                uint32_t cpacr = armreg_cpacr_read();
                    268:                cpacr |= __SHIFTIN(CPACR_ALL, cpacr_vfp);
                    269:                cpacr |= __SHIFTIN(CPACR_ALL, cpacr_vfp2);
                    270:                armreg_cpacr_write(cpacr);
1.1       rearnsha  271:
1.48      jmcneill  272:                arm_isb();
                    273:
1.7       matt      274:                /*
                    275:                 * If we could enable them, then they exist.
                    276:                 */
                    277:                cpacr = armreg_cpacr_read();
1.40      matt      278:                bool vfp_p = __SHIFTOUT(cpacr, cpacr_vfp2) == CPACR_ALL
                    279:                    && __SHIFTOUT(cpacr, cpacr_vfp) == CPACR_ALL;
1.28      matt      280:                if (!vfp_p) {
1.40      matt      281:                        aprint_normal_dev(ci->ci_dev,
                    282:                            "VFP access denied (CPACR=%#x)\n", cpacr);
1.28      matt      283:                        install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
                    284:                        ci->ci_vfp_id = 0;
1.37      matt      285:                        evcnt_attach_dynamic(&ci->ci_vfp_evs[0],
                    286:                            EVCNT_TYPE_TRAP, NULL, ci->ci_cpuname,
                    287:                            "vfp fpscr traps");
1.28      matt      288:                        return;
                    289:                }
1.6       matt      290:        }
                    291:
1.7       matt      292:        void *uh = install_coproc_handler(VFP_COPROC, vfp_test);
                    293:
                    294:        undefined_test = 0;
                    295:
1.21      matt      296:        const uint32_t fpsid = armreg_fpsid_read();
1.1       rearnsha  297:
                    298:        remove_coproc_handler(uh);
                    299:
                    300:        if (undefined_test != 0) {
1.4       matt      301:                aprint_normal_dev(ci->ci_dev, "No VFP detected\n");
                    302:                install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
                    303:                ci->ci_vfp_id = 0;
1.1       rearnsha  304:                return;
                    305:        }
                    306:
1.4       matt      307:        ci->ci_vfp_id = fpsid;
                    308:        switch (fpsid & ~ VFP_FPSID_REV_MSK) {
                    309:        case FPU_VFP10_ARM10E:
                    310:                model = "VFP10 R1";
                    311:                break;
                    312:        case FPU_VFP11_ARM11:
                    313:                model = "VFP11";
                    314:                break;
1.36      matt      315:        case FPU_VFP_MV88SV58XX:
                    316:                model = "VFP3";
                    317:                break;
1.7       matt      318:        case FPU_VFP_CORTEXA5:
                    319:        case FPU_VFP_CORTEXA7:
                    320:        case FPU_VFP_CORTEXA8:
                    321:        case FPU_VFP_CORTEXA9:
1.20      matt      322:        case FPU_VFP_CORTEXA15:
1.42      slp       323:        case FPU_VFP_CORTEXA15_QEMU:
1.50      skrll     324:        case FPU_VFP_CORTEXA53:
1.53    ! jmcneill  325:        case FPU_VFP_CORTEXA57:
1.37      matt      326:                if (armreg_cpacr_read() & CPACR_V7_ASEDIS) {
                    327:                        model = "VFP 4.0+";
                    328:                } else {
                    329:                        model = "NEON MPE (VFP 3.0+)";
                    330:                        cpu_neon_present = 1;
                    331:                }
1.6       matt      332:                break;
1.4       matt      333:        default:
1.36      matt      334:                aprint_normal_dev(ci->ci_dev, "unrecognized VFP version %#x\n",
1.4       matt      335:                    fpsid);
                    336:                install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
1.35      matt      337:                vfp_fpscr_changable = VFP_FPSCR_CSUM|VFP_FPSCR_ESUM
                    338:                    |VFP_FPSCR_RMODE;
                    339:                vfp_fpscr_default = 0;
1.4       matt      340:                return;
                    341:        }
1.1       rearnsha  342:
1.17      matt      343:        cpu_fpu_present = 1;
1.21      matt      344:        cpu_media_and_vfp_features[0] = armreg_mvfr0_read();
                    345:        cpu_media_and_vfp_features[1] = armreg_mvfr1_read();
1.1       rearnsha  346:        if (fpsid != 0) {
1.34      matt      347:                uint32_t f0 = armreg_mvfr0_read();
1.41      matt      348:                uint32_t f1 = armreg_mvfr1_read();
1.34      matt      349:                aprint_normal("vfp%d at %s: %s%s%s%s%s\n",
1.37      matt      350:                    device_unit(ci->ci_dev),
                    351:                    device_xname(ci->ci_dev),
1.34      matt      352:                    model,
                    353:                    ((f0 & ARM_MVFR0_ROUNDING_MASK) ? ", rounding" : ""),
                    354:                    ((f0 & ARM_MVFR0_EXCEPT_MASK) ? ", exceptions" : ""),
1.38      matt      355:                    ((f1 & ARM_MVFR1_D_NAN_MASK) ? ", NaN propagation" : ""),
1.34      matt      356:                    ((f1 & ARM_MVFR1_FTZ_MASK) ? ", denormals" : ""));
1.49      jmcneill  357:                aprint_debug("vfp%d: mvfr: [0]=%#x [1]=%#x\n",
1.37      matt      358:                    device_unit(ci->ci_dev), f0, f1);
                    359:                if (CPU_IS_PRIMARY(ci)) {
                    360:                        if (f0 & ARM_MVFR0_ROUNDING_MASK) {
                    361:                                vfp_fpscr_changable |= VFP_FPSCR_RMODE;
                    362:                        }
                    363:                        if (f1 & ARM_MVFR0_EXCEPT_MASK) {
                    364:                                vfp_fpscr_changable |= VFP_FPSCR_ESUM;
                    365:                        }
1.38      matt      366:                        // If hardware supports propagation of NaNs, select it.
1.37      matt      367:                        if (f1 & ARM_MVFR1_D_NAN_MASK) {
                    368:                                vfp_fpscr_default &= ~VFP_FPSCR_DN;
                    369:                                vfp_fpscr_changable |= VFP_FPSCR_DN;
                    370:                        }
                    371:                        // If hardware supports denormalized numbers, use it.
                    372:                        if (cpu_media_and_vfp_features[1] & ARM_MVFR1_FTZ_MASK) {
                    373:                                vfp_fpscr_default &= ~VFP_FPSCR_FZ;
                    374:                                vfp_fpscr_changable |= VFP_FPSCR_FZ;
                    375:                        }
                    376:                }
                    377:        }
                    378:        evcnt_attach_dynamic(&ci->ci_vfp_evs[0], EVCNT_TYPE_MISC, NULL,
                    379:            ci->ci_cpuname, "vfp coproc use");
                    380:        evcnt_attach_dynamic(&ci->ci_vfp_evs[1], EVCNT_TYPE_MISC, NULL,
                    381:            ci->ci_cpuname, "vfp coproc re-use");
                    382:        evcnt_attach_dynamic(&ci->ci_vfp_evs[2], EVCNT_TYPE_TRAP, NULL,
                    383:            ci->ci_cpuname, "vfp coproc fault");
1.1       rearnsha  384:        install_coproc_handler(VFP_COPROC, vfp_handler);
                    385:        install_coproc_handler(VFP_COPROC2, vfp_handler);
1.13      matt      386: #ifdef CPU_CORTEX
1.43      matt      387:        if (cpu_neon_present)
                    388:                install_coproc_handler(CORE_UNKNOWN_HANDLER, neon_handler);
1.13      matt      389: #endif
1.1       rearnsha  390: }
                    391:
                    392: /* The real handler for VFP bounces.  */
1.4       matt      393: static int
1.21      matt      394: vfp_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code)
1.1       rearnsha  395: {
1.4       matt      396:        struct cpu_info * const ci = curcpu();
1.1       rearnsha  397:
                    398:        /* This shouldn't ever happen.  */
                    399:        if (fault_code != FAULT_USER)
1.14      matt      400:                panic("VFP fault at %#x in non-user mode", frame->tf_pc);
1.1       rearnsha  401:
1.27      matt      402:        if (ci->ci_vfp_id == 0) {
1.1       rearnsha  403:                /* No VFP detected, just fault.  */
                    404:                return 1;
1.27      matt      405:        }
                    406:
                    407:        /*
1.47      matt      408:         * If we are just changing/fetching FPSCR, don't bother loading it
                    409:         * just emulate the instruction.
1.27      matt      410:         */
                    411:        if (!vfp_fpscr_handler(address, insn, frame, fault_code))
1.47      matt      412:                return 0;
1.27      matt      413:
1.47      matt      414:        /*
                    415:         * If we already own the FPU and it's enabled (and no exception), raise
                    416:         * SIGILL.  If there is an exception, drop through to raise a SIGFPE.
                    417:         */
1.46      matt      418:        if (curcpu()->ci_pcu_curlwp[PCU_FPU] == curlwp
1.47      matt      419:            && (armreg_fpexc_read() & (VFP_FPEXC_EX|VFP_FPEXC_EN)) == VFP_FPEXC_EN)
                    420:                return 1;
1.44      matt      421:
1.27      matt      422:        /*
                    423:         * Make sure we own the FP.
                    424:         */
                    425:        pcu_load(&arm_vfp_ops);
1.1       rearnsha  426:
1.21      matt      427:        uint32_t fpexc = armreg_fpexc_read();
                    428:        if (fpexc & VFP_FPEXC_EX) {
                    429:                ksiginfo_t ksi;
                    430:                KASSERT(fpexc & VFP_FPEXC_EN);
                    431:
1.37      matt      432:                curcpu()->ci_vfp_evs[2].ev_count++;
1.21      matt      433:
                    434:                /*
                    435:                 * Need the clear the exception condition so any signal
1.33      skrll     436:                 * and future use can proceed.
1.21      matt      437:                 */
1.31      skrll     438:                armreg_fpexc_write(fpexc & ~(VFP_FPEXC_EX|VFP_FPEXC_FSUM));
1.21      matt      439:
1.51      chs       440:                pcu_save(&arm_vfp_ops, curlwp);
1.33      skrll     441:
                    442:                /*
                    443:                 * XXX Need to emulate bounce instructions here to get correct
                    444:                 * XXX exception codes, etc.
                    445:                 */
1.21      matt      446:                KSI_INIT_TRAP(&ksi);
                    447:                ksi.ksi_signo = SIGFPE;
                    448:                if (fpexc & VFP_FPEXC_IXF)
                    449:                        ksi.ksi_code = FPE_FLTRES;
                    450:                else if (fpexc & VFP_FPEXC_UFF)
                    451:                        ksi.ksi_code = FPE_FLTUND;
                    452:                else if (fpexc & VFP_FPEXC_OFF)
                    453:                        ksi.ksi_code = FPE_FLTOVF;
                    454:                else if (fpexc & VFP_FPEXC_DZF)
                    455:                        ksi.ksi_code = FPE_FLTDIV;
                    456:                else if (fpexc & VFP_FPEXC_IOF)
                    457:                        ksi.ksi_code = FPE_FLTINV;
                    458:                ksi.ksi_addr = (uint32_t *)address;
                    459:                ksi.ksi_trap = 0;
                    460:                trapsignal(curlwp, &ksi);
                    461:                return 0;
                    462:        }
                    463:
1.4       matt      464:        /* Need to restart the faulted instruction.  */
                    465: //     frame->tf_pc -= INSN_SIZE;
                    466:        return 0;
                    467: }
1.1       rearnsha  468:
1.13      matt      469: #ifdef CPU_CORTEX
                    470: /* The real handler for NEON bounces.  */
                    471: static int
1.21      matt      472: neon_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code)
1.13      matt      473: {
                    474:        struct cpu_info * const ci = curcpu();
                    475:
                    476:        if (ci->ci_vfp_id == 0)
                    477:                /* No VFP detected, just fault.  */
                    478:                return 1;
                    479:
                    480:        if ((insn & 0xfe000000) != 0xf2000000
                    481:            && (insn & 0xfe000000) != 0xf4000000)
                    482:                /* Not NEON instruction, just fault.  */
                    483:                return 1;
                    484:
                    485:        /* This shouldn't ever happen.  */
                    486:        if (fault_code != FAULT_USER)
                    487:                panic("NEON fault in non-user mode");
                    488:
1.45      matt      489:        /* if we already own the FPU and it's enabled, raise SIGILL */
                    490:        if (curcpu()->ci_pcu_curlwp[PCU_FPU] == curlwp
                    491:            && (armreg_fpexc_read() & VFP_FPEXC_EN) != 0)
1.47      matt      492:                return 1;
1.43      matt      493:
1.13      matt      494:        pcu_load(&arm_vfp_ops);
                    495:
                    496:        /* Need to restart the faulted instruction.  */
                    497: //     frame->tf_pc -= INSN_SIZE;
                    498:        return 0;
                    499: }
                    500: #endif
                    501:
1.4       matt      502: static void
1.13      matt      503: vfp_state_load(lwp_t *l, u_int flags)
1.4       matt      504: {
                    505:        struct pcb * const pcb = lwp_getpcb(l);
                    506:        struct vfpreg * const fregs = &pcb->pcb_vfp;
1.1       rearnsha  507:
                    508:        /*
                    509:         * Instrument VFP usage -- if a process has not previously
                    510:         * used the VFP, mark it as having used VFP for the first time,
                    511:         * and count this event.
                    512:         *
                    513:         * If a process has used the VFP, count a "used VFP, and took
                    514:         * a trap to use it again" event.
                    515:         */
1.39      rmind     516:        if (__predict_false((flags & PCU_VALID) == 0)) {
1.37      matt      517:                curcpu()->ci_vfp_evs[0].ev_count++;
1.34      matt      518:                pcb->pcb_vfp.vfp_fpscr = vfp_fpscr_default;
1.4       matt      519:        } else {
1.37      matt      520:                curcpu()->ci_vfp_evs[1].ev_count++;
1.4       matt      521:        }
1.1       rearnsha  522:
1.39      rmind     523:        /*
                    524:         * If the VFP is already enabled we must be bouncing an instruction.
                    525:         */
                    526:        if (flags & PCU_REENABLE) {
                    527:                uint32_t fpexc = armreg_fpexc_read();
                    528:                armreg_fpexc_write(fpexc | VFP_FPEXC_EN);
                    529:                return;
                    530:        }
1.33      skrll     531:
1.39      rmind     532:        /*
                    533:         * Load and Enable the VFP (so that we can write the registers).
                    534:         */
                    535:        bool enabled = fregs->vfp_fpexc & VFP_FPEXC_EN;
                    536:        fregs->vfp_fpexc |= VFP_FPEXC_EN;
                    537:        armreg_fpexc_write(fregs->vfp_fpexc);
                    538:        if (enabled) {
1.4       matt      539:                /*
1.39      rmind     540:                 * If we think the VFP is enabled, it must have be
                    541:                 * disabled by vfp_state_release for another LWP so
                    542:                 * we can now just return.
1.4       matt      543:                 */
1.39      rmind     544:                return;
                    545:        }
1.13      matt      546:
1.39      rmind     547:        load_vfpregs(fregs);
                    548:        armreg_fpscr_write(fregs->vfp_fpscr);
1.13      matt      549:
1.39      rmind     550:        if (fregs->vfp_fpexc & VFP_FPEXC_EX) {
                    551:                /* Need to restore the exception handling state.  */
1.52      chs       552:                armreg_fpinst_write(fregs->vfp_fpinst);
1.39      rmind     553:                if (fregs->vfp_fpexc & VFP_FPEXC_FP2V)
1.52      chs       554:                        armreg_fpinst2_write(fregs->vfp_fpinst2);
1.1       rearnsha  555:        }
                    556: }
                    557:
                    558: void
1.39      rmind     559: vfp_state_save(lwp_t *l)
1.1       rearnsha  560: {
1.4       matt      561:        struct pcb * const pcb = lwp_getpcb(l);
1.39      rmind     562:        struct vfpreg * const fregs = &pcb->pcb_vfp;
1.21      matt      563:        uint32_t fpexc = armreg_fpexc_read();
1.33      skrll     564:
                    565:        /*
                    566:         * Enable the VFP (so we can read the registers).
                    567:         * Make sure the exception bit is cleared so that we can
                    568:         * safely dump the registers.
                    569:         */
1.21      matt      570:        armreg_fpexc_write((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX);
1.1       rearnsha  571:
1.4       matt      572:        fregs->vfp_fpexc = fpexc;
                    573:        if (fpexc & VFP_FPEXC_EX) {
                    574:                /* Need to save the exception handling state */
1.21      matt      575:                fregs->vfp_fpinst = armreg_fpinst_read();
                    576:                if (fpexc & VFP_FPEXC_FP2V)
                    577:                        fregs->vfp_fpinst2 = armreg_fpinst2_read();
1.1       rearnsha  578:        }
1.21      matt      579:        fregs->vfp_fpscr = armreg_fpscr_read();
1.13      matt      580:        save_vfpregs(fregs);
1.4       matt      581:
1.1       rearnsha  582:        /* Disable the VFP.  */
1.33      skrll     583:        armreg_fpexc_write(fpexc & ~VFP_FPEXC_EN);
1.1       rearnsha  584: }
                    585:
                    586: void
1.39      rmind     587: vfp_state_release(lwp_t *l)
1.1       rearnsha  588: {
1.4       matt      589:        struct pcb * const pcb = lwp_getpcb(l);
1.1       rearnsha  590:
1.39      rmind     591:        /*
                    592:         * Now mark the VFP as disabled (and our state
                    593:         * has been already saved or is being discarded).
                    594:         */
                    595:        pcb->pcb_vfp.vfp_fpexc &= ~VFP_FPEXC_EN;
1.1       rearnsha  596:
                    597:        /*
1.4       matt      598:         * Turn off the FPU so the next time a VFP instruction is issued
                    599:         * an exception happens.  We don't know if this LWP's state was
                    600:         * loaded but if we turned off the FPU for some other LWP, when
                    601:         * pcu_load invokes vfp_state_load it will see that VFP_FPEXC_EN
1.13      matt      602:         * is still set so it just restore fpexc and return since its
1.4       matt      603:         * contents are still sitting in the VFP.
1.1       rearnsha  604:         */
1.21      matt      605:        armreg_fpexc_write(armreg_fpexc_read() & ~VFP_FPEXC_EN);
1.1       rearnsha  606: }
                    607:
                    608: void
1.51      chs       609: vfp_savecontext(lwp_t *l)
1.1       rearnsha  610: {
1.51      chs       611:        pcu_save(&arm_vfp_ops, l);
1.1       rearnsha  612: }
                    613:
                    614: void
1.51      chs       615: vfp_discardcontext(lwp_t *l, bool used_p)
1.1       rearnsha  616: {
1.51      chs       617:        pcu_discard(&arm_vfp_ops, l, used_p);
1.25      matt      618: }
                    619:
                    620: bool
1.51      chs       621: vfp_used_p(const lwp_t *l)
1.25      matt      622: {
1.51      chs       623:        return pcu_valid_p(&arm_vfp_ops, l);
1.13      matt      624: }
                    625:
                    626: void
1.8       matt      627: vfp_getcontext(struct lwp *l, mcontext_t *mcp, int *flagsp)
                    628: {
1.51      chs       629:        if (vfp_used_p(l)) {
1.8       matt      630:                const struct pcb * const pcb = lwp_getpcb(l);
1.51      chs       631:
                    632:                pcu_save(&arm_vfp_ops, l);
1.8       matt      633:                mcp->__fpu.__vfpregs.__vfp_fpscr = pcb->pcb_vfp.vfp_fpscr;
                    634:                memcpy(mcp->__fpu.__vfpregs.__vfp_fstmx, pcb->pcb_vfp.vfp_regs,
                    635:                    sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx));
1.10      matt      636:                *flagsp |= _UC_FPU|_UC_ARM_VFP;
1.8       matt      637:        }
                    638: }
                    639:
                    640: void
                    641: vfp_setcontext(struct lwp *l, const mcontext_t *mcp)
                    642: {
                    643:        struct pcb * const pcb = lwp_getpcb(l);
1.51      chs       644:
                    645:        pcu_discard(&arm_vfp_ops, l, true);
1.8       matt      646:        pcb->pcb_vfp.vfp_fpscr = mcp->__fpu.__vfpregs.__vfp_fpscr;
                    647:        memcpy(pcb->pcb_vfp.vfp_regs, mcp->__fpu.__vfpregs.__vfp_fstmx,
                    648:            sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx));
                    649: }
                    650:
1.4       matt      651: #endif /* FPU_VFP */

CVSweb <webmaster@jp.NetBSD.org>