xref: /dflybsd-src/sys/dev/powermng/clockmod/clockmod.c (revision 805c8e8e4093ceca2e27510ad3a66d4de8060a55)

/*
 * Copyright (c) 2014 The DragonFly Project.  All rights reserved.
 *
 * This code is derived from software contributed to The DragonFly Project
 * by Sepherosa Ziehau <sepherosa@gmail.com>
 *
 * 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.
 * 3. Neither the name of The DragonFly Project nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific, prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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
 * COPYRIGHT HOLDERS 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/param.h>
#include <sys/conf.h>
#include <sys/kernel.h>
#include <sys/bus.h>
#include <sys/cpu_topology.h>
#include <sys/cpuhelper.h>
#include <sys/malloc.h>
#include <sys/module.h>
#include <sys/queue.h>
#include <sys/serialize.h>
#include <sys/sysctl.h>
#include <sys/systm.h>

#include <machine/specialreg.h>
#include <machine/cpufunc.h>
#include <machine/cputypes.h>
#include <machine/md_var.h>

struct clockmod_dom;

struct clockmod_softc {
	TAILQ_ENTRY(clockmod_softc) sc_link;
	struct clockmod_dom	*sc_dom;
	int			sc_cpuid;
};

struct clockmod_dom {
	TAILQ_ENTRY(clockmod_dom) dom_link;
	TAILQ_HEAD(, clockmod_softc) dom_list;
	struct sysctl_ctx_list	dom_sysctl_ctx;
	struct sysctl_oid	*dom_sysctl_tree;
	cpumask_t		dom_cpumask;
	char			dom_name[16];
	int			dom_select;
	uint32_t		dom_flags;
};

#define CLOCKMOD_DOM_FLAG_ACTIVE	0x1

struct clockmod_dom_ctrl {
	char			ctl_name[8];
	uint64_t		ctl_value;
};

static int	clockmod_dom_attach(struct clockmod_softc *);
static void	clockmod_dom_detach(struct clockmod_softc *);
static struct clockmod_dom *clockmod_dom_find(cpumask_t);
static struct clockmod_dom *clockmod_dom_create(cpumask_t);
static void	clockmod_dom_destroy(struct clockmod_dom *);

static int	clockmod_dom_sysctl_select(SYSCTL_HANDLER_ARGS);
static int	clockmod_dom_sysctl_members(SYSCTL_HANDLER_ARGS);
static int	clockmod_dom_sysctl_available(SYSCTL_HANDLER_ARGS);

static void	clockmod_identify(driver_t *, device_t);
static int	clockmod_probe(device_t);
static int	clockmod_attach(device_t);
static int	clockmod_detach(device_t);

static void	clockmod_select_handler(struct cpuhelper_msg *);
static int	clockmod_select(const struct clockmod_softc *,
		    const struct clockmod_dom_ctrl *);

static boolean_t clockmod_errata_duty(int);

static struct lwkt_serialize clockmod_dom_slize = LWKT_SERIALIZE_INITIALIZER;
static int	clockmod_dom_id;
static TAILQ_HEAD(, clockmod_dom) clockmod_dom_list =
    TAILQ_HEAD_INITIALIZER(clockmod_dom_list);
static int	clockmod_dom_nctrl;
static struct clockmod_dom_ctrl *clockmod_dom_controls;

static device_method_t clockmod_methods[] = {
	/* Device interface */
	DEVMETHOD(device_identify,	clockmod_identify),
	DEVMETHOD(device_probe,		clockmod_probe),
	DEVMETHOD(device_attach,	clockmod_attach),
	DEVMETHOD(device_detach,	clockmod_detach),

	DEVMETHOD_END
};

static driver_t clockmod_driver = {
	"clockmod",
	clockmod_methods,
	sizeof(struct clockmod_softc),
};

static devclass_t clockmod_devclass;
DRIVER_MODULE(clockmod, cpu, clockmod_driver, clockmod_devclass, NULL, NULL);

static void
clockmod_identify(driver_t *driver, device_t parent)
{
	device_t child;

	if (device_find_child(parent, "clockmod", -1) != NULL)
		return;

	if (cpu_vendor_id != CPU_VENDOR_INTEL)
		return;

	if ((cpu_feature & (CPUID_ACPI | CPUID_TM)) != (CPUID_ACPI | CPUID_TM))
		return;

	child = device_add_child(parent, "clockmod", device_get_unit(parent));
	if (child == NULL)
		device_printf(parent, "add clockmod failed\n");
}

