From f884535c04af30a9a2273755a42b263711f462f6 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 25 Oct 2023 14:41:55 +0100
Subject: [PATCH] AP_HAL_ChibiOS: cache values of io_dshot() and io_enabled()

enabled shared_up_dma to be fully compiled out
address some minor review comments
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp         | 44 ++++++++++---------
 libraries/AP_HAL_ChibiOS/RCOutput.h           |  9 ++++
 libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp  | 12 +++--
 libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp  |  2 +-
 .../hwdef/iomcu-f103-dshot/hwdef.dat          |  3 +-
 .../hwdef/scripts/chibios_hwdef.py            | 18 +++++++-
 6 files changed, 61 insertions(+), 27 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index d1f4366e994b22..365944433494fc 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -100,6 +100,10 @@ void RCOutput::init()
     if (AP_BoardConfig::io_enabled()) {
         // with IOMCU the local (FMU) channels start at 8
         chan_offset = 8;
+        iomcu_enabled = true;
+    }
+    if (AP_BoardConfig::io_dshot()) {
+        iomcu_dshot = true;
     }
 #endif
 
@@ -144,7 +148,7 @@ void RCOutput::init()
     }
 
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         iomcu.init();
     }
 #endif
@@ -442,7 +446,7 @@ void RCOutput::set_freq_group(pwm_group &group)
 void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
 {
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         // change frequency on IOMCU
         uint16_t io_chmask = chmask & 0xFF;
         if (io_chmask) {
@@ -499,7 +503,7 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
 void RCOutput::set_default_rate(uint16_t freq_hz)
 {
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         iomcu.set_default_rate(freq_hz);
     }
 #endif
@@ -526,7 +530,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
         _dshot_period_us = 1000UL;
         _dshot_rate = 0;
 #if HAL_WITH_IO_MCU
-        if (AP_BoardConfig::io_dshot()) {
+        if (iomcu_dshot) {
             iomcu.set_dshot_period(1000UL, 0);
         }
 #endif
@@ -542,7 +546,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
 #if HAL_WITH_IO_MCU
             // this is not strictly neccessary since the iomcu could run at a different rate,
             // but there is only one parameter to control this
-            if (AP_BoardConfig::io_dshot()) {
+            if (iomcu_dshot) {
                 iomcu.set_dshot_period(1000UL, 0);
             }
 #endif
@@ -566,7 +570,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
     }
     _dshot_period_us = 1000000UL / drate;
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_dshot()) {
+    if (iomcu_dshot) {
         iomcu.set_dshot_period(_dshot_period_us, _dshot_rate);
     }
 #endif
@@ -593,7 +597,7 @@ void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type)
             break;
     }
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_dshot()) {
+    if (iomcu_dshot) {
         iomcu.set_dshot_esc_type(dshot_esc_type);
     }
 #endif
@@ -670,7 +674,7 @@ uint16_t RCOutput::get_freq(uint8_t chan)
 void RCOutput::enable_ch(uint8_t chan)
 {
 #if HAL_WITH_IO_MCU
-    if (chan < chan_offset && AP_BoardConfig::io_enabled()) {
+    if (chan < chan_offset && iomcu_enabled) {
         iomcu.enable_ch(chan);
         return;
     }
@@ -686,7 +690,7 @@ void RCOutput::enable_ch(uint8_t chan)
 void RCOutput::disable_ch(uint8_t chan)
 {
 #if HAL_WITH_IO_MCU
-    if (chan < chan_offset && AP_BoardConfig::io_enabled()) {
+    if (chan < chan_offset && iomcu_enabled) {
         iomcu.disable_ch(chan);
         return;
     }
@@ -717,7 +721,7 @@ void RCOutput::write(uint8_t chan, uint16_t period_us)
 
 #if HAL_WITH_IO_MCU
     // handle IO MCU channels
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         iomcu.write_channel(chan, period_us);
     }
 #endif
@@ -1196,7 +1200,7 @@ void RCOutput::set_output_mode(uint32_t mask, const enum output_mode mode)
          mode == MODE_PWM_BRUSHED ||
          (mode >= MODE_PWM_DSHOT150 && mode <= MODE_PWM_DSHOT600)) &&
         iomcu_mask &&
-        AP_BoardConfig::io_enabled()) {
+        iomcu_enabled) {
         iomcu.set_output_mode(iomcu_mask, mode);
         return;
     }
@@ -1232,7 +1236,7 @@ RCOutput::output_mode RCOutput::get_output_mode(uint32_t& mask)
 void RCOutput::set_telem_request_mask(uint32_t mask)
 {
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_dshot() && (mask & ((1U<<chan_offset)-1))) {
+    if (iomcu_dshot && (mask & ((1U<<chan_offset)-1))) {
         iomcu.set_telem_request_mask(mask);
     }
 #endif
@@ -1255,7 +1259,7 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len)
 
 #if HAL_WITH_IO_MCU
     // fill in ch_mode array for IOMCU channels
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         uint8_t iomcu_mask;
         const output_mode iomcu_mode = iomcu.get_output_mode(iomcu_mask);
         for (uint8_t i = 0; i < chan_offset; i++ ) {
@@ -1318,7 +1322,7 @@ void RCOutput::cork(void)
 {
     corked = true;
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         iomcu.cork();
     }
 #endif
@@ -1332,7 +1336,7 @@ void RCOutput::push(void)
     corked = false;
     push_local();
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         iomcu.push();
     }
 #endif
@@ -1344,7 +1348,7 @@ void RCOutput::push(void)
 bool RCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
 {
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         return iomcu.enable_sbus_out(rate_hz);
     }
 #endif
@@ -2226,7 +2230,7 @@ void RCOutput::serial_end(void)
 AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void)
 {
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         safety_state = iomcu.get_safety_switch_state();
     }
 #endif
@@ -2242,7 +2246,7 @@ AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void)
 bool RCOutput::force_safety_on(void)
 {
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         return iomcu.force_safety_on();
     }
 #endif
