1/*
2 * Copyright 2020, Data61, CSIRO (ABN 41 687 119 230)
3 *
4 * SPDX-License-Identifier: GPL-2.0-only
5 */
6
7#pragma once
8
9#include <mode/machine/registerset.h>
10
11extern bool_t isFPUEnabledCached[CONFIG_MAX_NUM_NODES];
12
13#ifdef CONFIG_HAVE_FPU
14/* Store state in the FPU registers into memory. */
15static inline void saveFpuState(user_fpu_state_t *dest)
16{
17    word_t temp;
18
19    asm volatile(
20        /* SIMD and floating-point register file */
21        "stp     q0, q1, [%1, #16 * 0]      \n"
22        "stp     q2, q3, [%1, #16 * 2]      \n"
23        "stp     q4, q5, [%1, #16 * 4]      \n"
24        "stp     q6, q7, [%1, #16 * 6]      \n"
25        "stp     q8, q9, [%1, #16 * 8]      \n"
26        "stp     q10, q11, [%1, #16 * 10]   \n"
27        "stp     q12, q13, [%1, #16 * 12]   \n"
28        "stp     q14, q15, [%1, #16 * 14]   \n"
29        "stp     q16, q17, [%1, #16 * 16]   \n"
30        "stp     q18, q19, [%1, #16 * 18]   \n"
31        "stp     q20, q21, [%1, #16 * 20]   \n"
32        "stp     q22, q23, [%1, #16 * 22]   \n"
33        "stp     q24, q25, [%1, #16 * 24]   \n"
34        "stp     q26, q27, [%1, #16 * 26]   \n"
35        "stp     q28, q29, [%1, #16 * 28]   \n"
36        "stp     q30, q31, [%1, #16 * 30]   \n"
37
38        /* FP control and status registers */
39        "mrs     %0, fpsr                   \n"
40        "str     %w0, [%1, #16 * 32]        \n"
41        "mrs     %0, fpcr                   \n"
42        "str     %w0, [%1, #16 * 32 + 4]    \n"
43        : "=&r"(temp)
44        : "r"(dest)
45        : "memory"
46    );
47}
48
49/* Load FPU state from memory into the FPU registers. */
50static inline void loadFpuState(user_fpu_state_t *src)
51{
52    word_t temp;
53
54    asm volatile(
55        /* SIMD and floating-point register file */
56        "ldp     q0, q1, [%1, #16 * 0]      \n"
57        "ldp     q2, q3, [%1, #16 * 2]      \n"
58        "ldp     q4, q5, [%1, #16 * 4]      \n"
59        "ldp     q6, q7, [%1, #16 * 6]      \n"
60        "ldp     q8, q9, [%1, #16 * 8]      \n"
61        "ldp     q10, q11, [%1, #16 * 10]   \n"
62        "ldp     q12, q13, [%1, #16 * 12]   \n"
63        "ldp     q14, q15, [%1, #16 * 14]   \n"
64        "ldp     q16, q17, [%1, #16 * 16]   \n"
65        "ldp     q18, q19, [%1, #16 * 18]   \n"
66        "ldp     q20, q21, [%1, #16 * 20]   \n"
67        "ldp     q22, q23, [%1, #16 * 22]   \n"
68        "ldp     q24, q25, [%1, #16 * 24]   \n"
69        "ldp     q26, q27, [%1, #16 * 26]   \n"
70        "ldp     q28, q29, [%1, #16 * 28]   \n"
71        "ldp     q30, q31, [%1, #16 * 30]  \n"
72
73        /* FP control and status registers */
74        "ldr     %w0, [%1, #16 * 32]        \n"
75        "msr     fpsr, %0                   \n"
76        "ldr     %w0, [%1, #16 * 32 + 4]    \n"
77        "msr     fpcr, %0                   \n"
78        : "=&r"(temp)
79        : "r"(src)
80        : "memory"
81    );
82}
83
84/* Trap any FPU related instructions to EL2 */
85static inline void enableTrapFpu(void)
86{
87    word_t cptr;
88    MRS("cptr_el2", cptr);
89    cptr |= (BIT(10) | BIT(31));
90    MSR("cptr_el2", cptr);
91}
92
93/* Disable trapping FPU instructions to EL2 */
94static inline void disableTrapFpu(void)
95{
96    word_t cptr;
97    MRS("cptr_el2", cptr);
98    cptr &= ~(BIT(10) | BIT(31));
99    MSR("cptr_el2", cptr);
100}
101
102/* Enable FPU access in EL0 and EL1 */
103static inline void enableFpuEL01(void)
104{
105    word_t cpacr;
106    MRS("cpacr_el1", cpacr);
107    cpacr |= (3 << CPACR_EL1_FPEN);
108    MSR("cpacr_el1", cpacr);
109}
110
111/* Disable FPU access in EL0 */
112static inline void disableFpuEL0(void)
113{
114    word_t cpacr;
115    MRS("cpacr_el1", cpacr);
116    cpacr &= ~(3 << CPACR_EL1_FPEN);
117    cpacr |= (1 << CPACR_EL1_FPEN);
118    MSR("cpacr_el1", cpacr);
119}
120
121/* Enable the FPU to be used without faulting.
122 * Required even if the kernel attempts to use the FPU. */
123static inline void enableFpu(void)
124{
125    if (config_set(CONFIG_ARM_HYPERVISOR_SUPPORT)) {
126        disableTrapFpu();
127    } else {
128        enableFpuEL01();
129    }
130    isFPUEnabledCached[SMP_TERNARY(getCurrentCPUIndex(), 0)] = true;
131}
132
133static inline bool_t isFpuEnable(void)
134{
135    return isFPUEnabledCached[SMP_TERNARY(getCurrentCPUIndex(), 0)];
136}
137#endif /* CONFIG_HAVE_FPU */
138
139/* Disable the FPU so that usage of it causes a fault */
140static inline void disableFpu(void)
141{
142    if (config_set(CONFIG_ARM_HYPERVISOR_SUPPORT)) {
143        enableTrapFpu();
144    } else {
145        disableFpuEL0();
146    }
147    isFPUEnabledCached[SMP_TERNARY(getCurrentCPUIndex(), 0)] = false;
148}
149
150