Implement single-character virtio-console.paulliu-virtio-console

Signed-off-by: A. Cody Schuffelen <schuffelen@google.com>
[ Paul: pick from the Android tree. Rebase to the upstream ]
Signed-off-by: Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>
Link: https://git.pengutronix.de/cgit/barebox/tree/drivers/serial/virtio_console.c?id=180a551d542844b70012d7b94a415aacdcf31d45
Link: https://android.googlesource.com/platform/external/u-boot/+/4581f7c6e96d7332b534ae20de356a4554594d08

1 files changed, 90 insertions, 4 deletions

diff --git a/drivers/virtio/virtio_console.c b/drivers/virtio/virtio_console.c
index 75c5e1a4230..74bf371c629 100644
--- a/drivers/virtio/virtio_console.c
+++ b/drivers/virtio/virtio_console.c
@@ -2,6 +2,10 @@
 /*
  * Copyright (C) 2018, Tuomas Tynkkynen <tuomas.tynkkynen@iki.fi>
  * Copyright (C) 2018, Bin Meng <bmeng.cn@gmail.com>
+ * Copyright (C) 2006, 2007, 2009 Rusty Russell, IBM Corporation
+ * Copyright (C) 2009, 2010, 2011 Red Hat, Inc.
+ * Copyright (C) 2009, 2010, 2011 Amit Shah <amit.shah@redhat.com>
+ * Copyright (C) 2021 Ahmad Fatoum
  * Copyright (C) 2021, Google LLC, schuffelen@google.com (A. Cody Schuffelen)
  */
 
@@ -16,6 +20,9 @@
 #include "virtio_blk.h"
 
 struct virtio_console_priv {
+	struct virtqueue *receiveq_port0;
+	struct virtqueue *transmitq_port0;
+	unsigned char inbuf[1] __aligned(4);
 };
 
 static int virtio_console_bind(struct udevice *dev)
@@ -28,8 +35,46 @@ static int virtio_console_bind(struct udevice *dev)
 	return 0;
 }
 
+/*
+ * Create a scatter-gather list representing our input buffer and put
+ * it in the queue.
+ */
+static void add_inbuf(struct virtio_console_priv *priv)
+{
+	struct virtio_sg sg;
+	struct virtio_sg *sgs[1];
+
+	sgs[0] = &sg;
+	sg.addr = priv->inbuf;
+	sg.length = sizeof(priv->inbuf);
+
+	/* We should always be able to add one buffer to an empty queue. */
+	if (virtqueue_add(priv->receiveq_port0, sgs, 0, 1) < 0) {
+		debug("%s: virtqueue_add failed\n", __func__);
+		BUG();
+	}
+	virtqueue_kick(priv->receiveq_port0);
+}
+
 static int virtio_console_probe(struct udevice *dev)
 {
+	struct virtio_console_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	struct virtqueue *virtqueues[2];
+
+	ret = virtio_find_vqs(dev, 2, virtqueues);
+	if (ret < 0) {
+		debug("%s: virtio_find_vqs failed\n", __func__);
+		return ret;
+	}
+
+	priv->receiveq_port0 = virtqueues[0];
+	priv->transmitq_port0 = virtqueues[1];
+
+	/* Register the input buffer the first time. */
+	add_inbuf(priv);
+
 	return 0;
 }
 
@@ -38,18 +83,59 @@ static int virtio_console_serial_setbrg(struct udevice *dev, int baudrate)
 	return 0;
 }
 
-static int virtio_console_serial_getc(struct udevice *dev)
+static int virtio_console_serial_pending(struct udevice *dev, bool input)
 {
-	return -EAGAIN;
+	struct virtio_console_priv *priv = dev_get_priv(dev);
+	return virtqueue_poll(priv->receiveq_port0,
+			      priv->receiveq_port0->last_used_idx);
 }
 
-static int virtio_console_serial_pending(struct udevice *dev, bool input)
+static int virtio_console_serial_getc(struct udevice *dev)
 {
-	return 0;
+	struct virtio_console_priv *priv = dev_get_priv(dev);
+	unsigned char *in;
+	int ch;
+	unsigned int len = 0;
+
+	in = virtqueue_get_buf(priv->receiveq_port0, &len);
+	if (!in) {
+		return -EAGAIN;
+	} else if (len != 1) {
+		debug("%s: too much data: %d\n", __func__, len);
+		BUG();
+	}
+
+	ch = *in;
+
+	add_inbuf(priv);
+
+	return ch;
 }
 
 static int virtio_console_serial_putc(struct udevice *dev, const char ch)
 {
+	struct virtio_console_priv *priv = dev_get_priv(dev);
+	struct virtio_sg sg;
+	struct virtio_sg *sgs[1];
+	unsigned char buf[1] __aligned(4);
+	int ret = 0;
+
+	sg.addr = buf;
+	sg.length = sizeof(buf);
+	sgs[0] = &sg;
+	buf[0] = ch;
+
+	ret = virtqueue_add(priv->transmitq_port0, sgs, 1, 0);
+	if (ret) {
+		debug("%s: virtqueue_add failed\n", __func__);
+		return ret;
+	}
+
+	virtqueue_kick(priv->transmitq_port0);
+
+	while (!virtqueue_get_buf(priv->transmitq_port0, NULL)) {
+	}
+
 	return 0;
 }