From fc2ff3585622245583bb2b0f5a90a1a64741cf2a Mon Sep 17 00:00:00 2001
From: Filippo Gentile <filippogentile@disroot.org>
Date: Sat, 6 Jul 2024 14:27:50 +0200
Subject: [PATCH] server: Z21 reply queue, use encoded speed and steps

- To save space we use the same encoding as done
  in Z21 protocol.
- This way we also can directly compare without decoding
- Now also direction and emergency stop are checked
---
 .../hardware/protocol/z21/clientkernel.cpp    |  5 ++--
 server/src/hardware/protocol/z21/messages.cpp |  3 +-
 server/src/hardware/protocol/z21/messages.hpp | 29 ++++++++++++++++++-
 3 files changed, 33 insertions(+), 4 deletions(-)

diff --git a/server/src/hardware/protocol/z21/clientkernel.cpp b/server/src/hardware/protocol/z21/clientkernel.cpp
index d05d83aa..ce2f27c4 100644
--- a/server/src/hardware/protocol/z21/clientkernel.cpp
+++ b/server/src/hardware/protocol/z21/clientkernel.cpp
@@ -1047,9 +1047,10 @@ std::optional<ClientKernel::PendingRequest> ClientKernel::matchPendingReplyAndRe
       if(message.header() == LAN_X
           && static_cast<const LanX&>(message).xheader == LAN_X_LOCO_INFO)
       {
-        // TODO: Do extensive matching
         const LanXLocoInfo& locoInfo = static_cast<const LanXLocoInfo&>(message);
-        if(locoInfo.speedStep() != request->reply.speedStep)
+        if(locoInfo.speedAndDirection != request->reply.speedAndDirection)
+          continue;
+        if(locoInfo.speedSteps() != request->reply.speedSteps())
           continue;
       }
     }
diff --git a/server/src/hardware/protocol/z21/messages.cpp b/server/src/hardware/protocol/z21/messages.cpp
index 100b4512..93ce8cec 100644
--- a/server/src/hardware/protocol/z21/messages.cpp
+++ b/server/src/hardware/protocol/z21/messages.cpp
@@ -496,7 +496,8 @@ MessageReplyType getReplyType(const Message &message)
                 setLocoDrive.db0 >= 0x10 && setLocoDrive.db0 <= 0x13)
             {
               reply.address = setLocoDrive.address();
-              reply.speedStep = setLocoDrive.speedStep();
+              reply.speedAndDirection = setLocoDrive.speedAndDirection;
+              reply.setSpeedSteps(setLocoDrive.speedSteps());
               reply.setFlag(MessageReplyType::Flags::CheckSpeedStep);
             }
             else if(const auto& setLocoFunction = static_cast<const LanXSetLocoFunction&>(message);
diff --git a/server/src/hardware/protocol/z21/messages.hpp b/server/src/hardware/protocol/z21/messages.hpp
index ace42a62..2f6b8522 100644
--- a/server/src/hardware/protocol/z21/messages.hpp
+++ b/server/src/hardware/protocol/z21/messages.hpp
@@ -1749,6 +1749,30 @@ struct MessageReplyType
     m_flags = uint8_t(flags) << 4 | (uint8_t(priority()) & 0xF);
   }
 
+  inline uint8_t speedSteps() const
+  {
+    switch(speedStepsEncoded & LanXLocoInfo::db2_speed_steps_mask)
+    {
+      case LanXLocoInfo::db2_speed_steps_14:  return 14;
+      case LanXLocoInfo::db2_speed_steps_28:  return 28;
+      case LanXLocoInfo::db2_speed_steps_128: return 126;
+    }
+    return 0;
+  }
+
+  inline void setSpeedSteps(uint8_t value)
+  {
+    speedStepsEncoded &= ~LanXLocoInfo::db2_speed_steps_mask;
+    switch(value)
+    {
+      case 14:  speedStepsEncoded |= LanXLocoInfo::db2_speed_steps_14;  break;
+      case 28:  speedStepsEncoded |= LanXLocoInfo::db2_speed_steps_28;  break;
+      case 126:
+      case 128:
+      default:  speedStepsEncoded |= LanXLocoInfo::db2_speed_steps_128; break;
+    }
+  }
+
   static constexpr Header noReply = Header(0);
 
   Header header = noReply;
@@ -1756,7 +1780,10 @@ struct MessageReplyType
   uint8_t db0 = 0;
   uint16_t address = 0;
   uint8_t m_flags = uint8_t(Priority::Normal);
-  uint8_t speedStep = 0;
+
+  // Encoded as LAN_X_
+  uint8_t speedAndDirection = 0;
+  uint8_t speedStepsEncoded = 0;
 };
 
 MessageReplyType getReplyType(const Message &message);