[PATCH] serdev: add method to set stop bits

[Fatih Aşıcı <fatih.asici@xxxxxxxxxxxx>]

Adds serdev_device_set_stop_bits() which takes an int argument with
a value of 1 or 2.

An implementation for ttyport is also added.

Signed-off-by: Fatih Aşıcı <fatih.asici@xxxxxxxxxxxx>
---
 drivers/tty/serdev/core.c           | 11 +++++++++++
 drivers/tty/serdev/serdev-ttyport.c | 22 ++++++++++++++++++++++
 include/linux/serdev.h              |  3 +++
 3 files changed, 36 insertions(+)

diff --git a/drivers/tty/serdev/core.c b/drivers/tty/serdev/core.c
index a0ac16ee6575..1ca1c067e10f 100644
--- a/drivers/tty/serdev/core.c
+++ b/drivers/tty/serdev/core.c
@@ -371,6 +371,17 @@ int serdev_device_set_parity(struct serdev_device *serdev,
 }
 EXPORT_SYMBOL_GPL(serdev_device_set_parity);
 
+int serdev_device_set_stop_bits(struct serdev_device *serdev, int stop_bits)
+{
+	struct serdev_controller *ctrl = serdev->ctrl;
+
+	if (!ctrl || !ctrl->ops->set_stop_bits)
+		return -ENOTSUPP;
+
+	return ctrl->ops->set_stop_bits(ctrl, stop_bits);
+}
+EXPORT_SYMBOL_GPL(serdev_device_set_stop_bits);
+
 void serdev_device_wait_until_sent(struct serdev_device *serdev, long timeout)
 {
 	struct serdev_controller *ctrl = serdev->ctrl;
diff --git a/drivers/tty/serdev/serdev-ttyport.c b/drivers/tty/serdev/serdev-ttyport.c
index d1cdd2ab8b4c..820c632e77cc 100644
--- a/drivers/tty/serdev/serdev-ttyport.c
+++ b/drivers/tty/serdev/serdev-ttyport.c
@@ -217,6 +217,27 @@ static int ttyport_set_parity(struct serdev_controller *ctrl,
 	return 0;
 }
 
+static int ttyport_set_stop_bits(struct serdev_controller *ctrl, int stop_bits)
+{
+	struct serport *serport = serdev_controller_get_drvdata(ctrl);
+	struct tty_struct *tty = serport->tty;
+	struct ktermios ktermios = tty->termios;
+
+	if (stop_bits == 1)
+		ktermios.c_cflag &= ~CSTOPB;
+	else if (stop_bits == 2)
+		ktermios.c_cflag |= CSTOPB;
+	else
+		return -EINVAL;
+
+	tty_set_termios(tty, &ktermios);
+
+	if ((tty->termios.c_cflag & CSTOPB) != (ktermios.c_cflag & CSTOPB))
+		return -EINVAL;
+
+	return 0;
+}
+
 static void ttyport_wait_until_sent(struct serdev_controller *ctrl, long timeout)
 {
 	struct serport *serport = serdev_controller_get_drvdata(ctrl);
@@ -255,6 +276,7 @@ static const struct serdev_controller_ops ctrl_ops = {
 	.close = ttyport_close,
 	.set_flow_control = ttyport_set_flow_control,
 	.set_parity = ttyport_set_parity,
+	.set_stop_bits = ttyport_set_stop_bits,
 	.set_baudrate = ttyport_set_baudrate,
 	.wait_until_sent = ttyport_wait_until_sent,
 	.get_tiocm = ttyport_get_tiocm,
diff --git a/include/linux/serdev.h b/include/linux/serdev.h
index 070bf4e92df7..ec8bb6b4ac39 100644
--- a/include/linux/serdev.h
+++ b/include/linux/serdev.h
@@ -95,6 +95,7 @@ struct serdev_controller_ops {
 	void (*close)(struct serdev_controller *);
 	void (*set_flow_control)(struct serdev_controller *, bool);
 	int (*set_parity)(struct serdev_controller *, enum serdev_parity);
+	int (*set_stop_bits)(struct serdev_controller *, int);
 	unsigned int (*set_baudrate)(struct serdev_controller *, unsigned int);
 	void (*wait_until_sent)(struct serdev_controller *, long);
 	int (*get_tiocm)(struct serdev_controller *);
@@ -311,6 +312,8 @@ static inline int serdev_device_set_rts(struct serdev_device *serdev, bool enabl
 int serdev_device_set_parity(struct serdev_device *serdev,
 			     enum serdev_parity parity);
 
+int serdev_device_set_stop_bits(struct serdev_device *serdev, int stop_bits);
+
 /*
  * serdev hooks into TTY core
  */
-- 
2.19.1