[RFC PATCH 09/10] rpmsg: tty : Add the support of the flow control

[Arnaud Pouliquen <arnaud.pouliquen@foss.st.com>]

Add Software flow control support based on the RPMsg flow
control channel.

Signed-off-by: Arnaud Pouliquen <arnaud.pouliquen@foss.st.com>
---
 drivers/tty/rpmsg_tty.c | 50 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 50 insertions(+)

diff --git a/drivers/tty/rpmsg_tty.c b/drivers/tty/rpmsg_tty.c
index 29db413bbc03..fc1dc4a5ca9d 100644
--- a/drivers/tty/rpmsg_tty.c
+++ b/drivers/tty/rpmsg_tty.c
@@ -30,6 +30,7 @@ struct rpmsg_tty_port {
 	struct tty_port		port;	 /* TTY port data */
 	int			id;	 /* TTY rpmsg index */
 	struct rpmsg_device	*rpdev;	 /* rpmsg device */
+	bool			flow_stopped; /* remote device flow control */
 };
 
 static int rpmsg_tty_cb(struct rpmsg_device *rpdev, void *data, int len, void *priv, u32 src)
@@ -106,6 +107,9 @@ static unsigned int rpmsg_tty_write_room(struct tty_struct *tty)
 	struct rpmsg_tty_port *cport = tty->driver_data;
 	int size;
 
+	if (cport->flow_stopped)
+		return 0;
+
 	size = rpmsg_get_mtu(cport->rpdev->ept);
 	if (size < 0)
 		return 0;
@@ -118,6 +122,28 @@ static void rpmsg_tty_hangup(struct tty_struct *tty)
 	tty_port_hangup(tty->port);
 }
 
+static void rpmsg_tty_throttle(struct tty_struct *tty)
+{
+	struct rpmsg_tty_port *cport = tty->driver_data;
+	int ret;
+
+	/* Disable remote transmission */
+	ret = rpmsg_set_flow_control(cport->rpdev->ept, RPMSG_ADDR_ANY, 0);
+	if (ret && ret != ENXIO)
+		dev_err(tty->dev, "cannot send control (%d)\n", ret);
+};
+
+static void rpmsg_tty_unthrottle(struct tty_struct *tty)
+{
+	struct rpmsg_tty_port *cport = tty->driver_data;
+	int ret;
+
+	/* Enable remote transmission */
+	ret = rpmsg_set_flow_control(cport->rpdev->ept, RPMSG_ADDR_ANY, 1);
+	if (ret && ret != ENXIO)
+		dev_err(tty->dev, "cannot send control (%d)\n", ret);
+};
+
 static const struct tty_operations rpmsg_tty_ops = {
 	.install	= rpmsg_tty_install,
 	.open		= rpmsg_tty_open,
@@ -126,6 +152,8 @@ static const struct tty_operations rpmsg_tty_ops = {
 	.write_room	= rpmsg_tty_write_room,
 	.hangup		= rpmsg_tty_hangup,
 	.cleanup	= rpmsg_tty_cleanup,
+	.throttle	= rpmsg_tty_throttle,
+	.unthrottle	= rpmsg_tty_unthrottle,
 };
 
 static struct rpmsg_tty_port *rpmsg_tty_alloc_cport(void)
@@ -162,11 +190,32 @@ static void rpmsg_tty_destruct_port(struct tty_port *port)
 	kfree(cport);
 }
 
+static void rpmsg_tty_dtr_rts(struct tty_port *port, int raise)
+{
+	if (!port->tty)
+		return;
+
+	if (raise)
+		rpmsg_tty_unthrottle(port->tty);
+	else
+		rpmsg_tty_throttle(port->tty);
+}
+
 static const struct tty_port_operations rpmsg_tty_port_ops = {
 	.destruct = rpmsg_tty_destruct_port,
+	.dtr_rts  = rpmsg_tty_dtr_rts,
 };
 
 
+static int rpmsg_tty_dsr_cts(struct rpmsg_device *rpdev, void *priv, u32 state)
+{
+	struct rpmsg_tty_port *cport = dev_get_drvdata(&rpdev->dev);
+
+	cport->flow_stopped = !state;
+
+	return 0;
+}
+
 static int rpmsg_tty_probe(struct rpmsg_device *rpdev)
 {
 	struct rpmsg_tty_port *cport;
@@ -225,6 +274,7 @@ static struct rpmsg_driver rpmsg_tty_rpmsg_drv = {
 	.probe		= rpmsg_tty_probe,
 	.callback	= rpmsg_tty_cb,
 	.remove		= rpmsg_tty_remove,
+	.signals        = rpmsg_tty_dsr_cts,
 };
 
 static int __init rpmsg_tty_init(void)
-- 
2.25.1