From 84b1753b4ed87ef65c5df77b2de73f8d0e6b3642 Mon Sep 17 00:00:00 2001
From: Jan Niehusmann <jan@gondor.com>
Date: Sat, 24 Aug 2024 17:51:33 +0000
Subject: [PATCH] Port UART updates to rp235x-hal

The updates from #798, #837 and #838 were only applied to rp2040-hal.
This patch ports them to rp235x-hal.
---
 rp235x-hal/src/uart/peripheral.rs | 40 ++++++++++++++++++++++++++++++-
 rp235x-hal/src/uart/reader.rs     | 23 +++++++++++++++++-
 rp235x-hal/src/uart/writer.rs     | 10 ++++++--
 3 files changed, 69 insertions(+), 4 deletions(-)

diff --git a/rp235x-hal/src/uart/peripheral.rs b/rp235x-hal/src/uart/peripheral.rs
index 89fb74000..b0783084c 100644
--- a/rp235x-hal/src/uart/peripheral.rs
+++ b/rp235x-hal/src/uart/peripheral.rs
@@ -21,6 +21,7 @@ pub struct UartPeripheral<S: State, D: UartDevice, P: ValidUartPinout<D>> {
     device: D,
     _state: S,
     pins: P,
+    read_error: Option<ReadErrorType>,
 }
 
 impl<S: State, D: UartDevice, P: ValidUartPinout<D>> UartPeripheral<S, D, P> {
@@ -29,6 +30,7 @@ impl<S: State, D: UartDevice, P: ValidUartPinout<D>> UartPeripheral<S, D, P> {
             device: self.device,
             pins: self.pins,
             _state: state,
+            read_error: None,
         }
     }
 
@@ -48,6 +50,7 @@ impl<D: UartDevice, P: ValidUartPinout<D>> UartPeripheral<Disabled, D, P> {
             device,
             _state: Disabled,
             pins,
+            read_error: None,
         }
     }
 
@@ -88,6 +91,7 @@ impl<D: UartDevice, P: ValidUartPinout<D>> UartPeripheral<Disabled, D, P> {
             device,
             pins,
             _state: Enabled,
+            read_error: None,
         })
     }
 }
@@ -250,6 +254,7 @@ impl<D: UartDevice, P: ValidUartPinout<D>> UartPeripheral<Enabled, D, P> {
             device: reader.device,
             _state: Enabled,
             pins: reader.pins,
+            read_error: reader.read_error,
         }
     }
 }
@@ -260,6 +265,7 @@ impl<P: ValidUartPinout<UART0>> UartPeripheral<Enabled, UART0, P> {
         let reader = Reader {
             device: self.device,
             pins: self.pins,
+            read_error: self.read_error,
         };
         // Safety: reader and writer will never write to the same address
         let device_copy = unsafe { Peripherals::steal().UART0 };
@@ -278,6 +284,7 @@ impl<P: ValidUartPinout<UART1>> UartPeripheral<Enabled, UART1, P> {
         let reader = Reader {
             device: self.device,
             pins: self.pins,
+            read_error: self.read_error,
         };
         // Safety: reader and writer will never write to the same address
         let device_copy = unsafe { Peripherals::steal().UART1 };
@@ -469,9 +476,32 @@ impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::ErrorType
 }
 impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::Read for UartPeripheral<Enabled, D, P> {
     fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
-        nb::block!(self.read_raw(buf)).map_err(|e| e.err_type)
+        // If the last read stored an error, report it now
+        if let Some(err) = self.read_error.take() {
+            return Err(err);
+        }
+        match nb::block!(self.read_raw(buf)) {
+            Ok(bytes_read) => Ok(bytes_read),
+            Err(err) if !err.discarded.is_empty() => {
+                // If an error was reported but some bytes were already read,
+                // return the data now and store the error for the next
+                // invocation.
+                self.read_error = Some(err.err_type);
+                Ok(err.discarded.len())
+            }
+            Err(err) => Err(err.err_type),
+        }
+    }
+}
+
+impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::ReadReady
+    for UartPeripheral<Enabled, D, P>
+{
+    fn read_ready(&mut self) -> Result<bool, Self::Error> {
+        Ok(self.uart_is_readable())
     }
 }
+
 impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::Write for UartPeripheral<Enabled, D, P> {
     fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
         self.write_full_blocking(buf);
@@ -482,3 +512,11 @@ impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::Write for UartPeripheral
         Ok(())
     }
 }
+
+impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::WriteReady
+    for UartPeripheral<Enabled, D, P>
+{
+    fn write_ready(&mut self) -> Result<bool, Self::Error> {
+        Ok(self.uart_is_writable())
+    }
+}
diff --git a/rp235x-hal/src/uart/reader.rs b/rp235x-hal/src/uart/reader.rs
index 6126a4fcd..8df37e13c 100644
--- a/rp235x-hal/src/uart/reader.rs
+++ b/rp235x-hal/src/uart/reader.rs
@@ -190,6 +190,7 @@ pub(crate) fn read_full_blocking<D: UartDevice>(
 pub struct Reader<D: UartDevice, P: ValidUartPinout<D>> {
     pub(super) device: D,
     pub(super) pins: P,
+    pub(super) read_error: Option<ReadErrorType>,
 }
 
 impl<D: UartDevice, P: ValidUartPinout<D>> Reader<D, P> {
@@ -228,7 +229,27 @@ impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::ErrorType for Reader<D,
 
 impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::Read for Reader<D, P> {
     fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
-        nb::block!(self.read_raw(buf)).map_err(|e| e.err_type)
+        // If the last read stored an error, report it now
+        if let Some(err) = self.read_error.take() {
+            return Err(err);
+        }
+        match nb::block!(self.read_raw(buf)) {
+            Ok(bytes_read) => Ok(bytes_read),
+            Err(err) if !err.discarded.is_empty() => {
+                // If an error was reported but some bytes were already read,
+                // return the data now and store the error for the next
+                // invocation.
+                self.read_error = Some(err.err_type);
+                Ok(err.discarded.len())
+            }
+            Err(err) => Err(err.err_type),
+        }
+    }
+}
+
+impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::ReadReady for Reader<D, P> {
+    fn read_ready(&mut self) -> Result<bool, Self::Error> {
+        Ok(is_readable(&self.device))
     }
 }
 
diff --git a/rp235x-hal/src/uart/writer.rs b/rp235x-hal/src/uart/writer.rs
index 941fc9398..ac9c4779c 100644
--- a/rp235x-hal/src/uart/writer.rs
+++ b/rp235x-hal/src/uart/writer.rs
@@ -215,8 +215,8 @@ impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::ErrorType for Writer<D,
 
 impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::Write for Writer<D, P> {
     fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
-        self.write_full_blocking(buf);
-        Ok(buf.len())
+        let remaining = nb::block!(write_raw(&self.device, buf)).unwrap(); // Infallible
+        Ok(buf.len() - remaining.len())
     }
     fn flush(&mut self) -> Result<(), Self::Error> {
         nb::block!(transmit_flushed(&self.device)).unwrap(); // Infallible
@@ -224,6 +224,12 @@ impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::Write for Writer<D, P> {
     }
 }
 
+impl<D: UartDevice, P: ValidUartPinout<D>> embedded_io::WriteReady for Writer<D, P> {
+    fn write_ready(&mut self) -> Result<bool, Self::Error> {
+        Ok(uart_is_writable(&self.device))
+    }
+}
+
 impl<D: UartDevice, P: ValidUartPinout<D>> Write02<u8> for Writer<D, P> {
     type Error = Infallible;