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