/*      $NetBSD: fpu.c,v 1.42 2020/07/15 09:19:49 rin Exp $     */

/*
 * Copyright (C) 1996 Wolfgang Solfrank.
 * Copyright (C) 1996 TooLs GmbH.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *      This product includes software developed by TooLs GmbH.
 * 4. The name of TooLs GmbH may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY TOOLS GMBH ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL TOOLS GMBH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/cdefs.h>
__KERNEL_RCSID(0, "$NetBSD: fpu.c,v 1.42 2020/07/15 09:19:49 rin Exp $");

#include <sys/param.h>
#include <sys/proc.h>
#include <sys/systm.h>
#include <sys/atomic.h>
#include <sys/siginfo.h>
#include <sys/pcu.h>

#include <machine/pcb.h>
#include <machine/fpu.h>
#include <machine/psl.h>

static void fpu_state_load(lwp_t *, u_int);
static void fpu_state_save(lwp_t *);
static void fpu_state_release(lwp_t *);

const pcu_ops_t fpu_ops = {
        .pcu_id = PCU_FPU,
        .pcu_state_load = fpu_state_load,
        .pcu_state_save = fpu_state_save,
        .pcu_state_release = fpu_state_release,
};

bool
fpu_used_p(lwp_t *l)
{
        return pcu_valid_p(&fpu_ops, l);
}

void
fpu_mark_used(lwp_t *l)
{
        pcu_discard(&fpu_ops, l, true);
}

void
fpu_state_load(lwp_t *l, u_int flags)
{
#ifdef PPC_HAVE_FPU
        struct pcb * const pcb = lwp_getpcb(l);

        if ((flags & PCU_VALID) == 0) {
                memset(&pcb->pcb_fpu, 0, sizeof(pcb->pcb_fpu));
        }

        if ((flags & PCU_REENABLE) == 0) {
                const register_t msr = mfmsr();
                mtmsr((msr & ~PSL_EE) | PSL_FP);
                __asm volatile ("isync");

                fpu_load_from_fpreg(&pcb->pcb_fpu);
                __asm volatile ("sync");

                mtmsr(msr);
                __asm volatile ("isync");
        }

        curcpu()->ci_ev_fpusw.ev_count++;
        l->l_md.md_utf->tf_srr1 |= PSL_FP|(pcb->pcb_flags & (PCB_FE0|PCB_FE1));
#endif
}

/*
 * Save the contents of the current CPU's FPU to its PCB.
 */
void
fpu_state_save(lwp_t *l)
{
#ifdef PPC_HAVE_FPU
        struct pcb * const pcb = lwp_getpcb(l);

        const register_t msr = mfmsr();
        mtmsr((msr & ~PSL_EE) | PSL_FP);
        __asm volatile ("isync");

        fpu_unload_to_fpreg(&pcb->pcb_fpu);
        __asm volatile ("sync");

        mtmsr(msr);
        __asm volatile ("isync");
#endif
}

void
fpu_state_release(lwp_t *l)
{
#ifdef PPC_HAVE_FPU
        l->l_md.md_utf->tf_srr1 &= ~PSL_FP;
#endif
}

#define STICKYBITS      (FPSCR_VX|FPSCR_OX|FPSCR_UX|FPSCR_ZX|FPSCR_XX)
#define STICKYSHIFT     25
#define MASKBITS        (FPSCR_VE|FPSCR_OE|FPSCR_UE|FPSCR_ZE|FPSCR_XE)
#define MASKSHIFT       3

