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);