From e4f457646f216f36cd86fcfe54af0d7956d17932 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Kr=C3=B6ger?= <timokroeger93@gmail.com>
Date: Tue, 27 Dec 2022 10:20:51 +0100
Subject: [PATCH] rp: Fill and empty FIFOs in buffered uart interrupt

Fixes an issue where only the first byte was transmitted.
Should improve throughput aswell.
---
 embassy-rp/src/uart/buffered.rs | 40 ++++++++++++++++++++++++---------
 1 file changed, 29 insertions(+), 11 deletions(-)

diff --git a/embassy-rp/src/uart/buffered.rs b/embassy-rp/src/uart/buffered.rs
index e89970d1..1d1fe37c 100644
--- a/embassy-rp/src/uart/buffered.rs
+++ b/embassy-rp/src/uart/buffered.rs
@@ -402,27 +402,45 @@ pub(crate) unsafe fn on_interrupt<T: Instance>(_: *mut ()) {
         }
 
         let mut rx_writer = s.rx_buf.writer();
-        if !r.uartfr().read().rxfe() {
-            let val = r.uartdr().read().data();
-            if !rx_writer.push_one(val) {
-                warn!("RX buffer full, discard received byte");
+        let rx_buf = rx_writer.push_slice();
+        let mut n_read = 0;
+        for rx_byte in rx_buf {
+            if r.uartfr().read().rxfe() {
+                break;
             }
+            *rx_byte = r.uartdr().read().data();
+            n_read += 1;
+        }
+        if n_read > 0 {
+            rx_writer.push_done(n_read);
             s.rx_waker.wake();
         }
 
         // TX
         let mut tx_reader = s.tx_buf.reader();
-        if let Some(val) = tx_reader.pop_one() {
-            r.uartimsc().modify(|w| {
-                w.set_txim(true);
-            });
-            r.uartdr().write(|w| w.set_data(val));
-            s.tx_waker.wake();
-        } else {
+        let tx_buf = tx_reader.pop_slice();
+        if tx_buf.len() == 0 {
             // Disable interrupt until we have something to transmit again
             r.uartimsc().modify(|w| {
                 w.set_txim(false);
             });
+        } else {
+            r.uartimsc().modify(|w| {
+                w.set_txim(true);
+            });
+
+            let mut n_written = 0;
+            for tx_byte in tx_buf.iter_mut() {
+                if r.uartfr().read().txff() {
+                    break;
+                }
+                r.uartdr().write(|w| w.set_data(*tx_byte));
+                n_written += 1;
+            }
+            if n_written > 0 {
+                tx_reader.pop_done(n_written);
+                s.tx_waker.wake();
+            }
         }
     }
 }