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 <config.h>
10#include <mode/machine/registerset.h>
11#include <util.h>
12#include <mode/machine.h>
13
14
15#define CPACR_CP_10_SHIFT_POS        20
16#define CPACR_CP_11_SHIFT_POS        22
17#define CPACR_CP_ACCESS_DISABLE      0x0
18#define CPACR_CP_ACCESS_PL1          0x1
19#define CPACR_CP_ACCESS_PLX          0x3
20
21#define CPACR_D32DIS_BIT             30
22#define CPACR_ASEDIS_BIT             31
23
24#define FPSID_SW_BIT                 23
25#define FPSID_SUBARCH_SHIFT_POS      16
26
27#define FPEXC_EX_BIT                 31
28#define FPEXC_EN_BIT                 30
29
30#if defined(CONFIG_ARM_CORTEX_A7) || defined(CONFIG_ARM_CORTEX_A9)
31#define FPEXC_DEX_BIT                29
32#endif
33
34#define FPEXC_DEX_BIT                29
35#define FPEXC_FP2V_BIT               28
36
37extern bool_t isFPUEnabledCached[CONFIG_MAX_NUM_NODES];
38
39static void clearEnFPEXC(void)
40{
41    word_t fpexc;
42    VMRS(FPEXC, fpexc);
43    fpexc &= ~BIT(FPEXC_EN_BIT);
44    VMSR(FPEXC, fpexc);
45}
46
47#if defined(CONFIG_ARM_HYPERVISOR_SUPPORT) && defined(CONFIG_HAVE_FPU)
48
49#define HCPTR_CP10_BIT  10
50#define HCPTR_CP11_BIT  11
51#define HCPTR_TASE_BIT  15
52#define HCPTR_MASK      ~(BIT(HCPTR_CP10_BIT) | BIT(HCPTR_CP11_BIT) | BIT(HCPTR_TASE_BIT))
53
54/* enable FPU accesses in Hyp mode */
55static inline void enableFpuInstInHyp(void)
56{
57    if (!ARCH_NODE_STATE(armHSFPUEnabled)) {
58        setHCPTR(getHCPTR() & HCPTR_MASK);
59        ARCH_NODE_STATE(armHSFPUEnabled) = true;
60    }
61}
62
63/* trap PL0/PL1 FPU operations to Hyp mode and disable FPU accesses in Hyp */
64static inline void trapFpuInstToHyp(void)
65{
66    if (ARCH_NODE_STATE(armHSFPUEnabled)) {
67        setHCPTR(getHCPTR() | ~HCPTR_MASK);
68        ARCH_NODE_STATE(armHSFPUEnabled) = false;
69    }
70}
71
72#else
73
74static inline void enableFpuInstInHyp(void) {}
75static inline void trapFpuInstToHyp(void) {}
76
77#endif
78#ifdef CONFIG_HAVE_FPU
79
80/* This variable is set at init time to true if the FPU supports 32 registers (d0-d31) */
81extern bool_t isFPUD32SupportedCached;
82
83static void setEnFPEXC(void)
84{
85    word_t fpexc;
86    VMRS(FPEXC, fpexc);
87    fpexc |=  BIT(FPEXC_EN_BIT);
88    VMSR(FPEXC, fpexc);
89}
90/* Store state in the FPU registers into memory. */
91static inline void saveFpuState(user_fpu_state_t *dest)
92{
93    word_t fpexc;
94
95    /* Fetch FPEXC. */
96    VMRS(FPEXC, fpexc);
97
98#if defined(CONFIG_ARM_CORTEX_A7) || defined(CONFIG_ARM_CORTEX_A9)
99    /*
100    * Reset DEX bit to 0 in case a subarchitecture sets it.
101    * For example, Cortex-A7/A9 set this bit on deprecated vector VFP operations.
102    */
103    if (unlikely(fpexc & BIT(FPEXC_DEX_BIT))) {
104        fpexc &= ~BIT(FPEXC_DEX_BIT);
105        VMSR(FPEXC, fpexc);
106    }
107#endif
108
109    dest->fpexc = fpexc;
110
111    if (config_set(CONFIG_ARM_HYPERVISOR_SUPPORT)) {
112        /* before touching the regsiters, we need to set the EN bit */
113        setEnFPEXC();
114    }
115
116    /* We don't support asynchronous exceptions */
117    assert((dest->fpexc & BIT(FPEXC_EX_BIT)) == 0);
118
119    if (isFPUD32SupportedCached) {
120        register word_t regs_d16_d31 asm("ip") = (word_t) &dest->fpregs[16];
121        asm volatile(
122            ".word 0xeccc0b20        \n"    /*  vstmia  ip, {d16-d31} */
123            :
124            : "r"(regs_d16_d31)
125            : "memory"
126        );
127    }
128
129    register word_t regs_d0_d15 asm("r2") = (word_t) &dest->fpregs[0];
130    asm volatile(
131        /* Store d0 - d15 to memory */
132        ".word 0xec820b20       \n" /* vstmia  r2, {d0-d15}" */
133        :
134        : "r"(regs_d0_d15)
135    );
136
137    /* Store FPSCR. */
138    VMRS(FPSCR, dest->fpscr);
139
140    if (config_set(CONFIG_ARM_HYPERVISOR_SUPPORT)) {
141        /* Restore the FPEXC. */
142        VMSR(FPEXC, fpexc);
143    }
144}
145
146/* Enable the FPU to be used without faulting.
147 * Required even if the kernel attempts to use the FPU.
148 *
149 * The FPEXC_EN bit is not set immediately in HYP mode for
150 * the following reason:
151 *
152 *    A VM can set/clear the EN bit in the FPEXC in order to
153 *    trap FPU accesses, implementing its own save/restore
154 *    functions. Thus, we need to save the FPEXC without modifying
155 *    it.
156 *
157 */
158
159static inline void enableFpu(void)
160{
161#ifdef CONFIG_ARM_HYPERVISOR_SUPPORT
162    enableFpuInstInHyp();
163    if (!ARCH_NODE_STATE(armHSVCPUActive)) {
164        setEnFPEXC();
165    }
166#else
167    setEnFPEXC();
168#endif
169    isFPUEnabledCached[SMP_TERNARY(getCurrentCPUIndex(), 0)] = true;
170}
171
172/* Check if FPU is enable */
173static inline bool_t isFpuEnable(void)
174{
175    return isFPUEnabledCached[SMP_TERNARY(getCurrentCPUIndex(), 0)];
176}
177
178/* Load FPU state from memory into the FPU registers. */
179static inline void loadFpuState(user_fpu_state_t *src)
180{
181    if (config_set(CONFIG_ARM_HYPERVISOR_SUPPORT)) {
182        /* now we need to enable the EN bit in FPEXC */
183        setEnFPEXC();
184    }
185    register word_t regs_d16_d31 asm("r2") = (word_t) &src->fpregs[16];
186    if (isFPUD32SupportedCached) {
187        asm volatile(
188            ".word 0xecd20b20       \n" /*   vldmia  r2, {d16-d31} */
189            :: "r"(regs_d16_d31)
190        );
191    }
192
193    register word_t regs_d0_d15 asm("r0") = (word_t) &src->fpregs[0];
194    asm volatile(
195        /* Restore d0 - d15 from memory */
196        ".word 0xec900b20         \n"    /*  vldmia  r0, {d0-d15} */
197        :: "r"(regs_d0_d15)
198    );
199
200    /* Load FPSCR. */
201    VMSR(FPSCR, src->fpscr);
202
203    /* Restore FPEXC. */
204    VMSR(FPEXC, src->fpexc);
205}
206
207#endif /* CONFIG_HAVE_FPU */
208
209
210/* Disable the FPU so that usage of it causes a fault.
211 * In HYP mode, when a native thread is running:
212 *     if the EN FPEXC is set, trapFpuHyp causes ensures a trap to HYP mode;
213 *     if the EN is off, an undefined instruction exception is triggered.
214 *
215 * Either way, the kernel gets back in control.
216 * When a VM is running, always, a trap to HYP mode is triggered.
217 * Thus, we do not need to modify the EN bit of the FPEXC.
218 */
219static inline void disableFpu(void)
220{
221    if (config_set(CONFIG_ARM_HYPERVISOR_SUPPORT)) {
222        trapFpuInstToHyp();
223    } else {
224        clearEnFPEXC();
225    }
226    isFPUEnabledCached[SMP_TERNARY(getCurrentCPUIndex(), 0)] = false;
227}
228
229