Add support for relative cpufreq drivers. Such drivers modulate clock

frequency as a percentage of the base rate and do not change the base
rate directly.  The cpufreq framework combines these with absolute drivers
to produce synthesized levels made of one or more settings.

1 files changed, 192 insertions, 27 deletions

diff --git a/sys/kern/kern_cpu.c b/sys/kern/kern_cpu.c
index 9f9fa3ae29a7..67345a5a7015 100644
--- a/sys/kern/kern_cpu.c
+++ b/sys/kern/kern_cpu.c
@@ -57,6 +57,7 @@ __FBSDID("$FreeBSD$");
 struct cpufreq_softc {
 	struct cf_level			curr_level;
 	int				priority;
+	int				all_count;
 	struct cf_level_lst		all_levels;
 	device_t			dev;
 	struct sysctl_ctx_list		sysctl_ctx;
@@ -78,8 +79,12 @@ static int	cf_set_method(device_t dev, const struct cf_level *level,
 static int	cf_get_method(device_t dev, struct cf_level *level);
 static int	cf_levels_method(device_t dev, struct cf_level *levels,
 		    int *count);
-static int	cpufreq_insert_abs(struct cf_level_lst *list,
+static int	cpufreq_insert_abs(struct cpufreq_softc *sc,
 		    struct cf_setting *sets, int count);
+static int	cpufreq_expand_set(struct cpufreq_softc *sc,
+		    struct cf_setting_array *set_arr);
+static struct cf_level *cpufreq_dup_set(struct cpufreq_softc *sc,
+		    struct cf_level *dup, struct cf_setting *set);
 static int	cpufreq_curr_sysctl(SYSCTL_HANDLER_ARGS);
 static int	cpufreq_levels_sysctl(SYSCTL_HANDLER_ARGS);
 
@@ -166,7 +171,7 @@ cf_set_method(device_t dev, const struct cf_level *level, int priority)
 {
 	struct cpufreq_softc *sc;
 	const struct cf_setting *set;
-	int error;
+	int error, i;
 
 	sc = device_get_softc(dev);
 
@@ -187,7 +192,19 @@ cf_set_method(device_t dev, const struct cf_level *level, int priority)
 		}
 	}
 
-	/* TODO: Next, set any/all relative frequencies via their drivers. */
+	/* Next, set any/all relative frequencies via their drivers. */
+	for (i = 0; i < level->rel_count; i++) {
+		set = &level->rel_set[i];
+		if (!device_is_attached(set->dev)) {
+			error = ENXIO;
+			goto out;
+		}
+		error = CPUFREQ_DRV_SET(set->dev, set);
+		if (error) {
+			/* XXX Back out any successful setting? */
+			goto out;
+		}
+	}
 
 	/* Record the current level. */
 	sc->curr_level = *level;
@@ -241,7 +258,7 @@ cf_get_method(device_t dev, struct cf_level *level)
 		if (error)
 			continue;
 		for (i = 0; i < count; i++) {
-			if (CPUFREQ_CMP(set.freq, levels[i].abs_set.freq)) {
+			if (CPUFREQ_CMP(set.freq, levels[i].total_set.freq)) {
 				sc->curr_level = levels[i];
 				break;
 			}
@@ -279,13 +296,14 @@ out:
 static int
 cf_levels_method(device_t dev, struct cf_level *levels, int *count)
 {
+	struct cf_setting_array *set_arr;
 	struct cf_setting_lst rel_sets;
 	struct cpufreq_softc *sc;
 	struct cf_level *lev;
 	struct cf_setting *sets;
 	struct pcpu *pc;
 	device_t *devs;
-	int error, i, numdevs, numlevels, set_count, type;
+	int error, i, numdevs, set_count, type;
 	uint64_t rate;
 
 	if (levels == NULL || count == NULL)
@@ -303,7 +321,6 @@ cf_levels_method(device_t dev, struct cf_level *levels, int *count)
 	}
 
 	/* Get settings from all cpufreq drivers. */
-	numlevels = 0;
 	for (i = 0; i < numdevs; i++) {
 		if (!device_is_attached(devs[i]))
 			continue;
@@ -311,17 +328,27 @@ cf_levels_method(device_t dev, struct cf_level *levels, int *count)
 		error = CPUFREQ_DRV_SETTINGS(devs[i], sets, &set_count, &type);
 		if (error || set_count == 0)
 			continue;
-		error = cpufreq_insert_abs(&sc->all_levels, sets, set_count);
+
+		switch (type) {
+		case CPUFREQ_TYPE_ABSOLUTE:
+			error = cpufreq_insert_abs(sc, sets, set_count);
+			break;
+		case CPUFREQ_TYPE_RELATIVE:
+			set_arr = malloc(sizeof(*set_arr), M_TEMP, M_NOWAIT);
+			if (set_arr == NULL) {
+				error = ENOMEM;
+				goto out;
+			}
+			bcopy(sets, set_arr->sets, set_count * sizeof(*sets));
+			set_arr->count = set_count;
+			TAILQ_INSERT_TAIL(&rel_sets, set_arr, link);
+			break;
+		default:
+			error = EINVAL;
+			break;
+		}
 		if (error)
 			goto out;
-		numlevels += set_count;
-	}
-
-	/* If the caller doesn't have enough space, return the actual count. */
-	if (numlevels > *count) {
-		*count = numlevels;
-		error = E2BIG;
-		goto out;
 	}
 
 	/* If there are no absolute levels, create a fake one at 100%. */
@@ -334,21 +361,29 @@ cf_levels_method(device_t dev, struct cf_level *levels, int *count)
 		}
 		cpu_est_clockrate(pc->pc_cpuid, &rate);
 		sets[0].freq = rate / 1000000;
-		error = cpufreq_insert_abs(&sc->all_levels, sets, 1);
+		error = cpufreq_insert_abs(sc, sets, 1);
 		if (error)
 			goto out;
 	}
 
-	/* TODO: Create a combined list of absolute + relative levels. */
+	/* Create a combined list of absolute + relative levels. */
+	TAILQ_FOREACH(set_arr, &rel_sets, link)
+		cpufreq_expand_set(sc, set_arr);
+
+	/* If the caller doesn't have enough space, return the actual count. */
+	if (sc->all_count > *count) {
+		*count = sc->all_count;
+		error = E2BIG;
+		goto out;
+	}
+
+	/* Finally, output the list of levels. */
 	i = 0;
 	TAILQ_FOREACH(lev, &sc->all_levels, link) {
-		/* For now, just assume total freq equals absolute freq. */
-		lev->total_set = lev->abs_set;
-		lev->total_set.dev = NULL;
 		levels[i] = *lev;
 		i++;
 	}
-	*count = i;
+	*count = sc->all_count;
 	error = 0;
 
 out:
@@ -357,6 +392,11 @@ out:
 		TAILQ_REMOVE(&sc->all_levels, lev, link);
 		free(lev, M_TEMP);
 	}
+	while ((set_arr = TAILQ_FIRST(&rel_sets)) != NULL) {
+		TAILQ_REMOVE(&rel_sets, set_arr, link);
+		free(set_arr, M_TEMP);
+	}
+	sc->all_count = 0;
 	free(devs, M_TEMP);
 	free(sets, M_TEMP);
 	return (error);
@@ -367,17 +407,22 @@ out:
  * sorted order in the specified list.
  */
 static int
-cpufreq_insert_abs(struct cf_level_lst *list, struct cf_setting *sets,
+cpufreq_insert_abs(struct cpufreq_softc *sc, struct cf_setting *sets,
     int count)
 {
+	struct cf_level_lst *list;
 	struct cf_level *level, *search;
 	int i;
 
+	list = &sc->all_levels;
 	for (i = 0; i < count; i++) {
 		level = malloc(sizeof(*level), M_TEMP, M_NOWAIT | M_ZERO);
 		if (level == NULL)
 			return (ENOMEM);
 		level->abs_set = sets[i];
+		level->total_set = sets[i];
+		level->total_set.dev = NULL;
+		sc->all_count++;
 
 		if (TAILQ_EMPTY(list)) {
 			TAILQ_INSERT_HEAD(list, level, link);
@@ -385,7 +430,7 @@ cpufreq_insert_abs(struct cf_level_lst *list, struct cf_setting *sets,
 		}
 
 		TAILQ_FOREACH_REVERSE(search, list, cf_level_lst, link) {
-			if (sets[i].freq <= search->abs_set.freq) {
+			if (sets[i].freq <= search->total_set.freq) {
 				TAILQ_INSERT_AFTER(list, search, level, link);
 				break;
 			}
@@ -394,6 +439,129 @@ cpufreq_insert_abs(struct cf_level_lst *list, struct cf_setting *sets,
 	return (0);
 }
 
+/*
+ * Expand a group of relative settings, creating derived levels from them.
+ */
+static int
+cpufreq_expand_set(struct cpufreq_softc *sc, struct cf_setting_array *set_arr)
+{
+	struct cf_level *fill, *search;
+	struct cf_setting *set;
+	int i;
+
+	TAILQ_FOREACH(search, &sc->all_levels, link) {
+		/* Skip this level if we've already modified it. */
+		for (i = 0; i < search->rel_count; i++) {
+			if (search->rel_set[i].dev == set_arr->sets[0].dev)
+				break;
+		}
+		if (i != search->rel_count)
+			continue;
+
+		/* Add each setting to the level, duplicating if necessary. */
+		for (i = 0; i < set_arr->count; i++) {
+			set = &set_arr->sets[i];
+
+			/*
+			 * If this setting is less than 100%, split the level
+			 * into two and add this setting to the new level.
+			 */
+			fill = search;
+			if (set->freq < 10000)
+				fill = cpufreq_dup_set(sc, search, set);
+
+			/*
+			 * The new level was a duplicate of an existing level
+			 * so we freed it.  Go to the next setting.
+			 */
+			if (fill == NULL)
+				continue;
+
+			/* Add this setting to the existing or new level. */
+			KASSERT(fill->rel_count < MAX_SETTINGS,
+			    ("cpufreq: too many relative drivers (%d)",
+			    MAX_SETTINGS));
+			fill->rel_set[fill->rel_count] = *set;
+			fill->rel_count++;
+		}
+	}
+
+	return (0);
+}
+
+static struct cf_level *
+cpufreq_dup_set(struct cpufreq_softc *sc, struct cf_level *dup,
+    struct cf_setting *set)
+{
+	struct cf_level_lst *list;
+	struct cf_level *fill, *itr;
+	struct cf_setting *fill_set, *itr_set;
+	int i;
+
+	/*
+	 * Create a new level, copy it from the old one, and update the
+	 * total frequency and power by the percentage specified in the
+	 * relative setting.
+	 */
+	fill = malloc(sizeof(*fill), M_TEMP, M_NOWAIT);
+	if (fill == NULL)
+		return (NULL);
+	*fill = *dup;
+	fill_set = &fill->total_set;
+	fill_set->freq =
+	    ((uint64_t)fill_set->freq * set->freq) / 10000;
+	if (fill_set->power != CPUFREQ_VAL_UNKNOWN) {
+		fill_set->power = ((uint64_t)fill_set->power * set->freq)
+		    / 10000;
+	}
+	if (set->lat != CPUFREQ_VAL_UNKNOWN) {
+		if (fill_set->lat != CPUFREQ_VAL_UNKNOWN)
+			fill_set->lat += set->lat;
+		else
+			fill_set->lat = set->lat;
+	}
+
+	/*
+	 * If we copied an old level that we already modified (say, at 100%),
+	 * we need to remove that setting before adding this one.  Since we
+	 * process each setting array in order, we know any settings for this
+	 * driver will be found at the end.
+	 */
+	for (i = fill->rel_count; i != 0; i--) {
+		if (fill->rel_set[i - 1].dev != set->dev)
+			break;
+		fill->rel_count--;
+	}
+
+	/*
+	 * Insert the new level in sorted order.  If we find a duplicate,
+	 * free the new level.  We can do this since any existing level will
+	 * be guaranteed to have the same or less settings and thus consume
+	 * less power.  For example, a level with one absolute setting of
+	 * 800 Mhz uses less power than one composed of an absolute setting
+	 * of 1600 Mhz and a relative setting at 50%.
+	 */
+	list = &sc->all_levels;
+	if (TAILQ_EMPTY(list)) {
+		TAILQ_INSERT_HEAD(list, fill, link);
+	} else {
+		TAILQ_FOREACH_REVERSE(itr, list, cf_level_lst, link) {
+			itr_set = &itr->total_set;
+			if (CPUFREQ_CMP(fill_set->freq, itr_set->freq)) {
+				free(fill, M_TEMP);
+				fill = NULL;
+				break;
+			} else if (fill_set->freq < itr_set->freq) {
+				TAILQ_INSERT_AFTER(list, itr, fill, link);
+				sc->all_count++;
+				break;
+			}
+		}
+	}
+
+	return (fill);
+}
+
 static int
 cpufreq_curr_sysctl(SYSCTL_HANDLER_ARGS)
 {
@@ -482,11 +650,8 @@ cpufreq_register(device_t dev)
 	 * device per cpu.
 	 */
 	cf_dev = devclass_get_device(cpufreq_dc, 0);
-	if (cf_dev) {
-		device_printf(dev,
-		    "warning: only one cpufreq device at a time supported\n");
+	if (cf_dev)
 		return (0);
-	}
 
 	/* Add the child device and sysctls. */
 	cpu_dev = devclass_get_device(devclass_find("cpu"), 0);