From ef147671ef919eb81184e7e3f05883d32ba6d3e7 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <raphael.coeffic@frafos.com>
Date: Thu, 13 Oct 2022 18:26:47 +0200
Subject: [PATCH] Taranis: SBUS trainer driver for external module

---
 .../common/arm/stm32/stm32_usart_driver.cpp   | 19 ++--
 .../common/arm/stm32/trainer_driver.cpp       | 90 +++++++++++++++++++
 2 files changed, 103 insertions(+), 6 deletions(-)

diff --git a/radio/src/targets/common/arm/stm32/stm32_usart_driver.cpp b/radio/src/targets/common/arm/stm32/stm32_usart_driver.cpp
index fbc14556b..7300871ca 100644
--- a/radio/src/targets/common/arm/stm32/stm32_usart_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/stm32_usart_driver.cpp
@@ -99,10 +99,14 @@ void stm32_usart_deinit_rx_dma(const stm32_usart_t* usart)
 
   LL_DMA_DeInit(usart->rxDMA, usart->rxDMA_Stream);
 
-  // Enable IRQ based RX
-  LL_USART_EnableIT_RXNE(usart->USARTx);
-  NVIC_SetPriority(usart->IRQn, usart->IRQ_Prio);
-  NVIC_EnableIRQ(usart->IRQn);
+  if ((int32_t)(usart->IRQn) >= 0) {
+
+    // Enable IRQ based RX
+    LL_USART_EnableIT_RXNE(usart->USARTx);
+
+    NVIC_SetPriority(usart->IRQn, usart->IRQ_Prio);
+    NVIC_EnableIRQ(usart->IRQn);
+  }
 }
 
 void stm32_usart_init(const stm32_usart_t* usart, const etx_serial_init* params)
@@ -181,7 +185,10 @@ void stm32_usart_deinit(const stm32_usart_t* usart)
   if (usart->txDMA) {
     LL_DMA_DeInit(usart->txDMA, usart->txDMA_Stream);
   }
-  NVIC_DisableIRQ(usart->IRQn);
+
+  if ((int32_t)(usart->IRQn) >= 0) {
+    NVIC_DisableIRQ(usart->IRQn);
+  }
   LL_USART_DeInit(usart->USARTx);
   disable_usart_clock(usart->USARTx);
 
@@ -226,7 +233,7 @@ void stm32_usart_send_buffer(const stm32_usart_t* usart, const uint8_t * data, u
     // Please note that we don't use the buffer:
     // it should be set internally by the driver user
     // and each byte is returned individually by on_send()
-    LL_USART_EnableIT_TXE(usart->USARTx);
+    if ((int32_t)(usart->IRQn) >= 0) LL_USART_EnableIT_TXE(usart->USARTx);
   }
 }
 
diff --git a/radio/src/targets/common/arm/stm32/trainer_driver.cpp b/radio/src/targets/common/arm/stm32/trainer_driver.cpp
index 33e345f41..20c9cbc65 100644
--- a/radio/src/targets/common/arm/stm32/trainer_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/trainer_driver.cpp
@@ -338,3 +338,93 @@ extern "C" void TRAINER_MODULE_CPPM_TIMER_IRQHandler()
 #endif
 
 #endif // TRAINER_MODULE_CPPM
+
+#if defined(TRAINER_MODULE_SBUS)
+
+const etx_serial_init sbusTrainerParams = {
+    .baudrate = SBUS_BAUDRATE,
+    .parity = ETX_Parity_Even,
+    .stop_bits = ETX_StopBits_Two,
+    .word_length = ETX_WordLength_9,
+    .rx_enable = true,
+};
+
+// external module has a full-duplex USART
+#if defined(EXTMODULE_USART)
+#include "extmodule_serial_driver.h"
+
+static void* sbus_trainer_ctx = nullptr;
+
+void init_trainer_module_sbus()
+{
+  sbus_trainer_ctx = ExtmoduleSerialDriver.init(&sbusTrainerParams);
+}
+
+void stop_trainer_module_sbus()
+{
+  auto ctx = sbus_trainer_ctx;
+  if (ctx) {
+    sbus_trainer_ctx = nullptr;
+    ExtmoduleSerialDriver.deinit(ctx);
+  }
+}
+
+int trainerModuleSbusGetByte(uint8_t* data)
+{
+  auto ctx = sbus_trainer_ctx;
+  if (ctx) {
+    return ExtmoduleSerialDriver.getByte(ctx, data);
+  }
+
+  return 0;
+}
+
+#elif defined(TRAINER_MODULE_SBUS_USART)
+#include "stm32_usart_driver.h"
+
+static const LL_GPIO_InitTypeDef sbus_trainer_USART_PinDef = {
+  .Pin = TRAINER_MODULE_SBUS_GPIO_PIN,
+  .Mode = LL_GPIO_MODE_ALTERNATE,
+  .Speed = LL_GPIO_SPEED_FREQ_LOW,
+  .OutputType = LL_GPIO_OUTPUT_PUSHPULL,
+  .Pull = LL_GPIO_PULL_UP,
+  .Alternate = TRAINER_MODULE_SBUS_GPIO_AF,
+};
+
+static const stm32_usart_t sbus_trainer_USART = {
+  .USARTx = TRAINER_MODULE_SBUS_USART,
+  .GPIOx = TRAINER_MODULE_SBUS_GPIO,
+  .pinInit = &sbus_trainer_USART_PinDef,
+  .IRQn = (IRQn_Type)-1,
+  .IRQ_Prio = 0,
+  .txDMA = nullptr,
+  .txDMA_Stream = 0,
+  .txDMA_Channel = 0,
+  .rxDMA = TRAINER_MODULE_SBUS_DMA,
+  .rxDMA_Stream = TRAINER_MODULE_SBUS_DMA_STREAM_LL,
+  .rxDMA_Channel = TRAINER_MODULE_SBUS_DMA_CHANNEL,
+};
+
+static DMAFifo<32> sbus_trainer_fifo __DMA (TRAINER_MODULE_SBUS_DMA_STREAM);
+
+void init_trainer_module_sbus()
+{
+  stm32_usart_init(&sbus_trainer_USART, &sbusTrainerParams);
+  stm32_usart_init_rx_dma(&sbus_trainer_USART, sbus_trainer_fifo.buffer(),
+                          sbus_trainer_fifo.size());
+}
+
+void stop_trainer_module_sbus()
+{
+  stm32_usart_deinit(&sbus_trainer_USART);
+}
+
+int trainerModuleSbusGetByte(uint8_t* data)
+{
+  return sbus_trainer_fifo.pop(*data);
+}
+
+#else
+  #error "No available SBUS trainer implementation"
+#endif
+#endif // TRAINER_MODULE_SBUS