1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Copyright (C) 2015-2020 ARM Limited.
4  * Original author: Dave Martin <Dave.Martin@arm.com>
5  */
6 #include <errno.h>
7 #include <stddef.h>
8 #include <stdio.h>
9 #include <stdlib.h>
10 #include <string.h>
11 #include <unistd.h>
12 #include <sys/auxv.h>
13 #include <sys/ptrace.h>
14 #include <sys/types.h>
15 #include <sys/uio.h>
16 #include <sys/wait.h>
17 #include <asm/sigcontext.h>
18 #include <asm/ptrace.h>
19 
20 #include "../../kselftest.h"
21 
22 /* <linux/elf.h> and <sys/auxv.h> don't like each other, so: */
23 #ifndef NT_ARM_SVE
24 #define NT_ARM_SVE 0x405
25 #endif
26 
27 /* Number of registers filled in by sve_store_patterns */
28 #define NR_VREGS 5
29 
30 void sve_store_patterns(__uint128_t v[NR_VREGS]);
31 
32 static void dump(const void *buf, size_t size)
33 {
34 	size_t i;
35 	const unsigned char *p = buf;
36 
37 	for (i = 0; i < size; ++i)
38 		printf(" %.2x", *p++);
39 }
40 
41 static int check_vregs(const __uint128_t vregs[NR_VREGS])
42 {
43 	int i;
44 	int ok = 1;
45 
46 	for (i = 0; i < NR_VREGS; ++i) {
47 		printf("# v[%d]:", i);
48 		dump(&vregs[i], sizeof vregs[i]);
49 		putchar('\n');
50 
51 		if (vregs[i] != vregs[0])
52 			ok = 0;
53 	}
54 
55 	return ok;
56 }
57 
58 static int do_child(void)
59 {
60 	if (ptrace(PTRACE_TRACEME, -1, NULL, NULL))
61 		ksft_exit_fail_msg("PTRACE_TRACEME", strerror(errno));
62 
63 	if (raise(SIGSTOP))
64 		ksft_exit_fail_msg("raise(SIGSTOP)", strerror(errno));
65 
66 	return EXIT_SUCCESS;
67 }
68 
69 static struct user_sve_header *get_sve(pid_t pid, void **buf, size_t *size)
70 {
71 	struct user_sve_header *sve;
72 	void *p;
73 	size_t sz = sizeof *sve;
74 	struct iovec iov;
75 
76 	while (1) {
77 		if (*size < sz) {
78 			p = realloc(*buf, sz);
79 			if (!p) {
80 				errno = ENOMEM;
81 				goto error;
82 			}
83 
84 			*buf = p;
85 			*size = sz;
86 		}
87 
88 		iov.iov_base = *buf;
89 		iov.iov_len = sz;
90 		if (ptrace(PTRACE_GETREGSET, pid, NT_ARM_SVE, &iov))
91 			goto error;
92 
93 		sve = *buf;
94 		if (sve->size <= sz)
95 			break;
96 
97 		sz = sve->size;
98 	}
99 
100 	return sve;
101 
102 error:
103 	return NULL;
104 }
105 
106 static int set_sve(pid_t pid, const struct user_sve_header *sve)
107 {
108 	struct iovec iov;
109 
110 	iov.iov_base = (void *)sve;
111 	iov.iov_len = sve->size;
112 	return ptrace(PTRACE_SETREGSET, pid, NT_ARM_SVE, &iov);
113 }
114 
115 static void dump_sve_regs(const struct user_sve_header *sve, unsigned int num,
116 			  unsigned int vlmax)
117 {
118 	unsigned int vq;
119 	unsigned int i;
120 
121 	if ((sve->flags & SVE_PT_REGS_MASK) != SVE_PT_REGS_SVE)
122 		ksft_exit_fail_msg("Dumping non-SVE register\n");
123 
124 	if (vlmax > sve->vl)
125 		vlmax = sve->vl;
126 
127 	vq = sve_vq_from_vl(sve->vl);
128 	for (i = 0; i < num; ++i) {
129 		printf("# z%u:", i);
130 		dump((const char *)sve + SVE_PT_SVE_ZREG_OFFSET(vq, i),
131 		     vlmax);
132 		printf("%s\n", vlmax == sve->vl ? "" : " ...");
133 	}
134 }
135 
136 static int do_parent(pid_t child)
137 {
138 	int ret = EXIT_FAILURE;
139 	pid_t pid;
140 	int status;
141 	siginfo_t si;
142 	void *svebuf = NULL, *newsvebuf;
143 	size_t svebufsz = 0, newsvebufsz;
144 	struct user_sve_header *sve, *new_sve;
145 	struct user_fpsimd_state *fpsimd;
146 	unsigned int i, j;
147 	unsigned char *p;
148 	unsigned int vq;
149 
150 	/* Attach to the child */
151 	while (1) {
152 		int sig;
153 
154 		pid = wait(&status);
155 		if (pid == -1) {
156 			perror("wait");
157 			goto error;
158 		}
159 
160 		/*
161 		 * This should never happen but it's hard to flag in
162 		 * the framework.
163 		 */
164 		if (pid != child)
165 			continue;
166 
167 		if (WIFEXITED(status) || WIFSIGNALED(status))
168 			ksft_exit_fail_msg("Child died unexpectedly\n");
169 
170 		ksft_test_result(WIFSTOPPED(status), "WIFSTOPPED(%d)\n",
171 				 status);
172 		if (!WIFSTOPPED(status))
173 			goto error;
174 
175 		sig = WSTOPSIG(status);
176 
177 		if (ptrace(PTRACE_GETSIGINFO, pid, NULL, &si)) {
178 			if (errno == ESRCH)
179 				goto disappeared;
180 
181 			if (errno == EINVAL) {
182 				sig = 0; /* bust group-stop */
183 				goto cont;
184 			}
185 
186 			ksft_test_result_fail("PTRACE_GETSIGINFO: %s\n",
187 					      strerror(errno));
188 			goto error;
189 		}
190 
191 		if (sig == SIGSTOP && si.si_code == SI_TKILL &&
192 		    si.si_pid == pid)
193 			break;
194 
195 	cont:
196 		if (ptrace(PTRACE_CONT, pid, NULL, sig)) {
197 			if (errno == ESRCH)
198 				goto disappeared;
199 
200 			ksft_test_result_fail("PTRACE_CONT: %s\n",
201 					      strerror(errno));
202 			goto error;
203 		}
204 	}
205 
206 	sve = get_sve(pid, &svebuf, &svebufsz);
207 	if (!sve) {
208 		int e = errno;
209 
210 		ksft_test_result_fail("get_sve: %s\n", strerror(errno));
211 		if (e == ESRCH)
212 			goto disappeared;
213 
214 		goto error;
215 	} else {
216 		ksft_test_result_pass("get_sve\n");
217 	}
218 
219 	ksft_test_result((sve->flags & SVE_PT_REGS_MASK) == SVE_PT_REGS_FPSIMD,
220 			 "FPSIMD registers\n");
221 	if ((sve->flags & SVE_PT_REGS_MASK) != SVE_PT_REGS_FPSIMD)
222 		goto error;
223 
224 	fpsimd = (struct user_fpsimd_state *)((char *)sve +
225 					      SVE_PT_FPSIMD_OFFSET);
226 	for (i = 0; i < 32; ++i) {
227 		p = (unsigned char *)&fpsimd->vregs[i];
228 
229 		for (j = 0; j < sizeof fpsimd->vregs[i]; ++j)
230 			p[j] = j;
231 	}
232 
233 	if (set_sve(pid, sve)) {
234 		int e = errno;
235 
236 		ksft_test_result_fail("set_sve(FPSIMD): %s\n",
237 				      strerror(errno));
238 		if (e == ESRCH)
239 			goto disappeared;
240 
241 		goto error;
242 	}
243 
244 	vq = sve_vq_from_vl(sve->vl);
245 
246 	newsvebufsz = SVE_PT_SVE_ZREG_OFFSET(vq, 1);
247 	new_sve = newsvebuf = malloc(newsvebufsz);
248 	if (!new_sve) {
249 		errno = ENOMEM;
250 		perror(NULL);
251 		goto error;
252 	}
253 
254 	*new_sve = *sve;
255 	new_sve->flags &= ~SVE_PT_REGS_MASK;
256 	new_sve->flags |= SVE_PT_REGS_SVE;
257 	memset((char *)new_sve + SVE_PT_SVE_ZREG_OFFSET(vq, 0),
258 	       0, SVE_PT_SVE_ZREG_SIZE(vq));
259 	new_sve->size = SVE_PT_SVE_ZREG_OFFSET(vq, 1);
260 	if (set_sve(pid, new_sve)) {
261 		int e = errno;
262 
263 		ksft_test_result_fail("set_sve(ZREG): %s\n", strerror(errno));
264 		if (e == ESRCH)
265 			goto disappeared;
266 
267 		goto error;
268 	}
269 
270 	new_sve = get_sve(pid, &newsvebuf, &newsvebufsz);
271 	if (!new_sve) {
272 		int e = errno;
273 
274 		ksft_test_result_fail("get_sve(ZREG): %s\n", strerror(errno));
275 		if (e == ESRCH)
276 			goto disappeared;
277 
278 		goto error;
279 	}
280 
281 	ksft_test_result((new_sve->flags & SVE_PT_REGS_MASK) == SVE_PT_REGS_SVE,
282 			 "SVE registers\n");
283 	if ((new_sve->flags & SVE_PT_REGS_MASK) != SVE_PT_REGS_SVE)
284 		goto error;
285 
286 	dump_sve_regs(new_sve, 3, sizeof fpsimd->vregs[0]);
287 
288 	p = (unsigned char *)new_sve + SVE_PT_SVE_ZREG_OFFSET(vq, 1);
289 	for (i = 0; i < sizeof fpsimd->vregs[0]; ++i) {
290 		unsigned char expected = i;
291 
292 		if (__BYTE_ORDER == __BIG_ENDIAN)
293 			expected = sizeof fpsimd->vregs[0] - 1 - expected;
294 
295 		ksft_test_result(p[i] == expected, "p[%d] == expected\n", i);
296 		if (p[i] != expected)
297 			goto error;
298 	}
299 
300 	ret = EXIT_SUCCESS;
301 
302 error:
303 	kill(child, SIGKILL);
304 
305 disappeared:
306 	return ret;
307 }
308 
309 int main(void)
310 {
311 	int ret = EXIT_SUCCESS;
312 	__uint128_t v[NR_VREGS];
313 	pid_t child;
314 
315 	ksft_print_header();
316 	ksft_set_plan(20);
317 
318 	if (!(getauxval(AT_HWCAP) & HWCAP_SVE))
319 		ksft_exit_skip("SVE not available\n");
320 
321 	sve_store_patterns(v);
322 
323 	if (!check_vregs(v))
324 		ksft_exit_fail_msg("Initial check_vregs() failed\n");
325 
326 	child = fork();
327 	if (!child)
328 		return do_child();
329 
330 	if (do_parent(child))
331 		ret = EXIT_FAILURE;
332 
333 	ksft_print_cnts();
334 
335 	return ret;
336 }
337