1 /*
2 * Copyright (c) 2013-2015 Travis Geiselbrecht
3 *
4 * Use of this source code is governed by a MIT-style
5 * license that can be found in the LICENSE file or at
6 * https://opensource.org/licenses/MIT
7 */
8 #include <arch/arm.h>
9 #include <assert.h>
10 #include <lk/trace.h>
11 #include <stdbool.h>
12 #include <string.h>
13 #include <kernel/thread.h>
14
15 #define LOCAL_TRACE 0
16
is_16regs(void)17 static inline bool is_16regs(void) {
18 uint32_t mvfr0;
19 __asm__ volatile("vmrs %0, MVFR0" : "=r"(mvfr0));
20
21 return (mvfr0 & 0xf) == 1;
22 }
23
read_fpexc(void)24 static inline uint32_t read_fpexc(void) {
25 uint32_t val;
26 /* use legacy encoding of vmsr reg, fpexc */
27 __asm__("mrc p10, 7, %0, c8, c0, 0" : "=r" (val));
28 return val;
29 }
30
write_fpexc(uint32_t val)31 static inline void write_fpexc(uint32_t val) {
32 /* use legacy encoding of vmrs fpexc, reg */
33 __asm__ volatile("mcr p10, 7, %0, c8, c0, 0" :: "r" (val));
34 }
35
arm_fpu_set_enable(bool enable)36 void arm_fpu_set_enable(bool enable) {
37 /* set enable bit in fpexc */
38 write_fpexc(enable ? (1<<30) : 0);
39 }
40
41 #if ARM_WITH_VFP
arm_fpu_undefined_instruction(struct arm_iframe * frame)42 void arm_fpu_undefined_instruction(struct arm_iframe *frame) {
43 thread_t *t = get_current_thread();
44
45 if (unlikely(arch_in_int_handler())) {
46 panic("floating point code in irq context. pc 0x%x\n", frame->pc);
47 }
48
49 LTRACEF("enabling fpu on thread %p\n", t);
50
51 t->arch.fpused = true;
52 arm_fpu_thread_swap(NULL, t);
53
54 /* make sure the irq glue leaves the floating point unit enabled on the way out */
55 frame->fpexc |= (1<<30);
56 }
57
arm_fpu_thread_initialize(struct thread * t)58 void arm_fpu_thread_initialize(struct thread *t) {
59 /* zero the fpu register state */
60 memset(t->arch.fpregs, 0, sizeof(t->arch.fpregs));
61
62 t->arch.fpexc = (1<<30);
63 t->arch.fpscr = 0;
64 t->arch.fpused = false;
65 }
66
arm_fpu_thread_swap(struct thread * oldthread,struct thread * newthread)67 void arm_fpu_thread_swap(struct thread *oldthread, struct thread *newthread) {
68 LTRACEF("old %p (%d), new %p (%d)\n",
69 oldthread, oldthread ? oldthread->arch.fpused : 0,
70 newthread, newthread ? newthread->arch.fpused : 0);
71
72 if (oldthread) {
73 if (oldthread->arch.fpused) {
74 /* save the old state */
75 uint32_t fpexc;
76 fpexc = read_fpexc();
77
78 oldthread->arch.fpexc = fpexc;
79
80 /* make sure that the fpu is enabled, so the next instructions won't fault */
81 arm_fpu_set_enable(true);
82
83 __asm__ volatile("vmrs %0, fpscr" : "=r" (oldthread->arch.fpscr));
84 __asm__ volatile("vstm %0, { d0-d15 }" :: "r" (&oldthread->arch.fpregs[0]));
85 #if(!__ARM_ARCH_7R__)
86 if (!is_16regs()) {
87 __asm__ volatile("vstm %0, { d16-d31 }" :: "r" (&oldthread->arch.fpregs[16]));
88 }
89 #endif
90
91 arm_fpu_set_enable(false);
92 }
93 }
94
95 if (newthread) {
96 if (newthread->arch.fpused) {
97 // load the new state
98 arm_fpu_set_enable(true);
99 __asm__ volatile("vmsr fpscr, %0" :: "r" (newthread->arch.fpscr));
100
101 __asm__ volatile("vldm %0, { d0-d15 }" :: "r" (&newthread->arch.fpregs[0]));
102 #if(!__ARM_ARCH_7R__)
103 if (!is_16regs()) {
104 __asm__ volatile("vldm %0, { d16-d31 }" :: "r" (&newthread->arch.fpregs[16]));
105 }
106 #endif
107 write_fpexc(newthread->arch.fpexc);
108 } else {
109 arm_fpu_set_enable(false);
110 }
111 }
112 }
113 #endif
114