AP_HAL_ChibiOS: ensure that serial LED setup goes via an intermediate buffer

protect serial LED buffer transfers with mutex
protect serial LED buffer size with mutex
prevent serial LED interactions before rcout is ready
allocate a serial led buffer per channel that needs it
setup clock mask when needed

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -106,6 +106,8 @@ void RCOutput::init()
     hal.gpio->pinMode(56, 1);
     hal.gpio->pinMode(57, 1);
 #endif
+
+    _initialised = true;
 }
 
 /*
@@ -664,11 +666,11 @@ void RCOutput::set_group_mode(pwm_group &group)
         const uint8_t channels_per_group = 4;
         const uint16_t bit_length = bits_per_pixel * channels_per_group * group.serial_nleds + (pad_start_bits + pad_end_bits) * channels_per_group;
         const uint16_t buffer_length = bit_length * sizeof(uint32_t);
+
         if (!setup_group_DMA(group, rate, bit_period, active_high, buffer_length, true)) {
             group.current_mode = MODE_PWM_NONE;
             break;
         }
-
         // calculate min time between pulses
         group.dshot_pulse_time_us = 1000000UL * bit_length / rate;
         break;
@@ -931,23 +933,22 @@ void RCOutput::timer_tick(void)
     // like low rates
     if (!serial_group) {
         dshot_send_groups(true);
-    }
+    } else {
-
-    if (serial_led_pending && chMtxTryLock(&trigger_mutex)) {
-        serial_led_pending = false;
-        for (auto &group : pwm_group_list) {
-            if (group.serial_led_pending && (group.current_mode == MODE_NEOPIXEL || group.current_mode == MODE_PROFILED)) {
-                group.serial_led_pending = !serial_led_send(group);
-                group.prepared_send = group.serial_led_pending;
-                serial_led_pending |=  group.serial_led_pending;
-            }
-        }
-        chMtxUnlock(&trigger_mutex);
-    }
-    if (min_pulse_trigger_us == 0 ||
-        serial_group != nullptr) {
         return;
     }
+
+    // if we have enough time left send out LED data
+    if (serial_led_pending) {
+        serial_led_pending = false;
+        for (auto &group : pwm_group_list) {
+            serial_led_pending |= !serial_led_send(group);
+        }
+    }
+
+    if (min_pulse_trigger_us == 0) {
+        return;
+    }
+
     if (now > min_pulse_trigger_us &&
         now - min_pulse_trigger_us > 4000) {
         // trigger at a minimum of 250Hz
@@ -1183,12 +1184,27 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
  */
 bool RCOutput::serial_led_send(pwm_group &group)
 {
+    if (!group.serial_led_pending
+        || (group.current_mode != MODE_NEOPIXEL && group.current_mode != MODE_PROFILED)) {
+        return true;
+    }
+
 #ifndef DISABLE_DSHOT
     if (irq.waiter || !group.dma_handle->lock_nonblock()) {
         // doing serial output, don't send Serial LED pulses
         return false;
     }
 
+    {
+        WITH_SEMAPHORE(group.serial_led_mutex);
+
+        group.serial_led_pending = false;
+        group.prepared_send = false;
+
+        // fill the DMA buffer while we have the lock
+        fill_DMA_buffer_serial_led(group);
+    }
+
     // start sending the pulses out
     send_pulses_DMAR(group, group.dma_buffer_len);
 
@@ -1781,20 +1797,33 @@ uint32_t RCOutput::protocol_bitrate(const enum output_mode mode)
 */
 bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode, uint16_t clock_mask)
 {
-    uint8_t i;
+    if (!_initialised) {
+        return false;
+    }
+
+    uint8_t i = 0;
     pwm_group *grp = find_chan(chan, i);
     if (!grp) {
         return false;
     }
 
+    // we must hold the LED mutex while resizing the array
+    WITH_SEMAPHORE(grp->serial_led_mutex);
+    // nothing is as nothing does
+    if (grp->serial_nleds == num_leds
+        && mode == grp->current_mode
+        && grp->serial_led_data[i] != nullptr) {
+        return true;
+    }
+
     switch (mode) {
         case MODE_NEOPIXEL: {
-            grp->serial_nleds = MAX(num_leds, grp->serial_nleds);
+            num_leds = MAX(num_leds, grp->serial_nleds);
             break;
         }
         case MODE_PROFILED: {
             // ProfiLED requires two dummy LED's to mark end of transmission
-            grp->serial_nleds = MAX(num_leds + 2, grp->serial_nleds);
+            num_leds = MAX(num_leds + 2, grp->serial_nleds);
 
             // Enable any clock channels in the same group
             grp->clock_mask = 0;
@@ -1806,9 +1835,24 @@ bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, ou
 
             break;
         }
-        default: {
+        default:
             return false;
     }
+
+    // allocate the data storage array
+    if (grp->serial_nleds != num_leds || grp->serial_led_data[i] == nullptr) {
+        if (grp->serial_led_data[i] != nullptr) {
+            delete[] grp->serial_led_data[i];
+            grp->serial_led_data[i] = nullptr;
+        }
+        if (num_leds > 0) {
+            grp->serial_led_data[i] = new SerialLed[num_leds];
+            if (grp->serial_led_data[i] == nullptr) {
+                num_leds = 0;
+            }
+        }
+
+        grp->serial_nleds = num_leds;
     }
 
     set_output_mode(1U<<chan, mode);
@@ -1816,6 +1860,43 @@ bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, ou
     return grp->current_mode == mode;
 }
 
+#pragma GCC push_options
+#pragma GCC optimize("O2")
+// Fill the group DMA buffer with data to be output
+void RCOutput::fill_DMA_buffer_serial_led(pwm_group& group)
+{
+    memset(group.dma_buffer, 0, group.dma_buffer_len);
+    for (uint8_t j = 0; j < 4; j++) {
+        if (group.serial_led_data[j] == nullptr) {
+            if (group.current_mode == MODE_PROFILED && (group.clock_mask & 1U<<j) != 0) {
+                for (uint8_t i = 0; i < group.serial_nleds; i++) {
+                    _set_profiled_clock(&group, j, i);
+                }
+            }
+            continue;
+        }
+
+        for (uint8_t i = 0; i < group.serial_nleds; i++) {
+            const SerialLed& led = group.serial_led_data[j][i];
+            switch (group.current_mode) {
+                case MODE_NEOPIXEL:
+                    _set_neopixel_rgb_data(&group, j, i, led.red, led.green, led.blue);
+                    break;
+                case MODE_PROFILED: {
+                    if (i < group.serial_nleds - 2) {
+                        _set_profiled_rgb_data(&group, j, i, led.red, led.green, led.blue);
+                    } else {
+                        _set_profiled_blank_frame(&group, j, i);
+                    }
+                    break;
+                }
+                default:
+                    break;
+            }
+        }
+    }
+}
+
 /*
   setup neopixel (WS2812B) output data for a given output channel
   and a LED number. LED -1 is all LEDs
@@ -1883,6 +1964,7 @@ void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led)
         buf[b * stride] = BIT_1;
     }
 }
+#pragma GCC pop_options
 
 /*
   setup serial LED output data for a given output channel
@@ -1890,52 +1972,55 @@ void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led)
 */
 void RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue)
 {
-    uint8_t i;
+    if (!_initialised) {
-    pwm_group *grp = find_chan(chan, i);
-    if (!grp) {
         return;
     }
 
-    if (led >= grp->serial_nleds || (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED)) {
+    uint8_t i = 0;
+    pwm_group *grp = find_chan(chan, i);
+
+    WITH_SEMAPHORE(grp->serial_led_mutex);
+
+    if (!grp || grp->serial_nleds == 0) {
         return;
     }
 
     if (led == -1) {
         grp->prepared_send = true;
         for (uint8_t n=0; n<grp->serial_nleds; n++) {
-            set_serial_led_rgb_data(chan, n, red, green, blue);
+            serial_led_set_single_rgb_data(*grp, i, n, red, green, blue);
         }
         return;
-    } else if (!grp->prepared_send) {
+    }
+
     // if not ouput clock and trailing frames, run through all LED's to do it now
-        set_serial_led_rgb_data(chan, -1, 0, 0, 0);
+    if (!grp->prepared_send) {
+        for (uint8_t n=0; n<grp->serial_nleds; n++) {
+            serial_led_set_single_rgb_data(*grp, i, n, 0, 0, 0);
         }
+    }
+    serial_led_set_single_rgb_data(*grp, i, uint8_t(led), red, green, blue);
+}
 
-    switch (grp->current_mode) {
+/*
-        case MODE_NEOPIXEL: {
+  setup serial LED output data for a given output channel
-            _set_neopixel_rgb_data(grp, i, uint8_t(led), red, green, blue);
+  and a LED number. LED -1 is all LEDs
-            break;
+*/
-        }
+void RCOutput::serial_led_set_single_rgb_data(pwm_group& group, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue)
-
+{
-        case MODE_PROFILED: {
+    if (led >= group.serial_nleds || (group.current_mode != MODE_NEOPIXEL && group.current_mode != MODE_PROFILED)) {
-            if (led < grp->serial_nleds - 2) {
-                _set_profiled_rgb_data(grp, i, uint8_t(led), red, green, blue);
-            } else {
-                _set_profiled_blank_frame(grp, i, uint8_t(led));
-            }
-
-            for (uint8_t j = 0; j < 4; j++) {
-                if ((grp->clock_mask & 1U<<j) != 0) {
-                   _set_profiled_clock(grp, j, uint8_t(led));
-                }
-            }
-
-            break;
-        }
-
-        default: {
         return;
     }
+
+    switch (group.current_mode) {
+        case MODE_PROFILED:
+        case MODE_NEOPIXEL:
+            group.serial_led_data[idx][led].red = red;
+            group.serial_led_data[idx][led].green = green;
+            group.serial_led_data[idx][led].blue = blue;
+            break;
+        default:
+            break;
     }
 }
 
@@ -1944,17 +2029,25 @@ void RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t
 */
 void RCOutput::serial_led_send(const uint16_t chan)
 {
+    if (!_initialised) {
+        return;
+    }
+
     uint8_t i;
     pwm_group *grp = find_chan(chan, i);
     if (!grp) {
         return;
     }
-    if (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED) {
+
+    WITH_SEMAPHORE(grp->serial_led_mutex);
+
+    if (grp->serial_nleds == 0 || (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED)) {
         return;
     }
+
     if (grp->prepared_send) {
-        serial_led_pending = true; 
         grp->serial_led_pending = true;
+        serial_led_pending = true;
     }
 }
 

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -17,6 +17,7 @@
 #pragma once
 
 #include "AP_HAL_ChibiOS.h"
+#include <AP_HAL/Semaphores.h>
 #include "shared_dma.h"
 #include "ch.h"
 #include "hal.h"
@@ -199,6 +200,12 @@ private:
       RECV_COMPLETE = 4
     };
 
+    struct PACKED SerialLed {
+      uint8_t red;
+      uint8_t green;
+      uint8_t blue;
+    };
+
     /*
       DShot handling
      */
@@ -248,10 +255,16 @@ private:
         uint32_t dshot_pulse_time_us;
         uint32_t dshot_pulse_send_time_us;
         virtual_timer_t dma_timeout;
-        uint8_t serial_nleds;
+
+        // serial LED support
+        volatile uint8_t serial_nleds;
         uint8_t clock_mask;
-        bool serial_led_pending;
+        volatile bool serial_led_pending;
-        bool prepared_send;
+        volatile bool prepared_send;
+        HAL_Semaphore serial_led_mutex;
+        // structure to hold serial LED data until it can be transferred
+        // to the DMA buffer
+        SerialLed* serial_led_data[4];
 
         // serial output
         struct {
@@ -412,6 +425,8 @@ private:
     // iomcu output mode (pwm, oneshot or oneshot125)
     enum output_mode iomcu_mode = MODE_PWM_NORMAL;
 
+    volatile bool _initialised;
+
     bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; }
 
     // find a channel group given a channel number
@@ -446,7 +461,9 @@ private:
       return true if send was successful
     */
     bool serial_led_send(pwm_group &group);
-    bool serial_led_pending;
+    void serial_led_set_single_rgb_data(pwm_group& group, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue);
+    void fill_DMA_buffer_serial_led(pwm_group& group);
+    volatile bool serial_led_pending;
 
     void dma_allocate(Shared_DMA *ctx);
     void dma_deallocate(Shared_DMA *ctx);