diff --git a/drivers/tty/serial/8250.c b/drivers/tty/serial/8250.c index c10a6a909c7..b3b881bc471 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;