static int
clockmod_probe(device_t dev)
{
	device_set_desc(dev, "CPU clock modulation");
	return 0;
}

static int
clockmod_attach(device_t dev)
{
	struct clockmod_softc *sc = device_get_softc(dev);
	int error;

	sc->sc_cpuid = device_get_unit(dev);

	error = clockmod_dom_attach(sc);
	if (error) {
		device_printf(dev, "domain attach failed\n");
		return error;
	}

	return 0;
}

static int
clockmod_detach(device_t dev)
{
	clockmod_dom_detach(device_get_softc(dev));
	return 0;
}

static int
clockmod_dom_attach(struct clockmod_softc *sc)
{
	struct clockmod_softc *sc1;
	struct clockmod_dom *dom;
	cpumask_t mask, found_mask;
	int error = 0;

	CPUMASK_ASSZERO(found_mask);

	mask = get_cpumask_from_level(sc->sc_cpuid, CORE_LEVEL);
	if (CPUMASK_TESTZERO(mask))
		CPUMASK_ASSBIT(mask, sc->sc_cpuid);

	lwkt_serialize_enter(&clockmod_dom_slize);

	dom = clockmod_dom_find(mask);
	if (dom == NULL) {
		dom = clockmod_dom_create(mask);
		if (dom == NULL) {
			error = ENOMEM;
			goto back;
		}
	}

	sc->sc_dom = dom;
	TAILQ_INSERT_TAIL(&dom->dom_list, sc, sc_link);

	TAILQ_FOREACH(sc1, &dom->dom_list, sc_link)
		CPUMASK_ORBIT(found_mask, sc1->sc_cpuid);

	if (CPUMASK_CMPMASKEQ(found_mask, dom->dom_cpumask)) {
		/* All cpus in this domain is found */
		dom->dom_flags |= CLOCKMOD_DOM_FLAG_ACTIVE;
	}
back:
	lwkt_serialize_exit(&clockmod_dom_slize);
	return error;
}

static void
clockmod_dom_detach(struct clockmod_softc *sc)
{
	struct clockmod_dom *dom;

	lwkt_serialize_enter(&clockmod_dom_slize);

	dom = sc->sc_dom;
	sc->sc_dom = NULL;

	if (dom->dom_flags & CLOCKMOD_DOM_FLAG_ACTIVE) {
		struct clockmod_softc *sc1;

		/* Raise to 100% */
		TAILQ_FOREACH(sc1, &dom->dom_list, sc_link)
			clockmod_select(sc1, &clockmod_dom_controls[0]);
	}

	/* One cpu is leaving; domain is no longer active */
	dom->dom_flags &= ~CLOCKMOD_DOM_FLAG_ACTIVE;

	TAILQ_REMOVE(&dom->dom_list, sc, sc_link);
	if (TAILQ_EMPTY(&dom->dom_list))
		clockmod_dom_destroy(dom);

	lwkt_serialize_exit(&clockmod_dom_slize);
}

static struct clockmod_dom *
clockmod_dom_find(cpumask_t mask)
{
	struct clockmod_dom *dom;

	TAILQ_FOREACH(dom, &clockmod_dom_list, dom_link) {
		if (CPUMASK_CMPMASKEQ(dom->dom_cpumask, mask))
			return dom;
	}
	return NULL;
}

