From a6ebcddc546e6e654446188bd45f0097288dd67d Mon Sep 17 00:00:00 2001
From: Yong Cong Sin <yongcong.sin@gmail.com>
Date: Fri, 22 Oct 2021 00:02:29 +0800
Subject: [PATCH] driver: serial: uart_stm32: Calculate suitable PRESCALER
 value

Current driver set a fixed prescaler value for the lpuart
that caused certain baudrate configurations to fail due to
LPUARTDIV overflow the LPUART_BRR register.

This PR attempt to calculate a suitable PRESCALER for the
selected baudrate, throws error and return if it couldn't get
an optimal one.

Signed-off-by: Yong Cong Sin <yongcong.sin@gmail.com>
---
 drivers/serial/uart_stm32.c | 58 +++++++++++++++++++++++++++++++++++--
 1 file changed, 56 insertions(+), 2 deletions(-)

diff --git a/drivers/serial/uart_stm32.c b/drivers/serial/uart_stm32.c
index 0dfbad9f8cf..67ca8257d85 100644
--- a/drivers/serial/uart_stm32.c
+++ b/drivers/serial/uart_stm32.c
@@ -51,6 +51,34 @@ LOG_MODULE_REGISTER(uart_stm32);
 #define UART_STRUCT(dev)					\
 	((USART_TypeDef *)(DEV_CFG(dev))->uconf.base)
 
+#if HAS_LPUART_1
+#ifdef USART_PRESC_PRESCALER
+uint32_t lpuartdiv_calc(const uint64_t clock_rate, const uint16_t presc_idx,
+			const uint32_t baud_rate)
+{
+	uint64_t lpuartdiv;
+
+	lpuartdiv = clock_rate / LPUART_PRESCALER_TAB[presc_idx];
+	lpuartdiv *= LPUART_LPUARTDIV_FREQ_MUL;
+	lpuartdiv += baud_rate / 2;
+	lpuartdiv /= baud_rate;
+
+	return (uint32_t)lpuartdiv;
+}
+#else
+uint32_t lpuartdiv_calc(const uint64_t clock_rate, const uint32_t baud_rate)
+{
+	uint64_t lpuartdiv;
+
+	lpuartdiv = clock_rate * LPUART_LPUARTDIV_FREQ_MUL;
+	lpuartdiv += baud_rate / 2;
+	lpuartdiv /= baud_rate;
+
+	return (uint32_t)lpuartdiv;
+}
+#endif /* USART_PRESC_PRESCALER */
+#endif /* HAS_LPUART_1 */
+
 #define TIMEOUT 1000
 
 #ifdef CONFIG_PM
@@ -92,13 +120,39 @@ static inline void uart_stm32_set_baudrate(const struct device *dev,
 		return;
 	}
 
-
 #if HAS_LPUART_1
 	if (IS_LPUART_INSTANCE(UartInstance)) {
+		uint32_t lpuartdiv;
+#ifdef USART_PRESC_PRESCALER
+		uint8_t presc_idx;
+		uint32_t presc_val;
+
+		for (presc_idx = 0; presc_idx < ARRAY_SIZE(LPUART_PRESCALER_TAB); presc_idx++) {
+			lpuartdiv = lpuartdiv_calc(clock_rate, presc_idx, baud_rate);
+			if (lpuartdiv >= LPUART_BRR_MIN_VALUE && lpuartdiv <= LPUART_BRR_MASK) {
+				break;
+			}
+		}
+
+		if (presc_idx == ARRAY_SIZE(LPUART_PRESCALER_TAB)) {
+			LOG_ERR("Unable to set %s to %d", dev->name, baud_rate);
+			return;
+		}
+
+		presc_val = presc_idx << USART_PRESC_PRESCALER_Pos;
+
+		LL_LPUART_SetPrescaler(UartInstance, presc_val);
+#else
+		lpuartdiv = lpuartdiv_calc(clock_rate, baud_rate);
+		if (lpuartdiv < LPUART_BRR_MIN_VALUE || lpuartdiv > LPUART_BRR_MASK) {
+			LOG_ERR("Unable to set %s to %d", dev->name, baud_rate);
+			return;
+		}
+#endif /* USART_PRESC_PRESCALER */
 		LL_LPUART_SetBaudRate(UartInstance,
 				      clock_rate,
 #ifdef USART_PRESC_PRESCALER
-				      LL_USART_PRESCALER_DIV1,
+				      presc_val,
 #endif
 				      baud_rate);
 	} else {