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