USB: pl2303: refactor baud-rate divisor handling

Refactor baud-rate divisor handling.

Signed-off-by: Johan Hovold <jhovold@gmail.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

1 files changed, 25 insertions, 16 deletions

diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index 394903b4ecb7..44f4b546adeb 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -339,6 +339,28 @@ static speed_t pl2303_get_supported_baud_rate(speed_t baud)
 	return baud;
 }
 
+static speed_t pl2303_encode_baud_rate_divisor(unsigned char buf[4],
+								speed_t baud)
+{
+	unsigned int tmp;
+
+	/*
+	 * Apparently the formula is:
+	 * baudrate = 12M * 32 / (2^buf[1]) / buf[0]
+	 */
+	tmp = 12000000 * 32 / baud;
+	buf[3] = 0x80;
+	buf[2] = 0;
+	buf[1] = (tmp >= 256);
+	while (tmp >= 256) {
+		tmp >>= 2;
+		buf[1] <<= 1;
+	}
+	buf[0] = tmp;
+
+	return baud;
+}
+
 static void pl2303_encode_baud_rate(struct tty_struct *tty,
 					struct usb_serial_port *port,
 					u8 buf[4])
@@ -362,23 +384,10 @@ static void pl2303_encode_baud_rate(struct tty_struct *tty,
 	 */
 	baud = pl2303_get_supported_baud_rate(baud);
 
-	if (baud <= 115200) {
+	if (baud <= 115200)
 		put_unaligned_le32(baud, buf);
-	} else {
-		/*
-		 * Apparently the formula for higher speeds is:
-		 * baudrate = 12M * 32 / (2^buf[1]) / buf[0]
-		 */
-		unsigned tmp = 12000000 * 32 / baud;
-		buf[3] = 0x80;
-		buf[2] = 0;
-		buf[1] = (tmp >= 256);
-		while (tmp >= 256) {
-			tmp >>= 2;
-			buf[1] <<= 1;
-		}
-		buf[0] = tmp;
-	}
+	else
+		baud = pl2303_encode_baud_rate_divisor(buf, baud);
 
 	/* Save resulting baud rate */
 	tty_encode_baud_rate(tty, baud, baud);