From 76f6514b08d2f3aa3d798173ac57ef7b141b3724 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Tue, 16 Jan 2024 01:55:39 +0100
Subject: [PATCH] making TextIO more robust to bad input

---
 src/comms/streams/PacketCommander.cpp | 25 +++++++++++++++----------
 src/comms/streams/TextIO.cpp          | 20 ++++++++++++++------
 2 files changed, 29 insertions(+), 16 deletions(-)

diff --git a/src/comms/streams/PacketCommander.cpp b/src/comms/streams/PacketCommander.cpp
index 6c2a0d7..a8e1606 100644
--- a/src/comms/streams/PacketCommander.cpp
+++ b/src/comms/streams/PacketCommander.cpp
@@ -39,6 +39,10 @@ void PacketCommander::run() {
             handleRegisterPacket(!_io->is_complete(), curRegister);
             lastcommanderror = commanderror;
             lastcommandregister = curRegister;
+            if (commanderror) {
+                _io->in_sync = false;
+                //*_io << START_PACKET(PacketType::ALERT, 1) << '?' << END_PACKET;
+            }
         }
         else if (curr_packet.type == PacketType::SYNC) {            
             *_io << START_PACKET(PacketType::SYNC, 1);
@@ -46,8 +50,9 @@ void PacketCommander::run() {
             *_io << END_PACKET;
             // TODO flush packet
         }
-        else
-            _io->in_sync = false; // TODO flag in another way?
+        else {
+            _io->in_sync = false; // TODO it would be better just to reset the buffer, since we just saw a newline
+        }
 
         if (! _io->is_complete())
             _io->in_sync = false;
@@ -55,10 +60,11 @@ void PacketCommander::run() {
 };
 
 
+
 void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) {
     if (write) {
         bool ok = commsToRegister(reg);
-        commanderror = commanderror && !ok;
+        commanderror = commanderror || !ok;
     }
     if (!write || echo) {
         uint8_t size = SimpleFOCRegisters::regs->sizeOfRegister(reg);
@@ -70,7 +76,7 @@ void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) {
             // TODO flush packet
         }
     }
-}
+};
 
 
 
@@ -87,23 +93,22 @@ bool PacketCommander::commsToRegister(uint8_t reg){
         default:
             return SimpleFOCRegisters::regs->commsToRegister(*_io, reg, motors[curMotor]);
     }
-}
+};
 
 
 
 bool PacketCommander::registerToComms(uint8_t reg){
     switch (reg) {
-        case REG_STATUS:
+        case SimpleFOCRegister::REG_STATUS:
             // TODO implement status register
             return true;
-        case REG_MOTOR_ADDRESS:
+        case SimpleFOCRegister::REG_MOTOR_ADDRESS:
             *_io << curMotor;
             return true;
-        case REG_NUM_MOTORS:
+        case SimpleFOCRegister::REG_NUM_MOTORS:
             *_io << numMotors;
             return true;
         default:
             return SimpleFOCRegisters::regs->registerToComms(*_io, reg, motors[curMotor]);
     }
-}
-
+};
diff --git a/src/comms/streams/TextIO.cpp b/src/comms/streams/TextIO.cpp
index 22cb644..afd3bee 100644
--- a/src/comms/streams/TextIO.cpp
+++ b/src/comms/streams/TextIO.cpp
@@ -90,7 +90,7 @@ uint32_t TextIO::intFromBuffer() {
         }
     }
     return value;
-}
+};
 
 
 TextIO& TextIO::operator>>(float &value) {
@@ -150,7 +150,8 @@ TextIO& TextIO::operator>>(uint8_t &value) {
 
 TextIO& TextIO::operator>>(Packet &value) {
     while (!in_sync && _io.available() > 0) {
-        if (_io.read() == '\n') {
+        char skip = _io.read();
+        if (skip == '\n' || skip == '\r') {
             in_sync = true;
             buffer_len = 0;
         }
@@ -159,13 +160,16 @@ TextIO& TextIO::operator>>(Packet &value) {
         buffer_len = 0;
         buffer_index = 0;
     }
-    while (in_sync && _io.available()>0) {
+    int avail = _io.available();
+    while (in_sync && avail>0) {
         uint8_t peek = _io.peek();
         if (peek == '\n' || peek == '\r') {            
             // skip newlines and carriage returns
-            while (_io.available()>0 && (peek == '\n' || peek == '\r')) {
+            while (avail>0 && (peek == '\n' || peek == '\r')) {
                 _io.read(); // discard the \n
-                peek = _io.peek();
+                avail = _io.available();
+                if (avail>0)
+                    peek = _io.peek();
             }
             if (buffer_len>1) {
                 value.type = buffer[0];
@@ -174,10 +178,14 @@ TextIO& TextIO::operator>>(Packet &value) {
                 buffer_index = 1;
                 return *this;
             }
+            else
+                buffer_len = 0;
         }
         else {
-            if (buffer_len < SIMPLEFOC_TEXTIO_BUFFER_SIZE)
+            if (buffer_len < SIMPLEFOC_TEXTIO_BUFFER_SIZE) {
                 buffer[buffer_len++] = _io.read();
+                avail = _io.available();
+            }
             else {
                 buffer_len = 0;
                 in_sync = false;