/* SPDX-License-Identifier: GPL-2.0-or-later */
/*
 * Copyright 2015, Cyril Bur, IBM Corp.
 */

#include "basic_asm.h"
#include "fpu_asm.h"

FUNC_START(check_fpu)
        mr r4,r3
        li      r3,1 # assume a bad result
        lfd     f0,0(r4)
        fcmpu   cr1,f0,f14
        bne     cr1,1f
        lfd     f0,8(r4)
        fcmpu   cr1,f0,f15
        bne     cr1,1f
        lfd     f0,16(r4)
        fcmpu   cr1,f0,f16
        bne     cr1,1f
        lfd     f0,24(r4)
        fcmpu   cr1,f0,f17
        bne     cr1,1f
        lfd     f0,32(r4)
        fcmpu   cr1,f0,f18
        bne     cr1,1f
        lfd     f0,40(r4)
        fcmpu   cr1,f0,f19
        bne     cr1,1f
        lfd     f0,48(r4)
        fcmpu   cr1,f0,f20
        bne     cr1,1f
        lfd     f0,56(r4)
        fcmpu   cr1,f0,f21
        bne     cr1,1f
        lfd     f0,64(r4)
        fcmpu   cr1,f0,f22
        bne     cr1,1f
        lfd     f0,72(r4)
        fcmpu   cr1,f0,f23
        bne     cr1,1f
        lfd     f0,80(r4)
        fcmpu   cr1,f0,f24
        bne     cr1,1f
        lfd     f0,88(r4)
        fcmpu   cr1,f0,f25
        bne     cr1,1f
        lfd     f0,96(r4)
        fcmpu   cr1,f0,f26
        bne     cr1,1f
        lfd     f0,104(r4)
        fcmpu   cr1,f0,f27
        bne     cr1,1f
        lfd     f0,112(r4)
        fcmpu   cr1,f0,f28
        bne     cr1,1f
        lfd     f0,120(r4)
        fcmpu   cr1,f0,f29
        bne     cr1,1f
        lfd     f0,128(r4)
        fcmpu   cr1,f0,f30
        bne     cr1,1f
        lfd     f0,136(r4)
        fcmpu   cr1,f0,f31
        bne     cr1,1f
        li      r3,0 # Success!!!
1:      blr


// int check_all_fprs(double darray[32])
FUNC_START(check_all_fprs)
        PUSH_BASIC_STACK(8)
        mr      r4, r3  // r4 = darray
        li      r3, 1   // prepare for failure

        stfd    f31, STACK_FRAME_LOCAL(0, 0)(sp) // backup f31

        // Check regs f0-f30, using f31 as scratch
        .set i, 0
        .rept 31
        lfd     f31, (8 * i)(r4)        // load expected value
        fcmpu   cr0, i, f31             // compare
        bne     cr0, 1f                 // bail if mismatch
        .set i, i + 1
        .endr

        lfd     f31, STACK_FRAME_LOCAL(0, 0)(sp) // reload f31
        stfd    f30, STACK_FRAME_LOCAL(0, 0)(sp) // backup f30

        lfd     f30, (8 * 31)(r4)       // load expected value of f31
        fcmpu   cr0, f30, f31           // compare
        bne     cr0, 1f                 // bail if mismatch

        lfd     f30, STACK_FRAME_LOCAL(0, 0)(sp) // reload f30

        // Success
        li      r3, 0

1:      POP_BASIC_STACK(8)
        blr
FUNC_END(check_all_fprs)

FUNC_START(test_fpu)
        # r3 holds pointer to where to put the result of fork
        # r4 holds pointer to the pid
        # f14-f31 are non volatiles
        PUSH_BASIC_STACK(256)
        PUSH_FPU(256)
        std     r3,STACK_FRAME_PARAM(0)(sp) # Address of darray
        std r4,STACK_FRAME_PARAM(1)(sp) # Address of pid

        // Load FPRs with expected values
        OP_REGS lfd, 8, 0, 31, r3

        li      r0,__NR_fork
        sc

        # pass the result of the fork to the caller
        ld      r9,STACK_FRAME_PARAM(1)(sp)
        std     r3,0(r9)

        ld r3,STACK_FRAME_PARAM(0)(sp)
        bl check_all_fprs
        nop

        POP_FPU(256)
        POP_BASIC_STACK(256)
        blr
FUNC_END(test_fpu)

# int preempt_fpu(double *darray, int *threads_running, int *running)
# On starting will (atomically) decrement not_ready as a signal that the FPU
# has been loaded with darray. Will proceed to check the validity of the FPU
# registers while running is not zero.
FUNC_START(preempt_fpu)
        PUSH_BASIC_STACK(256)
        PUSH_FPU(256)
        std r3,STACK_FRAME_PARAM(0)(sp) # double *darray
        std r4,STACK_FRAME_PARAM(1)(sp) # int *threads_starting
        std r5,STACK_FRAME_PARAM(2)(sp) # int *running

        // Load FPRs with expected values
        OP_REGS lfd, 8, 0, 31, r3

        sync
        # Atomic DEC
        ld r3,STACK_FRAME_PARAM(1)(sp)
1:      lwarx r4,0,r3
        addi r4,r4,-1
        stwcx. r4,0,r3
        bne- 1b

2:      ld r3,STACK_FRAME_PARAM(0)(sp)
        bl check_all_fprs
        cmpdi r3,0
        bne 3f
        ld r4,STACK_FRAME_PARAM(2)(sp)
        ld r5,0(r4)
        cmpwi r5,0
        bne 2b

3:      POP_FPU(256)
        POP_BASIC_STACK(256)
        blr
FUNC_END(preempt_fpu)