/*
 * Copyright (c) 2015 Imre Vadász <imre@vdsz.com>
 *
 * 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.
 */

/* sensor driver for effective CPU frequency, using APERF/MPERF MSRs */

#include <sys/param.h>
#include <sys/bus.h>
#include <sys/module.h>
#include <sys/sensors.h>
#include <sys/systm.h>
#include <sys/thread2.h>

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

#include "cpu_if.h"

#define MSR_MPERF	0xE7
#define MSR_APERF	0xE8

struct aperf_softc {
	struct ksensordev	*sc_sensdev;
	struct ksensor		sc_sens;
	struct sensor_task	*sc_senstask;

	uint64_t		sc_aperf_prev;
	uint64_t		sc_mperf_prev;
	uint64_t		sc_tsc_prev;
	uint32_t		sc_flags;	/* APERF_FLAG_ */
};

#define APERF_FLAG_PREV_VALID	0x1

/*
 * Device methods.
 */
static void	aperf_identify(driver_t *, device_t);
static int	aperf_probe(device_t);
static int	aperf_attach(device_t);
static int	aperf_detach(device_t);

static void	aperf_sensor_task(void *);

static device_method_t aperf_methods[] = {
	/* Device interface */
	DEVMETHOD(device_identify,	aperf_identify),
	DEVMETHOD(device_probe,		aperf_probe),
	DEVMETHOD(device_attach,	aperf_attach),
	DEVMETHOD(device_detach,	aperf_detach),

	DEVMETHOD_END
};

static driver_t aperf_driver = {
	"aperf",
	aperf_methods,
	sizeof(struct aperf_softc),
};

static devclass_t aperf_devclass;
DRIVER_MODULE(aperf, cpu, aperf_driver, aperf_devclass, NULL, NULL);
MODULE_VERSION(aperf, 1);

static void
aperf_identify(driver_t *driver, device_t parent)
{
	uint32_t regs[4];
	device_t child;

	/* Make sure we're not being doubly invoked. */
	if (device_find_child(parent, "aperf", -1) != NULL)
		return;

	/* CPUID Fn0000_0006_ECX Effective Processor Frequency Interface */
	do_cpuid(0x00000006, regs);
	if ((regs[2] & 1) == 0)
		return;

	child = device_add_child(parent, "aperf", -1);
	if (child == NULL)
		device_printf(parent, "add aperf child failed\n");
}

static int
aperf_probe(device_t dev)
{
	if (resource_disabled("aperf", 0))
		return ENXIO;

	device_set_desc(dev, "CPU Frequency Sensor");

	return 0;
}

static int
aperf_attach(device_t dev)
{
	struct aperf_softc *sc = device_get_softc(dev);
	device_t parent;
	int cpu;

	parent = device_get_parent(dev);
	cpu = device_get_unit(parent);

	sc->sc_sensdev = CPU_GET_SENSDEV(parent);
	if (sc->sc_sensdev == NULL)
		return ENXIO;

	/*
	 * Add hw.sensors.cpuN.raw0 MIB.
	 */
	ksnprintf(sc->sc_sens.desc, sizeof(sc->sc_sens.desc),
	    "cpu%d freq", cpu);
	sc->sc_sens.type = SENSOR_FREQ;
	sensor_set_unknown(&sc->sc_sens);
	sensor_attach(sc->sc_sensdev, &sc->sc_sens);

	sc->sc_senstask = sensor_task_register2(sc, aperf_sensor_task, 1, cpu);

	return 0;
}

static int
aperf_detach(device_t dev)
{
	struct aperf_softc *sc = device_get_softc(dev);

	if (sc->sc_senstask != NULL)
		sensor_task_unregister2(sc->sc_senstask);

	if (sc->sc_sensdev != NULL)
		sensor_detach(sc->sc_sensdev, &sc->sc_sens);

	return 0;
}

static void
aperf_sensor_task(void *xsc)
{
	struct aperf_softc *sc = xsc;
	uint64_t aperf, mperf, tsc, freq;
	uint64_t aperf_diff, mperf_diff, tsc_diff;

	crit_enter();
	tsc = rdtsc_ordered();
	aperf = rdmsr(MSR_APERF);
	mperf = rdmsr(MSR_MPERF);
	crit_exit();

	if ((sc->sc_flags & APERF_FLAG_PREV_VALID) == 0) {
		sc->sc_aperf_prev = aperf;
		sc->sc_mperf_prev = mperf;
		sc->sc_tsc_prev = tsc;
		sc->sc_flags |= APERF_FLAG_PREV_VALID;
		return;
	}

	aperf_diff = aperf - sc->sc_aperf_prev;
	mperf_diff = mperf - sc->sc_mperf_prev;
	if (tsc_invariant)
		tsc_diff = tsc - sc->sc_tsc_prev;
	else
		tsc_diff = tsc_frequency;

	sc->sc_aperf_prev = aperf;
	sc->sc_mperf_prev = mperf;
	sc->sc_tsc_prev = tsc;

	/* Avoid division by zero */
	if (mperf_diff == 0)
		mperf_diff = 1;

	/* Using tsc_diff/1000 to avoid overflowing */
	freq = ((aperf_diff * (tsc_diff / 1000)) / mperf_diff) * 1000;
	sensor_set(&sc->sc_sens, freq, SENSOR_S_UNSPEC);
}