1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3  * Copyright (c) 2015, Linaro Limited
4  */
5 
6 #include <arm.h>
7 #include <assert.h>
8 #include <kernel/vfp.h>
9 #include "vfp_private.h"
10 
11 #ifdef ARM32
vfp_is_enabled(void)12 bool vfp_is_enabled(void)
13 {
14 	return !!(vfp_read_fpexc() & FPEXC_EN);
15 }
16 
vfp_enable(void)17 void vfp_enable(void)
18 {
19 	vfp_write_fpexc(vfp_read_fpexc() | FPEXC_EN);
20 }
21 
vfp_disable(void)22 void vfp_disable(void)
23 {
24 	vfp_write_fpexc(vfp_read_fpexc() & ~FPEXC_EN);
25 }
26 
vfp_lazy_save_state_init(struct vfp_state * state)27 void vfp_lazy_save_state_init(struct vfp_state *state)
28 {
29 	uint32_t fpexc = vfp_read_fpexc();
30 
31 	state->fpexc = fpexc;
32 	vfp_write_fpexc(fpexc & ~FPEXC_EN);
33 }
34 
vfp_lazy_save_state_final(struct vfp_state * state,bool force_save)35 void vfp_lazy_save_state_final(struct vfp_state *state, bool force_save)
36 {
37 	if ((state->fpexc & FPEXC_EN) || force_save) {
38 		uint32_t fpexc = vfp_read_fpexc();
39 
40 		assert(!(fpexc & FPEXC_EN));
41 		vfp_write_fpexc(fpexc | FPEXC_EN);
42 		state->fpscr = vfp_read_fpscr();
43 		vfp_save_extension_regs(state->reg);
44 		vfp_write_fpexc(fpexc);
45 	}
46 }
47 
vfp_lazy_restore_state(struct vfp_state * state,bool full_state)48 void vfp_lazy_restore_state(struct vfp_state *state, bool full_state)
49 {
50 
51 	if (full_state) {
52 		/*
53 		 * Only restore VFP registers if they have been touched as they
54 		 * otherwise are intact.
55 		 */
56 
57 		/* FPEXC is restored to what's in state->fpexc below */
58 		vfp_write_fpexc(vfp_read_fpexc() | FPEXC_EN);
59 
60 		vfp_write_fpscr(state->fpscr);
61 		vfp_restore_extension_regs(state->reg);
62 	}
63 	vfp_write_fpexc(state->fpexc);
64 }
65 #endif /* ARM32 */
66 
67 #ifdef ARM64
vfp_is_enabled(void)68 bool vfp_is_enabled(void)
69 {
70 	return (CPACR_EL1_FPEN(read_cpacr_el1()) & CPACR_EL1_FPEN_EL0EL1);
71 }
72 
vfp_enable(void)73 void vfp_enable(void)
74 {
75 	uint32_t val = read_cpacr_el1();
76 
77 	val |= (CPACR_EL1_FPEN_EL0EL1 << CPACR_EL1_FPEN_SHIFT);
78 	write_cpacr_el1(val);
79 	isb();
80 }
81 
vfp_disable(void)82 void vfp_disable(void)
83 {
84 	uint32_t val = read_cpacr_el1();
85 
86 	val &= ~(CPACR_EL1_FPEN_MASK << CPACR_EL1_FPEN_SHIFT);
87 	write_cpacr_el1(val);
88 	isb();
89 }
90 
vfp_lazy_save_state_init(struct vfp_state * state)91 void vfp_lazy_save_state_init(struct vfp_state *state)
92 {
93 	state->cpacr_el1 = read_cpacr_el1();
94 	vfp_disable();
95 }
96 
vfp_lazy_save_state_final(struct vfp_state * state,bool force_save)97 void vfp_lazy_save_state_final(struct vfp_state *state, bool force_save)
98 {
99 	if ((CPACR_EL1_FPEN(state->cpacr_el1) & CPACR_EL1_FPEN_EL0EL1) ||
100 	    force_save) {
101 		assert(!vfp_is_enabled());
102 		vfp_enable();
103 		state->fpcr = read_fpcr();
104 		state->fpsr = read_fpsr();
105 		vfp_save_extension_regs(state->reg);
106 		vfp_disable();
107 	}
108 }
109 
vfp_lazy_restore_state(struct vfp_state * state,bool full_state)110 void vfp_lazy_restore_state(struct vfp_state *state, bool full_state)
111 {
112 	if (full_state) {
113 		/*
114 		 * Only restore VFP registers if they have been touched as they
115 		 * otherwise are intact.
116 		 */
117 
118 		/* CPACR_EL1 is restored to what's in state->cpacr_el1 below */
119 		vfp_enable();
120 		write_fpcr(state->fpcr);
121 		write_fpsr(state->fpsr);
122 		vfp_restore_extension_regs(state->reg);
123 	}
124 	write_cpacr_el1(state->cpacr_el1);
125 	isb();
126 }
127 #endif /* ARM64 */
128