Validate the id16 values gathered from ACPI (previously a TODO item).

Style changes by me and njl.

Approved by:  	 njl (mentor)
Reviewed by:	 njl (mentor)
Submitted by: 	 Takeharu KATO <takeharu1219 at ybb.ne.jp>
PR:	  	 119350
MFC after:	 1 week

1 files changed, 59 insertions, 17 deletions

diff --git a/sys/i386/cpufreq/est.c b/sys/i386/cpufreq/est.c
index 32ee7bb403d8..885f06a912dd 100644
--- a/sys/i386/cpufreq/est.c
+++ b/sys/i386/cpufreq/est.c
@@ -902,6 +902,8 @@ static int	est_settings(device_t dev, struct cf_setting *sets, int *count);
 static int	est_set(device_t dev, const struct cf_setting *set);
 static int	est_get(device_t dev, struct cf_setting *set);
 static int	est_type(device_t dev, int *type);
+static int	est_set_id16(device_t dev, uint16_t id16, int need_check);
+static void	est_get_id16(uint16_t *id16_p);
 
 static device_method_t est_methods[] = {
 	/* Device interface */
@@ -1076,7 +1078,8 @@ est_acpi_info(device_t dev, freq_info **freqs)
 	struct cf_setting *sets;
 	freq_info *table;
 	device_t perf_dev;
-	int count, error, i;
+	int count, error, i, j;
+	uint16_t saved_id16;
 
 	perf_dev = device_find_child(device_get_parent(dev), "acpi_perf", -1);
 	if (perf_dev == NULL || !device_is_attached(perf_dev))
@@ -1098,19 +1101,31 @@ est_acpi_info(device_t dev, freq_info **freqs)
 		error = ENOMEM;
 		goto out;
 	}
-	for (i = 0; i < count; i++) {
+	for (i = 0, j = 0; i < count; i++) {
 		/*
-		 * TODO: Figure out validity checks for id16.  Linux checks
-		 * that the control and status values match.
+		 * Confirm id16 value is correct.
 		 */
-		table[i].freq = sets[i].freq;
-		table[i].volts = sets[i].volts;
-		table[i].id16 = sets[i].spec[0];
-		table[i].power = sets[i].power;
+		if (sets[i].freq > 0) {
+			est_get_id16(&saved_id16);
+			error = est_set_id16(dev, sets[i].spec[0], 1);
+			if (error != 0) {
+				if (bootverbose) 
+					device_printf(dev, "Invalid freq %u, "
+					    "ignored.\n", sets[i].freq);
+			} else {
+				table[j].freq = sets[i].freq;
+				table[j].volts = sets[i].volts;
+				table[j].id16 = sets[i].spec[0];
+				table[j].power = sets[i].power;
+				++j;
+			}
+			/* restore saved setting */
+			est_set_id16(dev, sets[i].spec[0], 0);
+		}
 	}
 
 	/* Mark end of table with a terminator. */
-	bzero(&table[i], sizeof(freq_info));
+	bzero(&table[j], sizeof(freq_info));
 
 	sc->acpi_settings = TRUE;
 	*freqs = table;
@@ -1149,6 +1164,39 @@ est_table_info(device_t dev, uint64_t msr, freq_info **freqs)
 	return (0);
 }
 
+static void
+est_get_id16(uint16_t *id16_p)
+{
+	*id16_p = rdmsr(MSR_PERF_STATUS) & 0xffff;
+}
+
+static int
+est_set_id16(device_t dev, uint16_t id16, int need_check)
+{
+	uint64_t msr;
+	uint16_t new_id16;
+	int ret = 0;
+
+	/* Read the current register, mask out the old, set the new id. */
+	msr = rdmsr(MSR_PERF_CTL);
+	msr = (msr & ~0xffff) | id16;
+	wrmsr(MSR_PERF_CTL, msr);
+	
+	/* Wait a short while for the new setting.  XXX Is this necessary? */
+	DELAY(EST_TRANS_LAT);
+	
+	if  (need_check) {
+		est_get_id16(&new_id16);		
+		if (new_id16 != id16) {
+			if (bootverbose) 
+				device_printf(dev, "Invalid id16 (set, cur) "
+				    "= (%u, %u)\n", id16, new_id16);
+			ret = ENXIO;
+		}
+	}
+	return (ret);
+}
+
 static freq_info *
 est_get_current(freq_info *freq_list)
 {
@@ -1162,7 +1210,7 @@ est_get_current(freq_info *freq_list)
 	 * we get a temporary invalid result.
 	 */
 	for (i = 0; i < 5; i++) {
-		id16 = rdmsr(MSR_PERF_STATUS) & 0xffff;
+		est_get_id16(&id16);
 		for (f = freq_list; f->id16 != 0; f++) {
 			if (f->id16 == id16)
 				return (f);
@@ -1201,7 +1249,6 @@ est_set(device_t dev, const struct cf_setting *set)
 {
 	struct est_softc *sc;
 	freq_info *f;
-	uint64_t msr;
 
 	/* Find the setting matching the requested one. */
 	sc = device_get_softc(dev);
@@ -1213,12 +1260,7 @@ est_set(device_t dev, const struct cf_setting *set)
 		return (EINVAL);
 
 	/* Read the current register, mask out the old, set the new id. */
-	msr = rdmsr(MSR_PERF_CTL);
-	msr = (msr & ~0xffff) | f->id16;
-	wrmsr(MSR_PERF_CTL, msr);
-
-	/* Wait a short while for the new setting.  XXX Is this necessary? */
-	DELAY(EST_TRANS_LAT);
+	est_set_id16(dev, f->id16, 0);
 
 	return (0);
 }