@@ -2256,7 +2260,7 @@ bool RCOutput::force_safety_on(void)
 void RCOutput::force_safety_off(void)
 {
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         iomcu.force_safety_off();
         return;
     }
@@ -2324,7 +2328,7 @@ void RCOutput::safety_update(void)
 void RCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
 {
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
+    if (iomcu_enabled) {
         iomcu.set_failsafe_pwm(chmask, period_us);
     }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index 2c0ffc6967b036..804446a30f7d97 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -331,7 +331,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
             uint8_t stream_id;
             uint8_t channel;
         } dma_ch[4];
+#ifdef HAL_TIM_UP_SHARED
         bool shared_up_dma; // do we need to wait for TIMx_UP DMA to be finished after use
+#endif
 #endif
         uint8_t alt_functions[4];
         ioline_t pal_lines[4];
@@ -517,6 +519,13 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
     static pwm_group pwm_group_list[];
     static const uint8_t NUM_GROUPS;
 
+#if HAL_WITH_IO_MCU
+    // cached values of AP_BoardConfig::io_enabled() and AP_BoardConfig::io_dshot()
+    // in case the user changes them
+    bool iomcu_enabled;
+    bool iomcu_dshot;
+#endif
+
     // offset of first local channel
     uint8_t chan_offset;
 
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
index b954c06f43e51b..43664570eb11c5 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
@@ -54,7 +54,7 @@ void RCOutput::set_bidir_dshot_mask(uint32_t mask)
 {
 #if HAL_WITH_IO_MCU
     const uint32_t iomcu_mask = ((1U<<chan_offset)-1);
-    if (AP_BoardConfig::io_dshot() && (mask & iomcu_mask)) {
+    if (iomcu_dshot && (mask & iomcu_mask)) {
         iomcu.set_bidir_dshot_mask(mask & iomcu_mask);
     }
 #endif
@@ -238,7 +238,11 @@ void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group)
         } else {
             osalDbgAssert(!group.bdshot.curr_ic_dma_handle, "IC DMA handle has not been released");
             group.bdshot.curr_ic_dma_handle = group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan];
+#ifdef HAL_TIM_UP_SHARED
             osalDbgAssert(group.shared_up_dma || !group.bdshot.curr_ic_dma_handle->is_locked(), "IC DMA handle is already locked");
+#else
+            osalDbgAssert(!group.bdshot.curr_ic_dma_handle->is_locked(), "IC DMA handle is already locked");
+#endif
             group.bdshot.curr_ic_dma_handle->lock();
             group.bdshot.enabled = true;
         }
@@ -461,7 +465,7 @@ void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t
         break;
     }
 }
