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
30void sve_store_patterns(__uint128_t v[NR_VREGS]);
31
32static 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
41static 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
58static 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
69static 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
102error:
103	return NULL;
104}
105
106static 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
115static 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
136static 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
302error:
303	kill(child, SIGKILL);
304
305disappeared:
306	return ret;
307}
308
309int 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