SRV_Channels: add mask semaphore

ArduPilot · Jan 8, 2025 · 26cbcd0 · 26cbcd0
1 parent 20c77ae
commit 26cbcd0
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 0 deletions.
diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h
@@ -605,6 +605,12 @@ class SRV_Channels {
     static Bitmask<SRV_Channel::k_nr_aux_servo_functions> function_mask;
     static bool initialised;
 
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+    // Prevents access to function_mask, functions[].channel_mask, and invalid_mask while they are being rebuilt
+    // (the race cannot happen on ChibiOS because the rebuild happens on the main thread and no callers can preempt it)
+    static HAL_Semaphore mask_semaphore;
+#endif
+
     // this static arrangement is to avoid having static objects in AP_Param tables
     static SRV_Channel *channels;
     static SRV_Channels *_singleton;

diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp
@@ -211,6 +211,10 @@ void SRV_Channels::update_aux_servo_function(void)
     if (!channels) {
         return;
     }
+
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+    WITH_SEMAPHORE(mask_semaphore);
+#endif
     function_mask.clearall();
 
     for (uint16_t i = 0; i < SRV_Channel::k_nr_aux_servo_functions; i++) {
@@ -506,6 +510,9 @@ SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t function)
     if (!initialised) {
         update_aux_servo_function();
     }
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+    WITH_SEMAPHORE(mask_semaphore);
+#endif
     return function_mask.get(uint16_t(function));
 }
 
@@ -556,6 +563,9 @@ bool SRV_Channels::set_aux_channel_default(SRV_Channel::Aux_servo_function_t fun
     channels[channel].type_setup = false;
     channels[channel].function.set_and_default(function);
     channels[channel].aux_servo_function_setup();
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+    WITH_SEMAPHORE(mask_semaphore);
+#endif
     function_mask.set((uint16_t)function);
     if (SRV_Channel::valid_function(function)) {
         functions[function].channel_mask |= 1U<<channel;
@@ -577,6 +587,9 @@ bool SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t function, uint
     }
 
     // Get the index of the first set bit, returns 0 if no bits are set
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+    WITH_SEMAPHORE(mask_semaphore);
+#endif
     const int first_chan = __builtin_ffs(functions[function].channel_mask);
     if (first_chan == 0) {
         return false;
@@ -603,6 +616,9 @@ void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function,
 {
     if (SRV_Channel::valid_function(function)) {
         functions[function].output_scaled = value;
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+        WITH_SEMAPHORE(mask_semaphore);
+#endif
         SRV_Channel::have_pwm_mask &= ~functions[function].channel_mask;
     }
 }
@@ -642,6 +658,9 @@ uint32_t SRV_Channels::get_output_channel_mask(SRV_Channel::Aux_servo_function_t
     if (!initialised) {
         update_aux_servo_function();
     }
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+    WITH_SEMAPHORE(mask_semaphore);
+#endif
     if (SRV_Channel::valid_function(function)) {
         return functions[function].channel_mask;
     }
@@ -678,6 +697,9 @@ void SRV_Channels::set_default_function(uint8_t chan, SRV_Channel::Aux_servo_fun
         const SRV_Channel::Aux_servo_function_t old = channels[chan].function;
         channels[chan].function.set_default(function);
         if (old != channels[chan].function && channels[chan].function == function) {
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+            WITH_SEMAPHORE(mask_semaphore);
+#endif
             function_mask.set((uint16_t)function);
         }
     }

diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp
@@ -52,6 +52,9 @@ bool SRV_Channels::disabled_passthrough;
 bool SRV_Channels::initialised;
 bool SRV_Channels::emergency_stop;
 Bitmask<SRV_Channel::k_nr_aux_servo_functions> SRV_Channels::function_mask;
+#if CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+HAL_Semaphore SRV_Channels::mask_semaphore;
+#endif
 SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions];
 SRV_Channels::slew_list *SRV_Channels::_slew;