1 /*
2 * Copyright (c) 2015 Intel Corporation
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining
5 * a copy of this software and associated documentation files
6 * (the "Software"), to deal in the Software without restriction,
7 * including without limitation the rights to use, copy, modify, merge,
8 * publish, distribute, sublicense, and/or sell copies of the Software,
9 * and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22 */
23
24 #include <lk/trace.h>
25 #include <arch/x86.h>
26 #include <arch/fpu.h>
27 #include <string.h>
28 #include <kernel/thread.h>
29
30 #define LOCAL_TRACE 0
31
32 #if X86_WITH_FPU
33
34 #define FPU_MASK_ALL_EXCEPTIONS 1
35
36 /* CPUID EAX = 1 return values */
37
38 #define ECX_SSE3 (0x00000001 << 0)
39 #define ECX_SSSE3 (0x00000001 << 9)
40 #define ECX_SSE4_1 (0x00000001 << 19)
41 #define ECX_SSE4_2 (0x00000001 << 20)
42 #define EDX_FXSR (0x00000001 << 24)
43 #define EDX_SSE (0x00000001 << 25)
44 #define EDX_SSE2 (0x00000001 << 26)
45 #define EDX_FPU (0x00000001 << 0)
46
47 #define FPU_CAP(ecx, edx) ((edx & EDX_FPU) != 0)
48
49 #define SSE_CAP(ecx, edx) ( \
50 ((ecx & (ECX_SSE3 | ECX_SSSE3 | ECX_SSE4_1 | ECX_SSE4_2)) != 0) || \
51 ((edx & (EDX_SSE | EDX_SSE2)) != 0) \
52 )
53
54 #define FXSAVE_CAP(ecx, edx) ((edx & EDX_FXSR) != 0)
55
56 static int fp_supported;
57 static thread_t *fp_owner;
58
59 /* FXSAVE area comprises 512 bytes starting with 16-byte aligned */
60 static uint8_t __ALIGNED(16) fpu_init_states[512]= {0};
61
get_cpu_cap(uint32_t * ecx,uint32_t * edx)62 static void get_cpu_cap(uint32_t *ecx, uint32_t *edx) {
63 uint32_t a, b;
64
65 cpuid(1, &a, &b, ecx, edx);
66 }
67
fpu_init(void)68 void fpu_init(void) {
69 uint32_t ecx = 0, edx = 0;
70 uint16_t fcw;
71 uint32_t mxcsr;
72
73 #ifdef ARCH_X86_64
74 uint64_t x;
75 #else
76 uint32_t x;
77 #endif
78
79 fp_supported = 0;
80 fp_owner = NULL;
81
82 get_cpu_cap(&ecx, &edx);
83
84 if (!FPU_CAP(ecx, edx) || !SSE_CAP(ecx, edx) || !FXSAVE_CAP(ecx, edx))
85 return;
86
87 fp_supported = 1;
88
89 /* No x87 emul, monitor co-processor */
90
91 x = x86_get_cr0();
92 x &= ~X86_CR0_EM;
93 x |= X86_CR0_NE;
94 x |= X86_CR0_MP;
95 x86_set_cr0(x);
96
97 /* Init x87 */
98 __asm__ __volatile__ ("finit");
99 __asm__ __volatile__("fstcw %0" : "=m" (fcw));
100 #if FPU_MASK_ALL_EXCEPTIONS
101 /* mask all exceptions */
102 fcw |= 0x3f;
103 #else
104 /* unmask all exceptions */
105 fcw &= 0xffc0;
106 #endif
107 __asm__ __volatile__("fldcw %0" : : "m" (fcw));
108
109 /* Init SSE */
110 x = x86_get_cr4();
111 x |= X86_CR4_OSXMMEXPT;
112 x |= X86_CR4_OSFXSR;
113 x &= ~X86_CR4_OSXSAVE;
114 x86_set_cr4(x);
115
116 __asm__ __volatile__("stmxcsr %0" : "=m" (mxcsr));
117 #if FPU_MASK_ALL_EXCEPTIONS
118 /* mask all exceptions */
119 mxcsr = (0x3f << 7);
120 #else
121 /* unmask all exceptions */
122 mxcsr &= 0x0000003f;
123 #endif
124 __asm__ __volatile__("ldmxcsr %0" : : "m" (mxcsr));
125
126 /* save fpu initial states, and used when new thread creates */
127 __asm__ __volatile__("fxsave %0" : "=m" (fpu_init_states));
128
129 x86_set_cr0(x86_get_cr0() | X86_CR0_TS);
130 return;
131 }
132
fpu_init_thread_states(thread_t * t)133 void fpu_init_thread_states(thread_t *t) {
134 t->arch.fpu_states = (vaddr_t *)ROUNDUP(((vaddr_t)t->arch.fpu_buffer), 16);
135 memcpy(t->arch.fpu_states, fpu_init_states, sizeof(fpu_init_states));
136 }
137
fpu_context_switch(thread_t * old_thread,thread_t * new_thread)138 void fpu_context_switch(thread_t *old_thread, thread_t *new_thread) {
139 if (fp_supported == 0)
140 return;
141
142 if (new_thread != fp_owner)
143 x86_set_cr0(x86_get_cr0() | X86_CR0_TS);
144 else
145 x86_set_cr0(x86_get_cr0() & ~X86_CR0_TS);
146
147 return;
148 }
149
fpu_dev_na_handler(void)150 void fpu_dev_na_handler(void) {
151 thread_t *self;
152
153 x86_set_cr0(x86_get_cr0() & ~X86_CR0_TS);
154
155 if (fp_supported == 0)
156 return;
157
158 self = get_current_thread();
159
160 LTRACEF("owner %p self %p\n", fp_owner, self);
161 if ((fp_owner != NULL) && (fp_owner != self)) {
162 __asm__ __volatile__("fxsave %0" : "=m" (*fp_owner->arch.fpu_states));
163 __asm__ __volatile__("fxrstor %0" : : "m" (*self->arch.fpu_states));
164 }
165
166 fp_owner = self;
167 return;
168 }
169 #endif
170
171 /* End of file */
172