From e63a2250cac1114376dd901f8acf3962cae6ca6d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 22 Nov 2023 11:33:33 +1100
Subject: [PATCH] AP_SerialManager: ensure registered ports are in sorted order

this avoids ordering issues if using both CAN and network serial ports
---
 .../AP_SerialManager/AP_SerialManager.cpp     | 20 +++++++++++++++++--
 libraries/AP_SerialManager/AP_SerialManager.h |  1 +
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp
index 382162db78..38b8124aaf 100644
--- a/libraries/AP_SerialManager/AP_SerialManager.cpp
+++ b/libraries/AP_SerialManager/AP_SerialManager.cpp
@@ -850,8 +850,24 @@ void AP_SerialManager::set_protocol_and_baud(uint8_t sernum, enum SerialProtocol
  */
 void AP_SerialManager::register_port(RegisteredPort *port)
 {
-    port->next = registered_ports;
-    registered_ports = port;
+    const auto idx = port->state.idx;
+    WITH_SEMAPHORE(port_sem);
+    /*
+      maintain the list in ID order
+     */
+    if (registered_ports == nullptr ||
+        registered_ports->state.idx >= idx) {
+        port->next = registered_ports;
+        registered_ports = port;
+        return;
+    }
+    for (auto p = registered_ports; p; p = p->next) {
+        if (p->next == nullptr || p->next->state.idx >= idx) {
+            port->next = p->next;
+            p->next = port;
+            break;
+        }
+    }
 }
 #endif // AP_SERIALMANAGER_REGISTER_ENABLED
 
diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h
index f106e79549..01d5d7bbd4 100644
--- a/libraries/AP_SerialManager/AP_SerialManager.h
+++ b/libraries/AP_SerialManager/AP_SerialManager.h
@@ -172,6 +172,7 @@ public:
         UARTState state;
     };
     RegisteredPort *registered_ports;
+    HAL_Semaphore port_sem;
 
     // register an externally managed port
     void register_port(RegisteredPort *port);