From a7e7629dabd1b0c47627f1f34c04b580e5fe8d59 Mon Sep 17 00:00:00 2001
From: Adam McQuilkin <46639306+ajmcquilkin@users.noreply.github.com>
Date: Fri, 15 Mar 2024 14:08:55 -0700
Subject: [PATCH] Added new helper function to shift buffer to first valid
 packet

---
 src/connections/stream_buffer.rs | 34 +++++++++++++++++++-------------
 1 file changed, 20 insertions(+), 14 deletions(-)

diff --git a/src/connections/stream_buffer.rs b/src/connections/stream_buffer.rs
index 1e73cfe..70d0191 100644
--- a/src/connections/stream_buffer.rs
+++ b/src/connections/stream_buffer.rs
@@ -121,6 +121,24 @@ impl StreamBuffer {
             });
         }
 
+        let framing_index = self.shift_buffer_to_first_valid_packet()?;
+
+        // Note: the framing index should always be 0 at this point, keeping for clarity
+        let incoming_packet_data_size = self.extract_data_size_from_header(framing_index)?;
+
+        self.validate_packet_in_buffer(incoming_packet_data_size, framing_index)?;
+
+        // Get packet data, excluding magic bytes
+        let packet_data =
+            self.extract_packet_from_buffer(incoming_packet_data_size, framing_index)?;
+
+        // Attempt to decode the current packet
+        let decoded_packet = protobufs::FromRadio::decode(packet_data.as_slice())?;
+
+        Ok(decoded_packet)
+    }
+
+    fn shift_buffer_to_first_valid_packet(&mut self) -> Result<usize, StreamBufferError> {
         let mut framing_index = self.get_framing_index()?;
 
         // Drop beginning of buffer if the framing byte is found later in the buffer.
@@ -143,19 +161,7 @@ impl StreamBuffer {
             framing_index = self.get_framing_index()?;
         }
 
-        // Note: the framing index should always be 0 at this point, keeping for clarity
-        let incoming_packet_data_size = self.extract_data_size_from_header(framing_index)?;
-
-        self.validate_packet_in_buffer(incoming_packet_data_size, framing_index)?;
-
-        // Get packet data, excluding magic bytes
-        let packet_data =
-            self.extract_packet_from_buffer(incoming_packet_data_size, framing_index)?;
-
-        // Attempt to decode the current packet
-        let decoded_packet = protobufs::FromRadio::decode(packet_data.as_slice())?;
-
-        Ok(decoded_packet)
+        Ok(framing_index)
     }
 
     // All valid packets start with the sequence [0x94 0xc3 size_msb size_lsb], where
@@ -191,7 +197,7 @@ impl StreamBuffer {
         // Check that the framing byte is correct, and fail if not
         if framing_byte != 0xc3 {
             warn!("Framing byte {} not equal to 0xc3", framing_byte);
-            self.get_framing_index()?; // Purge buffer to next packet
+            self.shift_buffer_to_first_valid_packet()?;
             return Err(StreamBufferError::IncorrectFramingByte { framing_byte });
         }