/* $Id: kernfpu.c,v 1.2 2010/03/21 19:36:47 kostik Exp $ */

#include <sys/types.h>
#include <sys/param.h>
#include <sys/conf.h>
#include <sys/kernel.h>
#include <sys/malloc.h>
#include <sys/module.h>
#include <sys/systm.h>
#include <sys/uio.h>

#include <machine/fpu.h>

static void dumpfp(const double *a) __attribute((unused));
void
dumpfp(const double *a)
{
	const char *x;
	int i;

	x = (const char *)a;
	for (i = 0; i < sizeof(double); i++)
		printf("%02x ", (unsigned char)x[i]);
	printf("\n");
}

static void fadd1(void *p) __attribute((noinline));
void
fadd1(void *p)
{
	static const double one = 1.0;

	__asm volatile(
	      "movsd	%1,%%xmm0\n\t"
	      "movsd	(%2),%%xmm1\n\t"
	      "addsd	%%xmm0,%%xmm1\n\t"
	      "movsd	%%xmm1,(%0)"
	      : "=r" (p)
	      : "m" (one), "r" (p)
	      : "memory");
}

struct kernfpu_priv {
	double x;
	int bytes_read;
	struct fpu_kern_ctx fpuctx;
};

MALLOC_DEFINE(M_KERNFPU, "kernfpu", "kernfpu");
static void
kernfpu_priv_dtr(void *priv)
{

	free(priv, M_KERNFPU);
}

static int
kernfpu_open(struct cdev *dev, int oflags, int devtype, struct thread *td)
{
	struct kernfpu_priv *priv;

	priv = malloc(sizeof(struct kernfpu_priv), M_KERNFPU, M_WAITOK |
	    M_ZERO);
	devfs_set_cdevpriv(priv, kernfpu_priv_dtr);
	return (0);
}

static int
kernfpu_close(struct cdev *dev, int fflag, int devtype, struct thread *td)
{

	devfs_clear_cdevpriv();
	return (0);
}

static int
kernfpu_write(struct cdev *dev, struct uio *uio, int ioflag)
{
	struct kernfpu_priv *priv;
	int error;

	error = devfs_get_cdevpriv((void **)&priv);
	if (error != 0)
		return (error);
	if (priv->bytes_read != 0)
		return (EBUSY);
	if (uio->uio_resid != sizeof(double))
		return (EIO);
	error = uiomove((char *)&priv->x, sizeof(double), uio);
	if (error != 0)
		return (error);
	priv->bytes_read = sizeof(double);
	return (0);
}

static int
kernfpu_read(struct cdev *dev, struct uio *uio, int ioflag)
{
	struct kernfpu_priv *priv;
	struct thread *td;
	int error;

	error = devfs_get_cdevpriv((void **)&priv);
	if (error != 0)
		return (error);
	if (uio->uio_resid != sizeof(double))
		return (EIO);
	td = curthread;
	error = fpu_kern_enter(td, &priv->fpuctx);
	if (error != 0)
		return (error);
	/* dumpfp(&priv->x); */
	fadd1(&priv->x);
	/* dumpfp(&priv->x); */
	priv->bytes_read = 0;
	error = fpu_kern_leave(td, &priv->fpuctx);
	if (error)
		return (error);
	error = uiomove(&priv->x, sizeof(double), uio);
	return (error);
}

static struct cdev *kernfpudev;

static struct cdevsw kernfpusw = {
	.d_version = D_VERSION,
	.d_open = kernfpu_open,
	.d_close = kernfpu_close,
	.d_read = kernfpu_read,
	.d_write = kernfpu_write
};

static int
kernfpu_modhandler(module_t mod, int what, void *arg)
{
	int error;

	error = 0;
	switch (what) {
	case MOD_LOAD:
		kernfpudev = make_dev(&kernfpusw, 0, UID_ROOT, GID_WHEEL,
		    0666, "kernfpu");
		break;

	case MOD_UNLOAD:
		if (kernfpudev != NULL)
			destroy_dev(kernfpudev);
		break;

	default:
		break;
	}

	return (error);
}

static moduledata_t kernfpu_moddata = {
	"kernfpu",
	kernfpu_modhandler,
	0
};

MODULE_VERSION(kernfpu, 1);
DECLARE_MODULE(kernfpu, kernfpu_moddata, SI_SUB_DRIVERS, SI_ORDER_ANY);