1/*
2 * SPDX-License-Identifier: BSD-3-Clause
3 * SPDX-FileCopyrightText: Copyright TF-RMM Contributors.
4 */
5
6/*
7 * void save_sve_zcr_fpu_state(unsigned long *data)
8 */
9
10#include <asm_macros.S>
11#include <sve.h>
12
13.globl save_sve_zcr_fpu_state
14.globl save_sve_z_state
15.globl save_sve_p_ffr_state
16.globl restore_sve_zcr_fpu_state
17.globl restore_sve_z_state
18.globl restore_sve_p_ffr_state
19
20func save_sve_zcr_fpu_state
21.arch_extension sve
22	/* Save the FPSR/FPCR */
23	mrs	x1, fpsr
24	mrs	x2, fpcr
25	stp	x1, x2, [x0], #16
26
27	/* Save the ZCR_EL1/EL2 */
28	mrs	x1, zcr_el1
29	mrs	x2, zcr_el2
30	stp	x1, x2, [x0]
31
32	ret
33.arch_extension nosve
34endfunc save_sve_zcr_fpu_state
35
36/*
37 * void save_sve_z_state(unsigned char *data)
38 */
39func save_sve_z_state
40.arch_extension sve
41	/* maximise VL */
42	mrs	x1, zcr_el2
43	orr	x2, x1, #SVE_VLA_ZCR_LEN_MAX
44	msr	zcr_el2, x2
45
46	/* Save the z register bank to memory. */
47	str 	z0, [x0, #0, MUL VL]
48	str	z1, [x0, #1, MUL VL]
49	str	z2, [x0, #2, MUL VL]
50	str	z3, [x0, #3, MUL VL]
51	str	z4, [x0, #4, MUL VL]
52	str	z5, [x0, #5, MUL VL]
53	str	z6, [x0, #6, MUL VL]
54	str	z7, [x0, #7, MUL VL]
55	str	z8, [x0, #8, MUL VL]
56	str	z9, [x0, #9, MUL VL]
57	str	z10, [x0, #10, MUL VL]
58	str	z11, [x0, #11, MUL VL]
59	str	z12, [x0, #12, MUL VL]
60	str	z13, [x0, #13, MUL VL]
61	str	z14, [x0, #14, MUL VL]
62	str	z15, [x0, #15, MUL VL]
63	str	z16, [x0, #16, MUL VL]
64	str	z17, [x0, #17, MUL VL]
65	str	z18, [x0, #18, MUL VL]
66	str	z19, [x0, #19, MUL VL]
67	str	z20, [x0, #20, MUL VL]
68	str	z21, [x0, #21, MUL VL]
69	str	z22, [x0, #22, MUL VL]
70	str	z23, [x0, #23, MUL VL]
71	str	z24, [x0, #24, MUL VL]
72	str	z25, [x0, #25, MUL VL]
73	str	z26, [x0, #26, MUL VL]
74	str	z27, [x0, #27, MUL VL]
75	str	z28, [x0, #28, MUL VL]
76	str	z29, [x0, #29, MUL VL]
77	str	z30, [x0, #30, MUL VL]
78	str	z31, [x0, #31, MUL VL]
79
80	/* restore back zcr */
81	msr	zcr_el2, x1
82
83	ret
84.arch_extension nosve
85endfunc save_sve_z_state
86
87/*
88 * void save_sve_p_ffr_state(unsigned char *data)
89 */
90func save_sve_p_ffr_state
91.arch_extension sve
92	/* maximise VL */
93	mrs	x1, zcr_el2
94	orr	x2, x1, #SVE_VLA_ZCR_LEN_MAX
95	msr	zcr_el2, x2
96
97	/* Save the P register bank to memory. */
98	str 	p0, [x0, #0, MUL VL]
99	str 	p1, [x0, #1, MUL VL]
100	str 	p2, [x0, #2, MUL VL]
101	str 	p3, [x0, #3, MUL VL]
102	str 	p4, [x0, #4, MUL VL]
103	str 	p5, [x0, #5, MUL VL]
104	str 	p6, [x0, #6, MUL VL]
105	str 	p7, [x0, #7, MUL VL]
106	str 	p8, [x0, #8, MUL VL]
107	str 	p9, [x0, #9, MUL VL]
108	str 	p10, [x0, #10, MUL VL]
109	str 	p11, [x0, #11, MUL VL]
110	str 	p12, [x0, #12, MUL VL]
111	str 	p13, [x0, #13, MUL VL]
112	str 	p14, [x0, #14, MUL VL]
113	str 	p15, [x0, #15, MUL VL]
114
115	/* Save the ffr register bank to memory. */
116	rdffr	p0.B
117	str 	p0, [x0, #16, MUL VL]
118
119	/* restore back zcr */
120	msr	zcr_el2, x1
121
122	ret
123.arch_extension nosve
124endfunc save_sve_p_ffr_state
125
126/*
127 * void restore_sve_zcr_fpu_state(unsigned long *data)
128 */
129func restore_sve_zcr_fpu_state
130.arch_extension sve
131	/* Load the FPSR/FPCR */
132	ldp	x1, x2, [x0], #16
133	msr	fpsr, x1
134	msr	fpcr, x2
135
136	/* Load the ZCR_EL1/EL2 */
137	ldp	x1, x2, [x0]
138	msr	zcr_el1, x1
139	msr	zcr_el2, x2
140
141	ret
142.arch_extension nosve
143endfunc restore_sve_zcr_fpu_state
144
145/*
146 * void restore_sve_z_state(unsigned char *data)
147 */
148func restore_sve_z_state
149.arch_extension sve
150	/* maximise VL */
151	mrs	x1, zcr_el2
152	orr	x2, x1, #SVE_VLA_ZCR_LEN_MAX
153	msr	zcr_el2, x2
154
155	/* Load the z register bank from memory. */
156	ldr 	z0, [x0, #0, MUL VL]
157	ldr	z1, [x0, #1, MUL VL]
158	ldr	z2, [x0, #2, MUL VL]
159	ldr	z3, [x0, #3, MUL VL]
160	ldr	z4, [x0, #4, MUL VL]
161	ldr	z5, [x0, #5, MUL VL]
162	ldr	z6, [x0, #6, MUL VL]
163	ldr	z7, [x0, #7, MUL VL]
164	ldr	z8, [x0, #8, MUL VL]
165	ldr	z9, [x0, #9, MUL VL]
166	ldr	z10, [x0, #10, MUL VL]
167	ldr	z11, [x0, #11, MUL VL]
168	ldr	z12, [x0, #12, MUL VL]
169	ldr	z13, [x0, #13, MUL VL]
170	ldr	z14, [x0, #14, MUL VL]
171	ldr	z15, [x0, #15, MUL VL]
172	ldr	z16, [x0, #16, MUL VL]
173	ldr	z17, [x0, #17, MUL VL]
174	ldr	z18, [x0, #18, MUL VL]
175	ldr	z19, [x0, #19, MUL VL]
176	ldr	z20, [x0, #20, MUL VL]
177	ldr	z21, [x0, #21, MUL VL]
178	ldr	z22, [x0, #22, MUL VL]
179	ldr	z23, [x0, #23, MUL VL]
180	ldr	z24, [x0, #24, MUL VL]
181	ldr	z25, [x0, #25, MUL VL]
182	ldr	z26, [x0, #26, MUL VL]
183	ldr	z27, [x0, #27, MUL VL]
184	ldr	z28, [x0, #28, MUL VL]
185	ldr	z29, [x0, #29, MUL VL]
186	ldr	z30, [x0, #30, MUL VL]
187	ldr	z31, [x0, #31, MUL VL]
188
189	/* restore back zcr */
190	msr	zcr_el2, x1
191
192	ret
193.arch_extension nosve
194endfunc restore_sve_z_state
195
196/*
197 * void restore_sve_p_ffr_state(unsigned char *data)
198 */
199func restore_sve_p_ffr_state
200.arch_extension sve
201	/* maximise VL */
202	mrs	x1, zcr_el2
203	orr	x2, x1, #SVE_VLA_ZCR_LEN_MAX
204	msr	zcr_el2, x2
205
206	/* Load the P register bank from memory. */
207	ldr 	p1, [x0, #1, MUL VL]
208	ldr 	p2, [x0, #2, MUL VL]
209	ldr 	p3, [x0, #3, MUL VL]
210	ldr 	p4, [x0, #4, MUL VL]
211	ldr 	p5, [x0, #5, MUL VL]
212	ldr 	p6, [x0, #6, MUL VL]
213	ldr 	p7, [x0, #7, MUL VL]
214	ldr 	p8, [x0, #8, MUL VL]
215	ldr 	p9, [x0, #9, MUL VL]
216	ldr 	p10, [x0, #10, MUL VL]
217	ldr 	p11, [x0, #11, MUL VL]
218	ldr 	p12, [x0, #12, MUL VL]
219	ldr 	p13, [x0, #13, MUL VL]
220	ldr 	p14, [x0, #14, MUL VL]
221	ldr 	p15, [x0, #15, MUL VL]
222
223	/* Load the ffr register bank from memory. */
224	ldr 	p0, [x0, #16, MUL VL]
225	wrffr	p0.B
226
227	/* restore P0 */
228	ldr 	p0, [x0]
229
230	/* restore back zcr */
231	msr	zcr_el2, x1
232
233	ret
234.arch_extension nosve
235endfunc restore_sve_p_ffr_state
236