selftests: arm64: Add stress tests for FPSMID and SVE context switching

#define sa_sz 32

#define sa_flags 8

#define sa_handler 0

#define sa_mask_sz 8

#define SIGUSR1 10

#define SIGTERM 15

#define SIGINT 2

#define SIGABRT 6

#define SA_NODEFER 1073741824

#define SA_SIGINFO 4

#define ucontext_regs 184

// SPDX-License-Identifier: GPL-2.0-only

// Copyright (C) 2015-2019 ARM Limited.

// Original author: Dave Martin <Dave.Martin@arm.com>

#ifndef ASSEMBLER_H

#define ASSEMBLER_H

.macro __for from:req, to:req

	.if (\from) == (\to)

		_for__body %\from

	.else

		__for \from, %(\from) + ((\to) - (\from)) / 2

		__for %(\from) + ((\to) - (\from)) / 2 + 1, \to

	.endif

.endm

.macro _for var:req, from:req, to:req, insn:vararg

	.macro _for__body \var:req

		.noaltmacro

		\insn

		.altmacro

	.endm

	.altmacro

	__for \from, \to

	.noaltmacro

	.purgem _for__body

.endm

.macro function name

	.macro endfunction

		.type \name, @function

		.purgem endfunction

	.endm

\name:

.endm

.macro define_accessor name, num, insn

	.macro \name\()_entry n

		\insn \n, 1

		ret

	.endm

function \name

	adr	x2, .L__accessor_tbl\@

	add	x2, x2, x0, lsl #3

	br	x2

.L__accessor_tbl\@:

	_for x, 0, (\num) - 1, \name\()_entry \x

endfunction

	.purgem \name\()_entry

.endm

#endif /* ! ASSEMBLER_H */

// SPDX-License-Identifier: GPL-2.0-only

// Copyright (C) 2015-2019 ARM Limited.

// Original author: Dave Martin <Dave.Martin@arm.com>

//

// Simple FPSIMD context switch test

// Repeatedly writes unique test patterns into each FPSIMD register

// and reads them back to verify integrity.

//

// for x in `seq 1 NR_CPUS`; do fpsimd-test & pids=$pids\ $! ; done

// (leave it running for as long as you want...)

// kill $pids

#include <asm/unistd.h>

#include "assembler.h"

#include "asm-offsets.h"

#define NVR	32

#define MAXVL_B	(128 / 8)

.macro _vldr Vn:req, Xt:req

	ld1	{v\Vn\().2d}, [x\Xt]

.endm

.macro _vstr Vn:req, Xt:req

	st1	{v\Vn\().2d}, [x\Xt]

.endm

// Generate accessor functions to read/write programmatically selected

// FPSIMD registers.

// x0 is the register index to access

// x1 is the memory address to read from (getv,setp) or store to (setv,setp)

// All clobber x0-x2

define_accessor setv, NVR, _vldr

define_accessor getv, NVR, _vstr

// Print a single character x0 to stdout

// Clobbers x0-x2,x8

