1 /*
2 * Copyright (c) 2015 Google Inc. All rights reserved
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
9 #include <arch/arm64.h>
10 #include <kernel/thread.h>
11 #include <lk/trace.h>
12
13 #define LOCAL_TRACE 0
14
15 static struct fpstate *current_fpstate[SMP_MAX_CPUS];
16
arm64_fpu_load_state(struct thread * t)17 static void arm64_fpu_load_state(struct thread *t) {
18 uint cpu = arch_curr_cpu_num();
19 struct fpstate *fpstate = &t->arch.fpstate;
20
21 if (fpstate == current_fpstate[cpu] && fpstate->current_cpu == cpu) {
22 LTRACEF("cpu %d, thread %s, fpstate already valid\n", cpu, t->name);
23 return;
24 }
25 LTRACEF("cpu %d, thread %s, load fpstate %p, last cpu %d, last fpstate %p\n",
26 cpu, t->name, fpstate, fpstate->current_cpu, current_fpstate[cpu]);
27 fpstate->current_cpu = cpu;
28 current_fpstate[cpu] = fpstate;
29
30
31 STATIC_ASSERT(sizeof(fpstate->regs) == 16 * 32);
32 __asm__ volatile("ldp q0, q1, [%0, #(0 * 32)]\n"
33 "ldp q2, q3, [%0, #(1 * 32)]\n"
34 "ldp q4, q5, [%0, #(2 * 32)]\n"
35 "ldp q6, q7, [%0, #(3 * 32)]\n"
36 "ldp q8, q9, [%0, #(4 * 32)]\n"
37 "ldp q10, q11, [%0, #(5 * 32)]\n"
38 "ldp q12, q13, [%0, #(6 * 32)]\n"
39 "ldp q14, q15, [%0, #(7 * 32)]\n"
40 "ldp q16, q17, [%0, #(8 * 32)]\n"
41 "ldp q18, q19, [%0, #(9 * 32)]\n"
42 "ldp q20, q21, [%0, #(10 * 32)]\n"
43 "ldp q22, q23, [%0, #(11 * 32)]\n"
44 "ldp q24, q25, [%0, #(12 * 32)]\n"
45 "ldp q26, q27, [%0, #(13 * 32)]\n"
46 "ldp q28, q29, [%0, #(14 * 32)]\n"
47 "ldp q30, q31, [%0, #(15 * 32)]\n"
48 "msr fpcr, %1\n"
49 "msr fpsr, %2\n"
50 :: "r"(fpstate), "r"(fpstate->fpcr), "r"(fpstate->fpsr));
51 }
52
arm64_fpu_save_state(struct thread * t)53 void arm64_fpu_save_state(struct thread *t) {
54 struct fpstate *fpstate = &t->arch.fpstate;
55 __asm__ volatile("stp q0, q1, [%2, #(0 * 32)]\n"
56 "stp q2, q3, [%2, #(1 * 32)]\n"
57 "stp q4, q5, [%2, #(2 * 32)]\n"
58 "stp q6, q7, [%2, #(3 * 32)]\n"
59 "stp q8, q9, [%2, #(4 * 32)]\n"
60 "stp q10, q11, [%2, #(5 * 32)]\n"
61 "stp q12, q13, [%2, #(6 * 32)]\n"
62 "stp q14, q15, [%2, #(7 * 32)]\n"
63 "stp q16, q17, [%2, #(8 * 32)]\n"
64 "stp q18, q19, [%2, #(9 * 32)]\n"
65 "stp q20, q21, [%2, #(10 * 32)]\n"
66 "stp q22, q23, [%2, #(11 * 32)]\n"
67 "stp q24, q25, [%2, #(12 * 32)]\n"
68 "stp q26, q27, [%2, #(13 * 32)]\n"
69 "stp q28, q29, [%2, #(14 * 32)]\n"
70 "stp q30, q31, [%2, #(15 * 32)]\n"
71 "mrs %0, fpcr\n"
72 "mrs %1, fpsr\n"
73 : "=r"(fpstate->fpcr), "=r"(fpstate->fpsr)
74 : "r"(fpstate));
75
76 LTRACEF("thread %s, fpcr %x, fpsr %x\n", t->name, fpstate->fpcr, fpstate->fpsr);
77 }
78
arm64_fpu_exception(struct arm64_iframe_long * iframe)79 void arm64_fpu_exception(struct arm64_iframe_long *iframe) {
80 uint32_t cpacr = ARM64_READ_SYSREG(cpacr_el1);
81 if (((cpacr >> 20) & 3) != 3) {
82 cpacr |= 3 << 20;
83 ARM64_WRITE_SYSREG(cpacr_el1, cpacr);
84 thread_t *t = get_current_thread();
85 if (likely(t))
86 arm64_fpu_load_state(t);
87 return;
88 }
89 }
90