int
fpu_get_fault_code(void)
{
        lwp_t * const l = curlwp;
        struct pcb * const pcb = lwp_getpcb(l);
        uint64_t fpscr64;
        uint32_t fpscr, ofpscr;
        int code;

#ifdef PPC_HAVE_FPU
        kpreempt_disable();

        struct cpu_info * const ci = curcpu();

        /*
         * If we got preempted, we may be running on a different CPU.  So we
         * need to check for that.
         */
        KASSERT(fpu_used_p(l));
        if (__predict_true(l->l_pcu_cpu[PCU_FPU] == ci)) {
                uint64_t tmp;
                const register_t msr = mfmsr();
                mtmsr((msr & ~PSL_EE) | PSL_FP);
                __asm volatile ("isync");
                __asm volatile (
                        "stfd   0,0(%[tmp])\n"          /* save f0 */
                        "mffs   0\n"                    /* get FPSCR */
                        "stfd   0,0(%[fpscr64])\n"      /* store a temp copy */
                        "mtfsb0 0\n"                    /* clear FPSCR_FX */
                        "mtfsb0 24\n"                   /* clear FPSCR_VE */
                        "mtfsb0 25\n"                   /* clear FPSCR_OE */
                        "mtfsb0 26\n"                   /* clear FPSCR_UE */
                        "mtfsb0 27\n"                   /* clear FPSCR_ZE */
                        "mtfsb0 28\n"                   /* clear FPSCR_XE */
                        "mffs   0\n"                    /* get FPSCR */
                        "stfd   0,0(%[fpscr])\n"        /* store it */
                        "lfd    0,0(%[tmp])\n"          /* restore f0 */
                    ::  [tmp] "b"(&tmp),
                        [fpscr] "b"(&pcb->pcb_fpu.fpscr),
                        [fpscr64] "b"(&fpscr64));
                mtmsr(msr);
                __asm volatile ("isync");
        } else {
                /*
                 * We got preempted to a different CPU so we need to save
                 * our FPU state.
                 */
                fpu_save(l);
                fpscr64 = *(uint64_t *)&pcb->pcb_fpu.fpscr;
                ((uint32_t *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD] &= ~MASKBITS;
        }

        kpreempt_enable();
#else /* !PPC_HAVE_FPU */
        fpscr64 = *(uint64_t *)&pcb->pcb_fpu.fpscr;
        ((uint32_t *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD] &= ~MASKBITS;
#endif

        /*
         * Now determine the fault type.  First we test to see if any of sticky
         * bits correspond to the enabled exceptions.  If so, we only test
         * those bits.  If not, we look at all the bits.  (In reality, we only
         * could get an exception if FPSCR_FEX changed state.  So we should
         * have at least one bit that corresponds).
         */
        ofpscr = (uint32_t)fpscr64;
        ofpscr &= ofpscr << (STICKYSHIFT - MASKSHIFT);
        fpscr = ((uint32_t *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD];
        if (fpscr & ofpscr & STICKYBITS)
                fpscr &= ofpscr;

        /*
         * Let's determine what the appropriate code is.
         */
        if (fpscr & FPSCR_VX)           code = FPE_FLTINV;
        else if (fpscr & FPSCR_OX)      code = FPE_FLTOVF;
        else if (fpscr & FPSCR_UX)      code = FPE_FLTUND;
        else if (fpscr & FPSCR_ZX)      code = FPE_FLTDIV;
        else if (fpscr & FPSCR_XX)      code = FPE_FLTRES;
        else                            code = 0;
        return code;
}

bool
fpu_save_to_mcontext(lwp_t *l, mcontext_t *mcp, unsigned int *flagp)
{
        KASSERT(l == curlwp);

        if (!pcu_valid_p(&fpu_ops, l))
                return false;

        struct pcb * const pcb = lwp_getpcb(l);

#ifdef PPC_HAVE_FPU
        /* If we're the FPU owner, dump its context to the PCB first. */
        pcu_save(&fpu_ops, l);
#endif
        (void)memcpy(mcp->__fpregs.__fpu_regs, pcb->pcb_fpu.fpreg,
            sizeof (mcp->__fpregs.__fpu_regs));
        mcp->__fpregs.__fpu_fpscr =
            ((int *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD];
        mcp->__fpregs.__fpu_valid = 1;
        *flagp |= _UC_FPU;
        return true;
}

void
fpu_restore_from_mcontext(lwp_t *l, const mcontext_t *mcp)
{
        if (!mcp->__fpregs.__fpu_valid)
                return;

        struct pcb * const pcb = lwp_getpcb(l);

#ifdef PPC_HAVE_FPU
        /* we don't need to save the state, just drop it */
        pcu_discard(&fpu_ops, l, true);
#endif
        (void)memcpy(&pcb->pcb_fpu.fpreg, &mcp->__fpregs.__fpu_regs,
            sizeof (pcb->pcb_fpu.fpreg));
        ((int *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD] = mcp->__fpregs.__fpu_fpscr;
}