UltraSPARC III cpufreq driver

Index: arch/sparc64/dev/schizo.c
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/dev/schizo.c,v
retrieving revision 1.31
diff -u -r1.31 schizo.c
--- arch/sparc64/dev/schizo.c	21 Jun 2013 20:09:58 -0000	1.31
+++ arch/sparc64/dev/schizo.c	8 Sep 2014 20:32:22 -0000
@@ -5,6 +5,7 @@
  * Copyright (c) 2002 Jason L. Wright ([email protected])
  * Copyright (c) 2003 Henric Jungheim
  * Copyright (c) 2008, 2009, 2010, 2012 Matthew R. Green
+ * Copyright (c) 2014 Joël Bertrand ([email protected])
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -41,6 +42,7 @@
 #include <sys/systm.h>
 #include <sys/time.h>
 #include <sys/reboot.h>
+#include <sys/cpufreq.h>
 
 #define _SPARC_BUS_DMA_PRIVATE
 #include <sys/bus.h>
@@ -86,6 +88,9 @@
 int schizo_safari_error(void *);
 int schizo_pci_error(void *);
 
+void schizo_get_cpufreq(void *, void *);
+void schizo_set_cpufreq(void *, void *);
+
 pci_chipset_tag_t schizo_alloc_chipset(struct schizo_pbm *, int,
     pci_chipset_tag_t);
 bus_space_tag_t schizo_alloc_mem_tag(struct schizo_pbm *);
@@ -136,6 +141,80 @@
 }
 
 void
+schizo_set_cpufreq(void *aux, void *freq)
+{
+	uint32_t	f;
+
+	uint64_t	cmd;
+	uint64_t	esctrl;
+
+	struct schizo_softc *sc;
+
+	f = (*((uint32_t *) freq));
+	sc = aux;
+
+	if (f == sc->sc_cf.cf_state[0].cfs_freq)
+		cmd = SCZ_ESCTRL_CLK_MAX;
+	else if (f == sc->sc_cf.cf_state[1].cfs_freq)
+		cmd = SCZ_ESCTRL_CLK_DIV_2;
+	else if (f == sc->sc_cf.cf_state[2].cfs_freq)
+		cmd = SCZ_ESCTRL_CLK_DIV_32;
+	else
+	{
+		printf("cpufreq failed: switching to max frequency\n");
+		cmd = SCZ_ESCTRL_CLK_MAX;
+	}
+
+	esctrl = schizo_read(sc, SCZ_ESCTRL);
+	esctrl &= (~SCZ_ESCTRL_CLK_MASK);
+	esctrl |= cmd;
+	schizo_write(sc, SCZ_ESCTRL, esctrl);
+
+	printf("schizo_set_cpufreq %u MHz\n", f);
+}
+
+void
+schizo_get_cpufreq(void *aux, void *freq)
+{
+	uint32_t	f;
+	uint64_t	esctrl;
+
+	struct schizo_softc *sc;
+
+	sc = aux;
+	esctrl = schizo_read(sc, SCZ_ESCTRL);
+
+	switch(esctrl & (~SCZ_ESCTRL_CLK_MASK))
+	{
+		default:
+		case SCZ_ESCTRL_CLK_MAX:
+			f = sc->sc_cf.cf_state[0].cfs_freq;
+			break;
+
+		case SCZ_ESCTRL_CLK_DIV_2:
+			f = sc->sc_cf.cf_state[1].cfs_freq;
+			break;
+
+		case SCZ_ESCTRL_CLK_DIV_32:
+			f = sc->sc_cf.cf_state[2].cfs_freq;
+			break;
+	}
+
+	(*((uint32_t *) freq)) = f;
+}
+
+static void
+schizo_setup_cpufreq(device_t dev)
+{
+	struct schizo_softc *sc = device_private(dev);
+	int ret;
+
+	ret = cpufreq_register(&(sc->sc_cf));
+	if (ret != 0)
+		aprint_error_dev(sc->sc_dev, "failed to register cpufreq\n");
+}
+
+void
 schizo_attach(device_t parent, device_t self, void *aux)
 {
 	struct schizo_softc *sc = device_private(self);
@@ -318,6 +397,26 @@
 	schizo_set_intr(sc, pbm, PIL_HIGH, schizo_safari_error, sc,
 	    SCZ_SERR_INO, "safari");
 
+	/*
+	 * Register with cpufreq(9)
+	 */
+
+	(void) memset(&(sc->sc_cf), 0, sizeof(struct cpufreq));
+
+	sc->sc_cf.cf_state[0].cfs_freq = (uint32_t) (curcpu()->ci_cpu_clockrate[1]);
+	sc->sc_cf.cf_state[1].cfs_freq = sc->sc_cf.cf_state[0].cfs_freq / 2;
+	sc->sc_cf.cf_state[2].cfs_freq = sc->sc_cf.cf_state[0].cfs_freq / 32;
+	sc->sc_cf.cf_state_count = 3;
+
+	sc->sc_cf.cf_mp = false;
+	sc->sc_cf.cf_cookie = sc;
+	sc->sc_cf.cf_get_freq = schizo_get_cpufreq;
+	sc->sc_cf.cf_set_freq = schizo_set_cpufreq;
+
+	(void) strlcpy(sc->sc_cf.cf_name, "safari", sizeof(sc->sc_cf.cf_name));
+
+	config_interrupts(sc->sc_dev, schizo_setup_cpufreq);
+
 	if (sc->sc_tomatillo) {
 		/*
 		 * Enable the IOCACHE.
Index: arch/sparc64/dev/schizoreg.h
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/dev/schizoreg.h,v
retrieving revision 1.9
diff -u -r1.9 schizoreg.h
--- arch/sparc64/dev/schizoreg.h	25 Mar 2012 03:13:08 -0000	1.9
+++ arch/sparc64/dev/schizoreg.h	8 Sep 2014 20:32:22 -0000
@@ -4,6 +4,7 @@
 /*
  * Copyright (c) 2002 Jason L. Wright ([email protected])
  * Copyright (c) 2010 Matthew R. Green
+ * Copyright (c) 2014 Joël Bertrand ([email protected])
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -15,7 +16,7 @@
  *    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 AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ e THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR BE LIABLE FOR ANY DIRECT,
@@ -76,8 +77,9 @@
 	volatile u_int64_t	ue_afar;
 	volatile u_int64_t	ce_afsr;
 	volatile u_int64_t	ce_afar;
+	volatile u_int64_t	esctrl;
 
-	volatile u_int64_t	_unused4[253942];
+	volatile u_int64_t	_unused4[253941];
 	struct schizo_pbm_regs pbm_a;
 	struct schizo_pbm_regs pbm_b;
 };
@@ -101,6 +103,7 @@
 #define	SCZ_UE_AFAR			0x10038
 #define	SCZ_CE_AFSR			0x10040
 #define	SCZ_CE_AFAR			0x10048
+#define SCZ_ESCTRL			0x10050
 
 /* These are relative to the PBM */
 #define	SCZ_PCI_IOMMU_CTRL		0x00200
@@ -298,3 +301,8 @@
 	u_int32_t	size_hi;
 	u_int32_t	size_lo;
 };
+
+#define SCZ_ESCTRL_CLK_MAX		0x0000000000000000UL
+#define SCZ_ESCTRL_CLK_DIV_2	0x0000000040000000UL
+#define SCZ_ESCTRL_CLK_DIV_32	0x0000000080000000UL
+#define SCZ_ESCTRL_CLK_MASK		0x00000000C0000000UL
Index: arch/sparc64/dev/schizovar.h
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/dev/schizovar.h,v
retrieving revision 1.6
diff -u -r1.6 schizovar.h
--- arch/sparc64/dev/schizovar.h	25 Mar 2012 03:13:08 -0000	1.6
+++ arch/sparc64/dev/schizovar.h	8 Sep 2014 20:32:22 -0000
@@ -64,6 +64,7 @@
 	int sc_busa;
 	int sc_tomatillo;
 	uint32_t sc_ver;
+	struct cpufreq sc_cf;
 };
 
 #define	schizo_read(sc,r) \