function putc

	str	x0, [sp, #-16]!

	mov	x0, #1			// STDOUT_FILENO

	mov	x1, sp

	mov	x2, #1

	mov	x8, #__NR_write

	svc	#0

	add	sp, sp, #16

	ret

endfunction

// Print a NUL-terminated string starting at address x0 to stdout

// Clobbers x0-x3,x8

function puts

	mov	x1, x0

	mov	x2, #0

0:	ldrb	w3, [x0], #1

	cbz	w3, 1f

	add	x2, x2, #1

	b	0b

1:	mov	w0, #1			// STDOUT_FILENO

	mov	x8, #__NR_write

	svc	#0

	ret

endfunction

// Utility macro to print a literal string

// Clobbers x0-x4,x8

.macro puts string

	.pushsection .rodata.str1.1, "aMS", 1

.L__puts_literal\@: .string "\string"

	.popsection

	ldr	x0, =.L__puts_literal\@

	bl	puts

.endm

// Print an unsigned decimal number x0 to stdout

// Clobbers x0-x4,x8

function putdec

	mov	x1, sp

	str	x30, [sp, #-32]!	// Result can't be > 20 digits

	mov	x2, #0

	strb	w2, [x1, #-1]!		// Write the NUL terminator

	mov	x2, #10

0:	udiv	x3, x0, x2		// div-mod loop to generate the digits

	msub	x0, x3, x2, x0

	add	w0, w0, #'0'

	strb	w0, [x1, #-1]!

	mov	x0, x3

	cbnz	x3, 0b

	ldrb	w0, [x1]

	cbnz	w0, 1f

	mov	w0, #'0'		// Print "0" for 0, not ""

	strb	w0, [x1, #-1]!

1:	mov	x0, x1

	bl	puts

	ldr	x30, [sp], #32

	ret

endfunction

// Print an unsigned decimal number x0 to stdout, followed by a newline

// Clobbers x0-x5,x8

function putdecn

	mov	x5, x30

	bl	putdec

	mov	x0, #'\n'

	bl	putc

	ret	x5

endfunction

// Clobbers x0-x3,x8

function puthexb

	str	x30, [sp, #-0x10]!

	mov	w3, w0

	lsr	w0, w0, #4

	bl	puthexnibble

	mov	w0, w3

	ldr	x30, [sp], #0x10

	// fall through to puthexnibble

endfunction

// Clobbers x0-x2,x8

function puthexnibble

	and	w0, w0, #0xf

	cmp	w0, #10

	blo	1f

	add	w0, w0, #'a' - ('9' + 1)

1:	add	w0, w0, #'0'

	b	putc

endfunction

// x0=data in, x1=size in, clobbers x0-x5,x8

function dumphex

	str	x30, [sp, #-0x10]!

	mov	x4, x0

	mov	x5, x1

0:	subs	x5, x5, #1

	b.lo	1f

	ldrb	w0, [x4], #1

	bl	puthexb

	b	0b

1:	ldr	x30, [sp], #0x10

	ret

endfunction

// Declare some storate space to shadow the SVE register contents:

.pushsection .text

.data

.align 4

vref:

	.space	MAXVL_B * NVR

scratch:

	.space	MAXVL_B

.popsection

// Trivial memory copy: copy x2 bytes, starting at address x1, to address x0.

// Clobbers x0-x3

function memcpy

	cmp	x2, #0

	b.eq	1f

0:	ldrb	w3, [x1], #1

	strb	w3, [x0], #1

	subs	x2, x2, #1

	b.ne	0b

1:	ret

endfunction

// Generate a test pattern for storage in SVE registers

// x0: pid	(16 bits)

// x1: register number (6 bits)

// x2: generation (4 bits)

function pattern

	orr	w1, w0, w1, lsl #16

	orr	w2, w1, w2, lsl #28

	ldr	x0, =scratch

	mov	w1, #MAXVL_B / 4

0:	str	w2, [x0], #4

	add	w2, w2, #(1 << 22)

	subs	w1, w1, #1

	bne	0b

	ret

endfunction

// Get the address of shadow data for FPSIMD V-register V<xn>

.macro _adrv xd, xn, nrtmp

	ldr	\xd, =vref

	mov	x\nrtmp, #16

	madd	\xd, x\nrtmp, \xn, \xd

.endm

// Set up test pattern in a FPSIMD V-register

// x0: pid

// x1: register number

// x2: generation

function setup_vreg

	mov	x4, x30

	mov	x6, x1

	bl	pattern

	_adrv	x0, x6, 2

	mov	x5, x0

	ldr	x1, =scratch

	bl	memcpy

	mov	x0, x6

	mov	x1, x5

	bl	setv

	ret	x4

endfunction

// Fill x1 bytes starting at x0 with 0xae (for canary purposes)

// Clobbers x1, x2.

function memfill_ae

	mov	w2, #0xae

	b	memfill

endfunction

// Fill x1 bytes starting at x0 with 0.

// Clobbers x1, x2.

function memclr

	mov	w2, #0

endfunction

	// fall through to memfill

// Trivial memory fill: fill x1 bytes starting at address x0 with byte w2

// Clobbers x1

function memfill

	cmp	x1, #0

	b.eq	1f

0:	strb	w2, [x0], #1

	subs	x1, x1, #1

	b.ne	0b

1:	ret

endfunction

// Trivial memory compare: compare x2 bytes starting at address x0 with

// bytes starting at address x1.

// Returns only if all bytes match; otherwise, the program is aborted.

// Clobbers x0-x5.

function memcmp

	cbz	x2, 1f

	mov	x5, #0

0:	ldrb	w3, [x0, x5]

	ldrb	w4, [x1, x5]

	add	x5, x5, #1

	cmp	w3, w4

	b.ne	barf

	subs	x2, x2, #1

	b.ne	0b

1:	ret

endfunction

// Verify that a FPSIMD V-register matches its shadow in memory, else abort

// x0: reg number

// Clobbers x0-x5.

function check_vreg

	mov	x3, x30

	_adrv	x5, x0, 6

	mov	x4, x0

	ldr	x7, =scratch

	mov	x0, x7

	mov	x1, x6

	bl	memfill_ae

	mov	x0, x4

	mov	x1, x7

	bl	getv

	mov	x0, x5

	mov	x1, x7

	mov	x2, x6

	mov	x30, x3

	b	memcmp

endfunction

// Any SVE register modified here can cause corruption in the main

// thread -- but *only* the registers modified here.

function irritator_handler

	// Increment the irritation signal count (x23):

	ldr	x0, [x2, #ucontext_regs + 8 * 23]

	add	x0, x0, #1

	str	x0, [x2, #ucontext_regs + 8 * 23]

	// Corrupt some random V-regs

	adr	x0, .text + (irritator_handler - .text) / 16 * 16

	movi	v0.8b, #7

	movi	v9.16b, #9

	movi	v31.8b, #31

	ret

endfunction

function terminate_handler

	mov	w21, w0

	mov	x20, x2

	puts	"Terminated by signal "

	mov	w0, w21

	bl	putdec

	puts	", no error, iterations="

	ldr	x0, [x20, #ucontext_regs + 8 * 22]

	bl	putdec

	puts	", signals="

	ldr	x0, [x20, #ucontext_regs + 8 * 23]

	bl	putdecn

	mov	x0, #0

	mov	x8, #__NR_exit

	svc	#0

endfunction

// w0: signal number

// x1: sa_action

// w2: sa_flags

// Clobbers x0-x6,x8

function setsignal

	str	x30, [sp, #-((sa_sz + 15) / 16 * 16 + 16)]!

	mov	w4, w0

	mov	x5, x1

	mov	w6, w2

	add	x0, sp, #16

	mov	x1, #sa_sz

	bl	memclr

	mov	w0, w4

	add	x1, sp, #16

	str	w6, [x1, #sa_flags]

	str	x5, [x1, #sa_handler]

	mov	x2, #0

	mov	x3, #sa_mask_sz

	mov	x8, #__NR_rt_sigaction

	svc	#0

	cbz	w0, 1f

	puts	"sigaction failure\n"

	b	.Labort

1:	ldr	x30, [sp], #((sa_sz + 15) / 16 * 16 + 16)

	ret

endfunction

// Main program entry point

.globl _start

function _start

_start:

	// Sanity-check and report the vector length

	mov	x19, #128

	cmp	x19, #128

	b.lo	1f

	cmp	x19, #2048

	b.hi	1f

	tst	x19, #(8 - 1)

	b.eq	2f

1:	puts	"Bad vector length: "

	mov	x0, x19

	bl	putdecn

	b	.Labort

2:	puts	"Vector length:\t"

	mov	x0, x19

	bl	putdec

	puts	" bits\n"

	// Obtain our PID, to ensure test pattern uniqueness between processes

	mov	x8, #__NR_getpid

	svc	#0

	mov	x20, x0

	puts	"PID:\t"

	mov	x0, x20

	bl	putdecn

	mov	x23, #0		// Irritation signal count

	mov	w0, #SIGINT

	adr	x1, terminate_handler

	mov	w2, #SA_SIGINFO

	bl	setsignal

	mov	w0, #SIGTERM

	adr	x1, terminate_handler

	mov	w2, #SA_SIGINFO

	bl	setsignal

	mov	w0, #SIGUSR1

	adr	x1, irritator_handler

	mov	w2, #SA_SIGINFO

	orr	w2, w2, #SA_NODEFER

	bl	setsignal

	mov	x22, #0		// generation number, increments per iteration

.Ltest_loop:

	mov	x21, #0		// Set up V-regs & shadow with test pattern

0:	mov	x0, x20

	mov	x1, x21

	and	x2, x22, #0xf

	bl	setup_vreg

	add	x21, x21, #1

	cmp	x21, #NVR

	b.lo	0b

// Can't do this when SVE state is volatile across SVC:

	mov	x8, #__NR_sched_yield	// Encourage preemption

	svc	#0

	mov	x21, #0

0:	mov	x0, x21

	bl	check_vreg

	add	x21, x21, #1

	cmp	x21, #NVR

	b.lo	0b

	add	x22, x22, #1

	b	.Ltest_loop

.Labort:

	mov	x0, #0

	mov	x1, #SIGABRT

	mov	x8, #__NR_kill

	svc	#0

endfunction

function barf

	mov	x10, x0	// expected data

	mov	x11, x1	// actual data

	mov	x12, x2	// data size

	puts	"Mistatch: PID="

	mov	x0, x20

	bl	putdec

	puts	", iteration="

	mov	x0, x22

	bl	putdec

	puts	", reg="

	mov	x0, x21

	bl	putdecn

	puts	"\tExpected ["

	mov	x0, x10

	mov	x1, x12

	bl	dumphex

	puts	"]\n\tGot      ["

	mov	x0, x11

	mov	x1, x12

	bl	dumphex

	puts	"]\n"

	mov	x8, #__NR_exit

	mov	x1, #1

	svc	#0

endfunction

// SPDX-License-Identifier: GPL-2.0-only

// Copyright (C) 2015-2019 ARM Limited.

// Original author: Dave Martin <Dave.Martin@arm.com>

//

// Simple Scalable Vector Extension context switch test

// Repeatedly writes unique test patterns into each SVE register

// and reads them back to verify integrity.

//

// for x in `seq 1 NR_CPUS`; do sve-test & pids=$pids\ $! ; done

// (leave it running for as long as you want...)

// kill $pids

#include <asm/unistd.h>

#include "assembler.h"

#include "asm-offsets.h"

#define NZR	32

#define NPR	16

#define MAXVL_B	(2048 / 8)

.arch_extension sve

.macro _sve_ldr_v zt, xn

	ldr	z\zt, [x\xn]

.endm

.macro _sve_str_v zt, xn

	str	z\zt, [x\xn]

.endm

.macro _sve_ldr_p pt, xn

	ldr	p\pt, [x\xn]

.endm

.macro _sve_str_p pt, xn

	str	p\pt, [x\xn]

.endm

// Generate accessor functions to read/write programmatically selected

// SVE registers.

// x0 is the register index to access

// x1 is the memory address to read from (getz,setp) or store to (setz,setp)

// All clobber x0-x2

define_accessor setz, NZR, _sve_ldr_v

define_accessor getz, NZR, _sve_str_v

define_accessor setp, NPR, _sve_ldr_p

define_accessor getp, NPR, _sve_str_p

// Print a single character x0 to stdout

// Clobbers x0-x2,x8

function putc

	str	x0, [sp, #-16]!

	mov	x0, #1			// STDOUT_FILENO

	mov	x1, sp

	mov	x2, #1

	mov	x8, #__NR_write

	svc	#0

	add	sp, sp, #16

	ret

endfunction

// Print a NUL-terminated string starting at address x0 to stdout

// Clobbers x0-x3,x8

function puts

	mov	x1, x0

	mov	x2, #0

0:	ldrb	w3, [x0], #1

	cbz	w3, 1f

	add	x2, x2, #1

	b	0b

1:	mov	w0, #1			// STDOUT_FILENO

	mov	x8, #__NR_write

	svc	#0

	ret

endfunction

// Utility macro to print a literal string

// Clobbers x0-x4,x8

.macro puts string

	.pushsection .rodata.str1.1, "aMS", 1

.L__puts_literal\@: .string "\string"

	.popsection

	ldr	x0, =.L__puts_literal\@

	bl	puts

.endm

// Print an unsigned decimal number x0 to stdout

// Clobbers x0-x4,x8

function putdec

	mov	x1, sp

	str	x30, [sp, #-32]!	// Result can't be > 20 digits

	mov	x2, #0

	strb	w2, [x1, #-1]!		// Write the NUL terminator

	mov	x2, #10

0:	udiv	x3, x0, x2		// div-mod loop to generate the digits

	msub	x0, x3, x2, x0

	add	w0, w0, #'0'

	strb	w0, [x1, #-1]!

	mov	x0, x3

	cbnz	x3, 0b

	ldrb	w0, [x1]

	cbnz	w0, 1f

	mov	w0, #'0'		// Print "0" for 0, not ""

	strb	w0, [x1, #-1]!

1:	mov	x0, x1

	bl	puts

	ldr	x30, [sp], #32

	ret

endfunction

// Print an unsigned decimal number x0 to stdout, followed by a newline

// Clobbers x0-x5,x8

function putdecn

	mov	x5, x30

	bl	putdec

	mov	x0, #'\n'