[PATCH 7/7] USB: serial: ftdi_sio: Fill c_*speed fields with real baudrate

["Marek Behún" <kabel@kernel.org>]

From: Pali Rohár <pali@kernel.org>

Calculate baudrate value in c_*speed fields to the real value which was set
to hardware. For this operation add a new set of functions divisor_to_baud
for each chip and use it for calculating the real baudrate value.

Each divisor_to_baud function is constructed as an inverse function of
corresponding baud_to_divisor function.

Signed-off-by: Pali Rohár <pali@kernel.org>
Tested-by: Marek Behún <kabel@kernel.org>
---
 drivers/usb/serial/ftdi_sio.c | 79 +++++++++++++++++++++++++++++++++++
 1 file changed, 79 insertions(+)

diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 3bf5750e76de..1f78ae695a1b 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -1167,6 +1167,23 @@ static u32 ftdi_sio_baud_to_divisor(int baud)
 	}
 }
 
+static int ftdi_sdio_divisor_to_baud(u32 divisor)
+{
+	switch (divisor) {
+	case ftdi_sio_b300: return 300;
+	case ftdi_sio_b600: return 600;
+	case ftdi_sio_b1200: return 1200;
+	case ftdi_sio_b2400: return 2400;
+	case ftdi_sio_b4800: return 4800;
+	case ftdi_sio_b9600: return 9600;
+	case ftdi_sio_b19200: return 19200;
+	case ftdi_sio_b38400: return 38400;
+	case ftdi_sio_b57600: return 57600;
+	case ftdi_sio_b115200: return 115200;
+	default: return 9600;
+	}
+}
+
 static unsigned short int ftdi_232am_baud_base_to_divisor(int baud, int base)
 {
 	unsigned short int divisor;
@@ -1189,11 +1206,27 @@ static unsigned short int ftdi_232am_baud_base_to_divisor(int baud, int base)
 	return divisor;
 }
 
+static int ftdi_232am_divisor_base_to_baud(unsigned short int divisor, int base)
+{
+	static const unsigned char divfrac_inv[4] = { 0, 4, 2, 1 };
+	unsigned int divisor3;
+	if (divisor == 0)
+		divisor = 1;
+	divisor3 = (GENMASK(13, 0) & divisor) << 3;
+	divisor3 |= divfrac_inv[(divisor >> 14) & 0x3];
+	return DIV_ROUND_CLOSEST(base, 2 * divisor3);
+}
+
 static unsigned short int ftdi_232am_baud_to_divisor(int baud)
 {
 	 return ftdi_232am_baud_base_to_divisor(baud, 48000000);
 }
 
+static int ftdi_232am_divisor_to_baud(unsigned short int divisor)
+{
+	return ftdi_232am_divisor_base_to_baud(divisor, 48000000);
+}
+
 static u32 ftdi_232bm_baud_base_to_divisor(int baud, int base)
 {
 	static const unsigned char divfrac[8] = { 0, 3, 2, 4, 1, 5, 6, 7 };
@@ -1212,11 +1245,30 @@ static u32 ftdi_232bm_baud_base_to_divisor(int baud, int base)
 	return divisor;
 }
 
+static int ftdi_232bm_divisor_base_to_baud(u32 divisor, int base)
+{
+	static const unsigned char divfrac_inv[8] = { 0, 4, 2, 1, 3, 5, 6, 7 };
+	u32 divisor3;
+	/* Deal with special cases for highest baud rates. */
+	if (divisor == 0)
+		divisor = 1;		/* 1.0 */
+	else if (divisor == 1)
+		divisor = 0x4001;	/* 1.5 */
+	divisor3 = (GENMASK(13, 0) & divisor) << 3;
+	divisor3 |= divfrac_inv[(divisor >> 14) & 0x7];
+	return DIV_ROUND_CLOSEST(base, 2 * divisor3);
+}
+
 static u32 ftdi_232bm_baud_to_divisor(int baud)
 {
 	 return ftdi_232bm_baud_base_to_divisor(baud, 48000000);
 }
 