static struct clockmod_dom *
clockmod_dom_create(cpumask_t mask)
{
	struct clockmod_dom *dom;
	int id;

	id = clockmod_dom_id++;
	dom = kmalloc(sizeof(*dom), M_DEVBUF, M_WAITOK | M_ZERO);

	TAILQ_INIT(&dom->dom_list);
	dom->dom_cpumask = mask;
	ksnprintf(dom->dom_name, sizeof(dom->dom_name), "clockmod_dom%d", id);

	sysctl_ctx_init(&dom->dom_sysctl_ctx);
	dom->dom_sysctl_tree = SYSCTL_ADD_NODE(&dom->dom_sysctl_ctx,
	    SYSCTL_STATIC_CHILDREN(_machdep), OID_AUTO, dom->dom_name,
	    CTLFLAG_RD, 0, "");
	if (dom->dom_sysctl_tree == NULL) {
		kprintf("%s: can't add sysctl node\n", dom->dom_name);
		kfree(dom, M_DEVBUF);
		return NULL;
	}

	SYSCTL_ADD_PROC(&dom->dom_sysctl_ctx,
	    SYSCTL_CHILDREN(dom->dom_sysctl_tree),
	    OID_AUTO, "members", CTLTYPE_STRING | CTLFLAG_RD,
	    dom, 0, clockmod_dom_sysctl_members, "A", "member cpus");

	SYSCTL_ADD_PROC(&dom->dom_sysctl_ctx,
	    SYSCTL_CHILDREN(dom->dom_sysctl_tree),
	    OID_AUTO, "available", CTLTYPE_STRING | CTLFLAG_RD,
	    dom, 0, clockmod_dom_sysctl_available, "A",
	    "available duty percent");

	SYSCTL_ADD_PROC(&dom->dom_sysctl_ctx,
	    SYSCTL_CHILDREN(dom->dom_sysctl_tree),
	    OID_AUTO, "select", CTLTYPE_STRING | CTLFLAG_RW,
	    dom, 0, clockmod_dom_sysctl_select, "A", "select duty");

	TAILQ_INSERT_TAIL(&clockmod_dom_list, dom, dom_link);

	if (clockmod_dom_controls == NULL) {
		int nctrl, step, i, shift, cnt;

#ifdef __x86_64__
		if (cpu_thermal_feature & CPUID_THERMAL_ECMD)
			shift = 0;
		else
#endif
			shift = 1;

		nctrl = 8 << (1 - shift);
		step = 10000 / nctrl;

		clockmod_dom_controls =
		    kmalloc(sizeof(struct clockmod_dom_ctrl) * nctrl, M_DEVBUF,
		    M_WAITOK | M_ZERO);

		if (bootverbose)
			kprintf("clock modulation:\n");

		cnt = 0;
		for (i = 0; i < nctrl; ++i) {
			struct clockmod_dom_ctrl *ctrl =
			    &clockmod_dom_controls[cnt];
			int duty;

			duty = 10000 - (i * step);
			if (clockmod_errata_duty(duty))
				continue;
			++cnt;

			ksnprintf(ctrl->ctl_name, sizeof(ctrl->ctl_name),
			    "%d.%02d%%", duty / 100, duty % 100);
			ctrl->ctl_value = (((nctrl - i) << shift) & 0xf);
			if (i != 0)
				ctrl->ctl_value |= 1 << 4;

			if (bootverbose) {
				kprintf("  0x%04jx %s\n",
				    (uintmax_t)ctrl->ctl_value,
				    ctrl->ctl_name);
			}
		}
		clockmod_dom_nctrl = cnt;
	}
	return dom;
}

static void
clockmod_dom_destroy(struct clockmod_dom *dom)
{
	KASSERT(TAILQ_EMPTY(&dom->dom_list),
	    ("%s: still has member cpus", dom->dom_name));
	TAILQ_REMOVE(&clockmod_dom_list, dom, dom_link);

	sysctl_ctx_free(&dom->dom_sysctl_ctx);
	kfree(dom, M_DEVBUF);

	if (TAILQ_EMPTY(&clockmod_dom_list)) {
		clockmod_dom_nctrl = 0;
		kfree(clockmod_dom_controls, M_DEVBUF);
		clockmod_dom_controls = NULL;
	}
}

static int
clockmod_dom_sysctl_members(SYSCTL_HANDLER_ARGS)
{
	struct clockmod_dom *dom = arg1;
	struct clockmod_softc *sc;
	int loop, error;

	lwkt_serialize_enter(&clockmod_dom_slize);

	loop = error = 0;
	TAILQ_FOREACH(sc, &dom->dom_list, sc_link) {
		char buf[16];

		if (error == 0 && loop)
			error = SYSCTL_OUT(req, " ", 1);
		if (error == 0) {
			ksnprintf(buf, sizeof(buf), "cpu%d", sc->sc_cpuid);
			error = SYSCTL_OUT(req, buf, strlen(buf));
		}
		++loop;
	}

	lwkt_serialize_exit(&clockmod_dom_slize);
	return error;
}

static int
clockmod_dom_sysctl_available(SYSCTL_HANDLER_ARGS)
{
	struct clockmod_dom *dom = arg1;
	int loop, error, i;

	lwkt_serialize_enter(&clockmod_dom_slize);

	if ((dom->dom_flags & CLOCKMOD_DOM_FLAG_ACTIVE) == 0) {
		error = SYSCTL_OUT(req, " ", 1);
		goto done;
	}

	loop = error = 0;
	for (i = 0; i < clockmod_dom_nctrl; ++i) {
		if (error == 0 && loop)
			error = SYSCTL_OUT(req, " ", 1);
		if (error == 0) {
			error = SYSCTL_OUT(req,
			    clockmod_dom_controls[i].ctl_name,
			    strlen(clockmod_dom_controls[i].ctl_name));
		}
		++loop;
	}
done:
	lwkt_serialize_exit(&clockmod_dom_slize);
	return error;
}

