ftdi_sio driver improvements

index 52d59be92034..b2febe8d573a 100644
@@ -1162,6 +1162,8 @@ static unsigned short int ftdi_232am_baud_base_to_divisor(int baud, int base)
 	int divisor3 = DIV_ROUND_CLOSEST(base, 2 * baud);
 	if ((divisor3 & 0x7) == 7)
 		divisor3++; /* round x.7/8 up to x+1 */
+	if (divisor3 > GENMASK(16, 0))
+		divisor3 = GENMASK(16, 0);
 	divisor = divisor3 >> 3;
 	divisor3 &= 0x7;
 	if (divisor3 == 1)

@@ -1186,6 +1188,8 @@ static u32 ftdi_232bm_baud_base_to_divisor(int baud, int base)
 	u32 divisor;
 	/* divisor shifted 3 bits to the left */
 	int divisor3 = DIV_ROUND_CLOSEST(base, 2 * baud);
+	if (divisor3 > GENMASK(16, 0))
+		divisor3 = GENMASK(16, 0);
 	divisor = divisor3 >> 3;
 	divisor |= (u32)divfrac[divisor3 & 0x7] << 14;
 	/* Deal with special cases for highest baud rates. */

@@ -1209,6 +1213,8 @@ static u32 ftdi_2232h_baud_base_to_divisor(int baud, int base)
 
 	/* hi-speed baud rate is 10-bit sampling instead of 16-bit */
 	divisor3 = DIV_ROUND_CLOSEST(8 * base, 10 * baud);
+	if (divisor3 > GENMASK(16, 0))
+		divisor3 = GENMASK(16, 0);
 
 	divisor = divisor3 >> 3;
 	divisor |= (u32)divfrac[divisor3 & 0x7] << 14;

index b2febe8d573a..bfa601fc14fe 100644
@@ -1335,7 +1335,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 		}
 		break;
 	case FT8U232AM: /* 8U232AM chip */
