1 files changed, 59 insertions, 18 deletions

diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 091b65587c7c..3081e46085ce 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -1776,13 +1776,30 @@ static unsigned int sci_scbrr_calc(struct sci_port *s, unsigned int bps,
         return ((freq + 16 * bps) / (32 * bps) - 1);
 }
 
+/* calculate frame length from SMR */
+static int sci_baud_calc_frame_len(unsigned int smr_val)
+{
+        int len = 10;
+
+        if (smr_val & SCSMR_CHR)
+                len--;
+        if (smr_val & SCSMR_PE)
+                len++;
+        if (smr_val & SCSMR_STOP)
+                len++;
+
+        return len;
+}
+
+
 /* calculate sample rate, BRR, and clock select for HSCIF */
 static void sci_baud_calc_hscif(unsigned int bps, unsigned long freq,
                                 int *brr, unsigned int *srr,
-                                unsigned int *cks)
+                                unsigned int *cks, int frame_len)
 {
-        int sr, c, br, err;
+        int sr, c, br, err, recv_margin;
         int min_err = 1000; /* 100% */
+        int recv_max_margin = 0;
 
         /* Find the combination of sample rate and clock select with the
            smallest deviation from the desired baud rate. */
@@ -1795,12 +1812,35 @@ static void sci_baud_calc_hscif(unsigned int bps, unsigned long freq,
                         err = DIV_ROUND_CLOSEST(freq, ((br + 1) * bps * sr *
                                                (1 << (2 * c + 1)) / 1000)) -
                                                1000;
+                        if (err < 0)
+                                continue;
+
+                        /* Calc recv margin
+                         * M: Receive margin (%)
+                         * N: Ratio of bit rate to clock (N = sampling rate)
+                         * D: Clock duty (D = 0 to 1.0)
+                         * L: Frame length (L = 9 to 12)
+                         * F: Absolute value of clock frequency deviation
+                         *
+                         *  M = |(0.5 - 1 / 2 * N) - ((L - 0.5) * F) -
+                         *      (|D - 0.5| / N * (1 + F))|
+                         *  NOTE: Usually, treat D for 0.5, F is 0 by this
+                         *        calculation.
+                         */
+                        recv_margin = abs((500 -
+                                        DIV_ROUND_CLOSEST(1000, sr << 1)) / 10);
                         if (min_err > err) {
                                 min_err = err;
-                                *brr = br;
-                                *srr = sr - 1;
-                                *cks = c;
-                        }
+                                recv_max_margin = recv_margin;
+                        } else if ((min_err == err) &&
+                                   (recv_margin > recv_max_margin))
+                                recv_max_margin = recv_margin;
+                        else
+                                continue;
+
+                        *brr = br;
+                        *srr = sr - 1;
+                        *cks = c;
                 }
         }
 
@@ -1834,10 +1874,19 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 {
         struct sci_port *s = to_sci_port(port);
         struct plat_sci_reg *reg;
-        unsigned int baud, smr_val, max_baud, cks = 0;
+        unsigned int baud, smr_val = 0, max_baud, cks = 0;
         int t = -1;
         unsigned int srr = 15;
 
+        if ((termios->c_cflag & CSIZE) == CS7)
+                smr_val |= SCSMR_CHR;
+        if (termios->c_cflag & PARENB)
+                smr_val |= SCSMR_PE;
+        if (termios->c_cflag & PARODD)
+                smr_val |= SCSMR_PE | SCSMR_ODD;
+        if (termios->c_cflag & CSTOPB)
+                smr_val |= SCSMR_STOP;
+
         /*
          * earlyprintk comes here early on with port->uartclk set to zero.
          * the clock framework is not up and running at this point so here
@@ -1851,8 +1900,9 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
         baud = uart_get_baud_rate(port, termios, old, 0, max_baud);
         if (likely(baud && port->uartclk)) {
                 if (s->cfg->type == PORT_HSCIF) {
+                        int frame_len = sci_baud_calc_frame_len(smr_val);
                         sci_baud_calc_hscif(baud, port->uartclk, &t, &srr,
-                                            &cks);
+                                            &cks, frame_len);
                 } else {
                         t = sci_scbrr_calc(s, baud, port->uartclk);
                         for (cks = 0; t >= 256 && cks <= 3; cks++)
@@ -1864,16 +1914,7 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 
         sci_reset(port);
 
-        smr_val = serial_port_in(port, SCSMR) & 3;
-
-        if ((termios->c_cflag & CSIZE) == CS7)
-                smr_val |= SCSMR_CHR;
-        if (termios->c_cflag & PARENB)
-                smr_val |= SCSMR_PE;
-        if (termios->c_cflag & PARODD)
-                smr_val |= SCSMR_PE | SCSMR_ODD;
-        if (termios->c_cflag & CSTOPB)
-                smr_val |= SCSMR_STOP;
+        smr_val |= serial_port_in(port, SCSMR) & 3;
 
         uart_update_timeout(port, termios->c_cflag, baud);