1 /*
2  * Copyright 2024 The Hafnium Authors.
3  *
4  * Use of this source code is governed by a BSD-style
5  * license that can be found in the LICENSE file or at
6  * https://opensource.org/licenses/BSD-3-Clause.
7  */
8 
9 #include "msr.h"
10 #include "sysregs.h"
11 
12 /** Disable FPU/Adv. SIMD/SVE traps. */
arch_sve_disable_traps(void)13 void arch_sve_disable_traps(void)
14 {
15 	uint64_t cptr_el2_val;
16 
17 	cptr_el2_val = read_msr(CPTR_EL2);
18 	/* Disable Adv. SIMD/SVE traps at EL2/1/0. */
19 	if (has_vhe_support()) {
20 		cptr_el2_val |= (CPTR_EL2_VHE_ZEN | CPTR_EL2_VHE_FPEN);
21 	} else {
22 		cptr_el2_val &= ~(CPTR_EL2_TFP | CPTR_EL2_TZ);
23 	}
24 
25 	write_msr(CPTR_EL2, cptr_el2_val);
26 	isb();
27 }
28 
29 /** Enable SVE traps (but leave FPU/Adv. SIMD traps disabled). */
arch_sve_enable_traps(void)30 void arch_sve_enable_traps(void)
31 {
32 	uint64_t cptr_el2_val;
33 
34 	cptr_el2_val = read_msr(CPTR_EL2);
35 	/* Enable SVE traps, disable Adv. SIMD traps at EL2/1/0. */
36 	if (has_vhe_support()) {
37 		cptr_el2_val &= ~CPTR_EL2_VHE_ZEN;
38 		cptr_el2_val |= CPTR_EL2_VHE_FPEN;
39 	} else {
40 		cptr_el2_val &= ~CPTR_EL2_TFP;
41 		cptr_el2_val |= CPTR_EL2_TZ;
42 	}
43 
44 	write_msr(CPTR_EL2, cptr_el2_val);
45 	isb();
46 }
47 
48 /** Returns the SVE implemented VL in bytes (constrained by ZCR_EL3.LEN) */
arch_sve_vector_length_get(void)49 static uint64_t arch_sve_vector_length_get(void)
50 {
51 	uint64_t vl;
52 
53 	__asm__ volatile(
54 		".arch_extension sve;"
55 		"rdvl %0, #1;"
56 		".arch_extension nosve;"
57 		: "=r"(vl));
58 
59 	return vl;
60 }
61 
arch_sve_configure_vector_length(void)62 void arch_sve_configure_vector_length(void)
63 {
64 	uint64_t vl_bits;
65 	uint32_t zcr_len;
66 
67 	/*
68 	 * Set ZCR_EL2.LEN to the maximum vector length permitted by the
69 	 * architecture which applies to EL2 and lower ELs (limited by the
70 	 * HW implementation).
71 	 * This is done so that the VL read by arch_cpu_sve_len_get isn't
72 	 * constrained by EL2 and thus indirectly retrieves the value
73 	 * constrained by EL3 which applies to EL3 and lower ELs (limited by
74 	 * the HW implementation).
75 	 */
76 	write_msr(MSR_ZCR_EL2, ZCR_LEN_MAX);
77 	isb();
78 
79 	vl_bits = arch_sve_vector_length_get() << 3;
80 	zcr_len = (vl_bits >> 7) - 1;
81 
82 	/*
83 	 * Set ZCR_EL2.LEN to the discovered value which contrains the VL at
84 	 * EL2 and lower ELs to the value set by EL3.
85 	 */
86 	write_msr(MSR_ZCR_EL2, zcr_len & ZCR_LEN_MASK);
87 	isb();
88 }
89