/*      $OpenBSD: vfp.c,v 1.5 2022/08/29 02:01:18 jsg Exp $     */

/*
 * Copyright (c) 2011 Dale Rahn <drahn@openbsd.org>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/proc.h>
#include <sys/user.h>

#include <arm/include/cpufunc.h>
#include <arm/include/vfp.h>
#include <arm/include/undefined.h>

static inline void
set_vfp_fpexc(uint32_t val)
{
        __asm volatile(
            ".fpu vfpv3\n"
            "vmsr fpexc, %0" :: "r" (val));
}

static inline uint32_t
get_vfp_fpexc(void)
{
        uint32_t val;
        __asm volatile(
            ".fpu vfpv3\n"
            "vmrs %0, fpexc" : "=r" (val));
        return val;
}

int vfp_fault(unsigned int, unsigned int, trapframe_t *, int, uint32_t);
void vfp_load(struct proc *p);
void vfp_store(struct fpreg *vfpsave);

void
vfp_init(void)
{
        uint32_t val;

        install_coproc_handler(10, vfp_fault);
        install_coproc_handler(11, vfp_fault);

        __asm volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val));
        val |= COPROC10 | COPROC11;
        __asm volatile("mcr p15, 0, %0, c1, c0, 2" :: "r" (val));
        __asm volatile("isb");
}

void
vfp_store(struct fpreg *vfpsave)
{
        uint32_t scratch;

        if (get_vfp_fpexc() & VFPEXC_EN) {
                __asm volatile(
                    ".fpu vfpv3\n"
                    "vstmia     %1!, {d0-d15}\n"        /* d0-d15 */
                    "vstmia     %1!, {d16-d31}\n"       /* d16-d31 */
                    "vmrs       %0, fpscr\n"
                    "str        %0, [%1]\n"             /* save vfpscr */
                : "=&r" (scratch) : "r" (vfpsave));
        }

        /* disable FPU */
        set_vfp_fpexc(0);
}

uint32_t
vfp_save(void)
{
        struct cpu_info *ci = curcpu();
        struct pcb *pcb = curpcb;
        struct proc *p = curproc;
        uint32_t fpexc;

        if (ci->ci_fpuproc == 0)
                return 0;

        fpexc = get_vfp_fpexc();

        if ((fpexc & VFPEXC_EN) == 0)
                return fpexc;   /* not enabled, nothing to do */

        if (fpexc & VFPEXC_EX)
                panic("vfp exceptional data fault, time to write more code");

        if (pcb->pcb_fpcpu == NULL || ci->ci_fpuproc == NULL ||
            !(pcb->pcb_fpcpu == ci && ci->ci_fpuproc == p)) {
                /* disable fpu before panic, otherwise recurse */
                set_vfp_fpexc(0);

                panic("FPU unit enabled when curproc and curcpu dont agree %p %p %p %p", pcb->pcb_fpcpu, ci, ci->ci_fpuproc, p);
        }

        vfp_store(&p->p_addr->u_pcb.pcb_fpstate);

        /*
         * NOTE: fpu state is saved but remains 'valid', as long as
         * curpcb()->pcb_fpucpu == ci && ci->ci_fpuproc == curproc()
         * is true FPU state is valid and can just be enabled without reload.
         */
        return fpexc;
}

void
vfp_enable(void)
{
        struct cpu_info *ci = curcpu();

        if (curproc->p_addr->u_pcb.pcb_fpcpu == ci &&
            ci->ci_fpuproc == curproc) {
                disable_interrupts(PSR_I|PSR_F);

                /* FPU state is still valid, just enable and go */
                set_vfp_fpexc(VFPEXC_EN);
        }
}

void
vfp_load(struct proc *p)
{
        struct cpu_info *ci = curcpu();
        struct pcb *pcb = &p->p_addr->u_pcb;
        uint32_t scratch = 0;
        int psw;

        /* do not allow a partially synced state here */
        psw = disable_interrupts(PSR_I|PSR_F);

        /*
         * p->p_pcb->pcb_fpucpu _may_ not be NULL here, but the FPU state
         * was synced on kernel entry, so we can steal the FPU state
         * instead of signalling and waiting for it to save
         */

        /* enable to be able to load ctx */
        set_vfp_fpexc(VFPEXC_EN);

        __asm volatile(
            ".fpu vfpv3\n"
            "vldmia     %1!, {d0-d15}\n"                /* d0-d15 */
            "vldmia     %1!, {d16-d31}\n"               /* d16-d31 */
            "ldr        %0, [%1]\n"                     /* set old vfpscr */
            "vmsr       fpscr, %0\n"
            : "=&r" (scratch) : "r" (&pcb->pcb_fpstate));

        ci->ci_fpuproc = p;
        pcb->pcb_fpcpu = ci;

        /* disable until return to userland */
        set_vfp_fpexc(0);

        restore_interrupts(psw);
}

int
vfp_fault(unsigned int pc, unsigned int insn, trapframe_t *tf, int fault_code,
    uint32_t fpexc)
{
        struct proc *p = curproc;
        struct pcb *pcb = &p->p_addr->u_pcb;

        if ((fpexc & VFPEXC_EN) != 0) {
                /*
                 * We probably ran into an unsupported instruction,
                 * like NEON on a non-NEON system. Let the process know.
                 */
                return 1;
        }

        /* we should be able to ignore old state of pcb_fpcpu ci_fpuproc */
        if ((pcb->pcb_flags & PCB_FPU) == 0) {
                pcb->pcb_flags |= PCB_FPU;
                memset(&pcb->pcb_fpstate, 0, sizeof(pcb->pcb_fpstate));
        }
        vfp_load(p);

        return 0;
}

void
vfp_discard(struct proc *p)
{
        struct cpu_info *ci = curcpu();

        if (curpcb->pcb_fpcpu == ci && ci->ci_fpuproc == p) {
                ci->ci_fpuproc = NULL;
                curpcb->pcb_fpcpu = NULL;
        }
}