From d126c11329c000a8fe312c10575f68e5d4841074 Mon Sep 17 00:00:00 2001
From: Eric Katzfey <eric.katzfey@modalai.com>
Date: Wed, 13 Nov 2024 17:09:16 -0800
Subject: [PATCH] AP_HAL_QURT: Move DSP to Apps Proc link message sequence
 checking into UART driver so it can be per link.

---
 libraries/AP_HAL_QURT/UARTDriver.cpp | 12 +++++++++++-
 libraries/AP_HAL_QURT/UARTDriver.h   |  5 ++++-
 libraries/AP_HAL_QURT/replace.cpp    |  5 -----
 3 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp
index f1a056df10..db91beb602 100644
--- a/libraries/AP_HAL_QURT/UARTDriver.cpp
+++ b/libraries/AP_HAL_QURT/UARTDriver.cpp
@@ -150,9 +150,19 @@ UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(uint8_t instance) : inst(instance)
     register_mavlink_data_callback(instance, _mavlink_data_cb, (void *) this);
 }
 
+void UARTDriver_MAVLinkUDP::check_rx_seq(uint32_t seq)
+{
+	if (seq != rx_seq)
+	{
+		HAP_PRINTF("Sequence mismatch for instance %u. Expected %u, got %u", inst, rx_seq, seq);
+	}
+	rx_seq++;
+}
+
 void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p)
 {
     auto *driver = (UARTDriver_MAVLinkUDP *)p;
+    driver->check_rx_seq(msg->seq);
     driver->_readbuf.write(msg->data, msg->data_length);
 }
 
@@ -183,7 +193,7 @@ bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void)
     }
     msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
     msg.inst = inst;
-    msg.seq = seq++;
+    msg.seq = tx_seq++;
     msg.data_length = _writebuf.read(msg.data, n);
 
     return qurt_rpc_send(msg);
diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h
index d27ef2d6b7..575e27707b 100644
--- a/libraries/AP_HAL_QURT/UARTDriver.h
+++ b/libraries/AP_HAL_QURT/UARTDriver.h
@@ -83,6 +83,8 @@ public:
 
     bool _write_pending_bytes(void) override;
 
+    void check_rx_seq(uint32_t seq);
+
     uint32_t bw_in_bytes_per_second() const override
     {
         return 250000 * 3;
@@ -95,7 +97,8 @@ public:
 private:
     static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p);
     uint8_t inst;
-    uint32_t seq;
+    uint32_t tx_seq;
+    uint32_t rx_seq;
 };
 
 /*
diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp
index 6f331f6a1f..969f697620 100644
--- a/libraries/AP_HAL_QURT/replace.cpp
+++ b/libraries/AP_HAL_QURT/replace.cpp
@@ -151,7 +151,6 @@ typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p)
 
 static mavlink_data_callback_t mav_cb[MAX_MAVLINK_INSTANCES];
 static void *mav_cb_ptr[MAX_MAVLINK_INSTANCES];
-static uint32_t expected_seq;
 
 void register_mavlink_data_callback(uint8_t instance, mavlink_data_callback_t func, void *p)
 {
@@ -172,10 +171,6 @@ int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes)
     if (msg->data_length + QURT_RPC_MSG_HEADER_LEN != data_len_in_bytes) {
         return 0;
     }
-    if (msg->seq != expected_seq) {
-        HAP_PRINTF("Bad sequence %u %u", msg->seq, expected_seq);
-    }
-    expected_seq = msg->seq + 1;
 
     switch (msg->msg_id) {
     case QURT_MSG_ID_MAVLINK_MSG: {