diff --git a/spinnyboy_rust/src/bin/peripherals/nextion.rs b/spinnyboy_rust/src/bin/peripherals/nextion.rs
index f6babf4..b6ae4ad 100644
--- a/spinnyboy_rust/src/bin/peripherals/nextion.rs
+++ b/spinnyboy_rust/src/bin/peripherals/nextion.rs
@@ -12,14 +12,14 @@ enum UART_STATEMACHINE {
 pub struct Nextion<'a> {
     interface: &'a mut Uart<'static, Blocking>,
     state: UART_STATEMACHINE,
-    idx: usize
+    idx: usize,
 }
 impl<'a> Nextion<'a> {
     pub fn new<'b>(uart: &'a mut Uart<'static, Blocking>) -> Self {
         Nextion {
             interface: uart,
             state: UART_STATEMACHINE::WaitingP,
-            idx:0,
+            idx: 0,
         }
     }
     pub fn send_command(&mut self, cmd: &[u8]) {
@@ -61,7 +61,7 @@ impl<'a> Nextion<'a> {
                             self.idx += 1;
                         } else {
                             self.reset();
-                           return  Err(RxError::FifoOverflowed);
+                            return Err(RxError::FifoOverflowed);
                         }
                     }
                 }
@@ -74,19 +74,29 @@ impl<'a> Nextion<'a> {
                         } else {
                             self.state = UART_STATEMACHINE::Terminator(n + 1)
                         }
-                    }
-                    self.state = UART_STATEMACHINE::Reading;
-                    if self.idx < buf.len() {
+                    } else {
+                        self.state = UART_STATEMACHINE::Reading;
+
+                        let needed: usize = (n + 1).into(); // number of bytes to reinsert (FFs + current byte)
+
+                        if self.idx + needed > buf.len() {
+                            self.reset();
+                            return Err(RxError::FifoOverflowed);
+                        }
+
+                        for _ in 0..n {
+                            buf[self.idx] = 0xff;
+                            self.idx += 1;
+                        }
+
                         buf[self.idx] = byte;
                         self.idx += 1;
-                    } else {
-                        self.reset();
-                        return Err(RxError::FifoOverflowed);
                     }
                 }
             }
         }
     }
+
     pub fn read_ready(&mut self) -> bool {
         self.interface.read_ready()
     }