tty: serial: qcom-geni-serial: Fix get_clk_div_rate() which otherwise could return...

In the logic around call to clk_round_rate(), for some corner conditions,
get_clk_div_rate() could return an sub-optimal clock rate. Also, if an
exact clock rate was not found lowest clock was being returned.

Search for suitable clock rate in 2 steps
a) exact match or within 2% tolerance
b) within 5% tolerance
This also takes care of corner conditions.

Fixes: c2194bc999d4 ("tty: serial: qcom-geni-serial: Remove uart frequency table. Instead, find suitable frequency with call to clk_round_rate")
Reviewed-by: Douglas Anderson <dianders@chromium.org>
Signed-off-by: Vijaya Krishna Nivarthi <quic_vnivarth@quicinc.com>
Link: https://lore.kernel.org/r/1657911343-1909-1-git-send-email-quic_vnivarth@quicinc.com
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/tty/serial/qcom_geni_serial.c b/drivers/tty/serial/qcom_geni_serial.c
index f8f9506..f754619 100644 (file)
--- a/drivers/tty/serial/qcom_geni_serial.c
+++ b/drivers/tty/serial/qcom_geni_serial.c
@@ -940,52 +940,63 @@ static int qcom_geni_serial_startup(struct uart_port *uport)
        return 0;
 }
 
-static unsigned long get_clk_div_rate(struct clk *clk, unsigned int baud,
-                       unsigned int sampling_rate, unsigned int *clk_div)
+static unsigned long find_clk_rate_in_tol(struct clk *clk, unsigned int desired_clk,
+                       unsigned int *clk_div, unsigned int percent_tol)
 {
-       unsigned long ser_clk;
-       unsigned long desired_clk;
-       unsigned long freq, prev;
+       unsigned long freq;
        unsigned long div, maxdiv;
-       int64_t mult;
-
-       desired_clk = baud * sampling_rate;
-       if (!desired_clk) {
-               pr_err("%s: Invalid frequency\n", __func__);
-               return 0;
-       }
+       u64 mult;
+       unsigned long offset, abs_tol, achieved;
 
+       abs_tol = div_u64((u64)desired_clk * percent_tol, 100);
        maxdiv = CLK_DIV_MSK >> CLK_DIV_SHFT;
-       prev = 0;
-
-       for (div = 1; div <= maxdiv; div++) {
-               mult = div * desired_clk;
-               if (mult > ULONG_MAX)
+       div = 1;
+       while (div <= maxdiv) {
+               mult = (u64)div * desired_clk;
+               if (mult != (unsigned long)mult)
                        break;
 
-               freq = clk_round_rate(clk, (unsigned long)mult);
-               if (!(freq % desired_clk)) {
-                       ser_clk = freq;
-                       break;
-               }
+               offset = div * abs_tol;
+               freq = clk_round_rate(clk, mult - offset);
 
-               if (!prev)
-                       ser_clk = freq;
-               else if (prev == freq)
+               /* Can only get lower if we're done */
+               if (freq < mult - offset)
                        break;
 
-               prev = freq;
-       }
+               /*
+                * Re-calculate div in case rounding skipped rates but we
+                * ended up at a good one, then check for a match.
+                */
+               div = DIV_ROUND_CLOSEST(freq, desired_clk);
+               achieved = DIV_ROUND_CLOSEST(freq, div);
+               if (achieved <= desired_clk + abs_tol &&
+                   achieved >= desired_clk - abs_tol) {
+                       *clk_div = div;
+                       return freq;
+               }
 
-       if (!ser_clk) {
-               pr_err("%s: Can't find matching DFS entry for baud %d\n",
-                                                               __func__, baud);
-               return ser_clk;
+               div = DIV_ROUND_UP(freq, desired_clk);
        }
 
-       *clk_div = ser_clk / desired_clk;
-       if (!(*clk_div))
-               *clk_div = 1;
+       return 0;
+}
+
+static unsigned long get_clk_div_rate(struct clk *clk, unsigned int baud,
+                       unsigned int sampling_rate, unsigned int *clk_div)
+{
+       unsigned long ser_clk;
+       unsigned long desired_clk;
+
+       desired_clk = baud * sampling_rate;
+       if (!desired_clk)
+               return 0;
+
+       /*
+        * try to find a clock rate within 2% tolerance, then within 5%
+        */
+       ser_clk = find_clk_rate_in_tol(clk, desired_clk, clk_div, 2);
+       if (!ser_clk)
+               ser_clk = find_clk_rate_in_tol(clk, desired_clk, clk_div, 5);
 
        return ser_clk;
 }
@@ -1020,8 +1031,15 @@ static void qcom_geni_serial_set_termios(struct uart_port *uport,
 
        clk_rate = get_clk_div_rate(port->se.clk, baud,
                sampling_rate, &clk_div);
-       if (!clk_rate)
+       if (!clk_rate) {
+               dev_err(port->se.dev,
+                       "Couldn't find suitable clock rate for %lu\n",
+                       baud * sampling_rate);
                goto out_restart_rx;
+       }
+
+       dev_dbg(port->se.dev, "desired_rate-%lu, clk_rate-%lu, clk_div-%u\n",
+                       baud * sampling_rate, clk_rate, clk_div);
 
        uport->uartclk = clk_rate;
        dev_pm_opp_set_rate(uport->dev, clk_rate);