>From 7bd4a1b35b6351bc358c539e49b3a1600a124c8d Mon Sep 17 00:00:00 2001
From: Gavin Shan <address@hidden>
Date: Fri, 21 Feb 2020 14:26:10 +1100
Subject: [PATCH] hw/char/pl011: Support TxFIFO and async transmission

Signed-off-by: Gavin Shan <address@hidden>
---
 hw/char/pl011.c         | 70 +++++++++++++++++++++++++++++++++++++----
 include/hw/char/pl011.h |  2 ++
 2 files changed, 66 insertions(+), 6 deletions(-)

diff --git a/hw/char/pl011.c b/hw/char/pl011.c
index 13e784f9d9..30d5aeb90a 100644
--- a/hw/char/pl011.c
+++ b/hw/char/pl011.c
@@ -169,6 +169,66 @@ static void pl011_set_read_trigger(PL011State *s)
         s->read_trigger = 1;
 }
 
+static gboolean pl011_xmit(GIOChannel *chan, GIOCondition cond, void *opaque)
+{
+    PL011State *s = (PL011State *)opaque;
+    int ret;
+
+    /* instant drain the fifo when there's no back-end */
+    if (!qemu_chr_fe_backend_connected(&s->chr)) {
+        s->write_count = 0;
+        return FALSE;
+    }
+
+    if (!s->write_count) {
+        return FALSE;
+    }
+
+    ret = qemu_chr_fe_write(&s->chr, s->write_fifo, s->write_count);
+    if (ret > 0) {
+        s->write_count -= ret;
+        memmove(s->write_fifo, s->write_fifo + ret, s->write_count);
+        s->flags &= ~PL011_FLAG_TXFF;
+        if (!s->write_count) {
+            s->flags |= PL011_FLAG_TXFE;
+        }
+    }
+
+    if (s->write_count) {
+        guint r = qemu_chr_fe_add_watch(&s->chr, G_IO_OUT | G_IO_HUP,
+                                        pl011_xmit, s);
+        if (!r) {
+            s->write_count = 0;
+            s->flags &= ~PL011_FLAG_TXFF;
+            s->flags |= PL011_FLAG_TXFE;
+            return FALSE;
+        }
+    }
+
+    s->int_level |= PL011_INT_TX;
+    pl011_update(s);
+    return FALSE;
+}
+
+static void pl011_write_fifo(void *opaque, const unsigned char *buf, int size)
+{
+    PL011State *s = (PL011State *)opaque;
+    int depth = (s->lcr & 0x10) ? 16 : 1;
+
+    if (size >= (depth - s->write_count)) {
+        size = depth - s->write_count;
+        s->flags |= PL011_FLAG_TXFF;
+    }
+
+    if (size > 0) {
+        memcpy(s->write_fifo + s->write_count, buf, size);
+	s->write_count += size;
+	s->flags &= ~PL011_FLAG_TXFE;
+    }
+
+    pl011_xmit(NULL, G_IO_OUT, s);
+}
+
 static void pl011_write(void *opaque, hwaddr offset,
                         uint64_t value, unsigned size)
 {
@@ -179,13 +239,8 @@ static void pl011_write(void *opaque, hwaddr offset,
 
     switch (offset >> 2) {
     case 0: /* UARTDR */
-        /* ??? Check if transmitter is enabled.  */
         ch = value;
-        /* XXX this blocks entire thread. Rewrite to use
-         * qemu_chr_fe_write and background I/O callbacks */
-        qemu_chr_fe_write_all(&s->chr, &ch, 1);
-        s->int_level |= PL011_INT_TX;
-        pl011_update(s);
+        pl011_write_fifo(opaque, &ch, 1);
         break;
     case 1: /* UARTRSR/UARTECR */
         s->rsr = 0;
@@ -207,6 +262,7 @@ static void pl011_write(void *opaque, hwaddr offset,
         if ((s->lcr ^ value) & 0x10) {
             s->read_count = 0;
             s->read_pos = 0;
+            s->write_count = 0;
         }
         s->lcr = value;
         pl011_set_read_trigger(s);
@@ -306,6 +362,7 @@ static const VMStateDescription vmstate_pl011 = {
         VMSTATE_UINT32(int_enabled, PL011State),
         VMSTATE_UINT32(int_level, PL011State),
         VMSTATE_UINT32_ARRAY(read_fifo, PL011State, 16),
+        VMSTATE_UINT8_ARRAY(write_fifo, PL011State, 16),
         VMSTATE_UINT32(ilpr, PL011State),
         VMSTATE_UINT32(ibrd, PL011State),
         VMSTATE_UINT32(fbrd, PL011State),
@@ -313,6 +370,7 @@ static const VMStateDescription vmstate_pl011 = {
         VMSTATE_INT32(read_pos, PL011State),
         VMSTATE_INT32(read_count, PL011State),
         VMSTATE_INT32(read_trigger, PL011State),
+        VMSTATE_INT32(write_count, PL011State),
         VMSTATE_END_OF_LIST()
     }
 };
diff --git a/include/hw/char/pl011.h b/include/hw/char/pl011.h
index 14187165c6..aeaf332eca 100644
--- a/include/hw/char/pl011.h
+++ b/include/hw/char/pl011.h
@@ -38,6 +38,7 @@ typedef struct PL011State {
     uint32_t int_enabled;
     uint32_t int_level;
     uint32_t read_fifo[16];
+    uint8_t  write_fifo[16];
     uint32_t ilpr;
     uint32_t ibrd;
     uint32_t fbrd;
@@ -45,6 +46,7 @@ typedef struct PL011State {
     int read_pos;
     int read_count;
     int read_trigger;
+    int write_count;
     CharBackend chr;
     qemu_irq irq[6];
     const unsigned char *id;
-- 
2.23.0