1// Copyright 2016 The Fuchsia Authors
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#include <arch/arm64.h>
9#include <assert.h>
10#include <bits.h>
11#include <kernel/thread.h>
12#include <trace.h>
13
14#define LOCAL_TRACE 0
15
16/* FPEN bits in the cpacr register
17 * 0 means all fpu instructions fault
18 * 3 means no faulting at all EL levels
19 * other values are not useful to us
20 */
21#define FPU_ENABLE_MASK (3 << 20)
22
23static inline bool is_fpu_enabled(uint32_t cpacr) {
24    return !!(BITS(cpacr, 21, 20) != 0);
25}
26
27static void arm64_fpu_load_state(struct thread* t) {
28    struct fpstate* fpstate = &t->arch.fpstate;
29
30    LTRACEF("cpu %u, thread %s, load fpstate %p\n", arch_curr_cpu_num(), t->name, fpstate);
31
32    static_assert(sizeof(fpstate->regs) == 16 * 32, "");
33    __asm__ volatile("ldp     q0, q1, [%0, #(0 * 32)]\n"
34                     "ldp     q2, q3, [%0, #(1 * 32)]\n"
35                     "ldp     q4, q5, [%0, #(2 * 32)]\n"
36                     "ldp     q6, q7, [%0, #(3 * 32)]\n"
37                     "ldp     q8, q9, [%0, #(4 * 32)]\n"
38                     "ldp     q10, q11, [%0, #(5 * 32)]\n"
39                     "ldp     q12, q13, [%0, #(6 * 32)]\n"
40                     "ldp     q14, q15, [%0, #(7 * 32)]\n"
41                     "ldp     q16, q17, [%0, #(8 * 32)]\n"
42                     "ldp     q18, q19, [%0, #(9 * 32)]\n"
43                     "ldp     q20, q21, [%0, #(10 * 32)]\n"
44                     "ldp     q22, q23, [%0, #(11 * 32)]\n"
45                     "ldp     q24, q25, [%0, #(12 * 32)]\n"
46                     "ldp     q26, q27, [%0, #(13 * 32)]\n"
47                     "ldp     q28, q29, [%0, #(14 * 32)]\n"
48                     "ldp     q30, q31, [%0, #(15 * 32)]\n"
49                     "msr     fpcr, %1\n"
50                     "msr     fpsr, %2\n" ::"r"(fpstate->regs),
51                     "r"((uint64_t)fpstate->fpcr),
52                     "r"((uint64_t)fpstate->fpsr));
53}
54
55__NO_SAFESTACK static void arm64_fpu_save_state(struct thread* t) {
56    struct fpstate* fpstate = &t->arch.fpstate;
57
58    LTRACEF("cpu %u, thread %s, save fpstate %p\n", arch_curr_cpu_num(), t->name, fpstate);
59
60    __asm__ volatile("stp     q0, q1, [%0, #(0 * 32)]\n"
61                     "stp     q2, q3, [%0, #(1 * 32)]\n"
62                     "stp     q4, q5, [%0, #(2 * 32)]\n"
63                     "stp     q6, q7, [%0, #(3 * 32)]\n"
64                     "stp     q8, q9, [%0, #(4 * 32)]\n"
65                     "stp     q10, q11, [%0, #(5 * 32)]\n"
66                     "stp     q12, q13, [%0, #(6 * 32)]\n"
67                     "stp     q14, q15, [%0, #(7 * 32)]\n"
68                     "stp     q16, q17, [%0, #(8 * 32)]\n"
69                     "stp     q18, q19, [%0, #(9 * 32)]\n"
70                     "stp     q20, q21, [%0, #(10 * 32)]\n"
71                     "stp     q22, q23, [%0, #(11 * 32)]\n"
72                     "stp     q24, q25, [%0, #(12 * 32)]\n"
73                     "stp     q26, q27, [%0, #(13 * 32)]\n"
74                     "stp     q28, q29, [%0, #(14 * 32)]\n"
75                     "stp     q30, q31, [%0, #(15 * 32)]\n" ::"r"(fpstate->regs));
76
77    // These are 32-bit values, but the msr instruction always uses a
78    // 64-bit destination register.
79    uint64_t fpcr, fpsr;
80    __asm__("mrs %0, fpcr\n"
81            : "=r"(fpcr));
82    __asm__("mrs %0, fpsr\n"
83            : "=r"(fpsr));
84    fpstate->fpcr = (uint32_t)fpcr;
85    fpstate->fpsr = (uint32_t)fpsr;
86
87    LTRACEF("thread %s, fpcr %x, fpsr %x\n", t->name, fpstate->fpcr, fpstate->fpsr);
88}
89
90/* save fpu state if the thread had dirtied it and disable the fpu */
91__NO_SAFESTACK void arm64_fpu_context_switch(struct thread* oldthread,
92                                             struct thread* newthread) {
93    uint64_t cpacr = ARM64_READ_SYSREG(cpacr_el1);
94    if (is_fpu_enabled((uint32_t)cpacr)) {
95        LTRACEF("saving state on thread %s\n", oldthread->name);
96
97        /* save the state */
98        arm64_fpu_save_state(oldthread);
99
100        /* disable the fpu again */
101        ARM64_WRITE_SYSREG(cpacr_el1, cpacr & ~FPU_ENABLE_MASK);
102    }
103}
104
105/* called because of a fpu instruction used exception */
106void arm64_fpu_exception(struct arm64_iframe_long* iframe, uint exception_flags) {
107    LTRACEF("cpu %u, thread %s, flags 0x%x\n", arch_curr_cpu_num(), get_current_thread()->name, exception_flags);
108
109    /* only valid to be called if exception came from lower level */
110    DEBUG_ASSERT(exception_flags & ARM64_EXCEPTION_FLAG_LOWER_EL);
111
112    uint64_t cpacr = ARM64_READ_SYSREG(cpacr_el1);
113    DEBUG_ASSERT(!is_fpu_enabled((uint32_t)cpacr));
114
115    /* enable the fpu */
116    cpacr |= FPU_ENABLE_MASK;
117    ARM64_WRITE_SYSREG(cpacr_el1, cpacr);
118
119    /* load the state from the current cpu */
120    thread_t* t = get_current_thread();
121    if (likely(t))
122        arm64_fpu_load_state(t);
123}
124