-		if (baud <= 3000000) {
+		if (baud >= 183 && baud <= 3000000) {
 			div_value = ftdi_232am_baud_to_divisor(baud);
 		} else {
 			dev_dbg(dev, "%s - Baud rate too high!\n", __func__);

@@ -1348,7 +1348,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 	case FT2232C: /* FT2232C chip */
 	case FT232RL: /* FT232RL chip */
 	case FTX:     /* FT-X series */
-		if (baud <= 3000000) {
+		if (baud >= 183 && baud <= 3000000) {
 			u16 product_id = le16_to_cpu(
 				port->serial->dev->descriptor.idProduct);
 			if (((product_id == FTDI_NDI_HUC_PID)		||

@@ -1372,7 +1372,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 	case FT232H:  /* FT232H chip */
 		if ((baud <= 12000000) && (baud >= 1200)) {
 			div_value = ftdi_2232h_baud_to_divisor(baud);
-		} else if (baud < 1200) {
+		} else if (baud >= 183 && baud < 1200) {
 			div_value = ftdi_232bm_baud_to_divisor(baud);
 		} else {
 			dev_dbg(dev, "%s - Baud rate too high!\n", __func__);

index bfa601fc14fe..fe8a7c5fa0e9 100644
@@ -1155,6 +1155,34 @@ static struct usb_serial_driver * const serial_drivers[] = {
  * ***************************************************************************
  */
 
+static u32 ftdi_sio_baud_to_divisor(int baud)
+{
+	switch (baud) {
+	case 300:
+		return ftdi_sio_b300;
+	case 600:
+		return ftdi_sio_b600;
+	case 1200:
+		return ftdi_sio_b1200;
+	case 2400:
+		return ftdi_sio_b2400;
+	case 4800:
+		return ftdi_sio_b4800;
+	case 9600:
+		return ftdi_sio_b9600;
+	case 19200:
+		return ftdi_sio_b19200;
+	case 38400:
+		return ftdi_sio_b38400;
+	case 57600:
+		return ftdi_sio_b57600;
+	case 115200:
+		return ftdi_sio_b115200;
+	default:
+		return -1;
+	}
+}
+
 static unsigned short int ftdi_232am_baud_base_to_divisor(int baud, int base)
 {
 	unsigned short int divisor;

@@ -1314,23 +1342,12 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 		baud = 9600;
 	switch (priv->chip_type) {
 	case SIO: /* SIO chip */
-		switch (baud) {
-		case 300: div_value = ftdi_sio_b300; break;
-		case 600: div_value = ftdi_sio_b600; break;
-		case 1200: div_value = ftdi_sio_b1200; break;
-		case 2400: div_value = ftdi_sio_b2400; break;
-		case 4800: div_value = ftdi_sio_b4800; break;
-		case 9600: div_value = ftdi_sio_b9600; break;
-		case 19200: div_value = ftdi_sio_b19200; break;
-		case 38400: div_value = ftdi_sio_b38400; break;
-		case 57600: div_value = ftdi_sio_b57600;  break;
-		case 115200: div_value = ftdi_sio_b115200; break;
-		} /* baud */
-		if (div_value == 0) {
+		div_value = ftdi_sio_baud_to_divisor(baud);
+		if (div_value == (u32)-1) {
 			dev_dbg(dev, "%s - Baudrate (%d) requested is not supported\n",
 				__func__,  baud);
-			div_value = ftdi_sio_b9600;
 			baud = 9600;
+			div_value = ftdi_sio_baud_to_divisor(baud);
 			div_okay = 0;
 		}
 		break;

index fe8a7c5fa0e9..1ab6bf48516f 100644
@@ -1315,7 +1315,7 @@ static int update_mctrl(struct usb_serial_port *port, unsigned int set,
 
 
 static u32 get_ftdi_divisor(struct tty_struct *tty,
-						struct usb_serial_port *port)
+			    struct usb_serial_port *port, speed_t old_baud)
 {
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 	struct device *dev = &port->dev;

@@ -1338,6 +1338,8 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 			__func__, priv->custom_divisor, baud);
 	}
 
+	if (!baud)
+		baud = old_baud;
 	if (!baud)
 		baud = 9600;
 	switch (priv->chip_type) {

@@ -1346,8 +1348,12 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 		if (div_value == (u32)-1) {
 			dev_dbg(dev, "%s - Baudrate (%d) requested is not supported\n",
 				__func__,  baud);
-			baud = 9600;
+			baud = old_baud ? old_baud : 9600;
 			div_value = ftdi_sio_baud_to_divisor(baud);
+			if (div_value == -1) {
+				baud = 9600;
+				div_value = ftdi_sio_baud_to_divisor(baud);
+			}
 			div_okay = 0;
 		}
 		break;

@@ -1356,8 +1362,11 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 			div_value = ftdi_232am_baud_to_divisor(baud);
 		} else {
 			dev_dbg(dev, "%s - Baud rate too high!\n", __func__);
-			baud = 9600;
-			div_value = ftdi_232am_baud_to_divisor(9600);
+			if (old_baud >= 183 && old_baud <= 3000000)
+				baud = old_baud;
+			else
+				baud = 9600;
+			div_value = ftdi_232am_baud_to_divisor(baud);
 			div_okay = 0;
 		}
 		break;

@@ -1379,9 +1388,12 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 			div_value = ftdi_232bm_baud_to_divisor(baud);
 		} else {
 			dev_dbg(dev, "%s - Baud rate too high!\n", __func__);
-			div_value = ftdi_232bm_baud_to_divisor(9600);
+			if (old_baud >= 183 && old_baud <= 3000000)
+				baud = old_baud;
+			else
+				baud = 9600;
+			div_value = ftdi_232bm_baud_to_divisor(baud);
 			div_okay = 0;
-			baud = 9600;
 		}
 		break;
 	case FT2232H: /* FT2232H chip */

@@ -1393,9 +1405,17 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 			div_value = ftdi_232bm_baud_to_divisor(baud);
 		} else {
 			dev_dbg(dev, "%s - Baud rate too high!\n", __func__);
-			div_value = ftdi_232bm_baud_to_divisor(9600);
+			if (old_baud >= 183 && old_baud < 1200) {
+				baud = old_baud;
+				div_value = ftdi_232bm_baud_to_divisor(baud);
+			} else {
+				if (old_baud >= 1200 && old_baud <= 12000000)
+					baud = old_baud;
+				else
+					baud = 9600;
+				div_value = ftdi_2232h_baud_to_divisor(baud);
+			}
 			div_okay = 0;
-			baud = 9600;
 		}
 		break;
 	} /* priv->chip_type */

@@ -1410,7 +1430,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 	return div_value;
 }
 
-static int change_speed(struct tty_struct *tty, struct usb_serial_port *port)
+static int change_speed(struct tty_struct *tty, struct usb_serial_port *port, speed_t old_baud)
 {
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 	u16 value;

@@ -1418,7 +1438,7 @@ static int change_speed(struct tty_struct *tty, struct usb_serial_port *port)
 	u32 index_value;
 	int rv;
 
-	index_value = get_ftdi_divisor(tty, port);
+	index_value = get_ftdi_divisor(tty, port, old_baud);
 	value = (u16)index_value;
 	index = (u16)(index_value >> 16);
 	if (priv->chip_type == FT2232C || priv->chip_type == FT2232H ||

@@ -1541,7 +1561,7 @@ static int set_serial_info(struct tty_struct *tty, struct serial_struct *ss)
 		if (priv->flags & ASYNC_SPD_MASK)
 			dev_warn_ratelimited(&port->dev, "use of SPD flags is deprecated\n");
 
-		change_speed(tty, port);
+		change_speed(tty, port, 9600);
 	}
 	mutex_unlock(&priv->cfg_lock);
 	return 0;

@@ -2790,9 +2810,12 @@ static void ftdi_set_termios(struct tty_struct *tty,
 		/* Drop RTS and DTR */
 		clear_mctrl(port, TIOCM_DTR | TIOCM_RTS);
 	} else {
+		speed_t old_baud =
+			old_termios ? tty_termios_baud_rate(old_termios) : 9600;
+
 		/* set the baudrate determined before */
 		mutex_lock(&priv->cfg_lock);
-		if (change_speed(tty, port))
+		if (change_speed(tty, port, old_baud))
 			dev_err(ddev, "%s urb failed to set baudrate\n", __func__);
 		mutex_unlock(&priv->cfg_lock);
 		/* Ensure RTS and DTR are raised when baudrate changed from 0 */

index 1ab6bf48516f..718c86db2900 100644
@@ -1333,7 +1333,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 	if (baud == 38400 &&
 	    ((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_CUST) &&
 	     (priv->custom_divisor)) {
-		baud = priv->baud_base / priv->custom_divisor;
+		baud = DIV_ROUND_CLOSEST(priv->baud_base, priv->custom_divisor);
 		dev_dbg(dev, "%s - custom divisor %d sets baud rate to %d\n",
 			__func__, priv->custom_divisor, baud);
 	}

index 718c86db2900..79b00912c81c 100644
@@ -1319,6 +1319,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 {
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 	struct device *dev = &port->dev;
+	int fix_custom_divisor = 0;
 	u32 div_value = 0;
 	int div_okay = 1;
 	int baud;

@@ -1333,6 +1334,7 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 	if (baud == 38400 &&
 	    ((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_CUST) &&
 	     (priv->custom_divisor)) {
+		fix_custom_divisor = 1;
 		baud = DIV_ROUND_CLOSEST(priv->baud_base, priv->custom_divisor);
 		dev_dbg(dev, "%s - custom divisor %d sets baud rate to %d\n",
 			__func__, priv->custom_divisor, baud);

@@ -1426,7 +1428,19 @@ static u32 get_ftdi_divisor(struct tty_struct *tty,
 			ftdi_chip_name[priv->chip_type]);
 	}
 
+	/* Fix deprecated async-compatible custom_divisor hack and update tty baud rate */
+	if (fix_custom_divisor) {
+		priv->custom_divisor = DIV_ROUND_CLOSEST(priv->baud_base, baud);
+		old_baud = baud;
+		baud = 38400;
+	}
+
 	tty_encode_baud_rate(tty, baud, baud);
+
+	/* For async-compatible custom_divisor store into TCGETS2 c_*speed fields real baud rate */
+	if (fix_custom_divisor)
+		tty->termios.c_ispeed = tty->termios.c_ospeed = old_baud;
+
 	return div_value;
 }
 

@@ -2699,6 +2713,8 @@ static void ftdi_set_termios(struct tty_struct *tty,
 		dev_dbg(ddev, "%s: forcing baud rate for this device\n", __func__);
 		tty_encode_baud_rate(tty, priv->force_baud,
 					priv->force_baud);
+		termios->c_ispeed = termios->c_ospeed =
+			DIV_ROUND_CLOSEST(priv->baud_base, priv->custom_divisor);
 	}
 
 	/* Force RTS-CTS if this device requires it. */

index 79b00912c81c..350ed14b014c 100644
@@ -1183,6 +1183,34 @@ 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;

@@ -1205,11 +1233,28 @@ 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 };

@@ -1228,11 +1273,31 @@ 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 };

@@ -1260,11 +1325,32 @@ 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))
 

@@ -1358,6 +1444,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) {

@@ -1371,6 +1458,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 */

@@ -1397,25 +1485,30 @@ 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 {
 				if (old_baud >= 1200 && old_baud <= 12000000)
 					baud = old_baud;
 				else
 					baud = 9600;
 				div_value = ftdi_2232h_baud_to_divisor(baud);
+				baud = ftdi_2232h_divisor_to_baud(div_value);
 			}
 			div_okay = 0;
 		}