-#endif
+#endif  // !defined(STM32F1)
 
 /*
   unlock DMA channel after a bi-directional dshot transaction completes
@@ -509,6 +513,8 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t*
     // we will not get data from active channels
     group->bdshot.curr_telem_chan = bdshot_find_next_ic_channel(*group);
 
+    // dshot commands are issued without a response coming back, this allows
+    // us to handle the next packet correctly without it looking like a failure
     if (group->bdshot.dma_tx_size > 0) {
         group->dshot_state = DshotState::RECV_COMPLETE;
     } else {
@@ -739,7 +745,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
     uint8_t normalized_chan = chan;
 #endif
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_dshot()) {
+    if (iomcu_dshot) {
         normalized_chan = chan + chan_offset;
     }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
index 52c801e985f7c8..c8ca5156fb7ce2 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
@@ -100,7 +100,7 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
     // not an FMU channel
     if (chan < chan_offset || chan == ALL_CHANNELS) {
 #if HAL_WITH_IO_MCU
-        if (AP_BoardConfig::io_dshot()) {
+        if (iomcu_dshot) {
             iomcu.send_dshot_command(command, chan, command_timeout_ms, repeat_count, priority);
         }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat
index 9f885359fe2ddb..10b71b35a9a0ad 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat
@@ -19,7 +19,7 @@ undef PA0 PA1 PB8 PB9
 
 # the order is important here as it determines the order that timers are used to sending dshot
 # TIM4 needs to go first so that TIM4_UP can be freed up to be used by input capture for TIM2
-PB8 TIM4_CH3 TIM4 PWM(3) GPIO(103) BIDIR
+PB8 TIM4_CH3 TIM4 PWM(3) GPIO(103) BIDIR UP_SHARED
 PB9 TIM4_CH4 TIM4 PWM(4) GPIO(104)
 PA0 TIM2_CH1 TIM2 PWM(1) GPIO(101)
 PA1 TIM2_CH2 TIM2 PWM(2) GPIO(102) BIDIR # DMA channel 7, shared with TIM4_UP and USART2_TX
@@ -43,7 +43,6 @@ define STM32_TIM_TIM4_CH3_DMA_CHAN 1
 define STM32_TIM_TIM3_CH4_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
 define STM32_TIM_TIM3_CH4_DMA_CHAN 1
 
-define HAL_TIM4_UP_SHARED 1
 undef SHARED_DMA_MASK
 define SHARED_DMA_MASK (1U<<STM32_TIM_TIM4_UP_DMA_STREAM | 1U<<STM32_TIM_TIM2_CH2_DMA_STREAM | 1U<<STM32_UART_USART2_TX_DMA_STREAM)
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
index 66beca4a63cb18..1aa188b63009d9 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
+++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
@@ -123,6 +123,9 @@ def __init__(self, bootloader=False, signed_fw=False, outdir=None, hwdef=[], def
         # integer defines
         self.intdefines = {}
 
+        # list of shared up timers
+        self.shared_up = []
+
     def is_int(self, str):
         '''check if a string is an integer'''
         try:
@@ -2059,6 +2062,7 @@ def write_PWM_config(self, f, ordered_timers):
         rc_in_int = None
         alarm = None
         bidir = None
+        up_shared = None
         pwm_out = []
         # start with the ordered list from the dma resolver
         pwm_timers = ordered_timers
@@ -2076,6 +2080,8 @@ def write_PWM_config(self, f, ordered_timers):
                         pwm_out.append(p)
                     if p.has_extra('BIDIR'):
                         bidir = p
+                    if p.has_extra('UP_SHARED'):
+                        up_shared = p
                     if p.type not in pwm_timers:
                         pwm_timers.append(p.type)
 
@@ -2162,6 +2168,10 @@ def write_PWM_config(self, f, ordered_timers):
         f.write('// PWM timer config\n')
         if bidir is not None:
             f.write('#define HAL_WITH_BIDIR_DSHOT\n')
+        if up_shared is not None:
+            f.write('#define HAL_TIM_UP_SHARED\n')
+            for t in self.shared_up:
+                f.write('#define HAL_%s_SHARED true\n' % t)
         for t in pwm_timers:
             n = int(t[3:])
             f.write('#define STM32_PWM_USE_TIM%u TRUE\n' % n)
@@ -2217,7 +2227,10 @@ def write_PWM_config(self, f, ordered_timers):
 # define HAL_IC%u_CH%u_DMA_CONFIG false, 0, 0
 #endif
 ''' % (n, i, n, i, n, i, n, i, n, i, n, i)
-                hal_icu_cfg += '}, HAL_TIM%u_UP_SHARED, \\' % n
+                if up_shared is not None:
+                    hal_icu_cfg += '}, HAL_TIM%u_UP_SHARED, \\' % n
+                else:
+                    hal_icu_cfg += '}, \\'
 
             f.write('''#if defined(STM32_TIM_TIM%u_UP_DMA_STREAM) && defined(STM32_TIM_TIM%u_UP_DMA_CHAN)
 # define HAL_PWM%u_DMA_CONFIG true, STM32_TIM_TIM%u_UP_DMA_STREAM, STM32_TIM_TIM%u_UP_DMA_CHAN
@@ -2730,6 +2743,7 @@ def write_hwdef_header(self, outfilename):
     def build_peripheral_list(self):
         '''build a list of peripherals for DMA resolver to work on'''
         peripherals = []
+        self.shared_up = []
         done = set()
         prefixes = ['SPI', 'USART', 'UART', 'I2C']
         periph_pins = self.allpins[:]
@@ -2780,6 +2794,8 @@ def build_peripheral_list(self):
                     (_, _, compl) = self.parse_timer(ch_label)
                     if ch_label not in peripherals and p.has_extra('BIDIR') and not compl:
                         peripherals.append(ch_label)
+                    if label not in self.shared_up and p.has_extra('UP_SHARED') and not compl:
+                        self.shared_up.append(label)
             done.add(type)
         return peripherals