From 031980c42f2aeec4ab70c807b3dc880f64588d13 Mon Sep 17 00:00:00 2001
From: Jan Niehusmann <jan@gondor.com>
Date: Wed, 1 May 2024 12:09:21 +0000
Subject: [PATCH] Avoid discarding already read bytes on error

Closes: #782
---
 rp2040-hal/src/uart/peripheral.rs | 23 ++++++++++++++++++++++-
 rp2040-hal/src/uart/reader.rs     | 17 ++++++++++++++++-
 2 files changed, 38 insertions(+), 2 deletions(-)

diff --git a/rp2040-hal/src/uart/peripheral.rs b/rp2040-hal/src/uart/peripheral.rs
index 160c56bc1..70a9d0b6d 100644
--- a/rp2040-hal/src/uart/peripheral.rs
+++ b/rp2040-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,
         })
     }
 }
@@ -245,6 +249,7 @@ impl<D: UartDevice, P: ValidUartPinout<D>> UartPeripheral<Enabled, D, P> {
             device: reader.device,
             _state: Enabled,
             pins: reader.pins,
+            read_error: reader.read_error,
         }
     }
 }
@@ -255,6 +260,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 };
@@ -273,6 +279,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 };
@@ -464,7 +471,21 @@ 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::Write for UartPeripheral<Enabled, D, P> {
diff --git a/rp2040-hal/src/uart/reader.rs b/rp2040-hal/src/uart/reader.rs
index f1d3692c9..c02e37abb 100644
--- a/rp2040-hal/src/uart/reader.rs
+++ b/rp2040-hal/src/uart/reader.rs
@@ -187,6 +187,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> {
@@ -224,7 +225,21 @@ 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),
+        }
     }
 }