From 95ac568b6137ee8d3a53d3ec911a7116c90e8d5d Mon Sep 17 00:00:00 2001
From: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>
Date: Tue, 6 Aug 2024 23:41:45 +0300
Subject: [PATCH] feat(nxp-drivers): add Linflex flush callback

Implement a flush callback for the Linflex UART driver to avoid cases
where the BL31 stage reinitializes the console while there is ongoing TX
initiated by the BL2.

Change-Id: Ic49852f809198362de1f993474c7c45f1439dc98
Signed-off-by: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>
---
 drivers/nxp/console/linflex_console.S    | 42 +++++++++++++++++++++++-
 plat/nxp/s32/s32g274ardb2/plat_helpers.S |  2 ++
 2 files changed, 43 insertions(+), 1 deletion(-)

diff --git a/drivers/nxp/console/linflex_console.S b/drivers/nxp/console/linflex_console.S
index abcbb594e..d8c10ef3f 100644
--- a/drivers/nxp/console/linflex_console.S
+++ b/drivers/nxp/console/linflex_console.S
@@ -18,6 +18,7 @@
 
 #define LINFLEX_LINSR		(0x8)
 #define LINSR_LINS_INITMODE	(0x00001000)
+#define LINSR_LINS_RX_TX_MODE	(0x00008000)
 #define LINSR_LINS_MASK		(0x0000F000)
 
 #define LINFLEX_UARTCR		(0x10)
@@ -48,9 +49,11 @@
  */
 .globl console_linflex_core_init
 .globl console_linflex_core_putc
+.globl console_linflex_core_flush
 
 .globl console_linflex_register
 .globl console_linflex_putc
+.globl console_linflex_flush
 
 /**
  * uint32_t get_ldiv_mult(uintptr_t baseaddr, uint32_t clock,
@@ -175,9 +178,28 @@ func console_linflex_register
 	str	x0, [x3, #CONSOLE_T_BASE]
 
 	mov	x0, x3
-	finish_console_register linflex, putc=1, getc=0, flush=0
+	finish_console_register linflex, putc=1, getc=0, flush=1
 endfunc console_linflex_register
 
+/**
+ * int console_linflex_core_flush(uintptr_t baseaddr);
+ *
+ * Loop while the TX fifo is not empty, depending on the selected UART mode.
+ *
+ * In:  x0 - Linflex base address
+ * Clobber list : x0 - x1
+ */
+func console_linflex_core_flush
+wait_rx_tx:
+	ldr	w1, [x0, LINFLEX_LINSR]
+	and	w1, w1, #LINSR_LINS_MASK
+	cmp	w1, #LINSR_LINS_RX_TX_MODE
+	b.eq	wait_rx_tx
+
+	mov	x0, #0
+	ret
+endfunc console_linflex_core_flush
+
 /**
  * int console_linflex_core_putc(int c, uintptr_t baseaddr);
 
@@ -257,3 +279,21 @@ puct_error:
 	mov	x0, #-EINVAL
 	ret
 endfunc console_linflex_putc
+
+/**
+ * int console_linflex_flush(console_t *console);
+ *
+ * Function to wait for the TX FIFO to be cleared.
+ * In : x0 - pointer to console_t struct
+ * Out: x0 - return -1 on error else return 0.
+ * Clobber list : x0 - x1
+ */
+func console_linflex_flush
+	cbz	x0, flush_error
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+
+	b	console_linflex_core_flush
+flush_error:
+	mov	x0, #-EINVAL
+	ret
+endfunc console_linflex_flush
diff --git a/plat/nxp/s32/s32g274ardb2/plat_helpers.S b/plat/nxp/s32/s32g274ardb2/plat_helpers.S
index 193c884da..10c00357d 100644
--- a/plat/nxp/s32/s32g274ardb2/plat_helpers.S
+++ b/plat/nxp/s32/s32g274ardb2/plat_helpers.S
@@ -38,6 +38,8 @@ endfunc plat_crash_console_putc
 
 /* void plat_crash_console_flush(void); */
 func plat_crash_console_flush
+	mov_imm	x0, UART_BASE
+	b	console_linflex_core_flush
 	ret
 endfunc plat_crash_console_flush