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);