From 315950dbe615de9068f00091a07fab2b6d819abb Mon Sep 17 00:00:00 2001
From: Bob Long <robertlong13@gmail.com>
Date: Thu, 9 Jan 2025 13:59:45 +1100
Subject: [PATCH] SRV_Channel: build bitmasks thread safe

---
 libraries/SRV_Channel/SRV_Channel_aux.cpp | 38 +++++++++++++++++------
 1 file changed, 28 insertions(+), 10 deletions(-)

diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp
index 3fb4e922744d7..189a7cb2c35ea 100644
--- a/libraries/SRV_Channel/SRV_Channel_aux.cpp
+++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp
@@ -211,23 +211,41 @@ void SRV_Channels::update_aux_servo_function(void)
     if (!channels) {
         return;
     }
-    function_mask.clearall();
 
-    for (uint16_t i = 0; i < SRV_Channel::k_nr_aux_servo_functions; i++) {
-        functions[i].channel_mask = 0;
-    }
-    invalid_mask = 0;
-
-    // set auxiliary ranges
+    // We build up new masks and set them at the end so that they are never
+    // in the middle of rebuilding if another thread tries to read them.
+    static Bitmask<SRV_Channel::k_nr_aux_servo_functions> new_function_mask;
+    new_function_mask.clearall();
+    uint32_t new_invalid_mask = 0;
+    
+    // Build up the new function and invalid masks, and set auxiliary ranges
     for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
         if (!channels[i].valid_function()) {
-            invalid_mask |= 1U<<i;
+            new_invalid_mask |= 1U<<i;
             continue;
         }
         const uint16_t function = channels[i].function.get();
         channels[i].aux_servo_function_setup();
-        function_mask.set(function);
-        functions[function].channel_mask |= 1U<<i;
+        new_function_mask.set(function);
+    }
+    function_mask = new_function_mask;
+    invalid_mask = new_invalid_mask;
+
+    // Set the channel masks for each function
+    // This nested loop runs worst case O(NUM_SERVO_CHANNELS^2), but this only
+    // runs once per second.
+    for (uint16_t i = 0; i < SRV_Channel::k_nr_aux_servo_functions; i++) {
+        if (!new_function_mask.get(i)) {
+            functions[i].channel_mask = 0;
+            continue;
+        }
+        SRV_Channel::servo_mask_t new_mask = 0;
+        for (uint8_t j = 0; j < NUM_SERVO_CHANNELS; j++) {
+            if (channels[j].function.get() == i) {
+                new_mask |= 1U<<j;
+            }
+        }
+        functions[i].channel_mask = new_mask;
     }
     initialised = true;
 }