From 5bb53cd3a5cb3f7f229b37966fc19ce390a83d38 Mon Sep 17 00:00:00 2001
From: =?utf8?q?Duje=20Mihanovi=C4=87?= <duje.mihanovic@skole.hr>
Date: Tue, 24 Dec 2024 18:41:46 +0100
Subject: [PATCH] restore upstream ns16550

---
 drivers/serial/ns16550.c | 60 ++++++++++++++++++++++++++++++++++------
 1 file changed, 52 insertions(+), 8 deletions(-)

diff --git a/drivers/serial/ns16550.c b/drivers/serial/ns16550.c
index 860b695e31..3f6860f391 100644
--- a/drivers/serial/ns16550.c
+++ b/drivers/serial/ns16550.c
@@ -108,7 +108,9 @@ static inline int serial_in_shift(void *addr, int shift)
 static void serial_out_dynamic(struct ns16550_plat *plat, u8 *addr,
 			       int value)
 {
-	if (plat->reg_width == 4) {
+	if (plat->flags & NS16550_FLAG_IO) {
+		outb(value, addr);
+	} else if (plat->reg_width == 4) {
 		if (plat->flags & NS16550_FLAG_ENDIAN) {
 			if (plat->flags & NS16550_FLAG_BE)
 				out_be32(addr, value);
@@ -126,7 +128,9 @@ static void serial_out_dynamic(struct ns16550_plat *plat, u8 *addr,
 
 static int serial_in_dynamic(struct ns16550_plat *plat, u8 *addr)
 {
-	if (plat->reg_width == 4) {
+	if (plat->flags & NS16550_FLAG_IO) {
+		return inb(addr);
+	} else if (plat->reg_width == 4) {
 		if (plat->flags & NS16550_FLAG_ENDIAN) {
 			if (plat->flags & NS16550_FLAG_BE)
 				return in_be32(addr);
@@ -223,6 +227,49 @@ static void ns16550_setbrg(struct ns16550 *com_port, int baud_divisor)
 
 void ns16550_init(struct ns16550 *com_port, int baud_divisor)
 {
+#if defined(CONFIG_XPL_BUILD) && defined(CONFIG_OMAP34XX)
+	/*
+	 * On some OMAP3/OMAP4 devices when UART3 is configured for boot mode
+	 * before SPL starts only THRE bit is set. We have to empty the
+	 * transmitter before initialization starts.
+	 */
+	if ((serial_in(&com_port->lsr) & (UART_LSR_TEMT | UART_LSR_THRE))
+	     == UART_LSR_THRE) {
+		if (baud_divisor != -1)
+			ns16550_setbrg(com_port, baud_divisor);
+		else {
+			// Re-use old baud rate divisor to flush transmit reg.
+			const int dll = serial_in(&com_port->dll);
+			const int dlm = serial_in(&com_port->dlm);
+			const int divisor = dll | (dlm << 8);
+			ns16550_setbrg(com_port, divisor);
+		}
+		serial_out(0, &com_port->mdr1);
+	}
+#endif
+
+	while (!(serial_in(&com_port->lsr) & UART_LSR_TEMT))
+		;
+
+	serial_out(CFG_SYS_NS16550_IER, &com_port->ier);
+#if defined(CONFIG_ARCH_OMAP2PLUS) || defined(CONFIG_OMAP_SERIAL)
+	serial_out(0x7, &com_port->mdr1);	/* mode select reset TL16C750*/
+#endif
+
+	serial_out(UART_MCRVAL, &com_port->mcr);
+	serial_out(ns16550_getfcr(com_port), &com_port->fcr);
+	/* initialize serial config to 8N1 before writing baudrate */
+	serial_out(UART_LCRVAL, &com_port->lcr);
+	if (baud_divisor != -1)
+		ns16550_setbrg(com_port, baud_divisor);
+#if defined(CONFIG_ARCH_OMAP2PLUS) || defined(CONFIG_SOC_DA8XX) || \
+	defined(CONFIG_OMAP_SERIAL)
+	/* /16 is proper to hit 115200 with 48MHz */
+	serial_out(0, &com_port->mdr1);
+#endif
+#if defined(CONFIG_ARCH_KEYSTONE)
+	serial_out(UART_REG_VAL_PWREMU_MGMT_UART_ENABLE, &com_port->regC);
+#endif
 }
 
 #if !CONFIG_IS_ENABLED(NS16550_MIN_FUNCTIONS)
@@ -278,21 +325,19 @@ int ns16550_tstc(struct ns16550 *com_port)
 
 static inline void _debug_uart_init(void)
 {
-	//struct ns16550 *com_port = (struct ns16550 *)CONFIG_VAL(DEBUG_UART_BASE);
-	//int baud_divisor;
+	struct ns16550 *com_port = (struct ns16550 *)CONFIG_VAL(DEBUG_UART_BASE);
+	int baud_divisor;
 
 	/* Wait until tx buffer is empty */
-	/*
 	while (!(serial_din(&com_port->lsr) & UART_LSR_TEMT))
 		;
-	*/
+
 	/*
 	 * We copy the code from above because it is already horribly messy.
 	 * Trying to refactor to nicely remove the duplication doesn't seem
 	 * feasible. The better fix is to move all users of this driver to
 	 * driver model.
 	 */
-	/*
 	baud_divisor = ns16550_calc_divisor(com_port, CONFIG_DEBUG_UART_CLOCK,
 					    CONFIG_BAUDRATE);
 	serial_dout(&com_port->ier, CFG_SYS_NS16550_IER);
@@ -303,7 +348,6 @@ static inline void _debug_uart_init(void)
 	serial_dout(&com_port->dll, baud_divisor & 0xff);
 	serial_dout(&com_port->dlm, (baud_divisor >> 8) & 0xff);
 	serial_dout(&com_port->lcr, UART_LCRVAL);
-	*/
 }
 
 static inline int NS16550_read_baud_divisor(struct ns16550 *com_port)
-- 
2.39.5