1 files changed, 48 insertions, 16 deletions

diff --git a/src/soc/amd/picasso/uart.c b/src/soc/amd/picasso/uart.c
index 2ef1979233..1aa42ef47b 100644
--- a/src/soc/amd/picasso/uart.c
+++ b/src/soc/amd/picasso/uart.c
@@ -41,15 +41,59 @@ uintptr_t get_uart_base(unsigned int idx)
 	return uart_info[idx].base;
 }
 
+static bool get_uart_idx(uintptr_t base, unsigned int *idx)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(uart_info); i++) {
+		if (base == uart_info[i].base) {
+			*idx = i;
+			return true;
+		}
+	}
+
+	return false;
+}
+
 void clear_uart_legacy_config(void)
 {
-	write16((void *)FCH_UART_LEGACY_DECODE, 0);
+	write16((void *)FCH_LEGACY_UART_DECODE, 0);
+}
+
+void set_uart_legacy_config(unsigned int uart_idx, unsigned int range_idx)
+{
+	uint16_t uart_legacy_decode;
+	uint8_t uart_map_offset;
+
+	if (uart_idx >= ARRAY_SIZE(uart_info) || range_idx >= ARRAY_SIZE(uart_info))
+		return;
+
+	uart_legacy_decode = read16((void *)FCH_LEGACY_UART_DECODE);
+	/* Map uart_idx to io range_idx */
+	uart_map_offset = range_idx * FCH_LEGACY_UART_MAP_SIZE + FCH_LEGACY_UART_MAP_SHIFT;
+	uart_legacy_decode &= ~(FCH_LEGACY_UART_MAP_MASK << uart_map_offset);
+	uart_legacy_decode |= uart_idx << uart_map_offset;
+	/* Enable io range */
+	uart_legacy_decode |= 1 << range_idx;
+	write16((void *)FCH_LEGACY_UART_DECODE, uart_legacy_decode);
+}
+
+static void enable_uart_legacy_decode(uintptr_t base)
+{
+	unsigned int idx;
+	const uint8_t range_idx[ARRAY_SIZE(uart_info)] = {
+		FCH_LEGACY_UART_RANGE_3F8,
+		FCH_LEGACY_UART_RANGE_2F8,
+		FCH_LEGACY_UART_RANGE_3E8,
+		FCH_LEGACY_UART_RANGE_2E8,
+	};
+
+	if (get_uart_idx(base, &idx)) {
+		set_uart_legacy_config(idx, range_idx[idx]);
+	}
 }
 
 void set_uart_config(unsigned int idx)
 {
 	uint32_t uart_ctrl;
-	uint16_t uart_leg;
 
 	if (idx >= ARRAY_SIZE(uart_info))
 		return;
@@ -62,20 +106,6 @@ void set_uart_config(unsigned int idx)
 		sm_pci_write32(SMB_UART_CONFIG, uart_ctrl);
 	}
 
-	if (CONFIG(PICASSO_UART_LEGACY) && idx != 3) {
-		/* Force 3F8 if idx=0, 2F8 if idx=1, 3E8 if idx=2 */
-
-		/* TODO: make clearer once PPR is updated */
-		uart_leg = (idx << 8) | (idx << 10) | (idx << 12) | (idx << 14);
-		if (idx == 0)
-			uart_leg |= 1 << FCH_LEGACY_3F8_SH;
-		else if (idx == 1)
-			uart_leg |= 1 << FCH_LEGACY_2F8_SH;
-		else if (idx == 2)
-			uart_leg |= 1 << FCH_LEGACY_3E8_SH;
-
-		write16((void *)FCH_UART_LEGACY_DECODE, uart_leg);
-	}
 }
 
 static const char *uart_acpi_name(const struct device *dev)
@@ -120,6 +150,8 @@ static void uart_enable(struct device *dev)
 	if (dev->enabled) {
 		power_on_aoac_device(dev_id);
 		wait_for_aoac_enabled(dev_id);
+		if (CONFIG(PICASSO_UART_LEGACY))
+			enable_uart_legacy_decode(dev->path.mmio.addr);
 	} else {
 		power_off_aoac_device(dev_id);
 	}