In FIFO mode, wait until UARTCR.TDFL_TFC(number Tx FIFO) entries reach 0
before entering INITM mode.
Failing to do so may lead to undefined behavior, such as:
- corrupted characters being printed.
- the device is not able to receive or transmit any character.

In linflex_set_termios, transmission and reception should be disabled
before entering INITM mode, as already done in linflex_setup_watermark.

This patch corrects the behavior that was previously addressed by the
earlycon workaround, making that workaround no longer necessary. The
next patch will remove it.

Fixes: 09864c1cdf5c ("tty: serial: Add linflexuart driver for S32V234")
Signed-off-by: Larisa Grigore <larisa.grigore@oss.nxp.com>
---
 drivers/tty/serial/fsl_linflexuart.c | 45 ++++++++++++++++++++++++++--
 1 file changed, 42 insertions(+), 3 deletions(-)

diff --git a/drivers/tty/serial/fsl_linflexuart.c b/drivers/tty/serial/fsl_linflexuart.c
index 768b3c67a614..c1d069dc8089 100644
--- a/drivers/tty/serial/fsl_linflexuart.c
+++ b/drivers/tty/serial/fsl_linflexuart.c
@@ -3,7 +3,7 @@
  * Freescale LINFlexD UART serial port driver
  *
  * Copyright 2012-2016 Freescale Semiconductor, Inc.
- * Copyright 2017-2019 NXP
+ * Copyright 2017-2019, 2021 NXP
  */
 
 #include <linux/console.h>
@@ -74,6 +74,17 @@
 
 #define LINFLEXD_UARTCR_ROSE		BIT(23)
 
+#define LINFLEXD_UARTCR_RDFLRFC_OFFSET	10
+#define LINFLEXD_UARTCR_RDFLRFC_MASK	(0x7 << LINFLEXD_UARTCR_RDFLRFC_OFFSET)
+#define LINFLEXD_UARTCR_RDFLRFC(uartcr)	(((uartcr) \
+					& LINFLEXD_UARTCR_RDFLRFC_MASK) >> \
+					LINFLEXD_UARTCR_RDFLRFC_OFFSET)
+#define LINFLEXD_UARTCR_TDFLTFC_OFFSET	13
+#define LINFLEXD_UARTCR_TDFLTFC_MASK	(0x7 << LINFLEXD_UARTCR_TDFLTFC_OFFSET)
+#define LINFLEXD_UARTCR_TDFLTFC(uartcr)	(((uartcr) \
+					& LINFLEXD_UARTCR_TDFLTFC_MASK) >> \
+					LINFLEXD_UARTCR_TDFLTFC_OFFSET)
+
 #define LINFLEXD_UARTCR_RFBM		BIT(9)
 #define LINFLEXD_UARTCR_TFBM		BIT(8)
 #define LINFLEXD_UARTCR_WL1		BIT(7)
@@ -140,6 +151,17 @@ static struct {
 } earlycon_buf;
 #endif
 
+static inline void linflex_wait_tx_fifo_empty(struct uart_port *port)
+{
+	unsigned long cr = readl(port->membase + UARTCR);
+
+	if (!(cr & LINFLEXD_UARTCR_TFBM))
+		return;
+
+	while (LINFLEXD_UARTCR_TDFLTFC(readl(port->membase + UARTCR)))
+		;
+}
+
 static void linflex_stop_tx(struct uart_port *port)
 {
 	unsigned long ier;
@@ -326,6 +348,11 @@ static void linflex_setup_watermark(struct uart_port *sport)
 	cr &= ~(LINFLEXD_UARTCR_RXEN | LINFLEXD_UARTCR_TXEN);
 	writel(cr, sport->membase + UARTCR);
 
+	/* In FIFO mode, we should make sure the fifo is empty
+	 * before entering INITM.
+	 */
+	linflex_wait_tx_fifo_empty(sport);
+
 	/* Enter initialization mode by setting INIT bit */
 
 	/* set the Linflex in master mode and activate by-pass filter */
@@ -412,8 +439,17 @@ linflex_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	uart_port_lock_irqsave(port, &flags);
 
-	cr = readl(port->membase + UARTCR);
-	old_cr = cr;
+	old_cr = readl(port->membase + UARTCR) &
+		~(LINFLEXD_UARTCR_RXEN | LINFLEXD_UARTCR_TXEN);
+	cr = old_cr;
+
+	/* In FIFO mode, we should make sure the fifo is empty
+	 * before entering INITM.
+	 */
+	linflex_wait_tx_fifo_empty(port);
+
+	/* disable transmit and receive */
+	writel(old_cr, port->membase + UARTCR);
 
 	/* Enter initialization mode by setting INIT bit */
 	cr1 = LINFLEXD_LINCR1_INIT | LINFLEXD_LINCR1_MME;
@@ -510,6 +546,9 @@ linflex_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	writel(cr1, port->membase + LINCR1);
 
+	cr |= (LINFLEXD_UARTCR_TXEN) | (LINFLEXD_UARTCR_RXEN);
+	writel(cr, port->membase + UARTCR);
+
 	uart_port_unlock_irqrestore(port, flags);
 }
 
-- 
2.47.0