Index: kern/subr_cpufreq.c
===================================================================
RCS file: /cvsroot/src/sys/kern/subr_cpufreq.c,v
retrieving revision 1.9
diff -u -r1.9 subr_cpufreq.c
--- kern/subr_cpufreq.c	12 Feb 2014 20:20:15 -0000	1.9
+++ kern/subr_cpufreq.c	8 Sep 2014 20:32:27 -0000
@@ -66,8 +66,10 @@
 	uint32_t c, i, j, k, m;
 	int rv;
 
+printf("<1>\n");
 	if (cold != 0)
 		return EBUSY;
+printf("<2>\n");
 
 	KASSERT(cf != NULL);
 	KASSERT(cf_backend != NULL);
@@ -78,7 +80,9 @@
 
 	mutex_enter(&cpufreq_lock);
 
+printf("<3>\n");
 	if (cf_backend->cf_init != false) {
+printf("<4>\n");
 		mutex_exit(&cpufreq_lock);
 		return EALREADY;
 	}
@@ -95,9 +99,11 @@
 	 * Sanity check the values and verify descending order.
 	 */
 	for (c = i = 0; i < cf->cf_state_count; i++) {
+printf("<5>\n");
 
 		CTASSERT(CPUFREQ_STATE_ENABLED != 0);
 		CTASSERT(CPUFREQ_STATE_DISABLED != 0);
+printf("<6>\n");
 
 		if (cf->cf_state[i].cfs_freq == 0)
 			continue;
@@ -128,15 +134,18 @@
 
 	cf_backend->cf_state_count = c;
 
+printf("<7>\n");
 	if (cf_backend->cf_state_count == 0) {
 		mutex_exit(&cpufreq_lock);
 		cpufreq_deregister();
+printf("<8>\n");
 		return EINVAL;
 	}
 
 	rv = cpufreq_latency();
 
 	if (rv != 0) {
+printf("<9>\n");
 		mutex_exit(&cpufreq_lock);
 		cpufreq_deregister();
 		return rv;