~hc/RK356X_SDK_RELEASE.git

// 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