static int
clockmod_dom_sysctl_select(SYSCTL_HANDLER_ARGS)
{
	struct clockmod_dom *dom = arg1;
	struct clockmod_softc *sc;
	const struct clockmod_dom_ctrl *ctrl = NULL;
	char duty[16];
	int error, i;

	lwkt_serialize_enter(&clockmod_dom_slize);
	KKASSERT(dom->dom_select >= 0 && dom->dom_select < clockmod_dom_nctrl);
	ksnprintf(duty, sizeof(duty), "%s",
	    clockmod_dom_controls[dom->dom_select].ctl_name);
	lwkt_serialize_exit(&clockmod_dom_slize);

	error = sysctl_handle_string(oidp, duty, sizeof(duty), req);
	if (error != 0 || req->newptr == NULL)
		return error;

	lwkt_serialize_enter(&clockmod_dom_slize);

	if ((dom->dom_flags & CLOCKMOD_DOM_FLAG_ACTIVE) == 0) {
		error = EOPNOTSUPP;
		goto back;
	}

	for (i = 0; i < clockmod_dom_nctrl; ++i) {
		ctrl = &clockmod_dom_controls[i];
		if (strcmp(duty, ctrl->ctl_name) == 0)
			break;
	}
	if (i == clockmod_dom_nctrl) {
		error = EINVAL;
		goto back;
	}
	dom->dom_select = i;

	TAILQ_FOREACH(sc, &dom->dom_list, sc_link)
		clockmod_select(sc, ctrl);
back:
	lwkt_serialize_exit(&clockmod_dom_slize);
	return error;
}

static void
clockmod_select_handler(struct cpuhelper_msg *msg)
{
	uint64_t ctl_value = *((const uint64_t *)msg->ch_cbarg);

#if 0
	if (bootverbose) {
		kprintf("cpu%d: clockmod 0x%04jx\n", mycpuid,
		    (uintmax_t)ctl_value);
	}
#endif
	wrmsr(MSR_THERM_CONTROL, ctl_value);
	cpuhelper_replymsg(msg, 0);
}

static int
clockmod_select(const struct clockmod_softc *sc,
    const struct clockmod_dom_ctrl *ctrl)
{
	struct cpuhelper_msg msg;

	cpuhelper_initmsg(&msg, &curthread->td_msgport,
	    clockmod_select_handler, __DECONST(void *, &ctrl->ctl_value),
	    MSGF_PRIORITY);
	return (cpuhelper_domsg(&msg, sc->sc_cpuid));
}

static boolean_t
clockmod_errata_duty(int duty)
{
	uint32_t model, stepping;

	/*
	 * This is obtained from the original p4tcc code.
	 *
	 * The original errata checking code in p4tcc is obviously wrong.
	 * However, I am no longer being able to find the errata mentioned
	 * in the code.  The guess is that the errata only affects family
	 * 0x0f CPUs, since:
	 * - The errata applies to only to model 0x00, 0x01 and 0x02 in
	 *   the original p4tcc code.
	 * - Software controlled clock modulation has been supported since
	 *   0f_00 and the model of the oldest family 0x06 CPUs supporting
	 *   this feature is 0x09.
	 */
	if (CPUID_TO_FAMILY(cpu_id) != 0xf)
		return FALSE;

	model = CPUID_TO_MODEL(cpu_id);
	stepping = cpu_id & 0xf;

	if (model == 0x6) {
		switch (stepping) {
		case 0x2:
		case 0x4:
		case 0x5:
			/* Hang w/ 12.50% and 25.00% */
			if (duty == 1250 || duty == 2500)
				return TRUE;
			break;
		}
	} else if (model == 0x2) {
		switch (stepping) {
		case 0x2:
		case 0x4:
		case 0x5:
		case 0x7:
		case 0x9:
			/* Hang w/ 12.50% */
			if (duty == 1250)
				return TRUE;
			break;
		}
	} else if (model == 0x1) {
		switch (stepping) {
		case 0x2:
		case 0x3:
			/* Hang w/ 12.50% and 25.00% */
			if (duty == 1250 || duty == 2500)
				return TRUE;
			break;
		}
	} else if (model == 0x0) {
		switch (stepping) {
		case 0x7:
		case 0xa:
			/* Hang w/ 12.50% and 25.00% */
			if (duty == 1250 || duty == 2500)
				return TRUE;
			break;
		}
	}
	return FALSE;
}