/*	$OpenBSD: fpu.c,v 1.4 2021/04/14 18:35:14 kettenis Exp $	*/

/*
 * Copyright (c) 2020 Mark Kettenis <kettenis@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/proc.h>
#include <sys/systm.h>
#include <sys/user.h>

#include <machine/cpufunc.h>
#include <machine/fenv.h>

void
save_vsx(struct proc *p)
{
	struct fpreg *fp = &p->p_addr->u_pcb.pcb_fpstate;

	mtmsr(mfmsr() | (PSL_FP|PSL_VEC|PSL_VSX));

	isync();

#define STXVVSR(n) \
	__asm volatile ("stxvd2x %%vs" #n ", 0, %0" :: "b"(&fp->fp_vsx[(n)]));

	STXVVSR(0);	STXVVSR(1);	STXVVSR(2);	STXVVSR(3);
	STXVVSR(4);	STXVVSR(5);	STXVVSR(6);	STXVVSR(7);
	STXVVSR(8);	STXVVSR(9);	STXVVSR(10);	STXVVSR(11);
	STXVVSR(12);	STXVVSR(13);	STXVVSR(14);	STXVVSR(15);
	STXVVSR(16);	STXVVSR(17);	STXVVSR(18);	STXVVSR(19);
	STXVVSR(20);	STXVVSR(21);	STXVVSR(22);	STXVVSR(23);
	STXVVSR(24);	STXVVSR(25);	STXVVSR(26);	STXVVSR(27);
	STXVVSR(28);	STXVVSR(29);	STXVVSR(30);	STXVVSR(31);
	STXVVSR(32);	STXVVSR(33);	STXVVSR(34);	STXVVSR(35);
	STXVVSR(36);	STXVVSR(37);	STXVVSR(38);	STXVVSR(39);
	STXVVSR(40);	STXVVSR(41);	STXVVSR(42);	STXVVSR(43);
	STXVVSR(44);	STXVVSR(45);	STXVVSR(46);	STXVVSR(47);
	STXVVSR(48);	STXVVSR(49);	STXVVSR(50);	STXVVSR(51);
	STXVVSR(52);	STXVVSR(53);	STXVVSR(54);	STXVVSR(55);
	STXVVSR(56);	STXVVSR(57);	STXVVSR(58);	STXVVSR(59);
	STXVVSR(60);	STXVVSR(61);	STXVVSR(62);	STXVVSR(63);

	__asm volatile ("mffs %%f0; stfd %%f0, 0(%0)"
	    :: "b"(&fp->fp_fpscr));
	__asm volatile ("mfvscr %%v0; stvewx %%v0, 0, %0"
	    :: "b"(&fp->fp_vscr));

	isync();

	mtmsr(mfmsr() & ~(PSL_FP|PSL_VEC|PSL_VSX));
}

void
restore_vsx(struct proc *p)
{
	struct pcb *pcb = &p->p_addr->u_pcb;
	struct fpreg *fp = &pcb->pcb_fpstate;

	if ((pcb->pcb_flags & (PCB_FPU|PCB_VEC|PCB_VSX)) == 0)
		memset(fp, 0, sizeof(*fp));

	mtmsr(mfmsr() | (PSL_FP|PSL_VEC|PSL_VSX));

	isync();

	__asm volatile ("lfd %%f0, 0(%0); mtfsf 0xff,%%f0"
	    :: "b"(&fp->fp_fpscr));
	__asm volatile ("vxor %%v0, %%v0, %%v0; lvewx %%v0, 0, %0; mtvscr %%v0"
	    :: "b"(&fp->fp_vscr));

#define LXVVSR(n) \
	__asm volatile ("lxvd2x %%vs" #n ", 0, %0" :: "b"(&fp->fp_vsx[(n)]));

	LXVVSR(0);	LXVVSR(1);	LXVVSR(2);	LXVVSR(3);
	LXVVSR(4);	LXVVSR(5);	LXVVSR(6);	LXVVSR(7);
	LXVVSR(8);	LXVVSR(9);	LXVVSR(10);	LXVVSR(11);
	LXVVSR(12);	LXVVSR(13);	LXVVSR(14);	LXVVSR(15);
	LXVVSR(16);	LXVVSR(17);	LXVVSR(18);	LXVVSR(19);
	LXVVSR(20);	LXVVSR(21);	LXVVSR(22);	LXVVSR(23);
	LXVVSR(24);	LXVVSR(25);	LXVVSR(26);	LXVVSR(27);
	LXVVSR(28);	LXVVSR(29);	LXVVSR(30);	LXVVSR(31);
	LXVVSR(32);	LXVVSR(33);	LXVVSR(34);	LXVVSR(35);
	LXVVSR(36);	LXVVSR(37);	LXVVSR(38);	LXVVSR(39);
	LXVVSR(40);	LXVVSR(41);	LXVVSR(42);	LXVVSR(43);
	LXVVSR(44);	LXVVSR(45);	LXVVSR(46);	LXVVSR(47);
	LXVVSR(48);	LXVVSR(49);	LXVVSR(50);	LXVVSR(51);
	LXVVSR(52);	LXVVSR(53);	LXVVSR(54);	LXVVSR(55);
	LXVVSR(56);	LXVVSR(57);	LXVVSR(58);	LXVVSR(59);
	LXVVSR(60);	LXVVSR(61);	LXVVSR(62);	LXVVSR(63);

	isync();

	mtmsr(mfmsr() & ~(PSL_FP|PSL_VEC|PSL_VSX));
}

int
fpu_sigcode(struct proc *p)
{
	struct trapframe *tf = p->p_md.md_regs;
	struct fpreg *fp = &p->p_addr->u_pcb.pcb_fpstate;
	int code = FPE_FLTINV;

	KASSERT(tf->srr1 & PSL_FP);
	tf->srr1 &= ~(PSL_FPU|PSL_VEC|PSL_VSX);
	save_vsx(p);

	if (fp->fp_fpscr & FE_INVALID)
		code = FPE_FLTINV;
	else if (fp->fp_fpscr & FE_DIVBYZERO)
		code = FPE_FLTDIV;
	else if (fp->fp_fpscr & FE_OVERFLOW)
		code = FPE_FLTOVF;
	else if (fp->fp_fpscr & FE_UNDERFLOW)
		code = FPE_FLTUND;
	else if (fp->fp_fpscr & FE_INEXACT)
		code = FPE_FLTRES;

	return code;
}