+static int ftdi_232bm_divisor_to_baud(u32 divisor)
+{
+	return ftdi_232bm_divisor_base_to_baud(divisor, 48000000);
+}
+
 static u32 ftdi_2232h_baud_base_to_divisor(int baud, int base)
 {
 	static const unsigned char divfrac[8] = { 0, 3, 2, 4, 1, 5, 6, 7 };
@@ -1244,11 +1296,31 @@ static u32 ftdi_2232h_baud_base_to_divisor(int baud, int base)
 	return divisor;
 }
 
+static int ftdi_2232h_divisor_base_to_baud(u32 divisor, int base)
+{
+	static const unsigned char divfrac_inv[8] = { 0, 4, 2, 1, 3, 5, 6, 7 };
+	u32 divisor3;
+	divisor &= GENMASK(16, 0);
+	/* Deal with special cases for highest baud rates. */
+	if (divisor == 0)
+		divisor = 1;		/* 1.0 */
+	else if (divisor == 1)
+		divisor = 0x4001;	/* 1.5 */
+	divisor3 = (GENMASK(13, 0) & divisor) << 3;
+	divisor3 |= divfrac_inv[(divisor >> 14) & 0x7];
+	return DIV_ROUND_CLOSEST(8 * base, 10 * divisor3);
+}
+
 static u32 ftdi_2232h_baud_to_divisor(int baud)
 {
 	 return ftdi_2232h_baud_base_to_divisor(baud, 120000000);
 }
 
+static int ftdi_2232h_divisor_to_baud(u32 divisor)
+{
+	return ftdi_2232h_divisor_base_to_baud(divisor, 120000000);
+}
+
 #define set_mctrl(port, set)		update_mctrl((port), (set), 0)
 #define clear_mctrl(port, clear)	update_mctrl((port), 0, (clear))
 
@@ -1342,6 +1414,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 			}
 			div_okay = 0;
 		}
+		baud = ftdi_sdio_divisor_to_baud(div_value);
 		break;
 	case FT8U232AM: /* 8U232AM chip */
 		if (baud >= 183 && baud <= 3000000) {
@@ -1352,6 +1425,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 			div_value = ftdi_232am_baud_to_divisor(baud);
 			div_okay = 0;
 		}
+		baud = ftdi_232am_divisor_to_baud(div_value);
 		break;
 	case FT232BM: /* FT232BM chip */
 	case FT2232C: /* FT2232C chip */
@@ -1375,22 +1449,27 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 			div_value = ftdi_232bm_baud_to_divisor(baud);
 			div_okay = 0;
 		}
+		baud = ftdi_232bm_divisor_to_baud(div_value);
 		break;
 	case FT2232H: /* FT2232H chip */
 	case FT4232H: /* FT4232H chip */
 	case FT232H:  /* FT232H chip */
 		if ((baud <= 12000000) && (baud >= 1200)) {
 			div_value = ftdi_2232h_baud_to_divisor(baud);
+			baud = ftdi_2232h_divisor_to_baud(div_value);
 		} else if (baud >= 183 && baud < 1200) {
 			div_value = ftdi_232bm_baud_to_divisor(baud);
+			baud = ftdi_232bm_divisor_to_baud(div_value);
 		} else {
 			dev_dbg(dev, "%s - Baud rate too high!\n", __func__);
 			if (old_baud >= 183 && old_baud < 1200) {
 				baud = old_baud;
 				div_value = ftdi_232bm_baud_to_divisor(baud);
+				baud = ftdi_232bm_divisor_to_baud(div_value);
 			} else {
 				baud = (old_baud >= 1200 && old_baud <= 12000000) ? old_baud : 9600;
 				div_value = ftdi_2232h_baud_to_divisor(baud);
+				baud = ftdi_2232h_divisor_to_baud(div_value);
 			}
 			div_okay = 0;
 		}
-- 
2.35.1