1 files changed, 25 insertions, 2 deletions

diff --git a/drivers/serial/sh-sci.c b/drivers/serial/sh-sci.c
index 3a13e58e9c5d..386fb878680c 100644
--- a/drivers/serial/sh-sci.c
+++ b/drivers/serial/sh-sci.c
@@ -82,6 +82,9 @@ struct sci_port {
         /* SCSCR initialization */
         unsigned int            scscr;
 
+        /* SCBRR calculation algo */
+        unsigned int            scbrr_algo_id;
+
 #ifdef CONFIG_HAVE_CLK
         /* Interface clock */
         struct clk              *iclk;
@@ -928,6 +931,27 @@ static void sci_shutdown(struct uart_port *port)
                 s->disable(port);
 }
 
+static unsigned int sci_scbrr_calc(unsigned int algo_id, unsigned int bps,
+                                   unsigned long freq)
+{
+        switch (algo_id) {
+        case SCBRR_ALGO_1:
+                return ((freq + 16 * bps) / (16 * bps) - 1);
+        case SCBRR_ALGO_2:
+                return ((freq + 16 * bps) / (32 * bps) - 1);
+        case SCBRR_ALGO_3:
+                return (((freq * 2) + 16 * bps) / (16 * bps) - 1);
+        case SCBRR_ALGO_4:
+                return (((freq * 2) + 16 * bps) / (32 * bps) - 1);
+        case SCBRR_ALGO_5:
+                return (((freq * 1000 / 32) / bps) - 1);
+        }
+
+        /* Warn, but use a safe default */
+        WARN_ON(1);
+        return ((freq + 16 * bps) / (32 * bps) - 1);
+}
+
 static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
                             struct ktermios *old)
 {
@@ -937,7 +961,7 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 
         baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk/16);
         if (likely(baud))
-                t = SCBRR_VALUE(baud, port->uartclk);
+                t = sci_scbrr_calc(s->scbrr_algo_id, baud, port->uartclk);
 
         do {
                 status = sci_in(port, SCxSR);
@@ -1108,7 +1132,6 @@ static void __devinit sci_init_single(struct platform_device *dev,
         sci_port->type          = sci_port->port.type = p->type;
 
         memcpy(&sci_port->irqs, &p->irqs, sizeof(p->irqs));
-
 }
 
 #ifdef CONFIG_SERIAL_SH_SCI_CONSOLE