cpu/intel/model_206ax: Only use supported C-states

When advertising C-state using the ACPI _CST object, make sure
to only advertise those that are supported by the CPU.
Downgrade if it's not and make sure to not advertise duplicate
states.

Add debug prints for the finally selected mapping of ACPI
C-state vs Intel CPU C-state.

Test: Tested on Lenovo X220.
      All C-states are still advertised as all are supported.

Change-Id: Iaaee050e0ce3c29c12e97f5819a29f485a7946c2
Signed-off-by: Patrick Rudolph <patrick.rudolph@9elements.com>
Reviewed-on: https://review.coreboot.org/c/coreboot/+/78194
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Kyösti Mälkki <kyosti.malkki@gmail.com>
diff --git a/src/cpu/intel/model_206ax/acpi.c b/src/cpu/intel/model_206ax/acpi.c
index c4f5839..f3bd67b 100644
--- a/src/cpu/intel/model_206ax/acpi.c
+++ b/src/cpu/intel/model_206ax/acpi.c
@@ -87,23 +87,77 @@
 	printk(BIOS_DEBUG, "\n");
 }
 
+/*
+ * Returns the supported C-state or the next lower one that
+ * is supported.
+ */
+static int get_supported_cstate(int cstate)
+{
+	uint8_t state, substate;
+	size_t i;
+
+	assert(cstate < NUM_C_STATES);
+
+	for (i = cstate; i > 0; i--) {
+		state = (cstate_map[i].resource.addrl >> 4) + 1;
+		substate = cstate_map[i].resource.addrl & 0xf;
+		if (cpu_get_c_substate_support(state) > substate)
+			break;
+	}
+
+	if (cstate != i)
+		printk(BIOS_INFO, "Requested C-state %s not supported, using %s instead\n",
+		       c_state_names[cstate], c_state_names[i]);
+
+	return i;
+}
+
 static void generate_C_state_entries(const struct device *dev)
 {
 	struct cpu_intel_model_206ax_config *conf = dev->chip_info;
 
-	const int acpi_cstates[3] = { conf->acpi_c1, conf->acpi_c2, conf->acpi_c3 };
+	int acpi_cstates[3] = { conf->acpi_c1, conf->acpi_c2, conf->acpi_c3 };
 
 	acpi_cstate_t acpi_cstate_map[ARRAY_SIZE(acpi_cstates)] = { 0 };
 	/* Count number of active C-states */
 	int count = 0;
 
 	for (int i = 0; i < ARRAY_SIZE(acpi_cstates); i++) {
-		if (acpi_cstates[i] > 0 && acpi_cstates[i] < ARRAY_SIZE(cstate_map)) {
-			acpi_cstate_map[count] = cstate_map[acpi_cstates[i]];
-			acpi_cstate_map[count].ctype = i + 1;
-			count++;
+		/* Remove invalid states */
+		if (acpi_cstates[i] >= ARRAY_SIZE(cstate_map)) {
+			printk(BIOS_ERR, "Invalid C-state in devicetree: %d\n",
+			       acpi_cstates[i]);
+			acpi_cstates[i] = 0;
+			continue;
+		}
+		/* Skip C0, it's always supported */
+		if (acpi_cstates[i] == 0)
+			continue;
+
+		/* Find supported state. Might downgrade a state. */
+		acpi_cstates[i] = get_supported_cstate(acpi_cstates[i]);
+
+		/* Remove duplicate states */
+		for (int j = i - 1; j >= 0; j--) {
+			if (acpi_cstates[i] == acpi_cstates[j]) {
+				acpi_cstates[i] = 0;
+				break;
+			}
 		}
 	}
+
+	/* Convert C-state to ACPI C-states */
+	for (int i = 0; i < ARRAY_SIZE(acpi_cstates); i++) {
+		if (acpi_cstates[i] == 0)
+			continue;
+		acpi_cstate_map[count] = cstate_map[acpi_cstates[i]];
+		acpi_cstate_map[count].ctype = i + 1;
+
+		count++;
+		printk(BIOS_DEBUG, "Advertising ACPI C State type C%d as CPU %s\n",
+		       i + 1, c_state_names[acpi_cstates[i]]);
+	}
+
 	acpigen_write_CST_package(acpi_cstate_map, count);
 }