3 files changed, 60 insertions, 25 deletions

diff --git a/src/soc/amd/picasso/Kconfig b/src/soc/amd/picasso/Kconfig
index 29ebc6dd2b..3b12089bf9 100644
--- a/src/soc/amd/picasso/Kconfig
+++ b/src/soc/amd/picasso/Kconfig
@@ -308,12 +308,10 @@ endchoice
 
 config PICASSO_UART_LEGACY
 	bool "Decode legacy I/O range"
-	depends on PICASSO_CONSOLE_UART # TODO: shouldn't depend on this
 	help
-	  Assign I/O 3F8, 2F8, etc. to a Picasso UART.  Only a single UART may
-	  decode legacy addresses and this option enables the one used for the
-	  console.  A UART accessed with I/O does not allow all the features
-	  of MMIO.  The MMIO decode is still present when this option is used.
+	  Assign I/O 3F8, 2F8, etc. to a Picasso UART. A UART accessed with I/O
+	  does not allow all the features of MMIO. The MMIO decode is still
+	  present when this option is used.
 
 config CONSOLE_UART_BASE_ADDRESS
 	depends on CONSOLE_SERIAL && PICASSO_CONSOLE_UART
diff --git a/src/soc/amd/picasso/include/soc/southbridge.h b/src/soc/amd/picasso/include/soc/southbridge.h
index 547f602db7..222858ab51 100644
--- a/src/soc/amd/picasso/include/soc/southbridge.h
+++ b/src/soc/amd/picasso/include/soc/southbridge.h
@@ -221,10 +221,14 @@
 #define   FCH_AOAC_STAT0		BIT(6)
 #define   FCH_AOAC_STAT1		BIT(7)
 
-#define FCH_UART_LEGACY_DECODE		0xfedc0020
-#define   FCH_LEGACY_3F8_SH		3
-#define   FCH_LEGACY_2F8_SH		1
-#define   FCH_LEGACY_3E8_SH		2
+#define FCH_LEGACY_UART_DECODE		(ALINK_AHB_ADDRESS + 0x20) /* 0xfedc0020 */
+#define   FCH_LEGACY_UART_MAP_SHIFT	8
+#define   FCH_LEGACY_UART_MAP_SIZE	2
+#define   FCH_LEGACY_UART_MAP_MASK	0x3
+#define   FCH_LEGACY_UART_RANGE_2E8	0
+#define   FCH_LEGACY_UART_RANGE_2F8	1
+#define   FCH_LEGACY_UART_RANGE_3E8	2
+#define   FCH_LEGACY_UART_RANGE_3F8	3
 
 #define PM1_LIMIT			16
 #define GPE0_LIMIT			28
@@ -279,6 +283,7 @@ void southbridge_final(void *chip_info);
 void southbridge_init(void *chip_info);
 void fch_pre_init(void);
 void fch_early_init(void);
+void set_uart_legacy_config(unsigned int uart_idx, unsigned int range_idx);
 
 /* Initialize all the i2c buses that are marked with early init. */
 void i2c_soc_early_init(void);
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);
 	}