loconet: improved message validation

server/src/hardware/commandstation/loconetserial.cpp

@@ -186,7 +186,7 @@ void LocoNetSerial::read()
           const Protocol::LocoNet::Message* message = reinterpret_cast<const Protocol::LocoNet::Message*>(pos);
 
           size_t drop = 0;
-          while((message->size() == 0 || (message->size() <= bytesTransferred && !Protocol::LocoNet::isChecksumValid(*message))) && drop < bytesTransferred)
+          while((message->size() == 0 || (message->size() <= bytesTransferred && !Protocol::LocoNet::isValid(*message))) && drop < bytesTransferred)
           {
             drop++;
             pos++;

server/src/hardware/protocol/loconet/loconet.cpp

@@ -78,7 +78,7 @@ bool LocoNet::send(const Message& message)
 {
   if(m_debugLog)
     logDebug("tx: " + to_string(message));
-  assert(isChecksumValid(message));
+  assert(isValid(message));
   return m_send(message);
 }
 
@@ -108,7 +108,7 @@ void LocoNet::receive(const Message& message)
 {
   // NOTE: this function is called async!
 
-  assert(isChecksumValid(message));
+  assert(isValid(message));
 
   if(m_debugLog)
     EventLoop::call([this, log="rx: " + to_string(message)](){ logDebug(log); });

server/src/hardware/protocol/loconet/messages.cpp

@@ -45,6 +45,17 @@ bool isChecksumValid(const Message& message)
   return calcChecksum(message) == reinterpret_cast<const uint8_t*>(&message)[message.size() - 1];
 }
 
+bool isValid(const Message& message)
+{
+  const uint8_t size = message.size();
+  if(size == 0 || !isChecksumValid(message))
+    return false;
+  for(uint8_t i = 1; i < size; i++) // bit 7 must be unset (except opcode)
+    if(reinterpret_cast<const uint8_t*>(&message)[i] & 0x80)
+      return false;
+  return true;
+}
+
 std::string to_string(const Message& message, bool raw)
 {
   std::string s;

server/src/hardware/protocol/loconet/messages.hpp

@@ -40,6 +40,7 @@ struct Message;
 uint8_t calcChecksum(const Message& msmessageg);
 void updateChecksum(Message& message);
 bool isChecksumValid(const Message& message);
+bool isValid(const Message& message);
 std::string to_string(const Message& message, bool raw = false);
 
 constexpr uint8_t SLOT_LOCO_MIN = 1;