1 files changed, 19 insertions, 3 deletions

diff --git a/drivers/tty/serial/sccnxp.c b/drivers/tty/serial/sccnxp.c
index c0a68eee4479..68a24a14f6b7 100644
--- a/drivers/tty/serial/sccnxp.c
+++ b/drivers/tty/serial/sccnxp.c
@@ -48,7 +48,6 @@
 #       define MR2_STOP1                (7 << 0)
 #       define MR2_STOP2                (0xf << 0)
 #define SCCNXP_SR_REG                   (0x01)
-#define SCCNXP_CSR_REG                  SCCNXP_SR_REG
 #       define SR_RXRDY                 (1 << 0)
 #       define SR_FULL                  (1 << 1)
 #       define SR_TXRDY                 (1 << 2)
@@ -57,6 +56,8 @@
 #       define SR_PE                    (1 << 5)
 #       define SR_FE                    (1 << 6)
 #       define SR_BRK                   (1 << 7)
+#define SCCNXP_CSR_REG                  (SCCNXP_SR_REG)
+#       define CSR_TIMER_MODE           (0x0d)
 #define SCCNXP_CR_REG                   (0x02)
 #       define CR_RX_ENABLE             (1 << 0)
 #       define CR_RX_DISABLE            (1 << 1)
@@ -83,9 +84,12 @@
 #       define IMR_RXRDY                (1 << 1)
 #       define ISR_TXRDY(x)             (1 << ((x * 4) + 0))
 #       define ISR_RXRDY(x)             (1 << ((x * 4) + 1))
+#define SCCNXP_CTPU_REG                 (0x06)
+#define SCCNXP_CTPL_REG                 (0x07)
 #define SCCNXP_IPR_REG                  (0x0d)
 #define SCCNXP_OPCR_REG                 SCCNXP_IPR_REG
 #define SCCNXP_SOP_REG                  (0x0e)
+#define SCCNXP_START_COUNTER_REG        SCCNXP_SOP_REG
 #define SCCNXP_ROP_REG                  (0x0f)
 
 /* Route helpers */
@@ -255,7 +259,7 @@ static int sccnxp_update_best_err(int a, int b, int *besterr)
 {
         int err = abs(a - b);
 
-        if ((*besterr < 0) || (*besterr > err)) {
+        if (*besterr > err) {
                 *besterr = err;
                 return 0;
         }
@@ -303,10 +307,22 @@ static const struct {
 static int sccnxp_set_baud(struct uart_port *port, int baud)
 {
         struct sccnxp_port *s = dev_get_drvdata(port->dev);
-        int div_std, tmp_baud, bestbaud = baud, besterr = -1;
+        int div_std, tmp_baud, bestbaud = INT_MAX, besterr = INT_MAX;
         struct sccnxp_chip *chip = s->chip;
         u8 i, acr = 0, csr = 0, mr0 = 0;
 
+        /* Find divisor to load to the timer preset registers */
+        div_std = DIV_ROUND_CLOSEST(port->uartclk, 2 * 16 * baud);
+        if ((div_std >= 2) && (div_std <= 0xffff)) {
+                bestbaud = DIV_ROUND_CLOSEST(port->uartclk, 2 * 16 * div_std);
+                sccnxp_update_best_err(baud, bestbaud, &besterr);
+                csr = CSR_TIMER_MODE;
+                sccnxp_port_write(port, SCCNXP_CTPU_REG, div_std >> 8);
+                sccnxp_port_write(port, SCCNXP_CTPL_REG, div_std);
+                /* Issue start timer/counter command */
+                sccnxp_port_read(port, SCCNXP_START_COUNTER_REG);
+        }
+
         /* Find best baud from table */
         for (i = 0; baud_std[i].baud && besterr; i++) {
                 if (baud_std[i].mr0 && !(chip->flags & SCCNXP_HAVE_MR0))