1 files changed, 20 insertions, 12 deletions

diff --git a/drivers/tty/serial/8250.c b/drivers/tty/serial/8250.c
index c10a6a909c76..b3b881bc4712 100644
--- a/drivers/tty/serial/8250.c
+++ b/drivers/tty/serial/8250.c
@@ -954,6 +954,23 @@ static int broken_efr(struct uart_8250_port *up)
         return 0;
 }
 
+static inline int ns16550a_goto_highspeed(struct uart_8250_port *up)
+{
+        unsigned char status;
+
+        status = serial_in(up, 0x04); /* EXCR2 */
+#define PRESL(x) ((x) & 0x30)
+        if (PRESL(status) == 0x10) {
+                /* already in high speed mode */
+                return 0;
+        } else {
+                status &= ~0xB0; /* Disable LOCK, mask out PRESL[01] */
+                status |= 0x10;  /* 1.625 divisor for baud_base --> 921600 */
+                serial_outp(up, 0x04, status);
+        }
+        return 1;
+}
+
 /*
  * We know that the chip has FIFOs.  Does it have an EFR?  The
  * EFR is located in the same register position as the IIR and
@@ -1025,12 +1042,8 @@ static void autoconfig_16550a(struct uart_8250_port *up)
                         quot = serial_dl_read(up);
                         quot <<= 3;
 
-                        status1 = serial_in(up, 0x04); /* EXCR2 */
-                        status1 &= ~0xB0; /* Disable LOCK, mask out PRESL[01] */
-                        status1 |= 0x10;  /* 1.625 divisor for baud_base --> 921600 */
-                        serial_outp(up, 0x04, status1);
-
-                        serial_dl_write(up, quot);
+                        if (ns16550a_goto_highspeed(up))
+                                serial_dl_write(up, quot);
 
                         serial_outp(up, UART_LCR, 0);
 
@@ -3025,15 +3038,10 @@ void serial8250_resume_port(int line)
         struct uart_8250_port *up = &serial8250_ports[line];
 
         if (up->capabilities & UART_NATSEMI) {
-                unsigned char tmp;
-
                 /* Ensure it's still in high speed mode */
                 serial_outp(up, UART_LCR, 0xE0);
 
-                tmp = serial_in(up, 0x04); /* EXCR2 */
-                tmp &= ~0xB0; /* Disable LOCK, mask out PRESL[01] */
-                tmp |= 0x10;  /* 1.625 divisor for baud_base --> 921600 */
-                serial_outp(up, 0x04, tmp);
+                ns16550a_goto_highspeed(up);
 
                 serial_outp(up, UART_LCR, 0);
                 up->port.uartclk = 921600*16;