/*	$NetBSD: fpu.c,v 1.3 2023/05/07 12:41:49 skrll Exp $	*/

/*-
 * Copyright (c) 2014 The NetBSD Foundation, Inc.
 * All rights reserved.
 *
 * This code is derived from software contributed to The NetBSD Foundation
 * by Matt Thomas of 3am Software Foundry.
 *
 * 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``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 THE FOUNDATION OR CONTRIBUTORS
 * 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.3 2023/05/07 12:41:49 skrll Exp $");

#include "opt_multiprocessor.h"

#include <sys/param.h>
#include <sys/mutex.h>
#include <sys/condvar.h>
#include <sys/cpu.h>
#include <sys/proc.h>
#include <sys/lwp.h>
#include <sys/pcu.h>

#include <riscv/locore.h>
#include <riscv/frame.h>

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

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

static void
fpu_disable(void)
{
	csr_sstatus_clear(SR_FS);
}

static void
fpu_enable(void)
{
	csr_sstatus_clear(SR_FS);
	csr_sstatus_set(__SHIFTIN(SR_FS_CLEAN, SR_FS));
}

void
fpu_state_save(lwp_t *l)
{
//	struct trapframe * const tf = l->l_md.md_utf;
	struct pcb * const pcb = lwp_getpcb(l);
	struct fpreg * const fp = &pcb->pcb_fpregs;

	KASSERT(l->l_pcu_cpu[PCU_FPU] == curcpu());

	curcpu()->ci_ev_fpu_saves.ev_count++;

	// Enable FPU to save FP state
	fpu_enable();

	// Save FCSR
	fp->r_fcsr = fcsr_read();

	// Save FP register values.
	__asm(	"fsd	f0, (0*%1)(%0)"
	"\n\t"	"fsd	f1, (1*%1)(%0)"
	"\n\t"	"fsd	f2, (2*%1)(%0)"
	"\n\t"	"fsd	f3, (3*%1)(%0)"
	"\n\t"	"fsd	f4, (4*%1)(%0)"
	"\n\t"	"fsd	f5, (5*%1)(%0)"
	"\n\t"	"fsd	f6, (6*%1)(%0)"
	"\n\t"	"fsd	f7, (7*%1)(%0)"
	"\n\t"	"fsd	f8, (8*%1)(%0)"
	"\n\t"	"fsd	f9, (9*%1)(%0)"
	"\n\t"	"fsd	f10, (10*%1)(%0)"
	"\n\t"	"fsd	f11, (11*%1)(%0)"
	"\n\t"	"fsd	f12, (12*%1)(%0)"
	"\n\t"	"fsd	f13, (13*%1)(%0)"
	"\n\t"	"fsd	f14, (14*%1)(%0)"
	"\n\t"	"fsd	f15, (15*%1)(%0)"
	"\n\t"	"fsd	f16, (16*%1)(%0)"
	"\n\t"	"fsd	f17, (17*%1)(%0)"
	"\n\t"	"fsd	f18, (18*%1)(%0)"
	"\n\t"	"fsd	f19, (19*%1)(%0)"
	"\n\t"	"fsd	f20, (20*%1)(%0)"
	"\n\t"	"fsd	f21, (21*%1)(%0)"
	"\n\t"	"fsd	f22, (22*%1)(%0)"
	"\n\t"	"fsd	f23, (23*%1)(%0)"
	"\n\t"	"fsd	f24, (24*%1)(%0)"
	"\n\t"	"fsd	f25, (25*%1)(%0)"
	"\n\t"	"fsd	f26, (26*%1)(%0)"
	"\n\t"	"fsd	f27, (27*%1)(%0)"
	"\n\t"	"fsd	f28, (28*%1)(%0)"
	"\n\t"	"fsd	f29, (29*%1)(%0)"
	"\n\t"	"fsd	f30, (30*%1)(%0)"
	"\n\t"	"fsd	f31, (31*%1)(%0)"
	   ::	"r"(fp->r_fpreg),
		"i"(sizeof(fp->r_fpreg[0])) : "memory");

	// Disable the FPU
	fpu_disable();
}

void
fpu_state_load(lwp_t *l, u_int flags)
{
	struct trapframe * const tf = l->l_md.md_utf;
	struct pcb * const pcb = lwp_getpcb(l);
	struct fpreg * const fp = &pcb->pcb_fpregs;

	KASSERT(l == curlwp);
//	KASSERT(l->l_pcu_cpu[PCU_FPU] == curcpu());

	// If this is the first time the state is being loaded, zero it first.
	if (__predict_false((flags & PCU_VALID) == 0)) {
		memset(fp, 0, sizeof(*fp));
	}

	// Enable the FP when this lwp return to userspace.
	tf->tf_sr &= ~SR_FS;
	tf->tf_sr |= __SHIFTIN(SR_FS_CLEAN, SR_FS);

	// If this is a simple reenable, set the FPU enable flag and return
	if (flags & PCU_REENABLE) {
		curcpu()->ci_ev_fpu_reenables.ev_count++;
		return;
	}

	curcpu()->ci_ev_fpu_loads.ev_count++;


	// Enabling to load FP state.  Interrupts will remain on.
	fpu_enable();

	// load FP registers and establish processes' FP context.
	__asm(	"fld	f0, (0*%1)(%0)"
	"\n\t"	"fld	f1, (1*%1)(%0)"
	"\n\t"	"fld	f2, (2*%1)(%0)"
	"\n\t"	"fld	f3, (3*%1)(%0)"
	"\n\t"	"fld	f4, (4*%1)(%0)"
	"\n\t"	"fld	f5, (5*%1)(%0)"
	"\n\t"	"fld	f6, (6*%1)(%0)"
	"\n\t"	"fld	f7, (7*%1)(%0)"
	"\n\t"	"fld	f8, (8*%1)(%0)"
	"\n\t"	"fld	f9, (9*%1)(%0)"
	"\n\t"	"fld	f10, (10*%1)(%0)"
	"\n\t"	"fld	f11, (11*%1)(%0)"
	"\n\t"	"fld	f12, (12*%1)(%0)"
	"\n\t"	"fld	f13, (13*%1)(%0)"
	"\n\t"	"fld	f14, (14*%1)(%0)"
	"\n\t"	"fld	f15, (15*%1)(%0)"
	"\n\t"	"fld	f16, (16*%1)(%0)"
	"\n\t"	"fld	f17, (17*%1)(%0)"
	"\n\t"	"fld	f18, (18*%1)(%0)"
	"\n\t"	"fld	f19, (19*%1)(%0)"
	"\n\t"	"fld	f20, (20*%1)(%0)"
	"\n\t"	"fld	f21, (21*%1)(%0)"
	"\n\t"	"fld	f22, (22*%1)(%0)"
	"\n\t"	"fld	f23, (23*%1)(%0)"
	"\n\t"	"fld	f24, (24*%1)(%0)"
	"\n\t"	"fld	f25, (25*%1)(%0)"
	"\n\t"	"fld	f26, (26*%1)(%0)"
	"\n\t"	"fld	f27, (27*%1)(%0)"
	"\n\t"	"fld	f28, (28*%1)(%0)"
	"\n\t"	"fld	f29, (29*%1)(%0)"
	"\n\t"	"fld	f30, (30*%1)(%0)"
	"\n\t"	"fld	f31, (31*%1)(%0)"
	   ::	"r"(fp->r_fpreg),
		"i"(sizeof(fp->r_fpreg[0])): "memory");

	// load FPCSR and disable FPU again
	fcsr_write(fp->r_fcsr);

	fpu_disable();
}

void
fpu_state_release(lwp_t *l)
{
	l->l_md.md_utf->tf_sr &= ~SR_FS;
	fpu_disable();
}