diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h
index 22a59c65f3..1f1f4e6c5a 100644
--- a/src/main/build/build_config.h
+++ b/src/main/build/build_config.h
@@ -65,6 +65,7 @@ typedef enum {
MCU_TYPE_AT32F435M,
MCU_TYPE_RP2350A,
MCU_TYPE_RP2350B,
+ MCU_TYPE_GD32F460,
MCU_TYPE_COUNT,
MCU_TYPE_UNKNOWN = 255,
} mcuTypeId_e;
diff --git a/src/main/common/utils.h b/src/main/common/utils.h
index c23155cfa1..fef918f2ee 100644
--- a/src/main/common/utils.h
+++ b/src/main/common/utils.h
@@ -70,7 +70,10 @@
#define STATIC_ASSERT(condition, name) static_assert((condition), #name)
#endif
+#ifdef BIT
+#undef BIT
#define BIT(x) (1 << (x))
+#endif
/*
http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c
index 958c9815d4..cbd8f8622d 100644
--- a/src/main/drivers/adc.c
+++ b/src/main/drivers/adc.c
@@ -57,6 +57,12 @@ uint8_t adcChannelByTag(ioTag_t ioTag)
ADCDevice adcDeviceByInstance(const ADC_TypeDef *instance)
{
+#if defined(USE_ADC_DEVICE_0)
+ if (instance == ADC0) {
+ return ADCDEV_0;
+ }
+#endif
+
if (instance == ADC1) {
return ADCDEV_1;
}
diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h
index 9e272d9e19..c5d4932903 100644
--- a/src/main/drivers/adc.h
+++ b/src/main/drivers/adc.h
@@ -26,8 +26,12 @@
#include "drivers/time.h"
#ifndef ADC_INSTANCE
+#if defined(USE_ADC_DEVICE_0)
+#define ADC_INSTANCE ADC0
+#else
#define ADC_INSTANCE ADC1
#endif
+#endif
#if defined(STM32F4) || defined(STM32F7) || defined(APM32F4)
#ifndef ADC1_DMA_STREAM
@@ -45,7 +49,14 @@
typedef enum ADCDevice {
ADCINVALID = -1,
+#if defined(USE_ADC_DEVICE_0)
+ ADCDEV_0 = 0,
+#if defined(ADC1)
+ ADCDEV_1,
+#endif
+#else
ADCDEV_1 = 0,
+#endif
#if defined(ADC2)
ADCDEV_2,
#endif
diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h
index add1083bf2..a83ac62ff5 100644
--- a/src/main/drivers/adc_impl.h
+++ b/src/main/drivers/adc_impl.h
@@ -51,6 +51,12 @@
#endif
#elif defined(APM32F4)
#define ADC_TAG_MAP_COUNT 16
+#elif defined(GD32F4)
+#ifdef USE_ADC_INTERNAL
+#define ADC_TAG_MAP_COUNT 19
+#else
+#define ADC_TAG_MAP_COUNT 16
+#endif
#else
#define ADC_TAG_MAP_COUNT 10
#endif
@@ -66,6 +72,18 @@ typedef struct adcTagMap_s {
// Encoding for adcTagMap_t.devices
+#if defined(USE_ADC_DEVICE_0)
+
+#define ADC_DEVICES_0 (1 << ADCDEV_0)
+#define ADC_DEVICES_1 (1 << ADCDEV_1)
+#define ADC_DEVICES_2 (1 << ADCDEV_2)
+#define ADC_DEVICES_3 (1 << ADCDEV_3)
+#define ADC_DEVICES_4 (1 << ADCDEV_4)
+#define ADC_DEVICES_01 ((1 << ADCDEV_0)|(1 << ADCDEV_1))
+#define ADC_DEVICES_23 ((1 << ADCDEV_2)|(1 << ADCDEV_3))
+#define ADC_DEVICES_012 ((1 << ADCDEV_0)|(1 << ADCDEV_1)|(1 << ADCDEV_2))
+#define ADC_DEVICES_234 ((1 << ADCDEV_2)|(1 << ADCDEV_3)|(1 << ADCDEV_4))
+#else
#define ADC_DEVICES_1 (1 << ADCDEV_1)
#define ADC_DEVICES_2 (1 << ADCDEV_2)
#define ADC_DEVICES_3 (1 << ADCDEV_3)
@@ -75,6 +93,7 @@ typedef struct adcTagMap_s {
#define ADC_DEVICES_34 ((1 << ADCDEV_3)|(1 << ADCDEV_4))
#define ADC_DEVICES_123 ((1 << ADCDEV_1)|(1 << ADCDEV_2)|(1 << ADCDEV_3))
#define ADC_DEVICES_345 ((1 << ADCDEV_3)|(1 << ADCDEV_4)|(1 << ADCDEV_5))
+#endif
typedef struct adcDevice_s {
ADC_TypeDef* ADCx;
@@ -83,7 +102,7 @@ typedef struct adcDevice_s {
#endif
#if !defined(USE_DMA_SPEC)
dmaResource_t* dmaResource;
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4)
uint32_t channel;
#endif
#endif // !defined(USE_DMA_SPEC)
@@ -164,3 +183,12 @@ void adcGetChannelValues(void);
#define TEMPSENSOR_CAL1_V (1.27f)
#define TEMPSENSOR_SLOPE (-4.13f) // mV/C
#endif
+
+#ifdef GD32F4
+#define VREFINT_EXPECTED (1489U) // 1.2/3.3*4095
+#define VREFINT_CAL_VREF (3300U)
+#define TEMPSENSOR_CAL_VREFANALOG (3300U)
+#define TEMPSENSOR_CAL1_TEMP ((int32_t) 25)
+#define TEMPSENSOR_CAL1_V (1.40f)
+#define TEMPSENSOR_SLOPE (-4.4f) // mV/C
+#endif
diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h
index e3b1ef17be..aa07235cd1 100644
--- a/src/main/drivers/nvic.h
+++ b/src/main/drivers/nvic.h
@@ -36,6 +36,12 @@
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority?
#define NVIC_PRIO_SERIALUART_TXDMA NVIC_BUILD_PRIORITY(1, 1) // Highest of all SERIALUARTx_TXDMA
+
+#if defined(USE_UART0)
+#define NVIC_PRIO_SERIALUART0_TXDMA NVIC_BUILD_PRIORITY(1, 1)
+#define NVIC_PRIO_SERIALUART0_RXDMA NVIC_BUILD_PRIORITY(1, 1)
+#define NVIC_PRIO_SERIALUART0 NVIC_BUILD_PRIORITY(1, 1)
+#endif
#define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 1)
@@ -91,6 +97,13 @@
#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING))))>>4)
#define NVIC_PRIORITY_SUB(prio) (((prio)&((0x0f>>(7-(NVIC_PRIORITY_GROUPING)))<<4))>>4)
+#elif defined(USE_GDBSP_DRIVER)
+
+#define NVIC_PRIORITY_GROUPING NVIC_PRIGROUP_PRE2_SUB2
+#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING>>8))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8)))))<<4)&0xf0)
+#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
+#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
+
#else
#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_2
diff --git a/src/main/drivers/rx/rx_pwm.c b/src/main/drivers/rx/rx_pwm.c
index 5042c73445..8039750855 100644
--- a/src/main/drivers/rx/rx_pwm.c
+++ b/src/main/drivers/rx/rx_pwm.c
@@ -322,7 +322,7 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture
}
}
-#ifdef USE_HAL_DRIVER
+#if defined(USE_HAL_DRIVER)
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
{
@@ -344,6 +344,24 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
HAL_TIM_IC_ConfigChannel(Handle, &TIM_ICInitStructure, channel);
HAL_TIM_IC_Start_IT(Handle,channel);
}
+#elif defined(USE_GDBSP_DRIVER)
+void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
+{
+ timer_ic_parameter_struct timer_icinitpara;
+
+ timer_channel_input_struct_para_init(&timer_icinitpara);
+ timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
+ timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
+ timer_icinitpara.icpolarity = polarity;
+
+ if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
+ timer_icinitpara.icfilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
+ } else {
+ timer_icinitpara.icfilter = 0x00;
+ }
+
+ timer_input_capture_config((uint32_t)tim, channel, &timer_icinitpara);
+}
#else
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
{
@@ -413,8 +431,11 @@ static void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
if (!motors[motorIndex].enabled || motors[motorIndex].channel.tim != pwmTimer) {
continue;
}
-
+#if defined(USE_GDBSP_DRIVER)
+ ppmCountDivisor = timerClock(pwmTimer) / timerPrescaler(pwmTimer);
+#else
ppmCountDivisor = timerClock(pwmTimer) / (pwmTimer->PSC + 1);
+#endif
return;
}
}
diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c
index 0f69dee869..50677d3641 100644
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@@ -111,9 +111,13 @@ static void serialEnableCC(softSerial_t *softSerial)
{
#ifdef USE_HAL_DRIVER
TIM_CCxChannelCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_ENABLE);
+#else
+#if defined(USE_GDBSP_DRIVER)
+ gd32_timer_input_capture_config(softSerial->timerHardware->tim, softSerial->timerHardware->channel, ENABLE);
#else
TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Enable);
#endif
+#endif
}
// switch to receive mode
@@ -139,8 +143,12 @@ static void serialInputPortDeActivate(softSerial_t *softSerial)
#ifdef USE_HAL_DRIVER
TIM_CCxChannelCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_DISABLE);
+#else
+#if defined(USE_GDBSP_DRIVER)
+ gd32_timer_input_capture_config(softSerial->timerHardware->tim, softSerial->timerHardware->channel, DISABLE);
#else
TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Disable);
+#endif
#endif
IOConfigGPIO(softSerial->rxIO, IOCFG_IN_FLOATING); // leave AF mode; serialOutputPortActivate will follow immediately
softSerial->rxActive = false;
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index 172719885e..be425e432a 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -444,6 +444,17 @@ const struct serialPortVTable uartVTable[] = {
}
};
+#ifdef USE_GDBSP_DRIVER
+// GDBSP driver uses IRQ handlers with different naming scheme
+#define UART_IRQHandler(type, number, dev) \
+ FAST_IRQ_HANDLER void CONCAT( \
+ _UART_GET_PREFIX(dev), \
+ number ## _IRQHandler)(void) \
+ { \
+ uartPort_t *uartPort = &(uartDevice[(dev)].port); \
+ uartIrqHandler(uartPort); \
+ }
+#else
#define UART_IRQHandler(type, number, dev) \
FAST_IRQ_HANDLER void type ## number ## _IRQHandler(void) \
{ \
@@ -451,6 +462,7 @@ const struct serialPortVTable uartVTable[] = {
uartIrqHandler(uartPort); \
} \
/**/
+#endif
#ifdef USE_UART1
UART_IRQHandler(USART, 1, UARTDEV_1) // USART1 Rx/Tx IRQ Handler
diff --git a/src/main/drivers/transponder_ir.h b/src/main/drivers/transponder_ir.h
index a4f8a44f35..f06cf4eef3 100644
--- a/src/main/drivers/transponder_ir.h
+++ b/src/main/drivers/transponder_ir.h
@@ -75,7 +75,7 @@
uint8_t erlt[TRANSPONDER_DMA_BUFFER_SIZE_ERLT]; // 91-200
} transponderIrDMABuffer_t;
-#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
+#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4)
typedef union transponderIrDMABuffer_s {
uint32_t arcitimer[TRANSPONDER_DMA_BUFFER_SIZE_ARCITIMER]; // 620
@@ -91,7 +91,7 @@ typedef struct transponder_s {
uint16_t bitToggleOne;
uint32_t dma_buffer_size;
- #if defined(STM32F4)|| defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST)
+ #if defined(STM32F4)|| defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) || defined(UNIT_TEST)
transponderIrDMABuffer_t transponderIrDMABuffer;
#endif
diff --git a/src/main/drivers/transponder_ir_arcitimer.c b/src/main/drivers/transponder_ir_arcitimer.c
index 4229af979e..498299ddc1 100644
--- a/src/main/drivers/transponder_ir_arcitimer.c
+++ b/src/main/drivers/transponder_ir_arcitimer.c
@@ -29,7 +29,7 @@
#include "drivers/transponder_ir.h"
#include "drivers/transponder_ir_arcitimer.h"
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) || defined(UNIT_TEST)
extern const struct transponderVTable arcitimerTansponderVTable;
static uint16_t dmaBufferOffset;
diff --git a/src/main/drivers/transponder_ir_erlt.c b/src/main/drivers/transponder_ir_erlt.c
index 54d603eb49..856d569062 100644
--- a/src/main/drivers/transponder_ir_erlt.c
+++ b/src/main/drivers/transponder_ir_erlt.c
@@ -28,7 +28,7 @@
#include "drivers/transponder_ir.h"
#include "drivers/transponder_ir_erlt.h"
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) || defined(UNIT_TEST)
static uint16_t dmaBufferOffset;
extern const struct transponderVTable erltTansponderVTable;
diff --git a/src/main/drivers/transponder_ir_ilap.c b/src/main/drivers/transponder_ir_ilap.c
index 9ecac736da..14a08d589a 100644
--- a/src/main/drivers/transponder_ir_ilap.c
+++ b/src/main/drivers/transponder_ir_ilap.c
@@ -28,7 +28,7 @@
#include "drivers/transponder_ir.h"
#include "drivers/transponder_ir_ilap.h"
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) || defined(UNIT_TEST)
static uint16_t dmaBufferOffset;
extern const struct transponderVTable ilapTansponderVTable;
diff --git a/src/main/pg/adc.c b/src/main/pg/adc.c
index b32de9545f..58d836a09a 100644
--- a/src/main/pg/adc.c
+++ b/src/main/pg/adc.c
@@ -39,6 +39,9 @@ PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0);
void pgResetFn_adcConfig(adcConfig_t *adcConfig)
{
adcConfig->device = ADC_DEV_TO_CFG(adcDeviceByInstance(ADC_INSTANCE));
+#if defined(USE_ADC_DEVICE_0)
+ adcConfig->dmaopt[ADCDEV_0] = ADC0_DMA_OPT;
+#endif
adcConfig->dmaopt[ADCDEV_1] = ADC1_DMA_OPT;
// These conditionals need to match the ones used in 'src/main/drivers/adc.h'.
#if defined(ADC2)
diff --git a/src/platform/GD32/adc_gd32.c b/src/platform/GD32/adc_gd32.c
new file mode 100755
index 0000000000..09a600afdd
--- /dev/null
+++ b/src/platform/GD32/adc_gd32.c
@@ -0,0 +1,324 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_ADC
+
+#include "build/debug.h"
+
+#include "drivers/dma_reqmap.h"
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "platform/rcc.h"
+#include "drivers/dma.h"
+#include "drivers/sensor.h"
+#include "drivers/adc.h"
+#include "drivers/adc_impl.h"
+
+#include "pg/adc.h"
+
+const adcDevice_t adcHardware[] = {
+ {
+ .ADCx = (ADC_TypeDef *)ADC0,
+ .rccADC = RCC_APB2(ADC0),
+#if !defined(USE_DMA_SPEC)
+ .dmaResource = (dmaResource_t *)ADC0_DMA_STREAM,
+ .channel = DMA_SUBPERI0
+#endif
+ },
+ {
+ .ADCx = (ADC_TypeDef *)ADC1,
+ .rccADC = RCC_APB2(ADC1),
+#if !defined(USE_DMA_SPEC)
+ .dmaResource = (dmaResource_t *)ADC1_DMA_STREAM,
+ .channel = DMA_SUBPERI1
+#endif
+ },
+ {
+ .ADCx = (ADC_TypeDef *)ADC2,
+ .rccADC = RCC_APB2(ADC2),
+#if !defined(USE_DMA_SPEC)
+ .dmaResource = (dmaResource_t *)ADC2_DMA_STREAM,
+ .channel = DMA_SUBPERI2
+#endif
+ }
+};
+
+/* note these could be packed up for saving space */
+const adcTagMap_t adcTagMap[] = {
+
+ { DEFIO_TAG_E__PC0, ADC_DEVICES_012, ADC_CHANNEL_10 },
+ { DEFIO_TAG_E__PC1, ADC_DEVICES_012, ADC_CHANNEL_11 },
+ { DEFIO_TAG_E__PC2, ADC_DEVICES_012, ADC_CHANNEL_12 },
+ { DEFIO_TAG_E__PC3, ADC_DEVICES_012, ADC_CHANNEL_13 },
+ { DEFIO_TAG_E__PC4, ADC_DEVICES_01, ADC_CHANNEL_14 },
+ { DEFIO_TAG_E__PC5, ADC_DEVICES_01, ADC_CHANNEL_15 },
+ { DEFIO_TAG_E__PB0, ADC_DEVICES_01, ADC_CHANNEL_8 },
+ { DEFIO_TAG_E__PB1, ADC_DEVICES_01, ADC_CHANNEL_9 },
+ { DEFIO_TAG_E__PA0, ADC_DEVICES_012, ADC_CHANNEL_0 },
+ { DEFIO_TAG_E__PA1, ADC_DEVICES_012, ADC_CHANNEL_1 },
+ { DEFIO_TAG_E__PA2, ADC_DEVICES_012, ADC_CHANNEL_2 },
+ { DEFIO_TAG_E__PA3, ADC_DEVICES_012, ADC_CHANNEL_3 },
+ { DEFIO_TAG_E__PA4, ADC_DEVICES_01, ADC_CHANNEL_4 },
+ { DEFIO_TAG_E__PA5, ADC_DEVICES_01, ADC_CHANNEL_5 },
+ { DEFIO_TAG_E__PA6, ADC_DEVICES_01, ADC_CHANNEL_6 },
+ { DEFIO_TAG_E__PA7, ADC_DEVICES_01, ADC_CHANNEL_7 },
+};
+
+
+static void adcInitDevice(uint32_t adc_periph, int channelCount)
+{
+ // Multiple injected channel seems to require scan conversion mode to be
+ // enabled even if main (non-injected) channel count is 1.
+#ifdef USE_ADC_INTERNAL
+ adc_special_function_config(adc_periph, ADC_SCAN_MODE, ENABLE);
+#else
+ if(channelCount > 1)
+ adc_special_function_config(adc_periph, ADC_SCAN_MODE, ENABLE);
+ else
+ adc_special_function_config(adc_periph, ADC_SCAN_MODE, DISABLE);
+#endif
+
+ adc_special_function_config(adc_periph, ADC_CONTINUOUS_MODE, ENABLE);
+ adc_resolution_config(adc_periph, ADC_RESOLUTION_12B);
+ adc_data_alignment_config(adc_periph, ADC_DATAALIGN_RIGHT);
+
+ /* routine channel config */
+ adc_external_trigger_source_config(adc_periph, ADC_ROUTINE_CHANNEL, ADC_EXTTRIG_ROUTINE_T0_CH0);
+ adc_external_trigger_config(adc_periph, ADC_ROUTINE_CHANNEL, EXTERNAL_TRIGGER_DISABLE);
+ adc_channel_length_config(adc_periph, ADC_ROUTINE_CHANNEL, channelCount);
+}
+
+#ifdef USE_ADC_INTERNAL
+
+static void adcInitInternalInjected(const adcConfig_t *config)
+{
+ uint32_t adc_periph = PERIPH_INT(ADC0);
+ adc_channel_16_to_18(ADC_TEMP_VREF_CHANNEL_SWITCH, ENABLE);
+ adc_discontinuous_mode_config(adc_periph, ADC_CHANNEL_DISCON_DISABLE, 0);
+ adc_channel_length_config(adc_periph, ADC_INSERTED_CHANNEL, 2);
+
+ adc_inserted_channel_config(adc_periph, 1, ADC_CHANNEL_17, ADC_SAMPLETIME_480); // ADC_Channel_Vrefint
+ adc_inserted_channel_config(adc_periph, 0, ADC_CHANNEL_16, ADC_SAMPLETIME_480); // ADC_Channel_TempSensor
+
+ adcVREFINTCAL = config->vrefIntCalibration ? config->vrefIntCalibration : VREFINT_EXPECTED;
+ adcTSCAL1 = config->tempSensorCalibration1 ? config->tempSensorCalibration1 : ((TEMPSENSOR_CAL1_V * 4095.0f) / 3.3f);
+
+ adcTSSlopeK = lrintf(3300.0f*1000.0f/4095.0f/TEMPSENSOR_SLOPE);
+}
+
+// Note on sampling time for temperature sensor and vrefint:
+// Both sources have minimum sample time of 10us.
+// With prescaler = 8:
+// 168MHz : fAPB2 = 84MHz, fADC = 10.5MHz, tcycle = 0.090us, 10us = 105cycle < 144cycle
+// 240MHz : fAPB2 = 120MHz, fADC = 15.0MHz, tcycle = 0.067usk 10us = 150cycle < 480cycle
+//
+// 480cycles@15.0MHz = 32us
+
+static bool adcInternalConversionInProgress = false;
+
+bool adcInternalIsBusy(void)
+{
+ if (adcInternalConversionInProgress) {
+ if (adc_flag_get(PERIPH_INT(ADC0), ADC_FLAG_EOIC) != RESET) {
+ adcInternalConversionInProgress = false;
+ }
+ }
+
+ return adcInternalConversionInProgress;
+}
+
+void adcInternalStartConversion(void)
+{
+ uint32_t adc_periph = PERIPH_INT(ADC0);
+ adc_flag_clear(adc_periph, ADC_FLAG_EOIC);
+ adc_software_trigger_enable(adc_periph, ADC_INSERTED_CHANNEL);
+
+ adcInternalConversionInProgress = true;
+}
+
+uint16_t adcInternalReadVrefint(void)
+{
+ return adc_inserted_data_read(PERIPH_INT(ADC0), ADC_INSERTED_CHANNEL_1);
+}
+
+uint16_t adcInternalReadTempsensor(void)
+{
+ return adc_inserted_data_read(PERIPH_INT(ADC0), ADC_INSERTED_CHANNEL_0);
+}
+#endif
+
+void adcInit(const adcConfig_t *config)
+{
+ uint8_t i;
+ uint8_t configuredAdcChannels = 0;
+
+ memset(&adcOperatingConfig, 0, sizeof(adcOperatingConfig));
+
+ if (config->vbat.enabled) {
+ adcOperatingConfig[ADC_BATTERY].tag = config->vbat.ioTag;
+ }
+
+ if (config->rssi.enabled) {
+ adcOperatingConfig[ADC_RSSI].tag = config->rssi.ioTag; //RSSI_ADC_CHANNEL;
+ }
+
+ if (config->external1.enabled) {
+ adcOperatingConfig[ADC_EXTERNAL1].tag = config->external1.ioTag; //EXTERNAL1_ADC_CHANNEL;
+ }
+
+ if (config->current.enabled) {
+ adcOperatingConfig[ADC_CURRENT].tag = config->current.ioTag; //CURRENT_METER_ADC_CHANNEL;
+ }
+
+ ADCDevice device = ADC_CFG_TO_DEV(config->device);
+
+ if (device == ADCINVALID) {
+ return;
+ }
+
+ adcDevice_t adc = adcHardware[device];
+
+ bool adcActive = false;
+ for (int i = 0; i < ADC_CHANNEL_COUNT; i++) {
+ if (!adcVerifyPin(adcOperatingConfig[i].tag, device)) {
+ continue;
+ }
+
+ adcActive = true;
+ IOInit(IOGetByTag(adcOperatingConfig[i].tag), OWNER_ADC_BATT + i, 0);
+ IOConfigGPIO(IOGetByTag(adcOperatingConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_OTYPE_OD, GPIO_PUPD_NONE));
+ adcOperatingConfig[i].adcChannel = adcChannelByTag(adcOperatingConfig[i].tag);
+ adcOperatingConfig[i].dmaIndex = configuredAdcChannels++;
+ adcOperatingConfig[i].sampleTime = ADC_SAMPLETIME_480;
+ adcOperatingConfig[i].enabled = true;
+ }
+
+#ifndef USE_ADC_INTERNAL
+ if (!adcActive) {
+ return;
+ }
+#endif
+
+ RCC_ClockCmd(adc.rccADC, ENABLE);
+
+ adc_sync_mode_config(ADC_SYNC_MODE_INDEPENDENT);
+ adc_clock_config(ADC_ADCCK_PCLK2_DIV8);
+ adc_sync_dma_config(ADC_SYNC_DMA_DISABLE);
+ adc_sync_delay_config(ADC_SYNC_DELAY_5CYCLE);
+
+#ifdef USE_ADC_INTERNAL
+ uint32_t adc_periph = PERIPH_INT(ADC0);
+ // If device is not ADC0 or there's no active channel, then initialize ADC0 separately
+ if (device != ADCDEV_0 || !adcActive) {
+ RCC_ClockCmd(adcHardware[ADCDEV_0].rccADC, ENABLE);
+ adcInitDevice(adc_periph, 2);
+ adc_enable(adc_periph);
+ }
+
+ // Initialize for injected conversion
+ adcInitInternalInjected(config);
+
+ if (!adcActive) {
+ return;
+ }
+#endif
+
+ adcInitDevice((uint32_t)(adc.ADCx), configuredAdcChannels); // Note type conversion.
+
+ uint8_t rank = 0;
+ for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
+ if (!adcOperatingConfig[i].enabled) {
+ continue;
+ }
+ adc_routine_channel_config((uint32_t)(adc.ADCx), rank++, adcOperatingConfig[i].adcChannel, adcOperatingConfig[i].sampleTime);
+ }
+
+ adc_dma_request_after_last_enable((uint32_t)(adc.ADCx));
+
+ adc_dma_mode_enable((uint32_t)(adc.ADCx));
+ adc_enable((uint32_t)(adc.ADCx));
+
+#ifdef USE_DMA_SPEC
+ const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, device, config->dmaopt[device]);
+
+ if (!dmaSpec || !dmaAllocate(dmaGetIdentifier(dmaSpec->ref), OWNER_ADC, RESOURCE_INDEX(device))) {
+ return;
+ }
+
+ dmaEnable(dmaGetIdentifier(dmaSpec->ref));
+
+ xDMA_DeInit(dmaSpec->ref);
+#else
+ if (!dmaAllocate(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0)) {
+ return;
+ }
+
+ dmaEnable(dmaGetIdentifier(adc.dmaResource));
+
+ xDMA_DeInit(adc.dmaResource);
+#endif
+
+ dma_single_data_parameter_struct dma_init_struct;
+ dma_single_data_para_struct_init(&dma_init_struct);
+ dma_init_struct.periph_addr = (uint32_t)(&ADC_RDATA((uint32_t)(adc.ADCx)));
+
+ uint32_t temp_dma_periph;
+ int temp_dma_channel;
+#ifdef USE_DMA_SPEC
+ gd32_dma_chbase_parse((uint32_t)dmaSpec->ref, &temp_dma_periph, &temp_dma_channel);
+ dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, dmaSpec->channel);
+#else
+ gd32_dma_chbase_parse((uint32_t)adc.dmaResource, &temp_dma_periph, &temp_dma_channel);
+ dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, adc.channel);
+#endif
+
+ dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ dma_init_struct.memory0_addr = (uint32_t)(adcValues);
+ dma_init_struct.memory_inc = configuredAdcChannels > 1 ? DMA_MEMORY_INCREASE_ENABLE : DMA_MEMORY_INCREASE_DISABLE;
+ dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_16BIT;
+ dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_ENABLE;
+ dma_init_struct.direction = DMA_PERIPH_TO_MEMORY;
+ dma_init_struct.number = configuredAdcChannels;
+ dma_init_struct.priority = DMA_PRIORITY_HIGH;
+
+#ifdef USE_DMA_SPEC
+ gd32_dma_init((uint32_t)dmaSpec->ref, &dma_init_struct);
+ xDMA_Cmd(dmaSpec->ref, ENABLE);
+#else
+ gd32_dma_init((uint32_t)adc.dmaResource, &dma_init_struct);
+ xDMA_Cmd(adc.dmaResource, ENABLE);
+#endif
+
+ adc_software_trigger_enable((uint32_t)(adc.ADCx), ADC_ROUTINE_CHANNEL);
+}
+
+void adcGetChannelValues(void)
+{
+ // Nothing to do
+}
+#endif
diff --git a/src/platform/GD32/audio_gd32.c b/src/platform/GD32/audio_gd32.c
new file mode 100755
index 0000000000..647e811c84
--- /dev/null
+++ b/src/platform/GD32/audio_gd32.c
@@ -0,0 +1,84 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "common/maths.h"
+
+#include "drivers/audio.h"
+
+void audioSetupIO(void)
+{
+ rcu_periph_clock_enable(RCU_DAC);
+ rcu_periph_clock_enable(RCU_TIMER5);
+
+ rcu_periph_clock_enable(RCU_GPIOA);
+ gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO_PIN_4);
+}
+
+void audioGenerateWhiteNoise(void)
+{
+ rcu_timer_clock_prescaler_config(RCU_TIMER_PSC_MUL4);
+
+ timer_deinit(TIMER5);
+ timer_parameter_struct timer_initpara;
+ timer_initpara.prescaler = 0;
+ timer_initpara.alignedmode = TIMER_COUNTER_EDGE;
+ timer_initpara.counterdirection = TIMER_COUNTER_UP;
+ timer_initpara.period = 0xFF;
+ timer_initpara.clockdivision = TIMER_CKDIV_DIV1;
+ timer_initpara.repetitioncounter = 0;
+ timer_init(TIMER5, &timer_initpara);
+
+ timer_master_slave_mode_config(TIMER5, TIMER_MASTER_SLAVE_MODE_ENABLE);
+ timer_master_output_trigger_source_select(TIMER5, TIMER_TRI_OUT_SRC_UPDATE);
+
+ timer_enable(TIMER5);
+
+ dac_deinit(DAC0);
+ dac_trigger_source_config(DAC0, DAC_OUT0, DAC_TRIGGER_T5_TRGO);
+ dac_trigger_enable(DAC0, DAC_OUT0);
+ dac_output_buffer_enable(DAC0, DAC_OUT0);
+
+ dac_wave_mode_config(DAC0, DAC_OUT0, DAC_WAVE_MODE_LFSR);
+ dac_lfsr_noise_config(DAC0, DAC_OUT0, DAC_LFSR_BITS10_0);
+ dac_data_set(DAC0, DAC_OUT0, DAC_ALIGN_12B_R, 0xD00);
+
+ dac_enable(DAC0, DAC_OUT0);
+}
+
+#define TONE_SPREAD 8
+
+void audioPlayTone(uint8_t tone)
+{
+ uint32_t autoreload_value = 64 + (MAX(TONE_MIN,MIN(tone, TONE_MAX)) * TONE_SPREAD);
+ timer_autoreload_value_config(TIMER5, autoreload_value);
+}
+
+void audioSilence(void)
+{
+ dac_disable(DAC0, DAC_OUT0);
+ timer_disable(TIMER5);
+}
diff --git a/src/platform/GD32/bus_i2c_gd32.c b/src/platform/GD32/bus_i2c_gd32.c
new file mode 100755
index 0000000000..4f2e7a919d
--- /dev/null
+++ b/src/platform/GD32/bus_i2c_gd32.c
@@ -0,0 +1,536 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#if defined(USE_I2C) && !defined(SOFT_I2C)
+
+#include "drivers/io.h"
+#include "drivers/time.h"
+#include "drivers/nvic.h"
+#include "platform/rcc.h"
+
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_impl.h"
+#include "drivers/bus_i2c_utils.h"
+
+static void i2c_er_handler(I2CDevice device);
+static void i2c_ev_handler(I2CDevice device);
+
+#if defined(GD32F4)
+#define IOCFG_I2C_PU IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_OD, GPIO_PUPD_PULLUP)
+#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_OD, GPIO_PUPD_NONE)
+#else // GD32F4
+#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ)
+#endif
+
+const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
+#ifdef USE_I2C_DEVICE_0
+ {
+ .device = I2CDEV_0,
+ .reg = (I2C_TypeDef *)I2C0,
+ .sclPins = {
+ I2CPINDEF(PB6, GPIO_AF_4),
+ I2CPINDEF(PB8, GPIO_AF_4),
+ },
+ .sdaPins = {
+ I2CPINDEF(PB7, GPIO_AF_4),
+ I2CPINDEF(PB9, GPIO_AF_4),
+ },
+ .rcc = RCC_APB1(I2C0),
+ .ev_irq = I2C0_EV_IRQn,
+ .er_irq = I2C0_ER_IRQn,
+ },
+#endif
+ #ifdef USE_I2C_DEVICE_1
+ {
+ .device = I2CDEV_1,
+ .reg = (I2C_TypeDef *)I2C1,
+ .sclPins = {
+ I2CPINDEF(PB10, GPIO_AF_4),
+ I2CPINDEF(PF1, GPIO_AF_4),
+ },
+ .sdaPins = {
+ I2CPINDEF(PB11, GPIO_AF_4),
+ I2CPINDEF(PF0, GPIO_AF_4),
+ },
+ .rcc = RCC_APB1(I2C1),
+ .ev_irq = I2C1_EV_IRQn,
+ .er_irq = I2C1_ER_IRQn,
+ },
+ #endif
+ #ifdef USE_I2C_DEVICE_2
+ {
+ .device = I2CDEV_2,
+ .reg = (I2C_TypeDef *)I2C2,
+ .sclPins = {
+ I2CPINDEF(PA8, GPIO_AF_4),
+ },
+ .sdaPins = {
+ I2CPINDEF(PC9, GPIO_AF_4),
+ },
+ .rcc = RCC_APB1(I2C2),
+ .ev_irq = I2C2_EV_IRQn,
+ .er_irq = I2C2_ER_IRQn,
+ },
+ #endif
+};
+
+i2cDevice_t i2cDevice[I2CDEV_COUNT];
+
+// State used by event handler ISR
+typedef struct {
+ uint8_t subaddress_sent; // flag to indicate if subaddess sent
+ uint8_t final_stop; // flag to indicate final bus condition
+ int8_t index; // index is signed -1 == send the subaddress
+} i2cEvState_t;
+ static i2cEvState_t i2c_ev_state[I2CDEV_COUNT];
+
+static volatile uint16_t i2cErrorCount = 0;
+
+void I2C0_ER_IRQHandler(void)
+{
+ i2c_er_handler(I2CDEV_0);
+}
+
+void I2C0_EV_IRQHandler(void)
+{
+ i2c_ev_handler(I2CDEV_0);
+}
+
+void I2C1_ER_IRQHandler(void)
+{
+ i2c_er_handler(I2CDEV_1);
+}
+
+void I2C1_EV_IRQHandler(void)
+{
+ i2c_ev_handler(I2CDEV_1);
+}
+
+void I2C2_ER_IRQHandler(void)
+{
+ i2c_er_handler(I2CDEV_2);
+}
+
+void I2C2_EV_IRQHandler(void)
+{
+ i2c_ev_handler(I2CDEV_2);
+}
+
+ static bool i2cHandleHardwareFailure(I2CDevice device)
+ {
+ i2cErrorCount++;
+ // reinit peripheral + clock out garbage
+ i2cInit(device);
+ return false;
+ }
+
+bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
+{
+ if (device == I2CINVALID || device >= I2CDEV_COUNT) {
+ return false;
+ }
+
+ uint32_t I2Cx = (uint32_t )(i2cDevice[device].reg);
+
+ if (!I2Cx) {
+ return false;
+ }
+
+ i2cState_t *state = &(i2cDevice[device].state);
+ if (state->busy) {
+ return false;
+ }
+
+ timeUs_t timeoutStartUs = microsISR();
+
+ state->addr = addr_ << 1;
+ state->reg = reg_;
+ state->writing = 1;
+ state->reading = 0;
+ state->write_p = data;
+ state->read_p = data;
+ state->bytes = len_;
+ state->busy = 1;
+ state->error = false;
+
+ if (!(I2C_CTL1(I2Cx) & I2C_CTL1_EVIE)) { // if we are restarting the driver
+ if (!(I2C_CTL0(I2Cx) & I2C_CTL0_START)) { // ensure sending a start
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for any stop to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ return i2cHandleHardwareFailure(device);
+ }
+ }
+ i2c_start_on_bus(I2Cx); // send the start for the new job
+ }
+ i2c_interrupt_enable(I2Cx, I2C_INT_ERR); // allow the interrupts to fire off again
+ i2c_interrupt_enable(I2Cx, I2C_INT_EV);
+ }
+
+ return true;
+}
+
+bool i2cBusy(I2CDevice device, bool *error)
+{
+ i2cState_t *state = &i2cDevice[device].state;
+
+ if (error) {
+ *error = state->error;
+ }
+ return state->busy;
+}
+
+static bool i2cWait(I2CDevice device)
+{
+ i2cState_t *state = &(i2cDevice[device].state);
+ timeUs_t timeoutStartUs = microsISR();
+
+ while (state->busy) {
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ return i2cHandleHardwareFailure(device) && i2cWait(device);
+ }
+ }
+
+ return !(state->error);
+}
+
+bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
+{
+ return i2cWriteBuffer(device, addr_, reg_, 1, &data) && i2cWait(device);
+}
+
+bool i2cReadBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
+{
+
+ if (device == I2CINVALID || device >= I2CDEV_COUNT) {
+ return false;
+ }
+
+ uint32_t I2Cx = (uint32_t )(i2cDevice[device].reg);
+ if (!I2Cx) {
+ return false;
+ }
+
+ i2cState_t *state = &i2cDevice[device].state;
+ if (state->busy) {
+ return false;
+ }
+
+ timeUs_t timeoutStartUs = microsISR();
+
+ state->addr = addr_ << 1;
+ state->reg = reg_;
+ state->writing = 0;
+ state->reading = 1;
+ state->read_p = buf;
+ state->write_p = buf;
+ state->bytes = len;
+ state->busy = 1;
+ state->error = false;
+
+ if (!(I2C_CTL1(I2Cx) & I2C_CTL1_EVIE)) { // if we are restarting the driver
+ if (!(I2C_CTL0(I2Cx) & I2C_CTL0_START)) { // ensure sending a start
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for any stop to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ return i2cHandleHardwareFailure(device);
+ }
+ }
+
+ /* enable acknowledge */
+ i2c_ack_config(I2Cx, I2C_ACK_ENABLE);
+ uint8_t wait_count = 0;
+ /* wait until I2C bus is idle */
+ while(i2c_flag_get(I2Cx, I2C_FLAG_I2CBSY)){
+ if(wait_count>10) {
+ break;
+ }
+ wait_count++;
+ delay(5);
+ }
+ if(2 == len) {
+ i2c_ackpos_config(I2Cx, I2C_ACKPOS_NEXT);
+ }
+ i2c_start_on_bus(I2Cx); // send the start for the new job
+ }
+ i2c_interrupt_enable(I2Cx, I2C_INT_ERR); // allow the interrupts to fire off again
+ i2c_interrupt_enable(I2Cx, I2C_INT_EV);
+ }
+
+ return true;
+}
+
+bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
+{
+ return i2cReadBuffer(device, addr_, reg_, len, buf) && i2cWait(device);
+}
+
+static void i2c_er_handler(I2CDevice device)
+{
+ uint32_t I2Cx = (uint32_t )(i2cDevice[device].hardware->reg);
+
+ i2cState_t *state = &i2cDevice[device].state;
+ timeUs_t timeoutStartUs = 0;
+
+ // Read the I2C status register
+ volatile uint32_t SR1Register = I2C_STAT0(I2Cx);
+
+ if (SR1Register & (I2C_STAT0_BERR | I2C_STAT0_LOSTARB | I2C_STAT0_AERR | I2C_STAT0_OUERR)) // an error
+ state->error = true;
+
+ if (SR1Register & (I2C_STAT0_BERR | I2C_STAT0_LOSTARB | I2C_STAT0_AERR)) {
+ // I2C_STAT0(I2Cx);
+ I2C_STAT1(I2Cx); // read second status register to clear ADDR if it is set (note that BTC will not be set after a NACK)
+ i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable the RBNE/TBE interrupt - prevent the ISR tailchaining onto the ERR
+ if (!(SR1Register & I2C_STAT0_LOSTARB) && !(I2C_CTL0(I2Cx) & I2C_CTL0_STOP)) {
+ if (I2C_CTL0(I2Cx) & I2C_CTL0_START) { // We are currently trying to send a start, this is very bad as start, stop will hang the peripheral
+ timeoutStartUs = microsISR();
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_START) { // wait for any start to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ break;
+ }
+ }
+ i2c_stop_on_bus(I2Cx); // send stop to finalise bus transaction
+ timeoutStartUs = microsISR();
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for stop to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ break;
+ }
+ }
+ i2cInit(device); // reset and configure the hardware
+ } else {
+ i2c_stop_on_bus(I2Cx); // stop to free up the bus
+ timeoutStartUs = microsISR();
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for stop to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ break;
+ }
+ }
+ i2c_interrupt_disable(I2Cx, I2C_INT_ERR); // Disable EV and ERR interrupts while bus inactive
+ i2c_interrupt_disable(I2Cx, I2C_INT_EV);
+ }
+ }
+ }
+ I2C_STAT0(I2Cx) &= ~(I2C_STAT0_BERR | I2C_STAT0_LOSTARB | I2C_STAT0_AERR | I2C_STAT0_OUERR); // reset all the error bits to clear the interrupt
+ state->busy = 0;
+}
+
+void i2c_ev_handler(I2CDevice device)
+{
+ uint32_t I2Cx = (uint32_t)(i2cDevice[device].hardware->reg);
+
+ i2cEvState_t *ev_state = &i2c_ev_state[device];
+ i2cState_t *state = &i2cDevice[device].state;
+
+ timeUs_t timeoutStartUs = 0;
+
+ uint8_t SReg_1 = I2C_STAT0(I2Cx); // read the status register here
+
+ if (SReg_1 & I2C_STAT0_SBSEND) { // we just sent a start
+ ev_state->index = 0; // reset the index
+ if (state->reading && (ev_state->subaddress_sent || 0xFF == state->reg)) { // we have sent the subaddr
+ ev_state->subaddress_sent = 1; // make sure this is set in case of no subaddress, so following code runs correctly
+ i2c_master_addressing(I2Cx, state->addr, I2C_RECEIVER); // send the address and set hardware mode
+ } else { // direction is TB, or we havent sent the sub and rep start
+ i2c_master_addressing(I2Cx, state->addr, I2C_TRANSMITTER); // send the address and set hardware mode
+ if (state->reg != 0xFF) // 0xFF as subaddress means it will be ignored, in TB or RB mode
+ ev_state->index = -1; // send a subaddress
+ }
+ } else if (SReg_1 & I2C_STAT0_ADDSEND) { // we just sent the address
+ // Read SR1,2 to clear ADDR
+ __DMB(); // memory fence to control hardware
+ if (state->bytes == 1 && state->reading && ev_state->subaddress_sent) { // we are receiving 1 byte
+ i2c_ack_config(I2Cx, I2C_ACK_DISABLE); // turn off ACK
+ __DMB();
+ I2C_STAT1(I2Cx); // clear ADDR after ACK is turned off
+ i2c_stop_on_bus(I2Cx); // program the stop
+ timeoutStartUs = microsISR();
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for stop to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ break;
+ }
+ }
+
+ ev_state->final_stop = 1;
+ i2c_interrupt_enable(I2Cx, I2C_INT_BUF);
+ } else {
+ I2C_STAT1(I2Cx); // clear the ADDR here
+ __DMB();
+ if (state->bytes == 2 && state->reading && ev_state->subaddress_sent) { // rx 2 bytes
+ i2c_ack_config(I2Cx, I2C_ACK_DISABLE); // turn off ACK
+ i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable TBE to allow the buffer to fill
+ } else if (state->bytes == 3 && state->reading && ev_state->subaddress_sent) { // rx 3 bytes
+ i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // make sure RBNE disabled so we get a BTC in two bytes time
+ } else { // receiving greater than three bytes, sending subaddress, or transmitting
+ i2c_interrupt_enable(I2Cx, I2C_INT_BUF);
+ }
+ }
+ } else if (SReg_1 & I2C_STAT0_BTC) { // Byte transfer finished
+ ev_state->final_stop = 1;
+ if (state->reading && ev_state->subaddress_sent) {
+ if (state->bytes > 2) {
+ i2c_ack_config(I2Cx, I2C_ACK_DISABLE); // turn off ACK
+ state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); // read data N-2
+ i2c_stop_on_bus(I2Cx); // program the Stop
+ timeoutStartUs = microsISR();
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for stop to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ break;
+ }
+ }
+
+ ev_state->final_stop = 1; // required to fix hardware
+ state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); // read data N - 1
+ i2c_interrupt_enable(I2Cx, I2C_INT_BUF);
+ } else {
+ if (ev_state->final_stop) {
+ i2c_stop_on_bus(I2Cx); // program the Stop
+ timeoutStartUs = microsISR();
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for stop to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ break;
+ }
+ }
+ } else {
+ i2c_start_on_bus(I2Cx); // program a rep start
+ }
+
+ state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); // read data N - 1
+ state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); // read data N
+ ev_state->index++;
+ }
+ } else {
+ if (ev_state->subaddress_sent || (state->writing)) {
+ if (ev_state->final_stop) {
+ i2c_stop_on_bus(I2Cx); // program the Stop
+ timeoutStartUs = microsISR();
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for stop to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ break;
+ }
+ }
+ } else {
+ i2c_start_on_bus(I2Cx); // program a rep start
+ }
+
+ ev_state->index++;
+ } else { // We need to send a subaddress
+ i2c_start_on_bus(I2Cx); // program the repeated Start
+ ev_state->subaddress_sent = 1; // this is set back to zero upon completion of the current task
+ }
+ }
+ // we must wait for the start to clear, otherwise we get constant BTC
+ timeUs_t timeoutStartUs = microsISR();
+ while (I2C_CTL0(I2Cx) & I2C_CTL0_START) { // wait for any start to finish sending
+ if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) {
+ break;
+ }
+ }
+ } else if (SReg_1 & I2C_STAT0_RBNE) { // Byte received
+ state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx);
+ if (state->bytes == (ev_state->index + 3))
+ i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable TBE to allow the buffer to flush
+ if (state->bytes == ev_state->index)
+ ev_state->index++;
+ } else if (SReg_1 & I2C_STAT0_TBE) { // Byte transmitted
+ if (ev_state->index != -1) {
+ if (state->bytes == (ev_state->index + 1) ) // we have sent all the data
+ i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable TXE to allow the buffer to flush
+ I2C_DATA(I2Cx) = state->write_p[ev_state->index++];
+ } else {
+ ev_state->index++;
+ if (state->reading || !(state->bytes)) // if receiving or sending 0 bytes, flush now
+ i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable TBE to allow the buffer to flush
+ I2C_DATA(I2Cx) = state->reg; // send the subaddress
+
+ }
+ }
+ if (ev_state->index == state->bytes + 1) {
+ ev_state->subaddress_sent = 0; // reset this here
+ if (ev_state->final_stop){
+ i2c_interrupt_disable(I2Cx, I2C_INT_ERR);
+ i2c_interrupt_disable(I2Cx, I2C_INT_EV);
+ } // If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTC
+
+ state->busy = 0;
+ }
+}
+
+void i2cInit(I2CDevice device)
+{
+ if (device == I2CINVALID)
+ return;
+
+ i2cDevice_t *pDev = &i2cDevice[device];
+ const i2cHardware_t *hw = pDev->hardware;
+ const IO_t scl = pDev->scl;
+ const IO_t sda = pDev->sda;
+
+ if (!hw || IOGetOwner(scl) || IOGetOwner(sda)) {
+ return;
+ }
+
+ uint32_t I2Cx = (uint32_t )i2cDevice[device].reg;
+ memset(&pDev->state, 0, sizeof(pDev->state));
+
+ IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device));
+ IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device));
+
+ // Enable RCC
+ RCC_ClockCmd(hw->rcc, ENABLE);
+
+ i2c_interrupt_disable(I2Cx, I2C_INT_EV);
+ i2cUnstick(scl, sda);
+
+ // Init pins
+ #ifdef GD32F4
+ IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sclAF);
+ IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sdaAF);
+ #else
+ IOConfigGPIO(scl, IOCFG_I2C);
+ IOConfigGPIO(sda, IOCFG_I2C);
+ #endif
+
+ i2c_deinit(I2Cx);
+ i2c_interrupt_disable(I2Cx, I2C_INT_ERR);
+ pDev->clockSpeed = ((pDev->clockSpeed > 400) ? 400 : pDev->clockSpeed);
+ i2c_clock_config(I2Cx, pDev->clockSpeed * 1000, I2C_DTCY_2);
+ i2c_mode_addr_config(I2Cx, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, 0);
+
+ i2c_stretch_scl_low_config(I2Cx, I2C_SCLSTRETCH_ENABLE);
+ i2c_enable(I2Cx);
+ i2c_ack_config(I2Cx, I2C_ACK_ENABLE);
+ // I2C Interrupt
+ nvic_irq_enable(hw->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
+ nvic_irq_enable(hw->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
+}
+
+uint16_t i2cGetErrorCounter(void)
+{
+ return i2cErrorCount;
+}
+
+#endif
diff --git a/src/platform/GD32/bus_spi_gd32.c b/src/platform/GD32/bus_spi_gd32.c
new file mode 100755
index 0000000000..db095edab3
--- /dev/null
+++ b/src/platform/GD32/bus_spi_gd32.c
@@ -0,0 +1,387 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_SPI
+
+// GD32F405 can't DMA to/from TCMSRAM
+#define IS_TCM(p) (((uint32_t)p & 0xffff0000) == 0x10000000)
+
+#include "common/maths.h"
+#include "drivers/bus.h"
+#include "drivers/bus_spi.h"
+#include "drivers/bus_spi_impl.h"
+#include "drivers/exti.h"
+#include "drivers/io.h"
+#include "platform/rcc.h"
+
+// Use DMA if possible if this many bytes are to be transferred
+#define SPI_DMA_THRESHOLD 8
+
+static spi_parameter_struct defaultInit = {
+ .device_mode = SPI_MASTER,
+ .trans_mode = SPI_TRANSMODE_FULLDUPLEX,
+ .frame_size = SPI_FRAMESIZE_8BIT,
+ .nss = SPI_NSS_SOFT,
+ .endian = SPI_ENDIAN_MSB,
+ .clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE,
+ .prescale = SPI_PSC_8
+};
+
+static uint32_t spiDivisorToBRbits(const SPI_TypeDef *instance, uint16_t divisor)
+{
+ // SPI1 and SPI2 are on APB1/AHB1 which PCLK is half that of APB2/AHB2.
+ if (instance == SPI1 || instance == SPI2) {
+ divisor /= 2; // Safe for divisor == 0 or 1
+ }
+
+ divisor = constrain(divisor, 2, 256);
+
+ return ((uint32_t)(ffs(divisor) - 2) << 3);
+}
+
+static void spiSetDivisorBRreg(SPI_TypeDef *instance, uint16_t divisor)
+{
+#define BR_BITS ((BIT(5) | BIT(4) | BIT(3)))
+ uint32_t spi_periph = PERIPH_INT(instance);
+ const uint32_t tempRegister = (SPI_CTL0(spi_periph) & ~BR_BITS);
+ SPI_CTL0(spi_periph) = (tempRegister | (spiDivisorToBRbits(instance, divisor) & BR_BITS));
+#undef BR_BITS
+}
+
+void spiInitDevice(SPIDevice device)
+{
+ spiDevice_t *spi = &(spiDevice[device]);
+
+ if (!spi->dev) {
+ return;
+ }
+
+ // Enable SPI clock
+ RCC_ClockCmd(spi->rcc, ENABLE);
+
+ IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device));
+ IOInit(IOGetByTag(spi->miso), OWNER_SPI_SDI, RESOURCE_INDEX(device));
+ IOInit(IOGetByTag(spi->mosi), OWNER_SPI_SDO, RESOURCE_INDEX(device));
+
+ IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG, spi->af);
+ IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_SDI_CFG, spi->af);
+ IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
+
+ uint32_t spi_periph = PERIPH_INT(spi->dev);
+ // Init SPI hardware
+ spi_i2s_deinit(spi_periph);
+
+ spi_dma_disable(spi_periph, SPI_DMA_TRANSMIT);
+ spi_dma_disable(spi_periph, SPI_DMA_RECEIVE);
+ spi_init(spi_periph, &defaultInit);
+ spi_crc_off(spi_periph);
+ spi_enable(spi_periph);
+}
+
+void spiInternalResetDescriptors(busDevice_t *bus)
+{
+ DMA_InitTypeDef *dmaGenerInitTx = bus->dmaInitTx;
+ dmaGenerInitTx->data_mode = DMA_DATA_MODE_SINGLE;
+ dmaGenerInitTx->sub_periph = bus->dmaTx->channel;
+ dma_single_data_parameter_struct *dmaInitTx = &dmaGenerInitTx->config.init_struct_s;
+
+ uint32_t spi_periph = PERIPH_INT(bus->busType_u.spi.instance);
+ dma_single_data_para_struct_init(dmaInitTx);
+ dmaInitTx->direction = DMA_MEMORY_TO_PERIPH;
+ dmaInitTx->circular_mode = DMA_CIRCULAR_MODE_DISABLE;
+ dmaInitTx->periph_addr = (uint32_t)&SPI_DATA(spi_periph);
+ dmaInitTx->priority = DMA_PRIORITY_LOW;
+ dmaInitTx->periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ dmaInitTx->periph_memory_width = DMA_PERIPH_WIDTH_8BIT;
+
+ if (bus->dmaRx) {
+ DMA_InitTypeDef *dmaGenerInitRx = bus->dmaInitRx;
+ dmaGenerInitRx->data_mode = DMA_DATA_MODE_SINGLE;
+ dmaGenerInitRx->sub_periph = bus->dmaRx->channel;
+ dma_single_data_parameter_struct *dmaInitRx = &dmaGenerInitRx->config.init_struct_s;
+
+ dma_single_data_para_struct_init(dmaInitRx);
+ dmaInitRx->direction = DMA_PERIPH_TO_MEMORY;
+ dmaInitRx->circular_mode = DMA_CIRCULAR_MODE_DISABLE;
+ dmaInitRx->periph_addr = (uint32_t)&SPI_DATA(spi_periph);
+ dmaInitRx->priority = DMA_PRIORITY_LOW;
+ dmaInitRx->periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ dmaInitRx->periph_memory_width = DMA_PERIPH_WIDTH_8BIT;
+ }
+}
+
+void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
+{
+ DMA_Stream_TypeDef *streamRegs = (DMA_Stream_TypeDef *)descriptor->ref;
+
+ // Disable the stream
+ REG32(streamRegs) = 0U;
+
+ // Clear any pending interrupt flags
+ dma_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF);
+}
+
+bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
+{
+ uint8_t b;
+ uint32_t spi_periph = PERIPH_INT(instance);
+
+ while (len--) {
+ b = txData ? *(txData++) : 0xFF;
+ while (spi_i2s_flag_get(spi_periph, I2S_FLAG_TBE) == RESET);
+ spi_i2s_data_transmit(spi_periph, b);
+
+ while (spi_i2s_flag_get(spi_periph, I2S_FLAG_RBNE) == RESET);
+ b = spi_i2s_data_receive(spi_periph);
+ if (rxData) {
+ *(rxData++) = b;
+ }
+ }
+
+ return true;
+}
+
+void spiInternalInitStream(const extDevice_t *dev, bool preInit)
+{
+ STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff;
+ STATIC_DMA_DATA_AUTO uint8_t dummyRxByte;
+ busDevice_t *bus = dev->bus;
+
+ volatile busSegment_t *segment = bus->curSegment;
+
+ if (preInit) {
+ // Prepare the init structure for the next segment to reduce inter-segment interval
+ segment++;
+ if(segment->len == 0) {
+ // There's no following segment
+ return;
+ }
+ }
+
+ int len = segment->len;
+
+ uint8_t *txData = segment->u.buffers.txData;
+
+ DMA_InitTypeDef *dmaGenerInitTx = bus->dmaInitTx;
+ dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
+ dmaGenerInitTx->sub_periph = dmaTx->channel;
+ dmaGenerInitTx->data_mode = DMA_DATA_MODE_SINGLE;
+ dma_single_data_parameter_struct *dmaInitTx = &dmaGenerInitTx->config.init_struct_s;
+
+ if (txData) {
+ dmaInitTx->memory0_addr = (uint32_t)txData;
+ dmaInitTx->memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+ } else {
+ dummyTxByte = 0xff;
+ dmaInitTx->memory0_addr = (uint32_t)&dummyTxByte;
+ dmaInitTx->memory_inc = DMA_MEMORY_INCREASE_DISABLE;
+ }
+ dmaInitTx->number = len;
+
+ if (dev->bus->dmaRx) {
+ uint8_t *rxData = segment->u.buffers.rxData;
+
+ DMA_InitTypeDef *dmaGenerInitRx = bus->dmaInitRx;
+ dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
+ dmaGenerInitRx->sub_periph = dmaRx->channel;
+ dmaGenerInitRx->data_mode = DMA_DATA_MODE_SINGLE;
+ dma_single_data_parameter_struct *dmaInitRx = &dmaGenerInitRx->config.init_struct_s;
+
+ if (rxData) {
+ dmaInitRx->memory0_addr = (uint32_t)rxData;
+ dmaInitRx->memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+ } else {
+ dmaInitRx->memory0_addr = (uint32_t)&dummyRxByte;
+ dmaInitRx->memory_inc = DMA_MEMORY_INCREASE_DISABLE;
+ }
+ // If possible use 16 bit memory writes to prevent atomic access issues on gyro data
+ if ((dmaInitRx->memory0_addr & 0x1) || (len & 0x1)) {
+ dmaInitRx->periph_memory_width = DMA_PERIPH_WIDTH_8BIT;
+ } else {
+ dmaInitRx->periph_memory_width = DMA_PERIPH_WIDTH_16BIT;
+ }
+ dmaInitRx->number = len;
+ }
+}
+
+void spiInternalStartDMA(const extDevice_t *dev)
+{
+ uint32_t spi_periph = PERIPH_INT(dev->bus->busType_u.spi.instance);
+ dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
+ dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
+ DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
+
+ if (dmaRx) {
+ DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
+
+ // Use the correct callback argument
+ dmaRx->userParam = (uint32_t)dev;
+
+ // Clear transfer flags
+ dma_flag_clear((uint32_t)dmaTx->dma, dmaTx->stream, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF);
+ dma_flag_clear((uint32_t)dmaRx->dma, dmaRx->stream, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF);
+
+ // Disable streams to enable update
+ REG32(streamRegsTx) = 0U;
+ REG32(streamRegsRx) = 0U;
+
+ /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
+ * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
+ */
+ xDMA_ITConfig(streamRegsRx, DMA_INT_FTF, ENABLE);
+
+ // Update streams
+
+ xDMA_Init(streamRegsTx, (dma_general_config_struct *)dev->bus->dmaInitTx);
+
+ dma_channel_subperipheral_select((uint32_t)(dmaTx->dma), dmaTx->stream, dmaTx->channel);
+
+ xDMA_Init(streamRegsRx, (dma_general_config_struct *)dev->bus->dmaInitRx);
+
+ dma_channel_subperipheral_select((uint32_t)(dmaRx->dma), dmaRx->stream, dmaRx->channel);
+
+ // Enable streams
+ dma_channel_enable((uint32_t)(dmaTx->dma), dmaTx->stream);
+ dma_channel_enable((uint32_t)(dmaRx->dma), dmaRx->stream);
+
+ /* Enable the SPI DMA Tx & Rx requests */
+ spi_dma_enable(spi_periph, SPI_DMA_TRANSMIT);
+ spi_dma_enable(spi_periph, SPI_DMA_RECEIVE);
+ } else {
+ // Use the correct callback argument
+ dmaTx->userParam = (uint32_t)dev;
+
+ // Clear transfer flags
+ dma_flag_clear((uint32_t)dmaTx->dma, dmaTx->stream, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF);
+
+ // Disable stream to enable update
+ REG32(streamRegsTx) = 0U;
+
+ xDMA_ITConfig(streamRegsTx, DMA_INT_FTF, ENABLE);
+
+ // Update stream
+ xDMA_Init(streamRegsTx, dev->bus->dmaInitTx);
+
+ // Enable stream
+ dma_channel_enable((uint32_t)(dmaTx->dma), dmaTx->stream);
+
+ /* Enable the SPI DMA Tx request */
+ spi_dma_enable(spi_periph, SPI_DMA_TRANSMIT);
+ }
+}
+
+void spiInternalStopDMA (const extDevice_t *dev)
+{
+ dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
+ dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
+ uint32_t spi_periph = PERIPH_INT(dev->bus->busType_u.spi.instance);
+ DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
+
+ if (dmaRx) {
+ DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
+
+ // Disable streams
+ REG32(streamRegsTx) = 0U;
+ REG32(streamRegsRx) = 0U;
+
+ spi_dma_disable(spi_periph, SPI_DMA_TRANSMIT);
+ spi_dma_disable(spi_periph, SPI_DMA_RECEIVE);
+ } else {
+ // Ensure the current transmission is complete
+ while (spi_i2s_flag_get(spi_periph, I2S_FLAG_TRANS));
+
+ // Drain the RX buffer
+ while (spi_i2s_flag_get(spi_periph, I2S_FLAG_RBNE)) {
+ SPI_DATA(spi_periph);
+ }
+
+ // Disable stream
+ REG32(streamRegsTx) = 0U;
+
+ spi_dma_disable(spi_periph, SPI_DMA_TRANSMIT);
+ }
+}
+
+// DMA transfer setup and start
+void spiSequenceStart(const extDevice_t *dev)
+{
+ busDevice_t *bus = dev->bus;
+ bool dmaSafe = dev->useDMA;
+ uint32_t xferLen = 0;
+ uint32_t segmentCount = 0;
+ uint32_t spi_periph = PERIPH_INT(bus->busType_u.spi.instance);
+
+ dev->bus->initSegment = true;
+
+ spi_disable(spi_periph);
+
+ // Switch bus speed
+ if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) {
+ spiSetDivisorBRreg(bus->busType_u.spi.instance, dev->busType_u.spi.speed);
+ bus->busType_u.spi.speed = dev->busType_u.spi.speed;
+ }
+
+ if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) {
+ // Switch SPI clock polarity/phase
+ SPI_CTL0(spi_periph) &= ~(SPI_CTL0_CKPL | SPI_CTL0_CKPH);
+
+ // Apply setting
+ if (dev->busType_u.spi.leadingEdge) {
+ SPI_CTL0(spi_periph) |= SPI_CK_PL_LOW_PH_1EDGE;
+ } else
+ {
+ SPI_CTL0(spi_periph) |= SPI_CK_PL_HIGH_PH_2EDGE;
+ }
+ bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge;
+ }
+
+ spi_enable(spi_periph);
+
+ // Check that any there are no attempts to DMA to/from CCD SRAM
+ for (busSegment_t *checkSegment = (busSegment_t *)bus->curSegment; checkSegment->len; checkSegment++) {
+ // Check there is no receive data as only transmit DMA is available
+ if (((checkSegment->u.buffers.rxData) && (IS_TCM(checkSegment->u.buffers.rxData) || (bus->dmaRx == (dmaChannelDescriptor_t *)NULL))) ||
+ ((checkSegment->u.buffers.txData) && IS_TCM(checkSegment->u.buffers.txData))) {
+ dmaSafe = false;
+ break;
+ }
+ // Note that these counts are only valid if dmaSafe is true
+ segmentCount++;
+ xferLen += checkSegment->len;
+ }
+
+ // Use DMA if possible
+ // If there are more than one segments, or a single segment with negateCS negated in the list terminator then force DMA irrespective of length
+ if (bus->useDMA && dmaSafe && ((segmentCount > 1) ||
+ (xferLen >= SPI_DMA_THRESHOLD) ||
+ !bus->curSegment[segmentCount].negateCS)) {
+ spiProcessSegmentsDMA(dev);
+ } else {
+ spiProcessSegmentsPolled(dev);
+ }
+}
+#endif
diff --git a/src/platform/GD32/camera_control_gd32.c b/src/platform/GD32/camera_control_gd32.c
new file mode 100755
index 0000000000..25a556b1e8
--- /dev/null
+++ b/src/platform/GD32/camera_control_gd32.c
@@ -0,0 +1,95 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include "platform.h"
+
+#ifdef USE_CAMERA_CONTROL
+
+#include
+
+#include "drivers/camera_control_impl.h"
+#include "drivers/nvic.h"
+#include "drivers/pwm_output.h"
+#include "platform/rcc.h"
+
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+#include "build/atomic.h"
+#endif
+
+#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+void cameraControlHardwarePwmInit(timerChannel_t *channel, const timerHardware_t *timerHardware, uint8_t inverted)
+{
+ pwmOutConfig(channel, timerHardware, timerClock((void *)TIMER5), CAMERA_CONTROL_PWM_RESOLUTION, 0, inverted);
+}
+#endif
+
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+void TIMER5_DAC_IRQHandler(void)
+{
+ cameraControlHi();
+ TIMER_INTF(TIMER5) = 0;
+}
+
+void TIMER6_IRQHandler(void)
+{
+ cameraControlLo();
+ TIMER_INTF(TIMER6) = 0;
+}
+
+void cameraControlSoftwarePwmInit(void)
+{
+ nvic_irq_enable(TIMER5_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ rcu_periph_clock_enable(RCU_TIMER5);
+ TIMER_PSC(TIMER5) = 0;
+
+ nvic_irq_enable(TIMER6_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ rcu_periph_clock_enable(RCU_TIMER6);
+ TIMER_PSC(TIMER6) = 0;
+}
+
+void cameraControlSoftwarePwmEnable(uint32_t hiTime, uint32_t period)
+{
+ timer_counter_value_config(TIMER5, hiTime);
+ timer_autoreload_value_config(TIMER5, period);
+
+ timer_counter_value_config(TIMER6, 0);
+ timer_autoreload_value_config(TIMER6, period);
+
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ timer_enable(TIMER5);
+ timer_enable(TIMER6);
+ }
+
+ timer_interrupt_enable(TIMER5, TIMER_INT_UP);
+ timer_interrupt_enable(TIMER6, TIMER_INT_UP);
+}
+
+void cameraControlSoftwarePwmDisable(void)
+{
+ timer_disable(TIMER5);
+ timer_disable(TIMER6);
+
+ TIMER_DMAINTEN(TIMER5) = 0;
+ TIMER_DMAINTEN(TIMER6) = 0;
+}
+#endif
+
+#endif // USE_CAMERA_CONTROL
diff --git a/src/platform/GD32/debug.c b/src/platform/GD32/debug.c
new file mode 100755
index 0000000000..038ed5a196
--- /dev/null
+++ b/src/platform/GD32/debug.c
@@ -0,0 +1,36 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include "platform.h"
+#include "build/debug.h"
+#include "drivers/io.h"
+
+void debugInit(void)
+{
+ IO_t io = IOGetByTag(DEFIO_TAG_E(PA13)); // SWDIO
+ if (IOGetOwner(io) == OWNER_FREE) {
+ IOInit(io, OWNER_SWD, 0);
+ }
+ io = IOGetByTag(DEFIO_TAG_E(PA14)); // SWCLK
+ if (IOGetOwner(io) == OWNER_FREE) {
+ IOInit(io, OWNER_SWD, 0);
+ }
+}
diff --git a/src/platform/GD32/dma_gd32.c b/src/platform/GD32/dma_gd32.c
new file mode 100755
index 0000000000..471bf6bf01
--- /dev/null
+++ b/src/platform/GD32/dma_gd32.c
@@ -0,0 +1,291 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_DMA
+
+#include "drivers/nvic.h"
+#include "drivers/dma.h"
+#include "platform/rcc.h"
+#include "drivers/resource.h"
+
+/*
+ * DMA descriptors.
+ */
+dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
+ DEFINE_DMA_CHANNEL(DMA0, 0, 0),
+ DEFINE_DMA_CHANNEL(DMA0, 1, 6),
+ DEFINE_DMA_CHANNEL(DMA0, 2, 16),
+ DEFINE_DMA_CHANNEL(DMA0, 3, 22),
+ DEFINE_DMA_CHANNEL(DMA0, 4, 32),
+ DEFINE_DMA_CHANNEL(DMA0, 5, 38),
+ DEFINE_DMA_CHANNEL(DMA0, 6, 48),
+ DEFINE_DMA_CHANNEL(DMA0, 7, 54),
+
+ DEFINE_DMA_CHANNEL(DMA1, 0, 0),
+ DEFINE_DMA_CHANNEL(DMA1, 1, 6),
+ DEFINE_DMA_CHANNEL(DMA1, 2, 16),
+ DEFINE_DMA_CHANNEL(DMA1, 3, 22),
+ DEFINE_DMA_CHANNEL(DMA1, 4, 32),
+ DEFINE_DMA_CHANNEL(DMA1, 5, 38),
+ DEFINE_DMA_CHANNEL(DMA1, 6, 48),
+ DEFINE_DMA_CHANNEL(DMA1, 7, 54),
+};
+
+/*
+ * DMA IRQ Handlers
+ */
+DEFINE_DMA_IRQ_HANDLER(0, 0, DMA0_CH0_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(0, 1, DMA0_CH1_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(0, 2, DMA0_CH2_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(0, 3, DMA0_CH3_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(0, 4, DMA0_CH4_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(0, 5, DMA0_CH5_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(0, 6, DMA0_CH6_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(0, 7, DMA0_CH7_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_CH0_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_CH1_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_CH2_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_CH3_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_CH4_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_CH5_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_CH6_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_CH7_HANDLER)
+
+#define DMA_RCU(x) ((x) == (void *)DMA0 ? RCC_AHB1(DMA0) : RCC_AHB1(DMA1))
+void dmaEnable(dmaIdentifier_e identifier)
+{
+ (void) identifier;
+ const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
+ RCC_ClockCmd(DMA_RCU(dmaDescriptors[index].dma), ENABLE);
+}
+
+#define RETURN_TCIF_FLAG(s, n) if (s == DMA0_CH ## n ## _BASE || s == DMA1_CH ## n ## _BASE) return DMA_IT_TCIF ## n
+
+static int gd32_dma_channel_get(uint32_t dma_chan_base, uint32_t dma_periph)
+{
+ uint32_t chan_base = (dma_chan_base - dma_periph);
+ int channel = DMA_CH0;
+
+ switch(chan_base) {
+ case 0x10:
+ channel = DMA_CH0;
+ break;
+ case 0x28:
+ channel = DMA_CH1;
+ break;
+ case 0x40:
+ channel = DMA_CH2;
+ break;
+ case 0x58:
+ channel = DMA_CH3;
+ break;
+ case 0x70:
+ channel = DMA_CH4;
+ break;
+ case 0x88:
+ channel = DMA_CH5;
+ break;
+ case 0xA0:
+ channel = DMA_CH6;
+ break;
+ case 0xB8:
+ channel = DMA_CH7;
+ break;
+ default:
+ break;
+ }
+ return channel;
+}
+
+static uint32_t dma_int_ftf_flag_get(uint32_t dma_chan_base)
+{
+ uint32_t status = 0;
+ uint32_t dma_periph;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ status = dma_interrupt_flag_get(dma_periph, channel ,DMA_INT_FLAG_FTF);
+
+ return status;
+}
+
+uint32_t dma_enable_status_get(uint32_t dma_chan_base)
+{
+ uint32_t dma_periph;
+ uint32_t status;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ status = (DMA_CHCTL(dma_periph, channel) & DMA_CHXCTL_CHEN);
+
+ return status;
+}
+
+void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
+{
+
+ const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
+
+ RCC_ClockCmd(DMA_RCU(dmaDescriptors[index].dma), ENABLE);
+ dmaDescriptors[index].irqHandlerCallback = callback;
+ dmaDescriptors[index].userParam = userParam;
+ dmaDescriptors[index].completeFlag = dma_int_ftf_flag_get((uint32_t)dmaDescriptors[index].ref);
+
+ nvic_irq_enable(dmaDescriptors[index].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
+}
+
+
+void gd32_dma_chbase_parse(uint32_t dma_chan_base, uint32_t *dma_periph, int *dma_channel)
+{
+ if (DMA1 > dma_chan_base) {
+ *dma_periph = DMA0;
+ } else {
+ *dma_periph = DMA1;
+ }
+
+ *dma_channel = gd32_dma_channel_get(dma_chan_base, *dma_periph);
+}
+
+void gd32_dma_init(uint32_t dma_chan_base, dma_single_data_parameter_struct *init_struct)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ dma_single_data_mode_init(dma_periph, channel, init_struct);
+}
+
+void gd32_dma_general_init(uint32_t dma_chan_base, dma_general_config_struct *init_struct)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ if(DMA_DATA_MODE_MULTI == init_struct->data_mode) {
+ dma_multi_data_mode_init(dma_periph, channel, (dma_multi_data_parameter_struct *)&init_struct->config.init_struct_m);
+ } else {
+ dma_single_data_mode_init(dma_periph, channel, (dma_single_data_parameter_struct *)&init_struct->config.init_struct_s);
+ }
+
+ dma_channel_subperipheral_select(dma_periph, channel, init_struct->sub_periph);
+}
+
+void gd32_dma_deinit(uint32_t dma_chan_base)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ dma_deinit(dma_periph, channel);
+}
+
+void gd32_dma_channel_state_config(uint32_t dma_chan_base, ControlStatus new_state)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ if(ENABLE == new_state) {
+ dma_flag_clear(dma_periph, channel, DMA_FLAG_FTF);
+ dma_flag_clear(dma_periph, channel, DMA_FLAG_HTF);
+ dma_channel_enable(dma_periph, channel);
+ } else {
+ dma_channel_disable(dma_periph, channel);
+ }
+}
+
+void gd32_dma_int_config(uint32_t dma_chan_base, uint32_t source, ControlStatus new_state)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ if(ENABLE == new_state) {
+ dma_interrupt_enable(dma_periph, channel, source);
+ } else {
+ dma_interrupt_disable(dma_periph, channel, source);
+ }
+}
+
+void gd32_dma_transnum_config(uint32_t dma_chan_base, uint32_t number)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ dma_transfer_number_config(dma_periph, channel, number);
+}
+
+uint16_t gd32_dma_transnum_get(uint32_t dma_chan_base)
+{
+ uint32_t dma_periph;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ return (uint16_t)dma_transfer_number_get(dma_periph, channel);
+}
+
+FlagStatus gd32_dma_flag_get(uint32_t dma_chan_base, uint32_t flag)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ return dma_flag_get(dma_periph, channel, flag);
+}
+
+void gd32_dma_flag_clear(uint32_t dma_chan_base, uint32_t flag)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ dma_flag_clear(dma_periph, channel, flag);
+}
+
+void gd32_dma_memory_addr_config(uint32_t dma_chan_base, uint32_t address, uint8_t memory_flag)
+{
+ uint32_t dma_periph ;
+ int channel;
+
+ gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel);
+
+ dma_memory_address_config(dma_periph, channel, memory_flag, address);
+}
+
+#endif
diff --git a/src/platform/GD32/dma_reqmap_mcu.c b/src/platform/GD32/dma_reqmap_mcu.c
new file mode 100755
index 0000000000..2cb47cc881
--- /dev/null
+++ b/src/platform/GD32/dma_reqmap_mcu.c
@@ -0,0 +1,216 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+
+#include "platform.h"
+
+#ifdef USE_DMA_SPEC
+
+#include "timer_def.h"
+#include "drivers/adc.h"
+#include "drivers/bus_spi.h"
+#include "drivers/dma_reqmap.h"
+#include "drivers/serial.h"
+#include "drivers/serial_uart.h"
+#include "drivers/serial_uart_impl.h"
+
+#include "pg/timerio.h"
+
+typedef struct dmaPeripheralMapping_s {
+ dmaPeripheral_e device;
+ uint8_t index;
+ dmaChannelSpec_t channelSpec[MAX_PERIPHERAL_DMA_OPTIONS];
+} dmaPeripheralMapping_t;
+
+typedef struct dmaTimerMapping_s {
+ TIM_TypeDef *tim;
+ uint8_t channel;
+ dmaChannelSpec_t channelSpec[MAX_TIMER_DMA_OPTIONS];
+} dmaTimerMapping_t;
+
+#define DMA(d, s, c) { DMA_CODE(d, s, c), (dmaResource_t *)DMA ## d ## _CH ## s ## _BASE, DMA_SUBPERI ## c }
+
+static const dmaPeripheralMapping_t dmaPeripheralMapping[] = {
+#ifdef USE_SPI
+ { DMA_PERIPH_SPI_SDO, SPIDEV_0, { DMA(1, 3, 3), DMA(1, 5, 3) } },
+ { DMA_PERIPH_SPI_SDI, SPIDEV_0, { DMA(1, 0, 3), DMA(1, 2, 3) } },
+ { DMA_PERIPH_SPI_SDO, SPIDEV_1, { DMA(0, 4, 0) } },
+ { DMA_PERIPH_SPI_SDI, SPIDEV_1, { DMA(0, 3, 0) } },
+ { DMA_PERIPH_SPI_SDO, SPIDEV_2, { DMA(0, 5, 0), DMA(0, 7, 0) } },
+ { DMA_PERIPH_SPI_SDI, SPIDEV_2, { DMA(0, 0, 0), DMA(0, 2, 0) } },
+
+#endif
+
+#ifdef USE_ADC
+ { DMA_PERIPH_ADC, ADCDEV_0, { DMA(1, 0, 0), DMA(1, 4, 0) } },
+ { DMA_PERIPH_ADC, ADCDEV_1, { DMA(1, 2, 1), DMA(1, 3, 1) } },
+ { DMA_PERIPH_ADC, ADCDEV_2, { DMA(1, 0, 2), DMA(1, 1, 2) } },
+#endif
+
+#ifdef USE_SDCARD_SDIO
+ { DMA_PERIPH_SDIO, 0, { DMA(1, 3, 4), DMA(1, 6, 4) } },
+#endif
+
+#ifdef USE_UART0
+ { DMA_PERIPH_UART_TX, UARTDEV_0, { DMA(1, 7, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_0, { DMA(1, 5, 4), DMA(1, 2, 4) } },
+#endif
+
+#ifdef USE_UART1
+ { DMA_PERIPH_UART_TX, UARTDEV_1, { DMA(0, 6, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_1, { DMA(0, 5, 4) } },
+#endif
+
+#ifdef USE_UART2
+ { DMA_PERIPH_UART_TX, UARTDEV_2, { DMA(0, 3, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_2, { DMA(0, 1, 4) } },
+#endif
+
+#ifdef USE_UART3
+ { DMA_PERIPH_UART_TX, UARTDEV_3, { DMA(0, 4, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_3, { DMA(0, 2, 4) } },
+#endif
+
+#ifdef USE_UART4
+ { DMA_PERIPH_UART_TX, UARTDEV_4, { DMA(0, 7, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_4, { DMA(0, 0, 4) } },
+#endif
+
+#ifdef USE_UART5
+ { DMA_PERIPH_UART_TX, UARTDEV_5, { DMA(1, 6, 5), DMA(1, 7, 5) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_5, { DMA(1, 1, 5), DMA(1, 2, 5) } },
+#endif
+};
+
+#define TC(chan) DEF_TIM_CHANNEL(CH_ ## chan)
+
+static const dmaTimerMapping_t dmaTimerMapping[] = {
+ // Generated from 'timer_def.h'
+ { (void *)TIMER0, TC(CH0), { DMA(1, 6, 0), DMA(1, 1, 6), DMA(1, 3, 6) } },
+ { (void *)TIMER0, TC(CH1), { DMA(1, 6, 0), DMA(1, 2, 6) } },
+ { (void *)TIMER0, TC(CH2), { DMA(1, 6, 0), DMA(1, 6, 6) } },
+ { (void *)TIMER0, TC(CH3), { DMA(1, 4, 6) } },
+
+ { (void *)TIMER1, TC(CH0), { DMA(0, 5, 3) } },
+ { (void *)TIMER1, TC(CH1), { DMA(0, 6, 3) } },
+ { (void *)TIMER1, TC(CH2), { DMA(0, 1, 3) } },
+ { (void *)TIMER1, TC(CH3), { DMA(0, 7, 3), DMA(0, 6, 3) } },
+
+ { (void *)TIMER2, TC(CH0), { DMA(0, 4, 5) } },
+ { (void *)TIMER2, TC(CH1), { DMA(0, 5, 5) } },
+ { (void *)TIMER2, TC(CH2), { DMA(0, 7, 5) } },
+ { (void *)TIMER2, TC(CH3), { DMA(0, 2, 5) } },
+
+ { (void *)TIMER3, TC(CH0), { DMA(0, 0, 2) } },
+ { (void *)TIMER3, TC(CH1), { DMA(0, 3, 2) } },
+ { (void *)TIMER3, TC(CH2), { DMA(0, 7, 2) } },
+
+ { (void *)TIMER4, TC(CH0), { DMA(0, 2, 6) } },
+ { (void *)TIMER4, TC(CH1), { DMA(0, 4, 6) } },
+ { (void *)TIMER4, TC(CH2), { DMA(0, 0, 6) } },
+ { (void *)TIMER4, TC(CH3), { DMA(0, 1, 6), DMA(0, 3, 6) } },
+
+ { (void *)TIMER7, TC(CH0), { DMA(1, 2, 0), DMA(1, 2, 7) } },
+ { (void *)TIMER7, TC(CH1), { DMA(1, 2, 0), DMA(1, 3, 7) } },
+ { (void *)TIMER7, TC(CH2), { DMA(1, 2, 0), DMA(1, 4, 7) } },
+ { (void *)TIMER7, TC(CH3), { DMA(1, 7, 7) } },
+};
+
+#undef TC
+#undef DMA
+
+const dmaChannelSpec_t *dmaGetChannelSpecByPeripheral(dmaPeripheral_e device, uint8_t index, int8_t opt)
+{
+ if (opt < 0 || opt >= MAX_PERIPHERAL_DMA_OPTIONS) {
+ return NULL;
+ }
+
+ for (const dmaPeripheralMapping_t *periph = dmaPeripheralMapping; periph < ARRAYEND(dmaPeripheralMapping); periph++) {
+ if (periph->device == device && periph->index == index && periph->channelSpec[opt].ref) {
+ return &periph->channelSpec[opt];
+ }
+ }
+
+ return NULL;
+}
+
+dmaoptValue_t dmaoptByTag(ioTag_t ioTag)
+{
+#ifdef USE_TIMER_MGMT
+ for (unsigned i = 0; i < MAX_TIMER_PINMAP_COUNT; i++) {
+ if (timerIOConfig(i)->ioTag == ioTag) {
+ return timerIOConfig(i)->dmaopt;
+ }
+ }
+#else
+ UNUSED(ioTag);
+#endif
+
+ return DMA_OPT_UNUSED;
+}
+
+const dmaChannelSpec_t *dmaGetChannelSpecByTimerValue(TIM_TypeDef *tim, uint8_t channel, dmaoptValue_t dmaopt)
+{
+ if (dmaopt < 0 || dmaopt >= MAX_TIMER_DMA_OPTIONS) {
+ return NULL;
+ }
+
+ for (unsigned i = 0 ; i < ARRAYLEN(dmaTimerMapping) ; i++) {
+ const dmaTimerMapping_t *timerMapping = &dmaTimerMapping[i];
+
+ if (timerMapping->tim == tim && timerMapping->channel == channel && timerMapping->channelSpec[dmaopt].ref) {
+ return &timerMapping->channelSpec[dmaopt];
+ }
+ }
+
+ return NULL;
+}
+
+const dmaChannelSpec_t *dmaGetChannelSpecByTimer(const timerHardware_t *timer)
+{
+ if (!timer) {
+ return NULL;
+ }
+
+ dmaoptValue_t dmaopt = dmaoptByTag(timer->tag);
+ return dmaGetChannelSpecByTimerValue(timer->tim, timer->channel, dmaopt);
+}
+
+// dmaGetOptionByTimer is called by pgResetFn_timerIOConfig to find out dmaopt for pre-configured timer.
+dmaoptValue_t dmaGetOptionByTimer(const timerHardware_t *timer)
+{
+ for (unsigned i = 0 ; i < ARRAYLEN(dmaTimerMapping); i++) {
+ const dmaTimerMapping_t *timerMapping = &dmaTimerMapping[i];
+ if (timerMapping->tim == timer->tim && timerMapping->channel == timer->channel) {
+ for (unsigned j = 0; j < MAX_TIMER_DMA_OPTIONS; j++) {
+ const dmaChannelSpec_t *dma = &timerMapping->channelSpec[j];
+ if (dma->ref == timer->dmaRefConfigured && dma->channel == timer->dmaChannelConfigured) {
+ return j;
+ }
+ }
+ }
+ }
+
+ return DMA_OPT_UNUSED;
+}
+
+#endif // USE_DMA_SPEC
diff --git a/src/platform/GD32/dma_reqmap_mcu.h b/src/platform/GD32/dma_reqmap_mcu.h
new file mode 100755
index 0000000000..1e11e27c00
--- /dev/null
+++ b/src/platform/GD32/dma_reqmap_mcu.h
@@ -0,0 +1,25 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#define MAX_PERIPHERAL_DMA_OPTIONS 2
+#define MAX_TIMER_DMA_OPTIONS 3
diff --git a/src/platform/GD32/dshot_bitbang.c b/src/platform/GD32/dshot_bitbang.c
new file mode 100755
index 0000000000..6bf1b21c09
--- /dev/null
+++ b/src/platform/GD32/dshot_bitbang.c
@@ -0,0 +1,764 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_DSHOT_BITBANG
+
+#include "build/debug.h"
+#include "build/debug_pin.h"
+
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "drivers/dma.h"
+#include "drivers/dma_reqmap.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_bitbang.h"
+#include "dshot_bitbang_impl.h"
+#include "drivers/dshot_command.h"
+#include "drivers/motor.h"
+#include "drivers/nvic.h"
+#include "pwm_output_dshot_shared.h"
+#include "drivers/dshot_bitbang_decode.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+
+#include "pg/motor.h"
+#include "pg/pinio.h"
+
+// DEBUG_DSHOT_TELEMETRY_COUNTS
+// 0 - Count of telemetry packets read
+// 1 - Count of missing edge
+// 2 - Count of reception not complete in time
+// 3 - Number of high bits before telemetry start
+
+// Maximum time to wait for telemetry reception to complete
+#define DSHOT_TELEMETRY_TIMEOUT 2000
+
+// For MCUs that use MPU to control DMA coherency, there might be a performance hit
+// on manipulating input buffer content especially if it is read multiple times,
+// as the buffer region is attributed as not cachable.
+// If this is not desirable, we should use manual cache invalidation.
+#ifdef USE_DSHOT_CACHE_MGMT
+#define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32)))
+#define BB_INPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32)))
+#else
+#if defined(GD32F4)
+#define BB_OUTPUT_BUFFER_ATTRIBUTE
+#define BB_INPUT_BUFFER_ATTRIBUTE
+#endif
+#endif // USE_DSHOT_CACHE_MGMT
+
+BB_OUTPUT_BUFFER_ATTRIBUTE uint32_t bbOutputBuffer[MOTOR_DSHOT_BUF_CACHE_ALIGN_LENGTH * MAX_SUPPORTED_MOTOR_PORTS];
+BB_INPUT_BUFFER_ATTRIBUTE uint16_t bbInputBuffer[DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_LENGTH * MAX_SUPPORTED_MOTOR_PORTS];
+
+uint8_t bbPuPdMode;
+FAST_DATA_ZERO_INIT timeUs_t dshotFrameUs;
+
+ const timerHardware_t bbTimerHardware[] = {
+ #if defined(GD32F4)
+ DEF_TIMER(TIMER7, CH0, NONE, 0, 1),
+ DEF_TIMER(TIMER7, CH1, NONE, 0, 1),
+ DEF_TIMER(TIMER7, CH2, NONE, 0, 1),
+ DEF_TIMER(TIMER7, CH3, NONE, 0, 0),
+
+ DEF_TIMER(TIMER0, CH0, NONE, 0, 1),
+ DEF_TIMER(TIMER0, CH0, NONE, 0, 2),
+ DEF_TIMER(TIMER0, CH1, NONE, 0, 1),
+ DEF_TIMER(TIMER0, CH2, NONE, 0, 1),
+ DEF_TIMER(TIMER0, CH3, NONE, 0, 0),
+
+ #else
+ #error MCU dependent code required
+ #endif
+ };
+
+static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
+
+static motorProtocolTypes_e motorProtocol;
+
+// DMA GPIO output buffer formatting
+
+static void bbOutputDataInit(uint32_t *buffer, uint16_t portMask, bool inverted)
+{
+ uint32_t resetMask;
+ uint32_t setMask;
+
+ if (inverted) {
+ resetMask = portMask;
+ setMask = (portMask << 16);
+ } else {
+ resetMask = (portMask << 16);
+ setMask = portMask;
+ }
+
+ int symbol_index;
+
+ for (symbol_index = 0; symbol_index < MOTOR_DSHOT_FRAME_BITS; symbol_index++) {
+ buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 0] |= setMask ; // Always set all ports
+ buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 1] = 0; // Reset bits are port dependent
+ buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 2] |= resetMask; // Always reset all ports
+ }
+
+ //
+ // output one more 'bit' that keeps the line level at idle to allow the ESC to sample the last bit
+ //
+ // Avoid CRC errors in the case of bi-directional d-shot. CRC errors can occur if the output is
+ // transitioned to an input before the signal has been sampled by the ESC as the sampled voltage
+ // may be somewhere between logic-high and logic-low depending on how the motor output line is
+ // driven or floating. On some MCUs it's observed that the voltage momentarily drops low on transition
+ // to input.
+
+ int hold_bit_index = MOTOR_DSHOT_FRAME_BITS * MOTOR_DSHOT_STATE_PER_SYMBOL;
+ buffer[hold_bit_index + 0] |= resetMask; // Always reset all ports
+ buffer[hold_bit_index + 1] = 0; // Never any change
+ buffer[hold_bit_index + 2] = 0; // Never any change
+}
+
+static void bbOutputDataSet(uint32_t *buffer, int pinNumber, uint16_t value, bool inverted)
+{
+ uint32_t middleBit;
+
+ if (inverted) {
+ middleBit = (1 << (pinNumber + 0));
+ } else {
+ middleBit = (1 << (pinNumber + 16));
+ }
+
+ for (int pos = 0; pos < 16; pos++) {
+ if (!(value & 0x8000)) {
+ buffer[pos * 3 + 1] |= middleBit;
+ }
+ value <<= 1;
+ }
+}
+
+static void bbOutputDataClear(uint32_t *buffer)
+{
+ // Middle position to no change
+ for (int bitpos = 0; bitpos < 16; bitpos++) {
+ buffer[bitpos * 3 + 1] = 0;
+ }
+}
+
+// bbPacer management
+
+static bbPacer_t *bbFindMotorPacer(TIM_TypeDef *tim)
+{
+ for (int i = 0; i < MAX_MOTOR_PACERS; i++) {
+
+ bbPacer_t *bbPacer = &bbPacers[i];
+
+ if (bbPacer->tim == NULL) {
+ bbPacer->tim = tim;
+ ++usedMotorPacers;
+ return bbPacer;
+ }
+
+ if (bbPacer->tim == tim) {
+ return bbPacer;
+ }
+ }
+
+ return NULL;
+}
+
+// bbPort management
+
+static bbPort_t *bbFindMotorPort(int portIndex)
+{
+ for (int i = 0; i < usedMotorPorts; i++) {
+ if (bbPorts[i].portIndex == portIndex) {
+ return &bbPorts[i];
+ }
+ }
+ return NULL;
+}
+
+static bbPort_t *bbAllocateMotorPort(int portIndex)
+{
+ if (usedMotorPorts >= MAX_SUPPORTED_MOTOR_PORTS) {
+ bbStatus = DSHOT_BITBANG_STATUS_TOO_MANY_PORTS;
+ return NULL;
+ }
+
+ bbPort_t *bbPort = &bbPorts[usedMotorPorts];
+
+ if (!bbPort->timhw) {
+ // No more pacer channel available
+ bbStatus = DSHOT_BITBANG_STATUS_NO_PACER;
+ return NULL;
+ }
+
+ bbPort->portIndex = portIndex;
+ bbPort->owner.owner = OWNER_DSHOT_BITBANG;
+ bbPort->owner.resourceIndex = RESOURCE_INDEX(portIndex);
+
+ ++usedMotorPorts;
+
+ return bbPort;
+}
+
+const timerHardware_t *dshotBitbangTimerGetAllocatedByNumberAndChannel(int8_t timerNumber, uint16_t timerChannel)
+{
+ for (int index = 0; index < usedMotorPorts; index++) {
+ const timerHardware_t *bitbangTimer = bbPorts[index].timhw;
+ if (bitbangTimer && timerGetTIMNumber(bitbangTimer->tim) == timerNumber && bitbangTimer->channel == timerChannel && bbPorts[index].owner.owner) {
+ return bitbangTimer;
+ }
+ }
+
+ return NULL;
+}
+
+const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer)
+{
+ for (int index = 0; index < usedMotorPorts; index++) {
+ const timerHardware_t *bitbangTimer = bbPorts[index].timhw;
+ if (bitbangTimer && bitbangTimer == timer) {
+ return &bbPorts[index].owner;
+ }
+ }
+
+ return &freeOwner;
+}
+
+// Return frequency of smallest change [state/sec]
+
+static uint32_t getDshotBaseFrequency(motorProtocolTypes_e pwmProtocolType)
+{
+ switch (pwmProtocolType) {
+ case(MOTOR_PROTOCOL_DSHOT600):
+ return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
+ case(MOTOR_PROTOCOL_DSHOT300):
+ return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
+ default:
+ case(MOTOR_PROTOCOL_DSHOT150):
+ return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
+ }
+
+ return 0;
+}
+
+static void bbSetupDma(bbPort_t *bbPort)
+{
+ const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(bbPort->dmaResource);
+ dmaEnable(dmaIdentifier);
+ bbPort->dmaSource = timerDmaSource(bbPort->timhw->channel);
+
+ bbPacer_t *bbPacer = bbFindMotorPacer(bbPort->timhw->tim);
+ bbPacer->dmaSources |= bbPort->dmaSource;
+
+ dmaSetHandler(dmaIdentifier, bbDMAIrqHandler, NVIC_BUILD_PRIORITY(2, 1), (uint32_t)bbPort);
+
+ bbDMA_ITConfig(bbPort);
+}
+
+FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
+{
+ dbgPinHi(0);
+
+ bbPort_t *bbPort = (bbPort_t *)descriptor->userParam;
+
+ bbDMA_Cmd(bbPort, DISABLE);
+
+ bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, DISABLE);
+
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_INT_FLAG_TAE)) {
+ DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_TAE); // Clear Transfer Error flag
+ bbDMA_Cmd(bbPort, DISABLE);
+ bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, DISABLE);
+ }
+
+ DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_FTF);
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
+ bbPort->telemetryPending = false;
+#ifdef DEBUG_COUNT_INTERRUPT
+ bbPort->inputIrq++;
+#endif
+ // Disable DMA as telemetry reception is complete
+ bbDMA_Cmd(bbPort, DISABLE);
+ } else {
+#ifdef DEBUG_COUNT_INTERRUPT
+ bbPort->outputIrq++;
+#endif
+
+ // Switch to input
+
+ bbSwitchToInput(bbPort);
+ bbPort->telemetryPending = true;
+
+ bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE);
+ }
+ }
+#endif
+ dbgPinLo(0);
+}
+
+// Setup bbPorts array elements so that they each have a TIM1 or TIM8 channel
+// in timerHardware array for BB-DShot.
+
+static void bbFindPacerTimer(void)
+{
+ for (int bbPortIndex = 0; bbPortIndex < MAX_SUPPORTED_MOTOR_PORTS; bbPortIndex++) {
+ for (unsigned timerIndex = 0; timerIndex < ARRAYLEN(bbTimerHardware); timerIndex++) {
+ const timerHardware_t *timer = &bbTimerHardware[timerIndex];
+ int timNumber = timerGetTIMNumber(timer->tim);
+ if ((motorConfig()->dev.useDshotBitbangedTimer == DSHOT_BITBANGED_TIMER_TIM1 && timNumber != 1)
+ || (motorConfig()->dev.useDshotBitbangedTimer == DSHOT_BITBANGED_TIMER_TIM8 && timNumber != 8)) {
+ continue;
+ }
+ bool timerConflict = false;
+ for (int channel = 0; channel < CC_CHANNELS_PER_TIMER; channel++) {
+ const timerHardware_t *timer = timerGetAllocatedByNumberAndChannel(timNumber, CC_CHANNEL_FROM_INDEX(channel));
+ const resourceOwner_e timerOwner = timerGetOwner(timer)->owner;
+ if (timerOwner != OWNER_FREE && timerOwner != OWNER_DSHOT_BITBANG) {
+ timerConflict = true;
+ break;
+ }
+ }
+
+ for (int index = 0; index < bbPortIndex; index++) {
+ const timerHardware_t* t = bbPorts[index].timhw;
+ if (timerGetTIMNumber(t->tim) == timNumber && timer->channel == t->channel) {
+ timerConflict = true;
+ break;
+ }
+ }
+
+ if (timerConflict) {
+ continue;
+ }
+
+#ifdef USE_DMA_SPEC
+ dmaoptValue_t dmaopt = dmaGetOptionByTimer(timer);
+ const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timer->tim, timer->channel, dmaopt);
+ dmaResource_t *dma = dmaChannelSpec->ref;
+#else
+ dmaResource_t *dma = timer->dmaRef;
+#endif
+ dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dma);
+ if (dmaGetOwner(dmaIdentifier)->owner == OWNER_FREE) {
+ bbPorts[bbPortIndex].timhw = timer;
+
+ break;
+ }
+ }
+ }
+}
+
+static void bbTimebaseSetup(bbPort_t *bbPort, motorProtocolTypes_e dshotProtocolType)
+{
+ uint32_t timerclock = timerClock(bbPort->timhw->tim);
+
+ uint32_t outputFreq = getDshotBaseFrequency(dshotProtocolType);
+ dshotFrameUs = 1000000 * 17 * 3 / outputFreq;
+ bbPort->outputARR = timerclock / outputFreq - 1;
+
+ // XXX Explain this formula
+ uint32_t inputFreq = outputFreq * 5 * 2 * DSHOT_BITBANG_TELEMETRY_OVER_SAMPLE / 24;
+ bbPort->inputARR = timerclock / inputFreq - 1;
+}
+
+//
+// bb only use pin info associated with timerHardware entry designated as TIM_USE_MOTOR;
+// it does not use the timer channel associated with the pin.
+//
+
+static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output)
+{
+ // Return if no GPIO is specified
+ if (!io) {
+ return false;
+ }
+
+ int pinIndex = IO_GPIOPinIdx(io);
+ int portIndex = IO_GPIOPortIdx(io);
+
+ bbPort_t *bbPort = bbFindMotorPort(portIndex);
+
+ if (!bbPort) {
+
+ // New port group
+
+ bbPort = bbAllocateMotorPort(portIndex);
+
+ if (bbPort) {
+ const timerHardware_t *timhw = bbPort->timhw;
+
+#ifdef USE_DMA_SPEC
+ const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timhw->tim, timhw->channel, dmaGetOptionByTimer(timhw));
+ bbPort->dmaResource = dmaChannelSpec->ref;
+ bbPort->dmaChannel = dmaChannelSpec->channel;
+#else
+ bbPort->dmaResource = timhw->dmaRef;
+ bbPort->dmaChannel = timhw->dmaChannel;
+#endif
+ }
+
+ if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
+ return false;
+ }
+
+ bbPort->gpio = IO_GPIO(io);
+
+ bbPort->portOutputCount = MOTOR_DSHOT_BUF_LENGTH;
+ bbPort->portOutputBuffer = &bbOutputBuffer[(bbPort - bbPorts) * MOTOR_DSHOT_BUF_CACHE_ALIGN_LENGTH];
+
+ bbPort->portInputCount = DSHOT_BB_PORT_IP_BUF_LENGTH;
+ bbPort->portInputBuffer = &bbInputBuffer[(bbPort - bbPorts) * DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_LENGTH];
+
+ bbTimebaseSetup(bbPort, pwmProtocolType);
+ bbTIM_TimeBaseInit(bbPort, bbPort->outputARR);
+ bbTimerChannelInit(bbPort);
+
+ bbSetupDma(bbPort);
+ bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_OUTPUT);
+ bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_INPUT);
+
+ bbDMA_ITConfig(bbPort);
+ }
+
+ bbMotors[motorIndex].pinIndex = pinIndex;
+ bbMotors[motorIndex].io = io;
+ bbMotors[motorIndex].output = output;
+ bbMotors[motorIndex].bbPort = bbPort;
+
+ IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+
+ // Setup GPIO_MODER and GPIO_ODR register manipulation values
+
+ bbGpioSetup(&bbMotors[motorIndex]);
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_INVERTED);
+ } else
+#endif
+ {
+ bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_NONINVERTED);
+ }
+
+ bbSwitchToOutput(bbPort);
+
+ bbMotors[motorIndex].configured = true;
+
+ return true;
+}
+
+static bool bbTelemetryWait(void)
+{
+ // Wait for telemetry reception to complete
+ bool telemetryPending;
+ bool telemetryWait = false;
+ const timeUs_t startTimeUs = micros();
+
+ do {
+ telemetryPending = false;
+ for (int i = 0; i < usedMotorPorts; i++) {
+ telemetryPending |= bbPorts[i].telemetryPending;
+ }
+
+ telemetryWait |= telemetryPending;
+
+ if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
+ break;
+ }
+ } while (telemetryPending);
+
+ if (telemetryWait) {
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
+ }
+
+ return telemetryWait;
+}
+
+static void bbUpdateInit(void)
+{
+ for (int i = 0; i < usedMotorPorts; i++) {
+ bbOutputDataClear(bbPorts[i].portOutputBuffer);
+ }
+}
+
+static bool bbDecodeTelemetry(void)
+{
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+#ifdef USE_DSHOT_TELEMETRY_STATS
+ const timeMs_t currentTimeMs = millis();
+#endif
+
+#ifdef USE_DSHOT_CACHE_MGMT
+ for (int i = 0; i < usedMotorPorts; i++) {
+ bbPort_t *bbPort = &bbPorts[i];
+ SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
+ }
+#endif
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
+#ifdef GD32F4
+ uint32_t rawValue = decode_bb_bitband(
+ bbMotors[motorIndex].bbPort->portInputBuffer,
+ bbMotors[motorIndex].bbPort->portInputCount,
+ bbMotors[motorIndex].pinIndex);
+#else
+ uint32_t rawValue = decode_bb(
+ bbMotors[motorIndex].bbPort->portInputBuffer,
+ bbMotors[motorIndex].bbPort->portInputCount,
+ bbMotors[motorIndex].pinIndex);
+#endif
+ if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
+ continue;
+ }
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
+ dshotTelemetryState.readCount++;
+
+ if (rawValue != DSHOT_TELEMETRY_INVALID) {
+ // Check EDT enable or store raw value
+ if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
+ dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
+ } else {
+ dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
+ }
+ } else {
+ dshotTelemetryState.invalidPacketCount++;
+ }
+#ifdef USE_DSHOT_TELEMETRY_STATS
+ updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
+#endif
+ }
+
+ dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
+ }
+#endif
+
+ return true;
+}
+
+static void bbWriteInt(uint8_t motorIndex, uint16_t value)
+{
+ bbMotor_t *const bbmotor = &bbMotors[motorIndex];
+
+ if (!bbmotor->configured) {
+ return;
+ }
+
+ // If there is a command ready to go overwrite the value and send that instead
+ if (dshotCommandIsProcessing()) {
+ value = dshotCommandGetCurrent(motorIndex);
+ if (value) {
+ bbmotor->protocolControl.requestTelemetry = true;
+ }
+ }
+
+ bbmotor->protocolControl.value = value;
+
+ uint16_t packet = prepareDshotPacket(&bbmotor->protocolControl);
+
+ bbPort_t *bbPort = bbmotor->bbPort;
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_INVERTED);
+ } else
+#endif
+ {
+ bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_NONINVERTED);
+ }
+}
+
+static void bbWrite(uint8_t motorIndex, float value)
+{
+ bbWriteInt(motorIndex, lrintf(value));
+}
+
+static void bbUpdateComplete(void)
+{
+ // If there is a dshot command loaded up, time it correctly with motor update
+
+ if (!dshotCommandQueueEmpty()) {
+ if (!dshotCommandOutputIsEnabled(dshotMotorCount)) {
+ return;
+ }
+ }
+
+ for (int i = 0; i < usedMotorPorts; i++) {
+ bbPort_t *bbPort = &bbPorts[i];
+#ifdef USE_DSHOT_CACHE_MGMT
+ SCB_CleanDCache_by_Addr(bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
+#endif
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
+ bbPort->inputActive = false;
+ bbSwitchToOutput(bbPort);
+ }
+ } else
+#endif
+ {
+ // Nothing to do
+ }
+
+ bbDMA_Cmd(bbPort, ENABLE);
+ }
+
+ lastSendUs = micros();
+ for (int i = 0; i < usedMotorPacers; i++) {
+ bbPacer_t *bbPacer = &bbPacers[i];
+ bbTIM_DMACmd(bbPacer->tim, bbPacer->dmaSources, ENABLE);
+ }
+}
+
+static bool bbEnableMotors(void)
+{
+ for (int i = 0; i < dshotMotorCount; i++) {
+ if (bbMotors[i].configured) {
+ IOConfigGPIO(bbMotors[i].io, bbMotors[i].iocfg);
+ }
+ }
+ return true;
+}
+
+static void bbDisableMotors(void)
+{
+ return;
+}
+
+static void bbShutdown(void)
+{
+ return;
+}
+
+static bool bbIsMotorEnabled(unsigned index)
+{
+ return bbMotors[index].enabled;
+}
+
+static void bbPostInit(void)
+{
+ bbFindPacerTimer();
+
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
+
+ if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) {
+ return;
+ }
+
+ bbMotors[motorIndex].enabled = true;
+ }
+}
+
+static const motorVTable_t bbVTable = {
+ .postInit = bbPostInit,
+ .enable = bbEnableMotors,
+ .disable = bbDisableMotors,
+ .isMotorEnabled = bbIsMotorEnabled,
+ .telemetryWait = bbTelemetryWait,
+ .decodeTelemetry = bbDecodeTelemetry,
+ .updateInit = bbUpdateInit,
+ .write = bbWrite,
+ .writeInt = bbWriteInt,
+ .updateComplete = bbUpdateComplete,
+ .convertExternalToMotor = dshotConvertFromExternal,
+ .convertMotorToExternal = dshotConvertToExternal,
+ .shutdown = bbShutdown,
+ .isMotorIdle = bbDshotIsMotorIdle,
+ .requestTelemetry = bbDshotRequestTelemetry,
+ .getMotorIO = bbGetMotorIO,
+};
+
+dshotBitbangStatus_e dshotBitbangGetStatus(void)
+{
+ return bbStatus;
+}
+
+bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)
+{
+ dbgPinLo(0);
+ dbgPinLo(1);
+
+ if (!device || !motorConfig) {
+ return false;
+ }
+
+ motorProtocol = motorConfig->motorProtocol;
+ device->vTable = &bbVTable;
+
+ dshotMotorCount = device->count;
+ bbStatus = DSHOT_BITBANG_STATUS_OK;
+
+#ifdef USE_DSHOT_TELEMETRY
+ useDshotTelemetry = motorConfig->useDshotTelemetry;
+#endif
+
+ memset(bbOutputBuffer, 0, sizeof(bbOutputBuffer));
+
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
+ const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
+ const timerHardware_t *timerHardware = timerGetConfiguredByTag(motorConfig->ioTags[reorderedMotorIndex]);
+ const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]);
+
+ uint8_t output = motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output;
+ bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP;
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ output ^= TIMER_OUTPUT_INVERTED;
+ }
+#endif
+
+ if (!IOIsFreeOrPreinit(io)) {
+ /* not enough motors initialised for the mixer or a break in the motors */
+ device->vTable = NULL;
+ dshotMotorCount = 0;
+ bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
+ return false;
+ }
+
+ int pinIndex = IO_GPIOPinIdx(io);
+
+ bbMotors[motorIndex].pinIndex = pinIndex;
+ bbMotors[motorIndex].io = io;
+ bbMotors[motorIndex].output = output;
+
+ bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_MODE_OUTPUT, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, bbPuPdMode);
+
+ IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+ IOConfigGPIO(io, bbMotors[motorIndex].iocfg);
+ if (output & TIMER_OUTPUT_INVERTED) {
+ IOLo(io);
+ } else {
+ IOHi(io);
+ }
+ }
+
+ return true;
+}
+
+#endif // USE_DSHOT_BB
diff --git a/src/platform/GD32/dshot_bitbang_stdperiph.c b/src/platform/GD32/dshot_bitbang_stdperiph.c
new file mode 100755
index 0000000000..e7e72bed37
--- /dev/null
+++ b/src/platform/GD32/dshot_bitbang_stdperiph.c
@@ -0,0 +1,296 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_DSHOT_BITBANG
+
+#include "build/atomic.h"
+#include "build/debug.h"
+#include "build/debug_pin.h"
+
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "drivers/dma.h"
+#include "drivers/dma_reqmap.h"
+#include "drivers/dshot.h"
+#include "dshot_bitbang_impl.h"
+#include "drivers/dshot_command.h"
+#include "drivers/motor.h"
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+#include "pg/motor.h"
+
+void bbGpioSetup(bbMotor_t *bbMotor)
+{
+ bbPort_t *bbPort = bbMotor->bbPort;
+ int pinIndex = bbMotor->pinIndex;
+
+ bbPort->gpioModeMask |= (CTL_CLTR(3) << (pinIndex * 2));
+ bbPort->gpioModeInput |= (GPIO_MODE_INPUT << (pinIndex * 2));
+ bbPort->gpioModeOutput |= (GPIO_MODE_OUTPUT << (pinIndex * 2));
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ bbPort->gpioIdleBSRR |= (1 << pinIndex); // BS (lower half)
+ } else
+#endif
+ {
+ bbPort->gpioIdleBSRR |= (1 << (pinIndex + 16)); // BR (higher half)
+ }
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ IOWrite(bbMotor->io, 1);
+ } else
+#endif
+ {
+ IOWrite(bbMotor->io, 0);
+ }
+}
+
+void bbTimerChannelInit(bbPort_t *bbPort)
+{
+ const timerHardware_t *timhw = bbPort->timhw;
+
+ timer_oc_parameter_struct timer_ocintpara;
+
+ timer_channel_output_struct_para_init(&timer_ocintpara);
+
+ timer_channel_output_mode_config((uint32_t)(timhw->tim), timhw->channel, TIMER_OC_MODE_PWM0);
+
+ timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_HIGH;
+ timer_ocintpara.outputstate = TIMER_CCX_ENABLE;
+ timer_ocintpara.ocpolarity = TIMER_OC_POLARITY_LOW;
+
+ timer_channel_output_pulse_value_config((uint32_t)(timhw->tim), timhw->channel, 10);
+
+ timer_disable((uint32_t)(bbPort->timhw->tim));
+
+ timerOCInit(timhw->tim, timhw->channel, &timer_ocintpara);
+ timerOCModeConfig(timhw->tim, timhw->channel, TIMER_OC_MODE_PWM0);
+
+#ifdef DEBUG_MONITOR_PACER
+ if (timhw->tag) {
+ IO_t io = IOGetByTag(timhw->tag);
+ IOConfigGPIOAF(io, IOCFG_AF_PP, timhw->alternateFunction);
+ IOInit(io, OWNER_DSHOT_BITBANG, 0);
+ timer_primary_output_config((uint32_t)(timhw->tim), ENABLE);
+ }
+#endif
+
+ // Enable and keep it running
+
+ timer_enable((uint32_t)(bbPort->timhw->tim));
+}
+
+#ifdef USE_DMA_REGISTER_CACHE
+
+static void bbLoadDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache)
+{
+ uint32_t dma_periph;
+ int dma_channel;
+ gd32_dma_chbase_parse((uint32_t)dmaResource, &dma_periph, &dma_channel);
+
+ DMA_CHCTL(dma_periph, dma_channel) = dmaRegCache->CHCTL;
+ DMA_CHCNT(dma_periph, dma_channel) = dmaRegCache->CHCNT;
+ DMA_CHPADDR(dma_periph, dma_channel) = dmaRegCache->CHPADDR;
+ DMA_CHM0ADDR(dma_periph, dma_channel) = dmaRegCache->CHM0ADDR;
+ DMA_CHFCTL(dma_periph, dma_channel) = dmaRegCache->CHFCTL;
+}
+
+static void bbSaveDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache)
+{
+ uint32_t dma_periph;
+ int dma_channel;
+ gd32_dma_chbase_parse((uint32_t)dmaResource, &dma_periph, &dma_channel);
+
+ dmaRegCache->CHCTL = DMA_CHCTL(dma_periph, dma_channel);
+ dmaRegCache->CHCNT = DMA_CHCNT(dma_periph, dma_channel);
+ dmaRegCache->CHPADDR = DMA_CHPADDR(dma_periph, dma_channel);
+ dmaRegCache->CHM0ADDR = DMA_CHM0ADDR(dma_periph, dma_channel);
+ dmaRegCache->CHFCTL = DMA_CHFCTL(dma_periph, dma_channel);
+}
+#endif
+
+void bbSwitchToOutput(bbPort_t * bbPort)
+{
+ dbgPinHi(1);
+ // Output idle level before switching to output
+ // Use BOP register for this
+ // Normal: Use CR (higher half)
+ // Inverted: Use BOP (lower half)
+
+ WRITE_REG(GPIO_BOP((uint32_t)bbPort->gpio), bbPort->gpioIdleBSRR);
+
+ // Set GPIO to output
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ MODIFY_REG(GPIO_CTL((uint32_t)bbPort->gpio), bbPort->gpioModeMask, bbPort->gpioModeOutput);
+ }
+
+ // Reinitialize port group DMA for output
+
+ dmaResource_t *dmaResource = bbPort->dmaResource;
+#ifdef USE_DMA_REGISTER_CACHE
+ bbLoadDMARegs(dmaResource, &bbPort->dmaRegOutput);
+#else
+ xDMA_DeInit(dmaResource);
+ xDMA_Init(dmaResource, &bbPort->outputDmaInit);
+ // Needs this, as it is DeInit'ed above...
+ xDMA_ITConfig(dmaResource, DMA_INT_FTF, ENABLE);
+#endif
+
+ // Reinitialize pacer timer for output
+
+ TIMER_CAR((uint32_t)(bbPort->timhw->tim)) = bbPort->outputARR;
+
+ bbPort->direction = DSHOT_BITBANG_DIRECTION_OUTPUT;
+
+ dbgPinLo(1);
+}
+
+#ifdef USE_DSHOT_TELEMETRY
+void bbSwitchToInput(bbPort_t *bbPort)
+{
+ dbgPinHi(1);
+
+ // Set GPIO to input
+
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ MODIFY_REG(GPIO_CTL((uint32_t)bbPort->gpio), bbPort->gpioModeMask, bbPort->gpioModeInput);
+ }
+
+ // Reinitialize port group DMA for input
+
+ dmaResource_t *dmaResource = bbPort->dmaResource;
+#ifdef USE_DMA_REGISTER_CACHE
+ bbLoadDMARegs(dmaResource, &bbPort->dmaRegInput);
+#else
+ xDMA_DeInit(dmaResource);
+ xDMA_Init(dmaResource, &bbPort->inputDmaInit);
+ // Needs this, as it is DeInit'ed above...
+ xDMA_ITConfig(dmaResource, DMA_INT_FTF, ENABLE);
+#endif
+
+ // Reinitialize pacer timer for input
+
+ TIMER_CNT((uint32_t)(bbPort->timhw->tim)) = 0;
+ TIMER_CAR((uint32_t)(bbPort->timhw->tim)) = bbPort->inputARR;
+
+ bbDMA_Cmd(bbPort, ENABLE);
+
+ bbPort->direction = DSHOT_BITBANG_DIRECTION_INPUT;
+
+ dbgPinLo(1);
+}
+#endif
+
+void bbDMAPreconfigure(bbPort_t *bbPort, uint8_t direction)
+{
+ DMA_InitTypeDef *gener_dmainit = (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) ? &bbPort->outputDmaInit : &bbPort->inputDmaInit;
+ gener_dmainit->data_mode = DMA_DATA_MODE_MULTI;
+ dma_multi_data_parameter_struct *dmainit = &gener_dmainit->config.init_struct_m;
+
+ dma_multi_data_para_struct_init(dmainit);
+
+ gener_dmainit->sub_periph = bbPort->dmaChannel;
+
+ dmainit->circular_mode = DMA_CIRCULAR_MODE_DISABLE;
+ dmainit->periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ dmainit->memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+ dmainit->critical_value = DMA_FIFO_STATUS_1_WORD;
+ dmainit->memory_burst_width = DMA_MEMORY_BURST_SINGLE;
+ dmainit->periph_burst_width = DMA_PERIPH_BURST_SINGLE;
+
+ if (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) {
+ dmainit->priority = DMA_PRIORITY_HIGH;
+ dmainit->direction = DMA_MEMORY_TO_PERIPH;
+ dmainit->number = bbPort->portOutputCount;
+ dmainit->periph_addr = (uint32_t)&GPIO_BOP((uint32_t)bbPort->gpio);
+ dmainit->periph_width = DMA_PERIPH_WIDTH_32BIT;
+ dmainit->memory0_addr = (uint32_t)bbPort->portOutputBuffer;
+ dmainit->memory_width = DMA_MEMORY_WIDTH_32BIT;
+
+#ifdef USE_DMA_REGISTER_CACHE
+ xDMA_Init(bbPort->dmaResource, gener_dmainit);
+ bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegOutput);
+#endif
+ } else {
+ dmainit->priority = DMA_PRIORITY_ULTRA_HIGH;
+ dmainit->direction = DMA_PERIPH_TO_MEMORY;
+ dmainit->number = bbPort->portInputCount;
+ dmainit->periph_addr = (uint32_t)&GPIO_ISTAT((uint32_t)bbPort->gpio);
+ dmainit->periph_width = DMA_PERIPH_WIDTH_16BIT;
+ dmainit->memory0_addr = (uint32_t)bbPort->portInputBuffer;
+ dmainit->memory_width = DMA_MEMORY_WIDTH_32BIT;
+
+#ifdef USE_DMA_REGISTER_CACHE
+ xDMA_Init(bbPort->dmaResource, gener_dmainit);
+ bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegInput);
+#endif
+ }
+}
+
+void bbTIM_TimeBaseInit(bbPort_t *bbPort, uint16_t period)
+{
+ timer_parameter_struct *init = &bbPort->timeBaseInit;
+
+ init->prescaler = 0; // Feed raw timerClock
+ init->clockdivision = TIMER_CKDIV_DIV1;
+ init->alignedmode = TIMER_COUNTER_EDGE;
+ init->counterdirection = TIMER_COUNTER_UP;
+ init->period = period;
+ init->repetitioncounter = 0;
+
+ timer_init((uint32_t)(bbPort->timhw->tim), init);
+ timer_auto_reload_shadow_enable((uint32_t)(bbPort->timhw->tim));
+}
+
+void bbTIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState)
+{
+ if(ENABLE == NewState){
+ timer_dma_enable((uint32_t)TIMx, TIM_DMASource);
+ } else {
+ timer_dma_disable((uint32_t)TIMx, TIM_DMASource);
+ }
+}
+
+void bbDMA_ITConfig(bbPort_t *bbPort)
+{
+ xDMA_ITConfig(bbPort->dmaResource, DMA_INT_FTF, ENABLE);
+}
+
+void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState)
+{
+ xDMA_Cmd(bbPort->dmaResource, NewState);
+}
+
+int bbDMA_Count(bbPort_t *bbPort)
+{
+ return xDMA_GetCurrDataCounter(bbPort->dmaResource);
+}
+
+#endif // USE_DSHOT_BB
diff --git a/src/platform/GD32/exti_gd32.c b/src/platform/GD32/exti_gd32.c
new file mode 100755
index 0000000000..69189a0254
--- /dev/null
+++ b/src/platform/GD32/exti_gd32.c
@@ -0,0 +1,188 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_EXTI
+
+#include "drivers/nvic.h"
+#include "drivers/io_impl.h"
+#include "drivers/exti.h"
+#include "platform/rcc.h"
+
+typedef struct {
+ extiCallbackRec_t* handler;
+} extiChannelRec_t;
+
+extiChannelRec_t extiChannelRecs[16];
+
+// IRQ grouping
+#define EXTI_IRQ_GROUPS 7
+// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
+static const uint8_t extiGroups[16] = { 0, 1, 2, 3, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6 };
+static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS];
+
+#if defined(GD32F4)
+static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
+ EXTI0_IRQn,
+ EXTI1_IRQn,
+ EXTI2_IRQn,
+ EXTI3_IRQn,
+ EXTI4_IRQn,
+ EXTI5_9_IRQn,
+ EXTI10_15_IRQn
+};
+#else
+# warning "Unknown MCU"
+#endif
+
+static uint32_t triggerLookupTable[] = {
+ [BETAFLIGHT_EXTI_TRIGGER_RISING] = EXTI_TRIG_RISING,
+ [BETAFLIGHT_EXTI_TRIGGER_FALLING] = EXTI_TRIG_FALLING,
+ [BETAFLIGHT_EXTI_TRIGGER_BOTH] = EXTI_TRIG_BOTH
+};
+
+void EXTIInit(void)
+{
+ /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
+ RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE);
+
+ memset(extiChannelRecs, 0, sizeof(extiChannelRecs));
+ memset(extiGroupPriority, 0xff, sizeof(extiGroupPriority));
+}
+
+void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn)
+{
+ self->fn = fn;
+}
+
+void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config, extiTrigger_t trigger)
+{
+ int chIdx = IO_GPIOPinIdx(io);
+
+ if (chIdx < 0) {
+ return;
+ }
+
+ int group = extiGroups[chIdx];
+
+ extiChannelRec_t *rec = &extiChannelRecs[chIdx];
+ rec->handler = cb;
+
+ EXTIDisable(io);
+
+ IOConfigGPIO(io, config);
+
+ syscfg_exti_line_config(IO_GPIO_PortSource(io), IO_GPIO_PinSource(io));
+
+ uint32_t extiLine = IO_EXTI_Line(io);
+
+ exti_init(extiLine, EXTI_INTERRUPT, triggerLookupTable[trigger]);
+
+ if (extiGroupPriority[group] > irqPriority) {
+ extiGroupPriority[group] = irqPriority;
+ nvic_irq_enable(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+ }
+}
+
+void EXTIRelease(IO_t io)
+{
+ // don't forget to match cleanup with config
+ EXTIDisable(io);
+
+ const int chIdx = IO_GPIOPinIdx(io);
+
+ if (chIdx < 0) {
+ return;
+ }
+
+ extiChannelRec_t *rec = &extiChannelRecs[chIdx];
+ rec->handler = NULL;
+}
+
+void EXTIEnable(IO_t io)
+{
+#if defined(GD32F4)
+ uint32_t extiLine = IO_EXTI_Line(io);
+
+ if (!extiLine) {
+ return;
+ }
+
+ EXTI_INTEN |= extiLine;
+#else
+# error "Unknown MCU"
+#endif
+}
+
+void EXTIDisable(IO_t io)
+{
+ (void) io;
+#if defined(GD32F4)
+ uint32_t extiLine = IO_EXTI_Line(io);
+
+ if (!extiLine) {
+ return;
+ }
+
+ EXTI_INTEN &= ~extiLine;
+ EXTI_PD = extiLine;
+#else
+# error "Unknown MCU"
+#endif
+}
+
+#define EXTI_EVENT_MASK 0xFFFF // first 16 bits only, see also definition of extiChannelRecs.
+
+static void EXTI_IRQHandler(uint32_t mask)
+{
+ uint32_t exti_active = (EXTI_INTEN & EXTI_PD) & mask;
+
+ EXTI_PD = exti_active; // clear pending mask (by writing 1)
+
+ while (exti_active) {
+ unsigned idx = 31 - __builtin_clz(exti_active);
+ uint32_t mask = 1 << idx;
+ extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler);
+ exti_active &= ~mask;
+ }
+}
+
+#define _EXTI_IRQ_HANDLER(name, mask) \
+ void name(void) { \
+ EXTI_IRQHandler(mask & EXTI_EVENT_MASK); \
+ } \
+ struct dummy \
+ /**/
+
+_EXTI_IRQ_HANDLER(EXTI0_IRQHandler, 0x0001);
+_EXTI_IRQ_HANDLER(EXTI1_IRQHandler, 0x0002);
+_EXTI_IRQ_HANDLER(EXTI2_IRQHandler, 0x0004);
+_EXTI_IRQ_HANDLER(EXTI3_IRQHandler, 0x0008);
+_EXTI_IRQ_HANDLER(EXTI4_IRQHandler, 0x0010);
+_EXTI_IRQ_HANDLER(EXTI5_9_IRQHandler, 0x03e0);
+_EXTI_IRQ_HANDLER(EXTI10_15_IRQHandler, 0xfc00);
+
+#endif
diff --git a/src/platform/GD32/include/platform/dma.h b/src/platform/GD32/include/platform/dma.h
new file mode 100755
index 0000000000..0a1531c1bd
--- /dev/null
+++ b/src/platform/GD32/include/platform/dma.h
@@ -0,0 +1,115 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include "platform.h"
+#include "drivers/resource.h"
+
+#if defined(GD32F4)
+#define PLATFORM_TRAIT_DMA_STREAM_REQUIRED 1
+#endif
+
+typedef enum {
+ DMA_NONE = 0,
+ DMA_FIRST_HANDLER = 1,
+ DMA0_CH0_HANDLER = DMA_FIRST_HANDLER,
+ DMA0_CH1_HANDLER,
+ DMA0_CH2_HANDLER,
+ DMA0_CH3_HANDLER,
+ DMA0_CH4_HANDLER,
+ DMA0_CH5_HANDLER,
+ DMA0_CH6_HANDLER,
+ DMA0_CH7_HANDLER,
+ DMA1_CH0_HANDLER,
+ DMA1_CH1_HANDLER,
+ DMA1_CH2_HANDLER,
+ DMA1_CH3_HANDLER,
+ DMA1_CH4_HANDLER,
+ DMA1_CH5_HANDLER,
+ DMA1_CH6_HANDLER,
+ DMA1_CH7_HANDLER,
+ DMA_LAST_HANDLER = DMA1_CH7_HANDLER
+} dmaIdentifier_e;
+
+/* DMA general initialize struct */
+typedef struct
+{
+ uint32_t single_mode; /* single or multiple mode */
+ int sub_periph; /* specify DMA channel peripheral */
+ void * general_init_s; /* single or multiple struct */
+} dma_general_init_struct;
+
+
+uint32_t dmaGetChannel(const uint8_t channel);
+
+#define DMA_DEVICE_NO(x) ((((x)-1) / 8) + 1)
+#define DMA_DEVICE_INDEX(x) ((((x)-1) % 8))
+#define DMA_OUTPUT_INDEX 0
+#define DMA_OUTPUT_STRING "DMA%d Channel %d:"
+#define DMA_INPUT_STRING "DMA%d_CH%d"
+
+#define DEFINE_DMA_CHANNEL(d, s, f) { \
+ .dma = (void *)d, \
+ .ref = (dmaResource_t *)d ## _CH ## s ## _BASE, \
+ .stream = s, \
+ .irqHandlerCallback = NULL, \
+ .flagsShift = f, \
+ .irqN = d ## _Channel ## s ## _IRQn, \
+ .userParam = 0, \
+ .owner.owner = 0, \
+ .owner.resourceIndex = 0 \
+ }
+
+#define DEFINE_DMA_IRQ_HANDLER(d, s, i) FAST_IRQ_HANDLER void DMA ## d ## _Channel ## s ## _IRQHandler(void) {\
+ const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \
+ dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \
+ if (handler) \
+ handler(&dmaDescriptors[index]); \
+ }
+
+#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) DMA_INTC1((uint32_t)d->dma) = (flag << (d->flagsShift - 32)); else DMA_INTC0((uint32_t)d->dma) = (flag << d->flagsShift)
+#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? DMA_INTF1((uint32_t)d->dma) & (flag << (d->flagsShift - 32)): DMA_INTF0((uint32_t)d->dma) & (flag << d->flagsShift))
+
+extern void gd32_dma_init(uint32_t dma_chan_base, dma_single_data_parameter_struct *init_struct);
+extern void gd32_dma_general_init(uint32_t dma_chan_base, dma_general_config_struct *init_struct);
+extern void gd32_dma_deinit(uint32_t dma_chan_base);
+extern void gd32_dma_channel_state_config(uint32_t dma_chan_base, ControlStatus new_state);
+extern void gd32_dma_int_config(uint32_t dma_chan_base, uint32_t source, ControlStatus new_state);
+extern void gd32_dma_transnum_config(uint32_t dma_chan_base, uint32_t number);
+extern uint16_t gd32_dma_transnum_get(uint32_t dma_chan_base);
+extern FlagStatus gd32_dma_flag_get(uint32_t dma_chan_base, uint32_t flag);
+extern void gd32_dma_flag_clear(uint32_t dma_chan_base, uint32_t flag);
+extern void gd32_dma_chbase_parse(uint32_t dma_chan_base, uint32_t *dma_periph, int *dma_channel);
+extern void gd32_dma_memory_addr_config(uint32_t dma_chan_base, uint32_t address, uint8_t memory_flag);
+
+#define xDMA_Init(dmaResource, initStruct) gd32_dma_general_init((uint32_t)dmaResource, initStruct)
+#define xDMA_DeInit(dmaResource) gd32_dma_deinit((uint32_t)dmaResource)
+#define xDMA_Cmd(dmaResource, newState) gd32_dma_channel_state_config((uint32_t)dmaResource, newState)
+#define xDMA_ITConfig(dmaResource, flags, newState) gd32_dma_int_config((uint32_t)(dmaResource), flags, newState)
+#define xDMA_GetCurrDataCounter(dmaResource) gd32_dma_transnum_get((uint32_t)(dmaResource))
+#define xDMA_SetCurrDataCounter(dmaResource, count) gd32_dma_transnum_config((uint32_t)(dmaResource), count)
+#define xDMA_GetFlagStatus(dmaResource, flags) gd32_dma_flag_get((uint32_t)(dmaResource), flags)
+#define xDMA_ClearFlag(dmaResource, flags) gd32_dma_flag_clear((uint32_t)(dmaResource), flags)
+#define xDMA_MemoryTargetConfig(dmaResource, address, target) gd32_dma_memory_addr_config((uint32_t)(dmaResource), address, target)
+
+extern uint32_t dma_enable_status_get(uint32_t dma_chan_base);
+#define IS_DMA_ENABLED(reg) (dma_enable_status_get((uint32_t)reg))
diff --git a/src/platform/GD32/include/platform/platform.h b/src/platform/GD32/include/platform/platform.h
new file mode 100755
index 0000000000..8f9e63b899
--- /dev/null
+++ b/src/platform/GD32/include/platform/platform.h
@@ -0,0 +1,316 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#if defined(GD32F405) || defined(GD32F407) || defined(GD32F425) || defined(GD32F427) || defined(GD32F450) || defined(GD32F460) || defined(GD32F470)
+
+#include "gd32f4xx.h"
+
+// Chip Unique ID on F4xx
+#define U_ID_0 (*(uint32_t*)0x1fff7a10)
+#define U_ID_1 (*(uint32_t*)0x1fff7a14)
+#define U_ID_2 (*(uint32_t*)0x1fff7a18)
+
+#ifndef GD32F4
+#define GD32F4
+#endif
+
+#endif
+
+#ifdef GD32F4
+
+/* DMA data mode type */
+typedef enum {
+ DMA_DATA_MODE_SINGLE = 0,
+ DMA_DATA_MODE_MULTI = 1
+} dma_data_mode_enum;
+
+/* DMA general configuration struct */
+typedef struct
+{
+ dma_data_mode_enum data_mode;
+ dma_subperipheral_enum sub_periph;
+ union {
+ dma_single_data_parameter_struct init_struct_s;
+ dma_multi_data_parameter_struct init_struct_m;
+ } config;
+} dma_general_config_struct; //todo: move to dma_bsp.h
+
+#endif // GD32F4
+
+#ifdef GD32F4
+
+#define SPI_TRAIT_AF_PORT 1
+#define SPI_TRAIT_AF_PIN 1
+#define I2C_TRAIT_STATE 1
+#define I2C_TRAIT_AF_PIN 1
+#define I2CDEV_COUNT 3
+#define PLATFORM_TRAIT_RCC 1
+#define UART_TRAIT_AF_PORT 1
+#define UART_TRAIT_AF_PIN 1
+#define UART_TRAIT_PINSWAP 1
+#define SERIAL_TRAIT_PIN_CONFIG 1
+#define DMA_TRAIT_CHANNEL 1
+
+#define USE_FAST_DATA
+#define USE_RPM_FILTER
+#define USE_DYN_IDLE
+#define USE_DYN_NOTCH_FILTER
+#define USE_ADC_INTERNAL
+#define USE_USB_CDC_HID
+#define USE_USB_MSC
+#define USE_PERSISTENT_MSC_RTC
+#define USE_MCO
+#define USE_DMA_SPEC
+#define USE_PERSISTENT_OBJECTS
+#define USE_LATE_TASK_STATISTICS
+
+#define SET_BIT(REG, BIT) ((REG) |= (BIT))
+#define CLEAR_BIT(REG, BIT) ((REG) &= ~(BIT))
+#define READ_BIT(REG, BIT) ((REG) & (BIT))
+#define CLEAR_REG(REG) ((REG) = (0x0))
+#define WRITE_REG(REG, VAL) ((REG) = (VAL))
+#define READ_REG(REG) ((REG))
+#define MODIFY_REG(REG, CLEARMASK, SETMASK) WRITE_REG((REG), (((READ_REG(REG)) & (~(CLEARMASK))) | (SETMASK)))
+
+typedef struct I2C_TypeDef I2C_TypeDef;
+typedef struct I2C_HandleTypeDef I2C_HandleTypeDef;
+typedef struct GPIO_TypeDef GPIO_TypeDef;
+typedef struct GPIO_InitTypeDef GPIO_InitTypeDef;
+typedef struct TIM_TypeDef TIM_TypeDef;
+typedef struct DMA_TypeDef DMA_TypeDef;
+typedef struct DMA_Stream_TypeDef DMA_Stream_TypeDef;
+typedef struct DMA_Channel_TypeDef DMA_Channel_TypeDef;
+typedef struct SPI_TypeDef SPI_TypeDef;
+typedef struct ADC_TypeDef ADC_TypeDef;
+typedef struct USART_TypeDef USART_TypeDef;
+typedef struct TIM_Cmd TIM_Cmd;
+typedef struct TIM_CtrlPWMOutputs TIM_CtrlPWMOutputs;
+typedef struct TIM_TimeBaseInit TIM_TimeBaseInit;
+typedef struct TIM_ARRPreloadConfig TIM_ARRPreloadConfig;
+typedef struct EXTI_TypeDef EXTI_TypeDef;
+typedef struct EXTI_InitTypeDef EXTI_InitTypeDef;
+#define DMA_InitTypeDef dma_general_config_struct
+#define TIM_OCInitTypeDef timer_oc_parameter_struct
+#define TIM_ICInitTypeDef timer_ic_parameter_struct
+#define TIM_OCStructInit timer_channel_output_struct_para_init
+#define TIM_TimeBaseInitTypeDef timer_parameter_struct
+
+#define TIM_ICPolarity_Falling TIMER_IC_POLARITY_FALLING
+#define TIM_ICPolarity_Rising TIMER_IC_POLARITY_RISING
+
+#define Bit_RESET 0
+
+#define DMA0_CH0_BASE (DMA0 + 0x10)
+#define DMA0_CH1_BASE (DMA0 + 0x28)
+#define DMA0_CH2_BASE (DMA0 + 0x40)
+#define DMA0_CH3_BASE (DMA0 + 0x58)
+#define DMA0_CH4_BASE (DMA0 + 0x70)
+#define DMA0_CH5_BASE (DMA0 + 0x88)
+#define DMA0_CH6_BASE (DMA0 + 0xA0)
+#define DMA0_CH7_BASE (DMA0 + 0xB8)
+#define DMA1_CH0_BASE (DMA1 + 0x10)
+#define DMA1_CH1_BASE (DMA1 + 0x28)
+#define DMA1_CH2_BASE (DMA1 + 0x40)
+#define DMA1_CH3_BASE (DMA1 + 0x58)
+#define DMA1_CH4_BASE (DMA1 + 0x70)
+#define DMA1_CH5_BASE (DMA1 + 0x88)
+#define DMA1_CH6_BASE (DMA1 + 0xA0)
+#define DMA1_CH7_BASE (DMA1 + 0xB8)
+
+extern void timerOCModeConfig(void *tim, uint8_t channel, uint16_t ocmode);
+extern void gd32_timer_input_capture_config(void* timer, uint16_t channel, uint8_t state);
+extern uint32_t timerPrescaler(const TIM_TypeDef *tim);
+
+#define UART_TX_BUFFER_ATTRIBUTE /* EMPTY */
+#define UART_RX_BUFFER_ATTRIBUTE /* EMPTY */
+
+#endif // GD32F4
+
+#define USE_ADC_DEVICE_0
+
+#define GPIOA_BASE GPIOA
+
+#if defined(GD32F4)
+#define TASK_GYROPID_DESIRED_PERIOD 125 // 125us = 8kHz
+#define SCHEDULER_DELAY_LIMIT 10
+#else
+#define TASK_GYROPID_DESIRED_PERIOD 1000 // 1000us = 1kHz
+#define SCHEDULER_DELAY_LIMIT 100
+#endif
+
+#define DEFAULT_CPU_OVERCLOCK 0
+
+#if defined(GD32F4)
+#define FAST_IRQ_HANDLER
+#endif
+
+#if defined(GD32F4)
+// F4 can't DMA to/from TCM (tightly-coupled memory) SRAM (where the stack lives)
+#define DMA_DATA_ZERO_INIT
+#define DMA_DATA
+#define STATIC_DMA_DATA_AUTO static
+#endif
+
+#if defined(GD32F4)
+// Data in RAM which is guaranteed to not be reset on hot reboot
+#define PERSISTENT __attribute__ ((section(".persistent_data"), aligned(4)))
+#endif
+
+#ifdef USE_DMA_RAM
+// Reserved for other GD series
+#else
+#define DMA_RAM
+#define DMA_RW_AXI
+#define DMA_RAM_R
+#define DMA_RAM_W
+#define DMA_RAM_RW
+
+#endif // USE_DMA_RAM
+
+#define USE_TIMER_MGMT
+#define USE_TIMER_AF
+
+#if defined(GD32F4)
+
+#define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5))
+
+#define IOCFG_OUT_PP IO_CONFIG(GPIO_MODE_OUTPUT, 0, GPIO_OTYPE_PP, GPIO_PUPD_NONE)
+#define IOCFG_OUT_PP_UP IO_CONFIG(GPIO_MODE_OUTPUT, 0, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP)
+#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_MODE_OUTPUT, GPIO_OSPEED_25MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE)
+#define IOCFG_OUT_OD IO_CONFIG(GPIO_MODE_OUTPUT, 0, GPIO_OTYPE_OD, GPIO_PUPD_NONE)
+#define IOCFG_AF_PP IO_CONFIG(GPIO_MODE_AF, 0, GPIO_OTYPE_PP, GPIO_PUPD_NONE)
+#define IOCFG_AF_PP_PD IO_CONFIG(GPIO_MODE_AF, 0, GPIO_OTYPE_PP, GPIO_PUPD_PULLDOWN)
+#define IOCFG_AF_PP_UP IO_CONFIG(GPIO_MODE_AF, 0, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP)
+#define IOCFG_AF_OD IO_CONFIG(GPIO_MODE_AF, 0, GPIO_OTYPE_PP, GPIO_PUPD_NONE)
+#define IOCFG_IPD IO_CONFIG(GPIO_MODE_INPUT, 0, 0, GPIO_PUPD_PULLDOWN)
+#define IOCFG_IPU IO_CONFIG(GPIO_MODE_INPUT, 0, 0, GPIO_PUPD_PULLUP)
+#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_MODE_INPUT, 0, 0, GPIO_PUPD_NONE)
+#define IOCFG_IPU_25 IO_CONFIG(GPIO_MODE_INPUT, GPIO_OSPEED_25MHZ, 0, GPIO_PUPD_PULLUP)
+
+#endif
+
+#if defined(GD32F4)
+#define FLASH_CONFIG_BUFFER_TYPE uint32_t
+#endif
+
+#if defined(GD32F4)
+#define SPI_IO_AF_CFG IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE)
+#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLDOWN)
+#define SPI_IO_AF_SDI_CFG IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP)
+#define SPI_IO_CS_CFG IO_CONFIG(GPIO_MODE_OUTPUT, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE)
+#define SPI_IO_CS_HIGH_CFG IO_CONFIG(GPIO_MODE_INPUT, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP)
+#else
+#error "Invalid GD32 MCU defined - requires SPI implementation"
+#endif
+
+#if defined(GD32F4)
+#if defined(GD32F425)
+#define SPIDEV_COUNT 3
+#elif defined(GD32F460)
+#define SPIDEV_COUNT 5
+#endif
+#else
+#define SPIDEV_COUNT 4
+#endif
+
+#define SPI_RX_DATA_REGISTER(base) ((base)->DR)
+
+#if defined(GD32F4)
+#define MAX_SPI_PIN_SEL 3
+#else
+#error Unknown MCU family
+#endif
+
+#if defined(GD32F4)
+#define USE_TX_IRQ_HANDLER
+#endif
+
+// all pins on given uart use same AF
+
+
+#if defined(GD32F4)
+#define UARTHARDWARE_MAX_PINS 4
+#endif
+
+#if defined(GD32F4)
+#define UART_REG_RXD(base) (USART_DATA((uint32_t)base))
+#define UART_REG_TXD(base) (USART_DATA((uint32_t)base))
+#endif
+
+#define SERIAL_TRAIT_PIN_CONFIG 1
+#define USB_DP_PIN PA12
+
+// Select UART prefix according to UART_DEV
+#define _UART_GET_PREFIX(dev) _UART_GET_PREFIX_##dev
+
+#define _UART_GET_PREFIX_UARTDEV_0 USART
+#define _UART_GET_PREFIX_UARTDEV_1 USART
+#define _UART_GET_PREFIX_UARTDEV_2 USART
+#define _UART_GET_PREFIX_UARTDEV_3 UART
+#define _UART_GET_PREFIX_UARTDEV_4 UART
+#define _UART_GET_PREFIX_UARTDEV_5 USART
+#define _UART_GET_PREFIX_UARTDEV_6 UART
+#define _UART_GET_PREFIX_UARTDEV_7 UART
+#define _UART_GET_PREFIX_UARTDEV_8 UART
+#define _UART_GET_PREFIX_UARTDEV_9 UART
+#define _UART_GET_PREFIX_UARTDEV_10 USART
+#define _UART_GET_PREFIX_UARTDEV_LP1 LPUART
+
+// #if defined(GD32F4)
+// We need to redefine ADC0, ADC1, etc.,
+// in the GD firmware library to be compatible with
+// such as the ADC_TypeDef * type in BF.
+#define GD_ADC0 ((ADC_TypeDef*)ADC_BASE)
+#define GD_ADC1 ((ADC_TypeDef*)(ADC_BASE + 0x100))
+#define GD_ADC2 ((ADC_TypeDef*)(ADC_BASE + 0x200))
+#undef ADC0
+#define ADC0 ((ADC_TypeDef*)GD_ADC0)
+#undef ADC1
+#define ADC1 ((ADC_TypeDef*)GD_ADC1)
+#undef ADC2
+#define ADC2 ((ADC_TypeDef*)GD_ADC2)
+
+#define GD_SPI0 ((SPI_TypeDef*)(SPI_BASE + 0x0000F800U))
+#define GD_SPI1 ((SPI_TypeDef*)SPI_BASE)
+#define GD_SPI2 ((SPI_TypeDef*)(SPI_BASE + 0x00000400U))
+#define GD_SPI3 ((SPI_TypeDef*)(SPI_BASE + 0x0000FC00U))
+#define GD_SPI4 ((SPI_TypeDef*)(SPI_BASE + 0x00011800U))
+#define GD_SPI5 ((SPI_TypeDef*)(SPI_BASE + 0x00011C00U))
+#undef SPI0
+#define SPI0 ((SPI_TypeDef*)GD_SPI0)
+#undef SPI1
+#define SPI1 ((SPI_TypeDef*)GD_SPI1)
+#undef SPI2
+#define SPI2 ((SPI_TypeDef*)GD_SPI2)
+#undef SPI3
+#define SPI3 ((SPI_TypeDef*)GD_SPI3)
+#undef SPI4
+#define SPI4 ((SPI_TypeDef*)GD_SPI4)
+#undef SPI5
+#define SPI5 ((SPI_TypeDef*)GD_SPI5)
+
+
+// We also need to convert the pointer to the uint32_t
+// type required by the GD firmware library.
+#define PERIPH_INT(periph) ((uint32_t)periph)
+// #endif
diff --git a/src/platform/GD32/io_gd32.c b/src/platform/GD32/io_gd32.c
new file mode 100755
index 0000000000..4721832e5b
--- /dev/null
+++ b/src/platform/GD32/io_gd32.c
@@ -0,0 +1,148 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include "platform.h"
+
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "platform/rcc.h"
+
+#include "common/utils.h"
+
+// io ports defs are stored in array by index now
+struct ioPortDef_s {
+ rccPeriphTag_t rcc;
+};
+
+#if defined(GD32F4)
+const struct ioPortDef_s ioPortDefs[] = {
+ { RCC_AHB1(PA) },
+ { RCC_AHB1(PB) },
+ { RCC_AHB1(PC) },
+ { RCC_AHB1(PD) },
+ { RCC_AHB1(PE) },
+ { RCC_AHB1(PF) },
+};
+#else
+# error "IO PortDefs not defined for MCU"
+#endif
+
+uint32_t IO_EXTI_Line(IO_t io)
+{
+ if (!io) {
+ return 0;
+ }
+#if defined(GD32F4)
+ return 1 << IO_GPIOPinIdx(io);
+#elif defined(SIMULATOR_BUILD)
+ return 0;
+#else
+# error "Unknown target type"
+#endif
+ return 0;
+}
+
+bool IORead(IO_t io)
+{
+ if (!io) {
+ return false;
+ }
+
+ return (GPIO_ISTAT(PERIPH_INT(IO_GPIO(io))) & IO_Pin(io));
+}
+
+void IOWrite(IO_t io, bool hi)
+{
+ if (!io) {
+ return;
+ }
+
+ if (hi) {
+ GPIO_BOP(PERIPH_INT(IO_GPIO(io))) = IO_Pin(io);
+ } else {
+ GPIO_BC(PERIPH_INT(IO_GPIO(io))) = IO_Pin(io);
+ }
+}
+
+void IOHi(IO_t io)
+{
+ if (!io) {
+ return;
+ }
+
+ GPIO_BOP(PERIPH_INT(IO_GPIO(io))) = IO_Pin(io);
+}
+
+void IOLo(IO_t io)
+{
+ if (!io) {
+ return;
+ }
+#if defined(GD32F4)
+ GPIO_BC(PERIPH_INT(IO_GPIO(io))) = IO_Pin(io);
+#endif
+}
+
+void IOToggle(IO_t io)
+{
+ if (!io) {
+ return;
+ }
+
+ uint32_t mask = IO_Pin(io);
+ // For GD32F4,use toggle register to toggle GPIO pin status
+#if defined(GD32F4)
+ GPIO_TG(PERIPH_INT(IO_GPIO(io))) = mask;
+#endif
+}
+
+#if defined(GD32F4)
+
+void IOConfigGPIO(IO_t io, ioConfig_t cfg)
+{
+ if (!io) {
+ return;
+ }
+
+ const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+ RCC_ClockCmd(rcc, ENABLE);
+
+ gpio_mode_set(PERIPH_INT(IO_GPIO(io)), ((cfg >> 0) & 0x03), ((cfg >> 5) & 0x03), IO_Pin(io));
+ gpio_output_options_set(PERIPH_INT(IO_GPIO(io)), ((cfg >> 4) & 0x01), ((cfg >> 2) & 0x03), IO_Pin(io));
+}
+
+void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
+{
+ if (!io) {
+ return;
+ }
+
+ const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+ RCC_ClockCmd(rcc, ENABLE);
+
+ gpio_af_set(PERIPH_INT(IO_GPIO(io)), af, IO_Pin(io));
+ gpio_mode_set(PERIPH_INT(IO_GPIO(io)), ((cfg >> 0) & 0x03), ((cfg >> 5) & 0x03), IO_Pin(io));
+ gpio_output_options_set(PERIPH_INT(IO_GPIO(io)), ((cfg >> 4) & 0x01), ((cfg >> 2) & 0x03), IO_Pin(io));
+}
+
+#else
+# warning MCU not set
+#endif
diff --git a/src/platform/GD32/light_ws2811strip_stdperiph.c b/src/platform/GD32/light_ws2811strip_stdperiph.c
new file mode 100755
index 0000000000..e99c9a20d8
--- /dev/null
+++ b/src/platform/GD32/light_ws2811strip_stdperiph.c
@@ -0,0 +1,223 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_LED_STRIP
+
+#include "build/debug.h"
+
+#include "common/color.h"
+
+#include "drivers/dma.h"
+#include "drivers/dma_reqmap.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "platform/rcc.h"
+#include "drivers/timer.h"
+
+#include "drivers/light_ws2811strip.h"
+
+static IO_t ws2811IO = IO_NONE;
+#if defined(GD32F4)
+static dmaResource_t *dmaRef = NULL;
+#else
+#error "No MCU definition in light_ws2811strip_stdperiph.c"
+#endif
+static TIM_TypeDef *timer = NULL;
+
+static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
+{
+#if defined(USE_WS2811_SINGLE_COLOUR)
+ static uint32_t counter = 0;
+#endif
+
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_INT_FLAG_FTF)) {
+#if defined(USE_WS2811_SINGLE_COLOUR)
+ counter++;
+ if (counter == WS2811_LED_STRIP_LENGTH) {
+ // Output low for 50us delay
+ memset(ledStripDMABuffer, 0, sizeof(ledStripDMABuffer));
+ } else if (counter == (WS2811_LED_STRIP_LENGTH + WS2811_DELAY_ITERATIONS)) {
+ counter = 0;
+ ws2811LedDataTransferInProgress = false;
+ xDMA_Cmd(descriptor->ref, DISABLE);
+ }
+#else
+ ws2811LedDataTransferInProgress = false;
+ xDMA_Cmd(descriptor->ref, DISABLE);
+#endif
+
+ DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_FTF);
+ }
+}
+
+bool ws2811LedStripHardwareInit(ioTag_t ioTag)
+{
+ if (!ioTag) {
+ return false;
+ }
+
+ timer_parameter_struct timer_initpara;
+ timer_oc_parameter_struct timer_ocintpara;
+ dma_single_data_parameter_struct dma_init_struct;
+
+ const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_LED_STRIP, 0);
+
+ if (timerHardware == NULL) {
+ return false;
+ }
+
+ timer = timerHardware->tim;
+
+#if defined(USE_DMA_SPEC)
+ const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
+
+ if (dmaSpec == NULL) {
+ return false;
+ }
+
+ dmaRef = dmaSpec->ref;
+#if defined(GD32F4)
+ uint32_t dmaChannel = dmaSpec->channel;
+#endif
+#else
+ dmaRef = timerHardware->dmaRef;
+#if defined(GD32F4)
+ uint32_t dmaChannel = timerHardware->dmaChannel;
+#endif
+#endif
+
+ if (dmaRef == NULL || !dmaAllocate(dmaGetIdentifier(dmaRef), OWNER_LED_STRIP, 0)) {
+ return false;
+ }
+
+ ws2811IO = IOGetByTag(ioTag);
+ IOInit(ws2811IO, OWNER_LED_STRIP, 0);
+ IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP), timerHardware->alternateFunction);
+
+ RCC_ClockCmd(timerRCC(timer), ENABLE);
+
+ // Stop timer
+ timer_disable((uint32_t)timer);
+
+ /* Compute the prescaler value */
+ uint16_t prescaler = timerGetPrescalerByDesiredMhz(timer, WS2811_TIMER_MHZ);
+ uint16_t period = timerGetPeriodByPrescaler(timer, prescaler, WS2811_CARRIER_HZ);
+
+ BIT_COMPARE_1 = period / 3 * 2;
+ BIT_COMPARE_0 = period / 3;
+
+ /* Time base configuration */
+ timer_struct_para_init(&timer_initpara);
+ timer_initpara.period = period; // 800kHz
+ timer_initpara.prescaler = prescaler;
+ timer_initpara.clockdivision = TIMER_CKDIV_DIV1;
+ timer_initpara.alignedmode = TIMER_COUNTER_EDGE;
+ timer_initpara.counterdirection = TIMER_COUNTER_UP;
+ timer_initpara.repetitioncounter = 0;
+ timer_init((uint32_t)timer, &timer_initpara);
+
+ /* PWM1 Mode configuration */
+ timer_channel_output_struct_para_init(&timer_ocintpara);
+ timer_channel_output_mode_config((uint32_t)timer, timerHardware->channel, TIMER_OC_MODE_PWM0);
+
+ if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+ timer_ocintpara.outputnstate = TIMER_CCXN_ENABLE;
+ timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
+ timer_ocintpara.ocnpolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIMER_OCN_IDLE_STATE_LOW : TIMER_OCN_POLARITY_HIGH;
+ } else {
+ timer_ocintpara.outputstate = TIMER_CCX_ENABLE;
+ timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_HIGH;
+ timer_ocintpara.ocpolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIMER_OC_IDLE_STATE_LOW : TIMER_OC_POLARITY_HIGH;
+ }
+
+ timer_channel_output_pulse_value_config((uint32_t)timer, timerHardware->channel, 0);
+
+ timerOCInit(timer, timerHardware->channel, &timer_ocintpara);
+ timerOCModeConfig(timer, timerHardware->channel, TIMER_OC_MODE_PWM0);
+ timerOCPreloadConfig(timer, timerHardware->channel, TIMER_OC_SHADOW_ENABLE);
+
+ timer_primary_output_config((uint32_t)timer, ENABLE);
+ timer_auto_reload_shadow_enable((uint32_t)timer);
+
+ if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+ timer_channel_complementary_output_state_config((uint32_t)timer, timerHardware->channel, TIMER_CCXN_ENABLE);
+ } else {
+ timer_channel_output_state_config((uint32_t)timer, timerHardware->channel, TIMER_CCX_ENABLE);
+ }
+
+ timer_enable((uint32_t)timer);
+
+ dmaEnable(dmaGetIdentifier(dmaRef));
+ dmaSetHandler(dmaGetIdentifier(dmaRef), WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0);
+
+ xDMA_DeInit(dmaRef);
+
+ /* configure DMA */
+ xDMA_Cmd(dmaRef, DISABLE);
+ xDMA_DeInit(dmaRef);
+ dma_single_data_para_struct_init(&dma_init_struct);
+ dma_init_struct.periph_addr = (uint32_t)timerCCR(timer, timerHardware->channel);
+ dma_init_struct.number = WS2811_DMA_BUFFER_SIZE;
+ dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+
+#if defined(GD32F4)
+
+ uint32_t temp_dma_periph;
+ int temp_dma_channel;
+
+ gd32_dma_chbase_parse((uint32_t)dmaRef, &temp_dma_periph, &temp_dma_channel);
+
+ dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, dmaChannel);
+
+ dma_init_struct.memory0_addr = (uint32_t)ledStripDMABuffer;
+ dma_init_struct.direction = DMA_MEMORY_TO_PERIPH;
+ dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_32BIT;
+ dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH;
+#endif
+
+#if defined(USE_WS2811_SINGLE_COLOUR)
+ dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_ENABLE;
+#else
+ dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_DISABLE;
+#endif
+
+ gd32_dma_init((uint32_t)dmaRef, &dma_init_struct);
+ timer_dma_enable((uint32_t)timer, timerDmaSource(timerHardware->channel));
+ xDMA_ITConfig(dmaRef, DMA_INT_FTF, ENABLE);
+
+ return true;
+}
+
+void ws2811LedStripDMAEnable(void)
+{
+ xDMA_SetCurrDataCounter(dmaRef, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred
+ timer_counter_value_config((uint32_t)timer, 0);
+ timer_enable((uint32_t)timer);
+ xDMA_Cmd(dmaRef, ENABLE);
+}
+#endif
diff --git a/src/platform/GD32/link/gd32_flash_f4_split.ld b/src/platform/GD32/link/gd32_flash_f4_split.ld
new file mode 100755
index 0000000000..b4d7ce5201
--- /dev/null
+++ b/src/platform/GD32/link/gd32_flash_f4_split.ld
@@ -0,0 +1,217 @@
+/*
+*****************************************************************************
+**
+** File : gd32_flash_f4_split.ld
+**
+** Abstract : Common linker script for GD32 devices.
+**
+*****************************************************************************
+*/
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Highest address of the user mode stack */
+_Hot_Reboot_Flags_Size = 16;
+_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM) - _Hot_Reboot_Flags_Size; /* end of RAM */
+
+/* Base address where the config is stored. */
+__config_start = ORIGIN(FLASH_CONFIG);
+__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG);
+
+/* Generate a link error if heap and stack don't fit into RAM */
+_Min_Heap_Size = 0x200; /* required amount of heap */
+_Min_Stack_Size = 0x800; /* required amount of stack */
+
+/* Define output sections */
+SECTIONS
+{
+ /*
+ * The ISR vector table is loaded at the beginning of the FLASH,
+ * But it is linked (space reserved) at the beginning of the VECTAB region,
+ * which is aliased either to FLASH or RAM.
+ * When linked to RAM, the table can optionally be copied from FLASH to RAM
+ * for table relocation.
+ */
+
+ _isr_vector_table_flash_base = LOADADDR(.vectors);
+ PROVIDE (isr_vector_table_flash_base = _isr_vector_table_flash_base);
+
+ .vectors :
+ {
+ . = ALIGN(4);
+ PROVIDE (isr_vector_table_base = .);
+ KEEP(*(.vectors)) /* Startup code */
+ . = ALIGN(4);
+ PROVIDE (isr_vector_table_end = .);
+ } >VECTAB AT> FLASH
+
+ /* System memory (read-only bootloader) interrupt vector */
+ .system_isr_vector (NOLOAD) :
+ {
+ . = ALIGN(4);
+ PROVIDE (system_isr_vector_table_base = .);
+ KEEP(*(.system_isr_vector)) /* Bootloader code */
+ . = ALIGN(4);
+ } >SYSTEM_MEMORY
+
+ /* The program code and other data goes into FLASH */
+ .text :
+ {
+ . = ALIGN(4);
+ *(.text) /* .text sections (code) */
+ *(.text*) /* .text* sections (code) */
+ *(.rodata) /* .rodata sections (constants, strings, etc.) */
+ *(.rodata*) /* .rodata* sections (constants, strings, etc.) */
+ *(.glue_7) /* glue arm to thumb code */
+ *(.glue_7t) /* glue thumb to arm code */
+ *(.eh_frame)
+
+ KEEP (*(.init))
+ KEEP (*(.fini))
+
+ . = ALIGN(4);
+ _etext = .; /* define a global symbols at end of code */
+ } >FLASH1
+
+
+ .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
+ .ARM : {
+ __exidx_start = .;
+ *(.ARM.exidx*)
+ __exidx_end = .;
+ } >MOVABLE_FLASH
+
+ .preinit_array :
+ {
+ PROVIDE_HIDDEN (__preinit_array_start = .);
+ KEEP (*(.preinit_array*))
+ PROVIDE_HIDDEN (__preinit_array_end = .);
+ } >MOVABLE_FLASH
+ .init_array :
+ {
+ PROVIDE_HIDDEN (__init_array_start = .);
+ KEEP (*(SORT(.init_array.*)))
+ KEEP (*(.init_array*))
+ PROVIDE_HIDDEN (__init_array_end = .);
+ } >MOVABLE_FLASH
+ .fini_array :
+ {
+ PROVIDE_HIDDEN (__fini_array_start = .);
+ KEEP (*(.fini_array*))
+ KEEP (*(SORT(.fini_array.*)))
+ PROVIDE_HIDDEN (__fini_array_end = .);
+ } >MOVABLE_FLASH
+ .pg_registry :
+ {
+ PROVIDE_HIDDEN (__pg_registry_start = .);
+ KEEP (*(.pg_registry))
+ KEEP (*(SORT(.pg_registry.*)))
+ PROVIDE_HIDDEN (__pg_registry_end = .);
+ } >MOVABLE_FLASH
+ .pg_resetdata :
+ {
+ PROVIDE_HIDDEN (__pg_resetdata_start = .);
+ KEEP (*(.pg_resetdata))
+ PROVIDE_HIDDEN (__pg_resetdata_end = .);
+ } >FLASH1
+
+ /* used by the startup to initialize data */
+ _sidata = LOADADDR(.data);
+
+ /* Initialized data sections goes into RAM, load LMA copy after code */
+ .data :
+ {
+ . = ALIGN(4);
+ _sdata = .; /* create a global symbol at data start */
+ *(.data) /* .data sections */
+ *(.data*) /* .data* sections */
+
+ . = ALIGN(4);
+ _edata = .; /* define a global symbol at data end */
+ } >RAM AT> MOVABLE_FLASH
+
+ /* Uninitialized data section */
+ . = ALIGN(4);
+ .bss (NOLOAD) :
+ {
+ /* This is used by the startup in order to initialize the .bss secion */
+ _sbss = .; /* define a global symbol at bss start */
+ __bss_start__ = _sbss;
+ *(.bss)
+ *(SORT_BY_ALIGNMENT(.bss*))
+ *(COMMON)
+
+ . = ALIGN(4);
+ _ebss = .; /* define a global symbol at bss end */
+ __bss_end__ = _ebss;
+ } >RAM
+
+ /* used during startup to initialized fastram_data */
+ _sfastram_idata = LOADADDR(.fastram_data);
+
+ /* Initialized FAST_DATA section for unsuspecting developers */
+ .fastram_data :
+ {
+ . = ALIGN(4);
+ _sfastram_data = .; /* create a global symbol at data start */
+ *(.fastram_data) /* .data sections */
+ *(.fastram_data*) /* .data* sections */
+
+ . = ALIGN(4);
+ _efastram_data = .; /* define a global symbol at data end */
+ } >FASTRAM AT> FLASH1
+
+ . = ALIGN(4);
+ .fastram_bss (NOLOAD) :
+ {
+ __fastram_bss_start__ = .;
+ *(.fastram_bss)
+ *(SORT_BY_ALIGNMENT(.fastram_bss*))
+ . = ALIGN(4);
+ __fastram_bss_end__ = .;
+ } >FASTRAM
+
+ .persistent_data (NOLOAD) :
+ {
+ __persistent_data_start__ = .;
+ *(.persistent_data)
+ . = ALIGN(4);
+ __persistent_data_end__ = .;
+ } >RAM
+
+ /* User_heap_stack section, used to check that there is enough RAM left */
+ _heap_stack_end = ORIGIN(STACKRAM) + LENGTH(STACKRAM) - _Hot_Reboot_Flags_Size;
+ _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size;
+ . = _heap_stack_begin;
+ ._user_heap_stack :
+ {
+ . = ALIGN(4);
+ PROVIDE ( end = . );
+ PROVIDE ( _end = . );
+ . = . + _Min_Heap_Size;
+ . = . + _Min_Stack_Size;
+ . = ALIGN(4);
+ } >STACKRAM = 0xa5
+
+ /* MEMORY_bank1 section, code must be located here explicitly */
+ /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */
+ .memory_b1_text :
+ {
+ *(.mb1text) /* .mb1text sections (code) */
+ *(.mb1text*) /* .mb1text* sections (code) */
+ *(.mb1rodata) /* read-only data (constants) */
+ *(.mb1rodata*)
+ } >MEMORY_B1
+
+ /* Remove information from the standard libraries */
+ /DISCARD/ :
+ {
+ libc.a ( * )
+ libm.a ( * )
+ libgcc.a ( * )
+ libnosys.a ( * )
+ }
+
+ .ARM.attributes 0 : { *(.ARM.attributes) }
+}
diff --git a/src/platform/GD32/link/gd32f405_425xg_flash.ld b/src/platform/GD32/link/gd32f405_425xg_flash.ld
new file mode 100755
index 0000000000..b82de71162
--- /dev/null
+++ b/src/platform/GD32/link/gd32f405_425xg_flash.ld
@@ -0,0 +1,44 @@
+/*
+*****************************************************************************
+**
+** File : gd32f405_425xg_flash.ld
+**
+** Abstract : Linker script for GD32F405_425xG Device with
+** 1024KByte FLASH, 256KByte SRAM (that include 64K TCM SRAM)
+**
+** Target : GD32F405_425xG
+**
+** Environment : Arm gcc toolchain
+**
+*****************************************************************************
+*/
+
+/*
+0x08000000 to 0x080FFFFF 1024K full flash,
+0x08000000 to 0x08003FFF 16K isr vector, startup code,
+0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1
+0x08008000 to 0x080FFFFF 992K firmware,
+*/
+
+/* memory map */
+MEMORY
+{
+ FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K
+ FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K
+ FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 992K
+
+ SYSTEM_MEMORY (rx) : ORIGIN = 0x1FFF0000, LENGTH = 29K
+
+ RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 192K
+ TCMRAM (rw) : ORIGIN = 0x10000000, LENGTH = 64K
+ BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
+ MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("STACKRAM", TCMRAM)
+REGION_ALIAS("FASTRAM", TCMRAM)
+REGION_ALIAS("VECTAB", RAM)
+
+REGION_ALIAS("MOVABLE_FLASH", FLASH1)
+
+INCLUDE "gd32_flash_f4_split.ld"
diff --git a/src/platform/GD32/link/gd32f460xg_flash.ld b/src/platform/GD32/link/gd32f460xg_flash.ld
new file mode 100755
index 0000000000..7b00ae0a95
--- /dev/null
+++ b/src/platform/GD32/link/gd32f460xg_flash.ld
@@ -0,0 +1,44 @@
+/*
+*****************************************************************************
+**
+** File : gd32_flash_f460xg.ld
+**
+** Abstract : Linker script for GD32F460xG Device with
+** 1024KByte FLASH, 512KByte SRAM (that include 64K TCM SRAM)
+**
+** Target : GD32F460xg
+**
+** Environment : Arm gcc toolchain
+**
+*****************************************************************************
+*/
+
+/*
+0x08000000 to 0x080FFFFF 1024K full flash,
+0x08000000 to 0x08003FFF 16K isr vector, startup code,
+0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1
+0x08008000 to 0x080FFFFF 992K firmware,
+*/
+
+/* memory map */
+MEMORY
+{
+ FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K
+ FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K
+ FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 992K
+
+ SYSTEM_MEMORY (rx) : ORIGIN = 0x1FFF0000, LENGTH = 29K
+
+ RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 448K
+ TCMRAM (rw) : ORIGIN = 0x10000000, LENGTH = 64K
+ BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
+ MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("STACKRAM", TCMRAM)
+REGION_ALIAS("FASTRAM", TCMRAM)
+REGION_ALIAS("VECTAB", RAM)
+
+REGION_ALIAS("MOVABLE_FLASH", FLASH1)
+
+INCLUDE "gd32_flash_f4_split.ld"
diff --git a/src/platform/GD32/mk/GD32F4.mk b/src/platform/GD32/mk/GD32F4.mk
new file mode 100755
index 0000000000..674129b1e7
--- /dev/null
+++ b/src/platform/GD32/mk/GD32F4.mk
@@ -0,0 +1,230 @@
+#
+# GD32F4 Make file include
+#
+
+CMSIS_DIR := $(LIB_MAIN_DIR)/CMSIS
+STDPERIPH_DIR = $(LIB_MAIN_DIR)/GD32F4/Drivers/GD32F4xx_standard_peripheral
+USB_LIB_DIR := $(LIB_MAIN_DIR)/GD32F4/Middlewares/GD32F4xx_usb_library
+
+STDPERIPH_SRC = \
+ gd32f4xx_adc.c \
+ gd32f4xx_dac.c \
+ gd32f4xx_dci.c \
+ gd32f4xx_dma.c \
+ gd32f4xx_exti.c \
+ gd32f4xx_fmc.c \
+ gd32f4xx_fwdgt.c \
+ gd32f4xx_gpio.c \
+ gd32f4xx_i2c.c \
+ gd32f4xx_ipa.c \
+ gd32f4xx_misc.c\
+ gd32f4xx_pmu.c \
+ gd32f4xx_rcu.c \
+ gd32f4xx_rtc.c \
+ gd32f4xx_sdio.c \
+ gd32f4xx_spi.c \
+ gd32f4xx_syscfg.c \
+ gd32f4xx_timer.c \
+ gd32f4xx_tli.c \
+ gd32f4xx_trng.c \
+ gd32f4xx_usart.c \
+ gd32f4xx_wwdgt.c
+
+VPATH := $(VPATH):$(STDPERIPH_DIR)/Source
+
+DEVICE_FLAGS = -DGD32F4XX
+
+ifneq ($(TARGET_MCU),$(filter $(TARGET_MCU),GDF450 GD32F470))
+STDPERIPH_SRC += gd32f4xx_exmc.c
+endif
+
+
+USBDEVICE_DIR = $(USB_LIB_DIR)/device
+
+USBDCORE_DIR = $(USBDEVICE_DIR)/core
+USBCORE_SRC = \
+ $(USBDCORE_DIR)/Source/usbd_core.c \
+ $(USBDCORE_DIR)/Source/usbd_enum.c \
+ $(USBDCORE_DIR)/Source/usbd_transc.c
+
+USBCDC_DIR = $(USBDEVICE_DIR)/class/cdc
+USBCDC_SRC = \
+ $(USBCDC_DIR)/Source/cdc_acm_core.c
+
+USBMSC_DIR = $(USBDEVICE_DIR)/class/msc
+USBMSC_SRC = \
+ $(USBMSC_DIR)/Source/usbd_msc_bbb.c \
+ $(USBMSC_DIR)/Source/usbd_msc_core.c \
+ $(USBMSC_DIR)/Source/usbd_msc_scsi.c
+
+USBHID_DIR = $(USBDEVICE_DIR)/class/hid
+USBHID_SRC = \
+ $(USBHID_DIR)/Source/standard_hid_core.c
+
+
+USBDRV_DIR = $(USB_LIB_DIR)/driver
+USBDRV_SRC = \
+ $(USB_LIB_DIR)/driver/Source/drv_usb_core.c \
+ $(USB_LIB_DIR)/driver/Source/drv_usb_dev.c \
+ $(USB_LIB_DIR)/driver/Source/drv_usbd_int.c
+
+
+
+USBSTD_DIR = $(USB_LIB_DIR)/ustd
+
+
+DEVICE_STDPERIPH_SRC := \
+ $(STDPERIPH_SRC) \
+ $(USBOTG_SRC) \
+ $(USBCORE_SRC) \
+ $(USBCDC_SRC) \
+ $(USBHID_SRC) \
+ $(USBMSC_SRC) \
+ $(USBDRV_SRC)
+
+
+VPATH := $(VPATH):$(CMSIS_DIR)/Core/Include:$(LIB_MAIN_DIR)/GD32F4/Drivers/CMSIS/GD/GD32F4xx/Source
+
+
+CMSIS_SRC := \
+ gd32f4xx_gpio.c \
+ gd32f4xx_rcu.c
+
+INCLUDE_DIRS := \
+ $(INCLUDE_DIRS) \
+ $(TARGET_PLATFORM_DIR) \
+ $(TARGET_PLATFORM_DIR)/include \
+ $(TARGET_PLATFORM_DIR)/startup \
+ $(STDPERIPH_DIR)/Include \
+ $(USBDEVICE_DIR)/class/msc/Include \
+ $(USBHID_DIR)/Include \
+ $(USBCDC_DIR)/Include \
+ $(USBMSC_DIR)/Include \
+ $(USBDCORE_DIR)/Include \
+ $(USBDRV_DIR)/Include \
+ $(USBSTD_DIR)/common \
+ $(USBSTD_DIR)/class/cdc \
+ $(USBSTD_DIR)/class/msc \
+ $(USBSTD_DIR)/class/hid \
+ $(CMSIS_DIR)/Core/Include \
+ $(LIB_MAIN_DIR)/GD32F4/Drivers/CMSIS/GD/GD32F4xx/Include \
+ $(TARGET_PLATFORM_DIR)/usb_f4
+
+DEVICE_FLAGS += -DUSE_STDPERIPH_DRIVER
+
+
+#Flags
+ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16
+
+ifeq ($(TARGET_MCU),GD32F407xx)
+DEVICE_FLAGS += -DGD32F407
+LD_SCRIPT = $(LINKER_DIR)/gd32_flash_f407_425.ld
+STARTUP_SRC = GD32/startup/startup_gd32f407_427.s
+MCU_FLASH_SIZE := 1024
+
+else ifeq ($(TARGET_MCU),GD32F460xg)
+DEVICE_FLAGS += -DGD32F460
+LD_SCRIPT = $(LINKER_DIR)/gd32f460xg_flash.ld
+STARTUP_SRC = GD32/startup/startup_gd32f460.S
+MCU_FLASH_SIZE := 1024
+
+else
+$(error Unknown MCU for F4 target)
+endif
+DEVICE_FLAGS += -DHXTAL_VALUE=$(HSE_VALUE)
+
+DSP_LIB := $(LIB_MAIN_DIR)/CMSIS/DSP
+DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM4
+DEVICE_FLAGS += -DUSE_GDBSP_DRIVER -DUSE_USB_FS
+DEVICE_FLAGS += -DVECT_TAB_SRAM
+
+MCU_COMMON_SRC = \
+ drivers/accgyro/accgyro_mpu.c \
+ drivers/dshot_bitbang_decode.c \
+ drivers/inverter.c \
+ GD32/adc_gd32.c \
+ GD32/audio_gd32.c \
+ GD32/bus_i2c_gd32.c \
+ GD32/bus_spi_gd32.c \
+ GD32/camera_control_gd32.c \
+ GD32/debug.c \
+ GD32/dma_gd32.c \
+ GD32/dma_reqmap_mcu.c \
+ GD32/dshot_bitbang.c \
+ GD32/dshot_bitbang_stdperiph.c \
+ GD32/exti_gd32.c \
+ GD32/io_gd32.c \
+ GD32/light_ws2811strip_stdperiph.c \
+ GD32/persistent_gd32.c \
+ GD32/rcu_gd32.c \
+ GD32/sdio_gdf4xx.c \
+ GD32/serial_uart_stdperiph.c \
+ GD32/serial_uart_gd32f4xx.c \
+ GD32/system_gd32f4xx.c \
+ GD32/pwm_output_gd32.c \
+ GD32/pwm_output_dshot.c \
+ GD32/timer_stdperiph.c \
+ GD32/timer_gd32f4xx.c \
+ GD32/transponder_ir_io_stdperiph.c \
+ drivers/adc.c \
+ drivers/bus_spi_config.c \
+ drivers/serial_escserial.c \
+ drivers/serial_pinconfig.c \
+ GD32/usb_f4/usbd_msc_desc.c \
+ GD32/startup/system_gd32f4xx.c
+
+VCP_SRC = \
+ GD32/usb_f4/gd32f4xx_it.c \
+ GD32/usb_f4/usb_bsp.c \
+ GD32/usb_f4/usbd_desc.c \
+ GD32/usb_f4/usb_cdc_hid.c \
+ GD32/usbd_cdc_vcp.c \
+ GD32/serial_usb_vcp.c \
+ drivers/usb_io.c
+
+MSC_SRC = \
+ drivers/usb_msc_common.c \
+ GD32/usb_msc_f4xx.c \
+ msc/usbd_storage.c \
+ msc/usbd_storage_emfat.c \
+ msc/emfat.c \
+ msc/emfat_file.c \
+ msc/usbd_storage_sd_spi.c \
+ msc/usbd_storage_sdio.c
+
+
+INCLUDE_DIRS += $(PLATFORM_DIR)/common/stm32
+
+MCU_COMMON_SRC += \
+ common/stm32/bus_i2c_pinconfig.c \
+ common/stm32/bus_spi_hw.c \
+ common/stm32/bus_spi_pinconfig.c \
+ common/stm32/config_flash.c \
+ common/stm32/dshot_bitbang_shared.c \
+ common/stm32/dshot_dpwm.c \
+ common/stm32/mco.c \
+ common/stm32/io_impl.c \
+ common/stm32/pwm_output_beeper.c \
+ common/stm32/pwm_output_dshot_shared.c \
+ common/stm32/serial_uart_hw.c \
+ common/stm32/serial_uart_pinconfig.c \
+ common/stm32/system.c
+
+SPEED_OPTIMISED_SRC += \
+ common/stm32/system.c \
+ common/stm32/bus_spi_hw.c \
+ common/stm32/pwm_output_dshot_shared.c \
+ common/stm32/dshot_bitbang_shared.c \
+ common/stm32/io_impl.c
+
+SIZE_OPTIMISED_SRC += \
+ GD32/serial_usb_vcp.c \
+ drivers/inverter.c \
+ drivers/serial_escserial.c \
+ drivers/serial_pinconfig.c \
+ drivers/bus_spi_config.c \
+ common/stm32/bus_i2c_pinconfig.c \
+ common/stm32/bus_spi_pinconfig.c \
+ common/stm32/config_flash.c \
+ common/stm32/pwm_output_beeper.c \
+ common/stm32/bus_spi_pinconfig.c
diff --git a/src/platform/GD32/persistent_gd32.c b/src/platform/GD32/persistent_gd32.c
new file mode 100755
index 0000000000..c4386f11f5
--- /dev/null
+++ b/src/platform/GD32/persistent_gd32.c
@@ -0,0 +1,85 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * An implementation of persistent data storage utilizing RTC backup data register.
+ * Retains values written across software resets and boot loader activities.
+ */
+
+#include
+#include "platform.h"
+
+#include "drivers/persistent.h"
+#include "drivers/system.h"
+
+#define PERSISTENT_OBJECT_MAGIC_VALUE (('B' << 24)|('e' << 16)|('f' << 8)|('1' << 0))
+
+uint32_t persistentObjectRead(persistentObjectId_e id)
+{
+ uint32_t value = REG32((RTC) + 0x50U + (id * 4U));
+
+ return value;
+}
+
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value)
+{
+ RTC_WPK = RTC_UNLOCK_KEY1;
+ RTC_WPK = RTC_UNLOCK_KEY2;
+
+ REG32(RTC + 0x50U + (id * 4U)) = value;
+
+ RTC_WPK = RTC_LOCK_KEY;
+}
+
+void persistentObjectRTCEnable(void)
+{
+ /* Enable access to the backup domain */
+ rcu_periph_clock_enable(RCU_PMU);
+ pmu_backup_write_enable();
+ rcu_periph_clock_enable(RCU_RTC);
+
+ rtc_register_sync_wait();
+
+ /* enable the write protection */
+ RTC_WPK = RTC_LOCK_KEY;
+ /* disable the write protection */
+ RTC_WPK = RTC_UNLOCK_KEY1;
+ RTC_WPK = RTC_UNLOCK_KEY2;
+}
+
+void persistentObjectInit(void)
+{
+ // Enable RTC and backup register access
+ persistentObjectRTCEnable();
+
+ // Check if the system was a software reset
+ uint32_t wasSoftReset = RCU_RSTSCK & RCU_RSTSCK_SWRSTF;
+
+ // Check if the magic value is present in the backup register
+ if (!wasSoftReset || (persistentObjectRead(PERSISTENT_OBJECT_MAGIC) != PERSISTENT_OBJECT_MAGIC_VALUE)) {
+ // Clear all persistent objects
+ for (int i = 1; i < PERSISTENT_OBJECT_COUNT; i++) {
+ persistentObjectWrite(i, 0);
+ }
+ // Write the magic value to indicate valid data
+ persistentObjectWrite(PERSISTENT_OBJECT_MAGIC, PERSISTENT_OBJECT_MAGIC_VALUE);
+ }
+}
diff --git a/src/platform/GD32/pwm_output_dshot.c b/src/platform/GD32/pwm_output_dshot.c
new file mode 100755
index 0000000000..05fe5d6593
--- /dev/null
+++ b/src/platform/GD32/pwm_output_dshot.c
@@ -0,0 +1,482 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_DSHOT
+
+#include "build/debug.h"
+
+#include "drivers/dma.h"
+#include "drivers/dma_reqmap.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "platform/rcc.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+#include "drivers/system.h"
+
+#if defined(GD32F4)
+#include "gd32f4xx.h"
+#endif
+
+#include "drivers/pwm_output.h"
+#include "drivers/dshot.h"
+#include "dshot_dpwm.h"
+#include "drivers/dshot_command.h"
+#include "pwm_output_dshot_shared.h"
+#include "drivers/motor.h"
+
+static uint16_t timer_oc_modes[MAX_SUPPORTED_MOTORS];
+static uint16_t timer_oc_pulses[MAX_SUPPORTED_MOTORS];
+
+#ifdef USE_DSHOT_TELEMETRY
+
+void dshotEnableChannels(unsigned motorCount)
+{
+ for (unsigned i = 0; i < motorCount; i++) {
+ if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) {
+ timer_channel_complementary_output_state_config((uint32_t)(dmaMotors[i].timerHardware->tim), dmaMotors[i].timerHardware->channel, TIMER_CCXN_ENABLE);
+ } else {
+ timer_channel_output_state_config((uint32_t)(dmaMotors[i].timerHardware->tim), dmaMotors[i].timerHardware->channel, TIMER_CCX_ENABLE);
+ }
+ }
+}
+
+#endif
+
+FAST_CODE void pwmDshotSetDirectionOutput(
+ motorDmaOutput_t * const motor
+#ifndef USE_DSHOT_TELEMETRY
+ ,timer_oc_parameter_struct *pOcInit, DMA_InitTypeDef* pGenerDmaInit
+#endif
+)
+{
+#ifdef USE_DSHOT_TELEMETRY
+ timer_oc_parameter_struct* pOcInit = &motor->ocInitStruct;
+ DMA_InitTypeDef* pGenerDmaInit = &motor->dmaInitStruct;
+#endif
+
+ const timerHardware_t * const timerHardware = motor->timerHardware;
+ TIM_TypeDef *timer = timerHardware->tim;
+
+ dmaResource_t *dmaRef = motor->dmaRef;
+
+#if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT_TELEMETRY)
+ if (useBurstDshot) {
+ dmaRef = timerHardware->dmaTimUPRef;
+ }
+#endif
+
+ xDMA_DeInit(dmaRef);
+
+#ifdef USE_DSHOT_TELEMETRY
+ motor->isInput = false;
+#endif
+
+ timerOCPreloadConfig(timer, timerHardware->channel, TIMER_OC_SHADOW_DISABLE);
+ timerOCInit(timer, timerHardware->channel, pOcInit);
+
+ timerOCModeConfig(timer, timerHardware->channel, timer_oc_modes[motor->index]);
+ timer_channel_output_pulse_value_config((uint32_t)timer, timerHardware->channel, timer_oc_pulses[motor->index]);
+ timerOCPreloadConfig(timer, timerHardware->channel, TIMER_OC_SHADOW_ENABLE);
+
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ pGenerDmaInit->config.init_struct_m.direction = DMA_MEMORY_TO_PERIPH;
+ } else
+#endif
+ {
+ pGenerDmaInit->config.init_struct_s.direction = DMA_MEMORY_TO_PERIPH;
+
+ }
+
+ xDMA_Init(dmaRef, pGenerDmaInit);
+
+ xDMA_ITConfig(dmaRef, DMA_INT_FTF, ENABLE);
+}
+
+#ifdef USE_DSHOT_TELEMETRY
+FAST_CODE
+static void pwmDshotSetDirectionInput(
+ motorDmaOutput_t * const motor
+)
+{
+ DMA_InitTypeDef* pGenerDmaInit = &motor->dmaInitStruct;
+
+ const timerHardware_t * const timerHardware = motor->timerHardware;
+ TIM_TypeDef *timer = timerHardware->tim;
+
+ dmaResource_t *dmaRef = motor->dmaRef;
+
+ xDMA_DeInit(dmaRef);
+
+ motor->isInput = true;
+ if (!inputStampUs) {
+ inputStampUs = micros();
+ }
+
+ timer_auto_reload_shadow_enable((uint32_t)timer);
+ TIMER_CAR((uint32_t)timer) = 0xffffffff;
+
+ timer_input_capture_config((uint32_t)timer, timerHardware->channel, &motor->icInitStruct);
+
+ if (useBurstDshot) {
+ pGenerDmaInit->config.init_struct_m.direction = DMA_PERIPH_TO_MEMORY;
+ } else {
+ pGenerDmaInit->config.init_struct_s.direction = DMA_PERIPH_TO_MEMORY;
+ }
+
+ xDMA_Init(dmaRef, pGenerDmaInit);
+}
+#endif
+
+void pwmCompleteDshotMotorUpdate(void)
+{
+ /* If there is a dshot command loaded up, time it correctly with motor update*/
+ if (!dshotCommandQueueEmpty()) {
+ if (!dshotCommandOutputIsEnabled(pwmMotorCount)) {
+ return;
+ }
+ }
+
+#ifdef GD32F4
+ if (!useBurstDshot) {
+ for(uint8_t i = 0; i < motorDeviceCount(); i++) {
+ memmove((uint8_t *)&dshotDmaBuffer[i][2], (uint8_t *)&dshotDmaBuffer[i][1], 60);
+ }
+ }
+#endif
+
+ for (int i = 0; i < dmaMotorTimerCount; i++) {
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ xDMA_SetCurrDataCounter(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength);
+ xDMA_Cmd(dmaMotorTimers[i].dmaBurstRef, ENABLE);
+ timer_dma_transfer_config((uint32_t)(dmaMotorTimers[i].timer), TIMER_DMACFG_DMATA_CH0CV, TIMER_DMACFG_DMATC_4TRANSFER);
+ timer_dma_enable((uint32_t)(dmaMotorTimers[i].timer), TIMER_DMA_UPD);
+ } else
+#endif
+ {
+ timer_counter_value_config((uint32_t)(dmaMotorTimers[i].timer), 0);
+ timer_dma_enable((uint32_t)(dmaMotorTimers[i].timer), dmaMotorTimers[i].timerDmaSources);
+ dmaMotorTimers[i].timerDmaSources = 0;
+ }
+ }
+}
+
+FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
+{
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_INT_FLAG_FTF)) {
+ motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
+#ifdef USE_DSHOT_TELEMETRY
+ dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
+#endif
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE);
+ timer_dma_disable((uint32_t)(motor->timerHardware->tim), TIMER_DMA_UPD);
+ } else
+#endif
+ {
+ xDMA_Cmd(motor->dmaRef, DISABLE);
+ timer_dma_disable((uint32_t)(motor->timerHardware->tim), motor->timerDmaSource);
+ }
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ pwmDshotSetDirectionInput(motor);
+ xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
+ xDMA_Cmd(motor->dmaRef, ENABLE);
+ timer_dma_enable((uint32_t)(motor->timerHardware->tim), motor->timerDmaSource);
+ dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
+ }
+#endif
+ DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_FTF);
+ DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_HTF);
+ }
+}
+
+bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output)
+{
+#ifdef USE_DSHOT_TELEMETRY
+#define OCINIT motor->ocInitStruct
+#define DMAINIT motor->dmaInitStruct
+#else
+ timer_oc_parameter_struct ocInitStruct;
+ DMA_InitTypeDef dmaInitStruct;
+#define OCINIT ocInitStruct
+#define DMAINIT dmaInitStruct
+#endif
+
+ dmaResource_t *dmaRef = NULL;
+#if defined(GD32F4)
+ uint32_t dmaChannel = 0;
+#endif
+
+#if defined(USE_DMA_SPEC)
+ const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
+
+ if (dmaSpec != NULL) {
+ dmaRef = dmaSpec->ref;
+#if defined(GD32F4)
+ dmaChannel = dmaSpec->channel;
+#endif
+ }
+#else
+ dmaRef = timerHardware->dmaRef;
+#if defined(GD32F4)
+ dmaChannel = timerHardware->dmaChannel;
+#endif
+#endif
+
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ dmaRef = timerHardware->dmaTimUPRef;
+#if defined(GD32F4)
+ dmaChannel = timerHardware->dmaTimUPChannel;
+#endif
+ }
+#endif
+
+ if (dmaRef == NULL) {
+ return false;
+ }
+
+ dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
+
+ bool dmaIsConfigured = false;
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier);
+ if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) {
+ dmaIsConfigured = true;
+ } else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) {
+ return false;
+ }
+ } else
+#endif
+ {
+ if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) {
+ return false;
+ }
+ }
+
+ motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
+
+ motor->dmaRef = dmaRef;
+
+ TIM_TypeDef *timer = timerHardware->tim;
+
+ // Boolean configureTimer is always true when different channels of the same timer are processed in sequence,
+ // causing the timer and the associated DMA initialized more than once.
+ // To fix this, getTimerIndex must be expanded to return if a new timer has been requested.
+ // However, since the initialization is idempotent, it is left as is in a favor of flash space (for now).
+ const uint8_t timerIndex = getTimerIndex(timer);
+ const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
+
+ motor->timer = &dmaMotorTimers[timerIndex];
+ motor->index = motorIndex;
+ motor->timerHardware = timerHardware;
+
+ const IO_t motorIO = IOGetByTag(timerHardware->tag);
+
+ uint8_t pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PUPD_PULLDOWN : GPIO_PUPD_PULLUP;
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ output ^= TIMER_OUTPUT_INVERTED;
+ }
+#endif
+
+ motor->iocfg = IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, pupMode);
+ IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
+
+ if (configureTimer) {
+ timer_parameter_struct timer_initpara;
+ timer_struct_para_init(&timer_initpara);
+
+ RCC_ClockCmd(timerRCC(timer), ENABLE);
+ timer_disable((uint32_t)timer);
+
+ timer_initpara.period = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
+ timer_initpara.prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
+ timer_initpara.clockdivision = TIMER_CKDIV_DIV1;
+ timer_initpara.alignedmode = TIMER_COUNTER_EDGE;
+ timer_initpara.counterdirection = TIMER_COUNTER_UP;
+ timer_initpara.repetitioncounter = 0;
+ timer_init((uint32_t)timer, &timer_initpara);
+ }
+
+ timer_channel_output_struct_para_init(&OCINIT);
+ timer_channel_output_mode_config((uint32_t)timer, timerHardware->channel, TIMER_OC_MODE_PWM0);
+ timer_oc_modes[motor->index] = TIMER_OC_MODE_PWM0;
+
+ if (output & TIMER_OUTPUT_N_CHANNEL) {
+ OCINIT.outputnstate = TIMER_CCXN_ENABLE;
+ OCINIT.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
+ OCINIT.ocnpolarity = (output & TIMER_OUTPUT_INVERTED) ? TIMER_OCN_POLARITY_LOW : TIMER_OCN_POLARITY_HIGH;
+ } else {
+ OCINIT.outputstate = TIMER_CCX_ENABLE;
+ OCINIT.ocidlestate = TIMER_OC_IDLE_STATE_HIGH;
+ OCINIT.ocpolarity = (output & TIMER_OUTPUT_INVERTED) ? TIMER_OC_POLARITY_LOW : TIMER_OC_POLARITY_HIGH;
+ }
+
+ timer_oc_pulses[motor->index] = 0;
+ timer_channel_output_pulse_value_config((uint32_t)timer, timerHardware->channel, 0);
+
+#ifdef USE_DSHOT_TELEMETRY
+
+ timer_channel_input_struct_para_init(&motor->icInitStruct);
+
+ motor->icInitStruct.icselection = TIMER_IC_SELECTION_DIRECTTI;
+ motor->icInitStruct.icpolarity = TIMER_IC_POLARITY_BOTH_EDGE;
+ motor->icInitStruct.icprescaler = TIMER_IC_PSC_DIV1;
+ motor->icInitStruct.icfilter = 2;
+ timer_input_capture_config((uint32_t)timer, timerHardware->channel, &motor->icInitStruct);
+#endif
+
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ motor->timer->dmaBurstRef = dmaRef;
+ motor->dmaInitStruct.data_mode = DMA_DATA_MODE_MULTI;
+ } else
+#endif
+ {
+ motor->timerDmaSource = timerDmaSource(timerHardware->channel);
+ motor->timer->timerDmaSources &= ~motor->timerDmaSource;
+ motor->dmaInitStruct.data_mode = DMA_DATA_MODE_SINGLE;
+ }
+
+ xDMA_Cmd(dmaRef, DISABLE);
+ xDMA_DeInit(dmaRef);
+
+ if (!dmaIsConfigured) {
+ dmaEnable(dmaIdentifier);
+ }
+
+ if (useBurstDshot) {
+ dma_multi_data_para_struct_init(&DMAINIT.config.init_struct_m);
+ DMAINIT.data_mode = DMA_DATA_MODE_MULTI;
+ } else {
+ dma_single_data_para_struct_init(&DMAINIT.config.init_struct_s);
+ DMAINIT.data_mode = DMA_DATA_MODE_SINGLE;
+ }
+
+ uint32_t temp_dma_periph;
+ int temp_dma_channel;
+
+ motor->dmaInitStruct.sub_periph = dmaChannel;
+
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
+
+ gd32_dma_chbase_parse((uint32_t)dmaRef, &temp_dma_periph, &temp_dma_channel);
+
+ DMAINIT.config.init_struct_m.memory0_addr = (uint32_t)motor->timer->dmaBurstBuffer;
+ DMAINIT.config.init_struct_m.direction = DMA_MEMORY_TO_PERIPH;
+ DMAINIT.config.init_struct_m.memory_burst_width = DMA_MEMORY_BURST_SINGLE;
+ DMAINIT.config.init_struct_m.periph_burst_width = DMA_PERIPH_BURST_SINGLE;
+ DMAINIT.config.init_struct_m.periph_addr = (uint32_t)&TIMER_DMATB((uint32_t)timerHardware->tim);
+ DMAINIT.config.init_struct_m.number = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
+ DMAINIT.config.init_struct_m.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ DMAINIT.config.init_struct_m.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+ DMAINIT.config.init_struct_m.periph_width = DMA_PERIPH_WIDTH_32BIT;
+ DMAINIT.config.init_struct_m.memory_width = DMA_MEMORY_WIDTH_32BIT;
+ DMAINIT.config.init_struct_m.circular_mode = DMA_CIRCULAR_MODE_DISABLE;
+ DMAINIT.config.init_struct_m.priority = DMA_PRIORITY_HIGH;
+
+ } else
+#endif
+ {
+ motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
+
+ gd32_dma_chbase_parse((uint32_t)dmaRef, &temp_dma_periph, &temp_dma_channel);
+
+ DMAINIT.config.init_struct_s.memory0_addr = (uint32_t)motor->dmaBuffer;
+ DMAINIT.config.init_struct_s.direction = DMA_MEMORY_TO_PERIPH;
+ DMAINIT.config.init_struct_s.periph_addr = (uint32_t)timerChCCR(timerHardware);
+ DMAINIT.config.init_struct_s.number = DSHOT_DMA_BUFFER_SIZE;
+ DMAINIT.config.init_struct_s.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ DMAINIT.config.init_struct_s.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+ DMAINIT.config.init_struct_s.periph_memory_width = DMA_PERIPH_WIDTH_32BIT;
+ DMAINIT.config.init_struct_s.circular_mode = DMA_CIRCULAR_MODE_DISABLE;
+ DMAINIT.config.init_struct_s.priority = DMA_PRIORITY_HIGH;
+
+ }
+
+ // XXX Consolidate common settings in the next refactor
+
+ motor->dmaRef = dmaRef;
+
+#ifdef USE_DSHOT_TELEMETRY
+ motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
+ (16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
+ motor->timer->outputPeriod = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
+ pwmDshotSetDirectionOutput(motor);
+#else
+ pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
+#endif
+
+ dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, dmaChannel);
+
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ if (!dmaIsConfigured) {
+ dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
+ }
+ } else
+#endif
+ {
+ dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
+ }
+
+ timer_enable((uint32_t)timer);
+
+ if (output & TIMER_OUTPUT_N_CHANNEL) {
+ timer_channel_complementary_output_state_config((uint32_t)timer, timerHardware->channel, TIMER_CCXN_ENABLE);
+ } else {
+ timer_channel_output_state_config((uint32_t)timer, timerHardware->channel, TIMER_CCX_ENABLE);
+ }
+
+ if (configureTimer) {
+ timer_auto_reload_shadow_enable((uint32_t)timer);
+ timer_primary_output_config((uint32_t)timer, ENABLE);
+ timer_enable((uint32_t)timer);
+ }
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ // avoid high line during startup to prevent bootloader activation
+ *timerChCCR(timerHardware) = 0xffff;
+ }
+#endif
+ motor->configured = true;
+
+ return true;
+}
+
+#endif
diff --git a/src/platform/GD32/pwm_output_gd32.c b/src/platform/GD32/pwm_output_gd32.c
new file mode 100755
index 0000000000..ab7ae130a0
--- /dev/null
+++ b/src/platform/GD32/pwm_output_gd32.c
@@ -0,0 +1,275 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_PWM_OUTPUT
+
+#include "drivers/io.h"
+#include "drivers/pwm_output.h"
+#include "drivers/pwm_output_impl.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+
+#include "drivers/motor_impl.h"
+
+static bool useContinuousUpdate = true;
+
+static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
+{
+ timer_oc_parameter_struct timer_ocintpara;
+
+ timer_channel_output_struct_para_init(&timer_ocintpara);
+ timer_channel_output_mode_config((uint32_t)tim, channel, TIMER_OC_MODE_PWM0);
+
+ if (output & TIMER_OUTPUT_N_CHANNEL) {
+ timer_ocintpara.outputnstate = TIMER_CCXN_ENABLE;
+ timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
+ timer_ocintpara.ocnpolarity = (output & TIMER_OUTPUT_INVERTED) ? TIMER_OCN_POLARITY_LOW : TIMER_OCN_POLARITY_HIGH;
+ } else {
+ timer_ocintpara.outputstate = TIMER_CCX_ENABLE;
+ timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_HIGH;
+ timer_ocintpara.ocpolarity = (output & TIMER_OUTPUT_INVERTED) ? TIMER_OC_POLARITY_LOW : TIMER_OC_POLARITY_HIGH;
+ }
+ timer_channel_output_pulse_value_config((uint32_t)tim, channel, value);
+
+ timerOCInit(tim, channel, &timer_ocintpara);
+ timerOCModeConfig(tim, channel, TIMER_OC_MODE_PWM0);
+ timerOCPreloadConfig(tim, channel, TIMER_OC_SHADOW_ENABLE);
+}
+
+void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
+{
+ configTimeBase(timerHardware->tim, period, hz);
+ pwmOCConfig(timerHardware->tim,
+ timerHardware->channel,
+ value,
+ inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
+ );
+
+ timer_primary_output_config((uint32_t)(timerHardware->tim), ENABLE);
+ timer_enable((uint32_t)(timerHardware->tim));
+
+ channel->ccr = timerChCCR(timerHardware);
+ channel->tim = timerHardware->tim;
+ *channel->ccr = 0;
+}
+
+static void pwmWriteStandard(uint8_t index, float value)
+{
+ /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
+ *pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset);
+}
+
+static void pwmShutdownPulsesForAllMotors(void)
+{
+ for (int index = 0; index < pwmMotorCount; index++) {
+ // Set the compare register to 0, which stops the output pulsing if the timer overflows
+ if (pwmMotors[index].channel.ccr) {
+ *pwmMotors[index].channel.ccr = 0;
+ }
+ }
+}
+
+static void pwmDisableMotors(void)
+{
+ pwmShutdownPulsesForAllMotors();
+}
+
+static void pwmCompleteMotorUpdate(void)
+{
+ if (useContinuousUpdate) {
+ return;
+ }
+
+ for (int index = 0; index < pwmMotorCount; index++) {
+ if (pwmMotors[index].forceOverflow) {
+ timerForceOverflow(pwmMotors[index].channel.tim);
+ }
+ // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
+ // This compare register will be set to the output value on the next main loop.
+ *pwmMotors[index].channel.ccr = 0;
+ }
+}
+
+static float pwmConvertFromExternal(uint16_t externalValue)
+{
+ return (float)externalValue;
+}
+
+static uint16_t pwmConvertToExternal(float motorValue)
+{
+ return (uint16_t)motorValue;
+}
+
+static const motorVTable_t motorPwmVTable = {
+ .postInit = NULL,
+ .enable = pwmEnableMotors,
+ .disable = pwmDisableMotors,
+ .isMotorEnabled = pwmIsMotorEnabled,
+ .shutdown = pwmShutdownPulsesForAllMotors,
+ .convertExternalToMotor = pwmConvertFromExternal,
+ .convertMotorToExternal = pwmConvertToExternal,
+ .write = pwmWriteStandard,
+ .decodeTelemetry = NULL,
+ .updateComplete = pwmCompleteMotorUpdate,
+ .requestTelemetry = NULL,
+ .isMotorIdle = NULL,
+ .getMotorIO = pwmGetMotorIO,
+};
+
+bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)
+{
+ memset(pwmMotors, 0, sizeof(pwmMotors));
+
+ pwmMotorCount = device->count;
+ device->vTable = &motorPwmVTable;
+ useContinuousUpdate = motorConfig->useContinuousUpdate;
+
+ float sMin = 0;
+ float sLen = 0;
+ switch (motorConfig->motorProtocol) {
+ default:
+ case MOTOR_PROTOCOL_ONESHOT125:
+ sMin = 125e-6f;
+ sLen = 125e-6f;
+ break;
+ case MOTOR_PROTOCOL_ONESHOT42:
+ sMin = 42e-6f;
+ sLen = 42e-6f;
+ break;
+ case MOTOR_PROTOCOL_MULTISHOT:
+ sMin = 5e-6f;
+ sLen = 20e-6f;
+ break;
+ case MOTOR_PROTOCOL_BRUSHED:
+ sMin = 0;
+ useContinuousUpdate = true;
+ idlePulse = 0;
+ break;
+ case MOTOR_PROTOCOL_PWM :
+ sMin = 1e-3f;
+ sLen = 1e-3f;
+ useContinuousUpdate = true;
+ idlePulse = 0;
+ break;
+ }
+
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) {
+ const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
+ const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
+ const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
+
+ if (timerHardware == NULL) {
+ /* not enough motors initialised for the mixer or a break in the motors */
+ device->vTable = NULL;
+ pwmMotorCount = 0;
+ /* TODO: block arming and add reason system cannot arm */
+ return false;
+ }
+
+ pwmMotors[motorIndex].io = IOGetByTag(tag);
+ IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
+
+ IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
+
+ /* standard PWM outputs */
+ // margin of safety is 4 periods when unsynced
+ const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
+
+ const uint32_t clock = timerClock(timerHardware->tim);
+ /* used to find the desired timer frequency for max resolution */
+ const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
+ const uint32_t hz = clock / prescaler;
+ const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff;
+
+ /*
+ if brushed then it is the entire length of the period.
+ TODO: this can be moved back to periodMin and periodLen
+ once mixer outputs a 0..1 float value.
+ */
+ pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
+ pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000);
+
+ pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion);
+
+ bool timerAlreadyUsed = false;
+ for (int i = 0; i < motorIndex; i++) {
+ if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) {
+ timerAlreadyUsed = true;
+ break;
+ }
+ }
+ pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed;
+ pwmMotors[motorIndex].enabled = true;
+ }
+
+ return true;
+}
+
+pwmOutputPort_t *pwmGetMotors(void)
+{
+ return pwmMotors;
+}
+
+#ifdef USE_SERVOS
+static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
+
+void pwmWriteServo(uint8_t index, float value)
+{
+ if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) {
+ *servos[index].channel.ccr = lrintf(value);
+ }
+}
+
+void servoDevInit(const servoDevConfig_t *servoConfig)
+{
+ for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
+ const ioTag_t tag = servoConfig->ioTags[servoIndex];
+
+ if (!tag) {
+ continue;
+ }
+
+ servos[servoIndex].io = IOGetByTag(tag);
+
+ IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
+
+ const timerHardware_t *timer = timerAllocate(tag, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
+
+ if (timer == NULL) {
+ /* flag failure and disable ability to arm */
+ break;
+ }
+
+ IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
+
+ pwmOutConfig(&servos[servoIndex].channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
+ servos[servoIndex].enabled = true;
+ }
+}
+#endif // USE_SERVOS
+#endif // USE_PWM_OUTPUT
diff --git a/src/platform/GD32/rcu_gd32.c b/src/platform/GD32/rcu_gd32.c
new file mode 100755
index 0000000000..448c3af700
--- /dev/null
+++ b/src/platform/GD32/rcu_gd32.c
@@ -0,0 +1,89 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include "platform.h"
+#include "platform/rcc.h"
+
+void rcu_periph_clk_config(volatile uint32_t *reg, uint32_t mask, FunctionalState NewState)
+{
+ if (NewState == ENABLE) {
+ *reg |= mask;
+ } else {
+ *reg &= ~mask;
+ }
+}
+
+static void rcu_periph_rst_config(volatile uint32_t *reg, uint32_t mask, FunctionalState NewState)
+{
+ if (NewState == ENABLE) {
+ *reg |= mask;
+ } else {
+ *reg &= ~mask;
+ }
+}
+
+void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
+{
+ int tag = periphTag >> 5;
+ uint32_t mask = 1 << (periphTag & 0x1f);
+
+ switch (tag) {
+ case RCC_AHB1:
+ rcu_periph_clk_config(&RCU_AHB1EN, mask, NewState);
+ break;
+ case RCC_AHB2:
+ rcu_periph_clk_config(&RCU_AHB2EN, mask, NewState);
+ break;
+ case RCC_AHB3:
+ rcu_periph_clk_config(&RCU_AHB3EN, mask, NewState);
+ break;
+ case RCC_APB1:
+ rcu_periph_clk_config(&RCU_APB1EN, mask, NewState);
+ break;
+ case RCC_APB2:
+ rcu_periph_clk_config(&RCU_APB2EN, mask, NewState);
+ break;
+ }
+}
+
+void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
+{
+ int tag = periphTag >> 5;
+ uint32_t mask = 1 << (periphTag & 0x1f);
+
+ switch (tag) {
+ case RCC_AHB1:
+ rcu_periph_rst_config(&RCU_AHB1RST, mask, NewState);
+ break;
+ case RCC_AHB2:
+ rcu_periph_rst_config(&RCU_AHB2RST, mask, NewState);
+ break;
+ case RCC_AHB3:
+ rcu_periph_rst_config(&RCU_AHB3RST, mask, NewState);
+ break;
+ case RCC_APB1:
+ rcu_periph_rst_config(&RCU_APB1RST, mask, NewState);
+ break;
+ case RCC_APB2:
+ rcu_periph_rst_config(&RCU_APB2RST, mask, NewState);
+ break;
+ }
+}
diff --git a/src/platform/GD32/sdio_gdf4xx.c b/src/platform/GD32/sdio_gdf4xx.c
new file mode 100755
index 0000000000..460fe94151
--- /dev/null
+++ b/src/platform/GD32/sdio_gdf4xx.c
@@ -0,0 +1,1547 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_SDCARD_SDIO
+
+#include "drivers/sdmmc_sdio.h"
+#include "gd32f4xx_gpio.h"
+#include "gd32f4xx_sdio.h"
+
+#include "pg/sdio.h"
+
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+#include "platform/rcc.h"
+#include "drivers/dma.h"
+#include "drivers/light_led.h"
+
+#include "build/debug.h"
+
+#define DMA_CHANNEL_4 ((uint32_t)0x08000000)
+#define DMA_MINC_ENABLE ((uint32_t)DMA_CHXCTL_MNAGA)
+#define DMA_MDATAALIGN_WORD ((uint32_t)DMA_MEMORY_WIDTH_32BIT)
+#define DMA_PDATAALIGN_WORD ((uint32_t)DMA_PERIPH_WIDTH_32BIT)
+#define DMA_MBURST_INC4 ((uint32_t)DMA_MEMORY_BURST_4_BEAT)
+#define DMA_PBURST_INC4 ((uint32_t)DMA_PERIPH_BURST_4_BEAT)
+
+#define BLOCK_SIZE ((uint32_t)(512))
+
+#define INTC_CLEAR_MASK_CH3 ((uint32_t)0x0F40000)
+#define INTC_CLEAR_MASK_CH6 ((uint32_t)0x003D000)
+
+#define SDIO_INTC_STATIC_FLAGS ((uint32_t)(SDIO_INTC_CCRCERRC | SDIO_INTC_DTCRCERRC | SDIO_INTC_CMDTMOUTC |\
+ SDIO_INTC_DTTMOUTC | SDIO_INTC_TXUREC | SDIO_INTC_RXOREC |\
+ SDIO_INTC_CMDRECVC | SDIO_INTC_CMDSENDC | SDIO_INTC_DTENDC |\
+ SDIO_INTC_DTBLKENDC))
+
+#define SD_SOFTWARE_COMMAND_TIMEOUT ((uint32_t)0x00220000)
+
+#define SD_OCR_ADDR_OUT_OF_RANGE ((uint32_t)0x80000000)
+#define SD_OCR_ADDR_MISALIGNED ((uint32_t)0x40000000)
+#define SD_OCR_BLOCK_LEN_ERR ((uint32_t)0x20000000)
+#define SD_OCR_ERASE_SEQ_ERR ((uint32_t)0x10000000)
+#define SD_OCR_BAD_ERASE_PARAM ((uint32_t)0x08000000)
+#define SD_OCR_WRITE_PROT_VIOLATION ((uint32_t)0x04000000)
+#define SD_OCR_LOCK_UNLOCK_FAILED ((uint32_t)0x01000000)
+#define SD_OCR_COM_CRC_FAILED ((uint32_t)0x00800000)
+#define SD_OCR_ILLEGAL_CMD ((uint32_t)0x00400000)
+#define SD_OCR_CARD_ECC_FAILED ((uint32_t)0x00200000)
+#define SD_OCR_CC_ERROR ((uint32_t)0x00100000)
+#define SD_OCR_GENERAL_UNKNOWN_ERROR ((uint32_t)0x00080000)
+#define SD_OCR_STREAM_READ_UNDERRUN ((uint32_t)0x00040000)
+#define SD_OCR_STREAM_WRITE_OVERRUN ((uint32_t)0x00020000)
+#define SD_OCR_CID_CSD_OVERWRITE ((uint32_t)0x00010000)
+#define SD_OCR_WP_ERASE_SKIP ((uint32_t)0x00008000)
+#define SD_OCR_CARD_ECC_DISABLED ((uint32_t)0x00004000)
+#define SD_OCR_ERASE_RESET ((uint32_t)0x00002000)
+#define SD_OCR_AKE_SEQ_ERROR ((uint32_t)0x00000008)
+#define SD_OCR_ERRORBITS ((uint32_t)0xFDFFE008)
+
+#define SD_R6_GENERAL_UNKNOWN_ERROR ((uint32_t)0x00002000)
+#define SD_R6_ILLEGAL_CMD ((uint32_t)0x00004000)
+#define SD_R6_COM_CRC_FAILED ((uint32_t)0x00008000)
+
+#define SD_VOLTAGE_WINDOW_SD ((uint32_t)0x80100000)
+#define SD_RESP_HIGH_CAPACITY ((uint32_t)0x40000000)
+#define SD_RESP_STD_CAPACITY ((uint32_t)0x00000000)
+#define SD_CHECK_PATTERN ((uint32_t)0x000001AA)
+
+#define SD_MAX_VOLT_TRIAL ((uint32_t)0x0000FFFF)
+#define SD_ALLZERO ((uint32_t)0x00000000)
+
+#define SD_WIDE_BUS_SUPPORT ((uint32_t)0x00040000)
+#define SD_SINGLE_BUS_SUPPORT ((uint32_t)0x00010000)
+#define SD_CARD_LOCKED ((uint32_t)0x02000000)
+
+#define SD_0TO7BITS ((uint32_t)0x000000FF)
+#define SD_8TO15BITS ((uint32_t)0x0000FF00)
+#define SD_16TO23BITS ((uint32_t)0x00FF0000)
+#define SD_24TO31BITS ((uint32_t)0xFF000000)
+#define SD_MAX_DATA_LENGTH ((uint32_t)0x01FFFFFF)
+
+#define SD_CCCC_ERASE ((uint32_t)0x00000020)
+
+#define SD_SDIO_SEND_IF_COND ((uint32_t)SD_CMD_HS_SEND_EXT_CSD)
+
+#define SD_BUS_WIDE_1B ((uint32_t)0x00000000)
+#define SD_BUS_WIDE_4B SDIO_BUSMODE_4BIT
+#define SD_BUS_WIDE_8B SDIO_BUSMODE_8BIT
+
+#define SD_CMD_RESPONSE_SHORT SDIO_RESPONSETYPE_SHORT
+#define SD_CMD_RESPONSE_LONG SDIO_RESPONSETYPE_LONG
+
+#define SD_DATABLOCK_SIZE_8B SDIO_DATABLOCKSIZE_8BYTES
+#define SD_DATABLOCK_SIZE_64B SDIO_DATABLOCKSIZE_64BYTES
+#define SD_DATABLOCK_SIZE_512B SDIO_DATABLOCKSIZE_512BYTES
+
+#define CLKCTL_CLEAR_MASK ((uint32_t)(SDIO_CLKCTL_DIV | SDIO_CLKCTL_CLKPWRSAV |\
+ SDIO_CLKCTL_CLKBYP | SDIO_CLKCTL_BUSMODE |\
+ SDIO_CLKCTL_CLKEDGE | SDIO_CLKCTL_HWCLKEN | SDIO_CLKCTL_DIV8))
+
+#define DATACTRL_CLEAR_MASK ((uint32_t)(SDIO_DATACTL_DATAEN | SDIO_DATACTL_DATADIR |\
+ SDIO_DATACTL_TRANSMOD | SDIO_DATACTL_BLKSZ))
+
+#define CMDCTL_CLEAR_MASK ((uint32_t)(SDIO_CMDCTL_CMDIDX | SDIO_CMDCTL_CMDRESP |\
+ SDIO_CMDCTL_INTWAIT | SDIO_CMDCTL_WAITDEND |\
+ SDIO_CMDCTL_CSMEN | SDIO_CMDCTL_SUSPEND))
+
+#define SDIO_INIT_CLK_DIV ((uint8_t)0x76)
+#define SDIO_CLK_DIV ((uint8_t)0x02)
+
+#define SD_CMD_GO_IDLE_STATE ((uint8_t)0) // Resets the SD memory card.
+#define SD_CMD_SEND_OP_COND ((uint8_t)1) // Sends host capacity support information and activates the card's initialization process.
+#define SD_CMD_ALL_SEND_CID ((uint8_t)2) // Asks any card connected to the host to send the CID numbers on the CMD line.
+#define SD_CMD_SET_REL_ADDR ((uint8_t)3) // Asks the card to publish a new relative address (RCA).
+#define SD_CMD_HS_SWITCH ((uint8_t)6) // Checks switchable function (mode 0) and switch card function (mode 1).
+#define SD_CMD_SEL_DESEL_CARD ((uint8_t)7) // Selects the card by its own relative address and gets deselected by any other address
+#define SD_CMD_HS_SEND_EXT_CSD ((uint8_t)8) // Sends SD Memory Card interface condition, which includes host supply voltage information
+ // and asks the card whether card supports voltage.
+#define SD_CMD_SEND_CSD ((uint8_t)9) // Addressed card sends its card specific data (CSD) on the CMD line.
+#define SD_CMD_SEND_CID ((uint8_t)10) // Addressed card sends its card identification (CID) on the CMD line.
+#define SD_CMD_STOP_TRANSMISSION ((uint8_t)12) // Forces the card to stop transmission.
+#define SD_CMD_SEND_STATUS ((uint8_t)13) // Addressed card sends its status register.
+#define SD_CMD_SET_BLOCKLEN ((uint8_t)16) // Sets the block length (in bytes for SDSC) for all following block commands
+ // (read, write, lock). Default block length is fixed to 512 Bytes. Not effective
+ // for SDHS and SDXC.
+#define SD_CMD_READ_SINGLE_BLOCK ((uint8_t)17) // Reads single block of size selected by SET_BLOCKLEN in case of SDSC, and a block of
+ // fixed 512 bytes in case of SDHC and SDXC.
+#define SD_CMD_READ_MULT_BLOCK ((uint8_t)18) // Continuously transfers data blocks from card to host until interrupted by
+ // STOP_TRANSMISSION command.
+#define SD_CMD_WRITE_SINGLE_BLOCK ((uint8_t)24) // Writes single block of size selected by SET_BLOCKLEN in case of SDSC, and a block of
+ // fixed 512 bytes in case of SDHC and SDXC.
+#define SD_CMD_WRITE_MULT_BLOCK ((uint8_t)25) // Continuously writes blocks of data until a STOP_TRANSMISSION follows.
+#define SD_CMD_SD_ERASE_GRP_START ((uint8_t)32) // Sets the address of the first write block to be erased. (For SD card only).
+#define SD_CMD_SD_ERASE_GRP_END ((uint8_t)33) // Sets the address of the last write block of the continuous range to be erased.
+ // system set by switch function command (CMD6).
+#define SD_CMD_ERASE ((uint8_t)38) // Reserved for SD security applications.
+#define SD_CMD_FAST_IO ((uint8_t)39) // SD card doesn't support it (Reserved).
+#define SD_CMD_APP_CMD ((uint8_t)55) // Indicates to the card that the next command is an application specific command rather
+ // than a standard command.
+
+#define SD_NO_RESPONSE ((int8_t)-1)
+
+/* Following commands are SD Card Specific commands.
+ SDIO_APP_CMD should be sent before sending these commands. */
+#define SD_CMD_APP_SD_SET_BUSWIDTH ((uint8_t)6) // (ACMD6) Defines the data bus width to be used for data transfer. The allowed data bus
+ // widths are given in SCR register.
+#define SD_CMD_SD_APP_STATUS ((uint8_t)13) // (ACMD13) Sends the SD status.
+#define SD_CMD_SD_APP_OP_COND ((uint8_t)41) // (ACMD41) Sends host capacity support information (HCS) and asks the accessed card to
+ // send its operating condition register (OCR) content in the response on the CMD line.
+#define SD_CMD_SD_APP_SEND_SCR ((uint8_t)51) // Reads the SD Configuration Register (SCR).
+
+#define SDIO_DIR_TX 1
+#define SDIO_DIR_RX 0
+
+#define SDIO_DMA_ST3 1
+
+
+typedef enum {
+ SD_SINGLE_BLOCK = 0, // Single block operation
+ SD_MULTIPLE_BLOCK = 1, // Multiple blocks operation
+} SD_Operation_t;
+
+typedef struct {
+ uint32_t CSD[4]; // SD card specific data table
+ uint32_t CID[4]; // SD card identification number table
+ volatile uint32_t TransferComplete; // SD transfer complete flag in non blocking mode
+ volatile uint32_t TransferError; // SD transfer error flag in non blocking mode
+ volatile uint32_t RXCplt; // SD RX Complete is equal 0 when no transfer
+ volatile uint32_t TXCplt; // SD TX Complete is equal 0 when no transfer
+ volatile uint32_t Operation; // SD transfer operation (read/write)
+} SD_Handle_t;
+
+typedef enum {
+ SD_CARD_READY = ((uint32_t)0x00000001), // Card state is ready
+ SD_CARD_IDENTIFICATION = ((uint32_t)0x00000002), // Card is in identification state
+ SD_CARD_STANDBY = ((uint32_t)0x00000003), // Card is in standby state
+ SD_CARD_TRANSFER = ((uint32_t)0x00000004), // Card is in transfer state
+ SD_CARD_SENDING = ((uint32_t)0x00000005), // Card is sending an operation
+ SD_CARD_RECEIVING = ((uint32_t)0x00000006), // Card is receiving operation information
+ SD_CARD_PROGRAMMING = ((uint32_t)0x00000007), // Card is in programming state
+ SD_CARD_DISCONNECTED = ((uint32_t)0x00000008), // Card is disconnected
+ SD_CARD_ERROR = ((uint32_t)0x000000FF) // Card is in error state
+} SD_CardState_t;
+
+
+static SD_Handle_t SD_Handle;
+static SD_CardInfo_t SD_CardInfo;
+static uint32_t SD_Status;
+static uint32_t SD_CardRCA;
+static SD_CardType_t SD_CardType;
+static volatile uint32_t TimeOut;
+static DMA_Stream_TypeDef *dmaStream;
+static uint32_t dma_periph_sdio;
+static int dma_channel_sdio;
+
+
+static void SD_DataTransferInit (uint32_t Size, uint32_t DataBlockSize, bool IsItReadFromCard);
+static SD_Error_t SD_TransmitCommand (uint32_t Command, uint32_t Argument, int8_t ResponseType);
+static SD_Error_t SD_CmdResponse (uint8_t SD_CMD, int8_t ResponseType);
+static void SD_GetResponse (uint32_t* pResponse);
+static SD_Error_t CheckOCR_Response (uint32_t Response_R1);
+static void SD_DMA_Complete (uint32_t dma_periph, dma_channel_enum channelx);
+static SD_Error_t SD_InitializeCard (void);
+
+static SD_Error_t SD_PowerON (void);
+static SD_Error_t SD_WideBusOperationConfig (uint32_t WideMode);
+static SD_Error_t SD_FindSCR (uint32_t *pSCR);
+
+void SDIO_DMA_ST3_IRQHandler(dmaChannelDescriptor_t *dma);
+void SDIO_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma);
+
+
+/*!
+ \brief Prepare the state machine for transfer
+ \param[in] Size: data transfer size
+ \param[in] DataBlockSize: data block size
+ \param[in] IsItReadFromCard: transfer direction flag
+ \param[out] none
+ \retval none
+*/
+static void SD_DataTransferInit(uint32_t Size, uint32_t DataBlockSize, bool IsItReadFromCard)
+ {
+ uint32_t Direction;
+
+ SDIO_DATATO = SD_DATATIMEOUT;
+ SDIO_DATALEN = Size;
+ Direction = (IsItReadFromCard == true) ? SDIO_DATACTL_DATADIR : 0;
+ SDIO_DATACTL |= (uint32_t)(DataBlockSize | Direction | SDIO_DATACTL_DATAEN | 0x01);
+ return;
+}
+
+/*!
+ \brief Send the command to SDIO
+ \param[in] Command: SDIO command
+ \param[in] Argument: command argument
+ \param[in] ResponseType: response type (must provide the response size)
+ \param[out] none
+ \retval SD Card error state
+*/
+static SD_Error_t SD_TransmitCommand(uint32_t Command, uint32_t Argument, int8_t ResponseType)
+{
+ SD_Error_t ErrorState;
+
+ SDIO_INTC = SDIO_INTC_STATIC_FLAGS; // Clear the Command Flags
+ /* disable the CSM */
+ SDIO_CMDCTL &= ~SDIO_CMDCTL_CSMEN;
+ SDIO_CMDAGMT = (uint32_t)Argument; // Set the SDIO Argument value
+ SDIO_CMDCTL = (uint32_t)(Command | SDIO_CMDCTL_CSMEN); // Set SDIO command parameters
+ if((Argument == 0) && (ResponseType == 0)) ResponseType = SD_NO_RESPONSE; // Go idle command
+ ErrorState = SD_CmdResponse(Command & SDIO_CMDCTL_CMDIDX, ResponseType);
+ SDIO_INTC = SDIO_INTC_STATIC_FLAGS; // Clear the Command Flags
+
+ return ErrorState;
+}
+
+/*!
+ \brief Checks for error conditions for any response (R2, R3, etc.)
+ \param[in] SD_CMD: the sent command Index
+ \param[in] ResponseType: response type
+ \param[out] none
+ \retval SD Card error state
+*/
+static SD_Error_t SD_CmdResponse(uint8_t SD_CMD, int8_t ResponseType)
+{
+ uint32_t Response_R1;
+ uint32_t TimeOut;
+ uint32_t Flag;
+
+ if(ResponseType == SD_NO_RESPONSE) {
+ Flag = SDIO_STAT_CMDSEND;
+ } else {
+ Flag = SDIO_STAT_CCRCERR | SDIO_STAT_CMDRECV | SDIO_STAT_CMDTMOUT;
+ }
+
+ TimeOut = SD_SOFTWARE_COMMAND_TIMEOUT;
+ do {
+ SD_Status = SDIO_STAT;
+ TimeOut--;
+ } while(((SD_Status & Flag) == 0) && (TimeOut > 0));
+
+ if(ResponseType <= 0) {
+ if(TimeOut == 0) {
+ return SD_CMD_RSP_TIMEOUT;
+ } else {
+ return SD_OK;
+ }
+ }
+
+ if((SDIO_STAT & SDIO_STAT_CMDTMOUT) != 0) {
+ return SD_CMD_RSP_TIMEOUT;
+ }
+
+ if(ResponseType == 3) {
+ if(TimeOut == 0) {
+ return SD_CMD_RSP_TIMEOUT; // Card is not V2.0 compliant or card does not support the set voltage range
+ } else {
+ return SD_OK; // Card is SD V2.0 compliant
+ }
+ }
+
+ if((SDIO_STAT & SDIO_STAT_CCRCERR) != 0) {
+ return SD_CMD_CRC_FAIL;
+ }
+ if(ResponseType == 2) {
+ return SD_OK;
+ }
+ if((uint8_t)SDIO_RSPCMDIDX != SD_CMD) {
+ return SD_ILLEGAL_CMD;
+ }
+
+ Response_R1 = SDIO_RESP0; // We have received response, retrieve it for analysis
+
+ if(ResponseType == 1) {
+ return CheckOCR_Response(Response_R1);
+ } else if(ResponseType == 6) {
+ if((Response_R1 & (SD_R6_GENERAL_UNKNOWN_ERROR | SD_R6_ILLEGAL_CMD | SD_R6_COM_CRC_FAILED)) == SD_ALLZERO) {
+ SD_CardRCA = Response_R1;
+ }
+ if((Response_R1 & SD_R6_GENERAL_UNKNOWN_ERROR) == SD_R6_GENERAL_UNKNOWN_ERROR) {
+ return SD_GENERAL_UNKNOWN_ERROR;
+ }
+ if((Response_R1 & SD_R6_ILLEGAL_CMD) == SD_R6_ILLEGAL_CMD) {
+ return SD_ILLEGAL_CMD;
+ }
+ if((Response_R1 & SD_R6_COM_CRC_FAILED) == SD_R6_COM_CRC_FAILED) {
+ return SD_COM_CRC_FAILED;
+ }
+ }
+
+ return SD_OK;
+}
+
+/*!
+ \brief Analyze the OCR response and return the appropriate error code
+ \param[in] Response_R1: OCR Response code
+ \param[out] none
+ \retval SD Card error state
+*/
+static SD_Error_t CheckOCR_Response(uint32_t Response_R1)
+{
+ if((Response_R1 & SD_OCR_ERRORBITS) == SD_ALLZERO) return SD_OK;
+ if((Response_R1 & SD_OCR_ADDR_OUT_OF_RANGE) == SD_OCR_ADDR_OUT_OF_RANGE) return SD_ADDR_OUT_OF_RANGE;
+ if((Response_R1 & SD_OCR_ADDR_MISALIGNED) == SD_OCR_ADDR_MISALIGNED) return SD_ADDR_MISALIGNED;
+ if((Response_R1 & SD_OCR_BLOCK_LEN_ERR) == SD_OCR_BLOCK_LEN_ERR) return SD_BLOCK_LEN_ERR;
+ if((Response_R1 & SD_OCR_ERASE_SEQ_ERR) == SD_OCR_ERASE_SEQ_ERR) return SD_ERASE_SEQ_ERR;
+ if((Response_R1 & SD_OCR_BAD_ERASE_PARAM) == SD_OCR_BAD_ERASE_PARAM) return SD_BAD_ERASE_PARAM;
+ if((Response_R1 & SD_OCR_WRITE_PROT_VIOLATION) == SD_OCR_WRITE_PROT_VIOLATION) return SD_WRITE_PROT_VIOLATION;
+ if((Response_R1 & SD_OCR_LOCK_UNLOCK_FAILED) == SD_OCR_LOCK_UNLOCK_FAILED) return SD_LOCK_UNLOCK_FAILED;
+ if((Response_R1 & SD_OCR_COM_CRC_FAILED) == SD_OCR_COM_CRC_FAILED) return SD_COM_CRC_FAILED;
+ if((Response_R1 & SD_OCR_ILLEGAL_CMD) == SD_OCR_ILLEGAL_CMD) return SD_ILLEGAL_CMD;
+ if((Response_R1 & SD_OCR_CARD_ECC_FAILED) == SD_OCR_CARD_ECC_FAILED) return SD_CARD_ECC_FAILED;
+ if((Response_R1 & SD_OCR_CC_ERROR) == SD_OCR_CC_ERROR) return SD_CC_ERROR;
+ if((Response_R1 & SD_OCR_GENERAL_UNKNOWN_ERROR) == SD_OCR_GENERAL_UNKNOWN_ERROR)return SD_GENERAL_UNKNOWN_ERROR;
+ if((Response_R1 & SD_OCR_STREAM_READ_UNDERRUN) == SD_OCR_STREAM_READ_UNDERRUN) return SD_STREAM_READ_UNDERRUN;
+ if((Response_R1 & SD_OCR_STREAM_WRITE_OVERRUN) == SD_OCR_STREAM_WRITE_OVERRUN) return SD_STREAM_WRITE_OVERRUN;
+ if((Response_R1 & SD_OCR_CID_CSD_OVERWRITE) == SD_OCR_CID_CSD_OVERWRITE) return SD_CID_CSD_OVERWRITE;
+ if((Response_R1 & SD_OCR_WP_ERASE_SKIP) == SD_OCR_WP_ERASE_SKIP) return SD_WP_ERASE_SKIP;
+ if((Response_R1 & SD_OCR_CARD_ECC_DISABLED) == SD_OCR_CARD_ECC_DISABLED) return SD_CARD_ECC_DISABLED;
+ if((Response_R1 & SD_OCR_ERASE_RESET) == SD_OCR_ERASE_RESET) return SD_ERASE_RESET;
+ if((Response_R1 & SD_OCR_AKE_SEQ_ERROR) == SD_OCR_AKE_SEQ_ERROR) return SD_AKE_SEQ_ERROR;
+
+ return SD_OK;
+}
+
+/*!
+ \brief Get response from SD device
+ \param[in] none
+ \param[out] pResponse: pointer to response buffer
+ \retval none
+*/
+static void SD_GetResponse(uint32_t* pResponse)
+{
+ pResponse[0] = SDIO_RESP0;
+ pResponse[1] = SDIO_RESP1;
+ pResponse[2] = SDIO_RESP2;
+ pResponse[3] = SDIO_RESP3;
+}
+
+/*!
+ \brief SD DMA transfer complete RX and TX callback
+ \param[in] dma_periph: DMA peripheral
+ \param[in] channelx: DMA channel
+ \param[out] none
+ \retval none
+*/
+static void SD_DMA_Complete(uint32_t dma_periph, dma_channel_enum channelx)
+{
+ if (SD_Handle.RXCplt) {
+ if (SD_Handle.Operation == ((SDIO_DIR_RX << 1) | SD_MULTIPLE_BLOCK)) {
+ /* Send stop command in multiblock write */
+ SD_TransmitCommand((SD_CMD_STOP_TRANSMISSION | SD_CMD_RESPONSE_SHORT), 0, 1);
+ }
+
+ SDIO_DATACTL &= (uint32_t)~((uint32_t)SDIO_DATACTL_DMAEN);
+
+ /* Clear all the static flags */
+ SDIO_INTC = SDIO_INTC_STATIC_FLAGS;
+
+ /* Clear flag */
+ SD_Handle.RXCplt = 0;
+
+ /* Disable the channel */
+ DMA_CHCTL(dma_periph, channelx) &= ~DMA_CHXCTL_CHEN;
+ } else {
+ /* Enable Dataend IE */
+ SDIO_INTEN |= SDIO_INTEN_DTENDIE;
+ }
+}
+
+/*!
+ \brief Prepare the DMA transfer
+ \param[in] pBuffer: pointer to the buffer that will contain the data to transmit
+ \param[in] BlockSize: the SD card Data block size (must be 512 bytes)
+ \param[in] NumberOfBlocks: number of blocks to write
+ \param[in] dir: transfer direction
+ \param[out] none
+ \retval none
+ \note BlockSize must be 512 bytes
+*/
+static SD_Error_t SD_InitializeCard(void)
+{
+ SD_Error_t ErrorState = SD_OK;
+
+ if((SDIO_PWRCTL & SDIO_PWRCTL_PWRCTL) != 0)
+ {
+ if(SD_CardType != SD_SECURE_DIGITAL_IO) {
+ // Send CMD2 ALL_SEND_CID
+ if((ErrorState = SD_TransmitCommand((SD_CMD_ALL_SEND_CID | SD_CMD_RESPONSE_LONG), 0, 2)) != SD_OK) {
+ return ErrorState;
+ }
+
+ // Get Card identification number data
+ SD_GetResponse(SD_Handle.CID);
+ }
+
+ if((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) ||
+ (SD_CardType == SD_SECURE_DIGITAL_IO_COMBO) || (SD_CardType == SD_HIGH_CAPACITY)) {
+ // Send CMD3 SET_REL_ADDR with argument 0
+ // SD Card publishes its RCA.
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SET_REL_ADDR | SD_CMD_RESPONSE_SHORT), 0, 6)) != SD_OK) {
+ return ErrorState;
+ }
+ }
+
+ if(SD_CardType != SD_SECURE_DIGITAL_IO) {
+ // Send CMD9 SEND_CSD with argument as card's RCA
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SEND_CSD | SD_CMD_RESPONSE_LONG), SD_CardRCA, 2)) == SD_OK) {
+ // Get Card Specific Data
+ SD_GetResponse(SD_Handle.CSD);
+ }
+ }
+ } else {
+ ErrorState = SD_REQUEST_NOT_APPLICABLE;
+ }
+
+ return ErrorState;
+}
+
+/*!
+ \brief Reads block(s) from a specified address in a card. The Data transfer is managed by DMA mode
+ \param[in] ReadAddress: address from where data is to be read
+ \param[in] buffer: pointer to the buffer that will contain the received data
+ \param[in] BlockSize: SD card Data block size (must be 512 bytes)
+ \param[in] NumberOfBlocks: number of blocks to read
+ \param[out] none
+ \retval SD Card error state
+ \note This API should be followed by the function SD_CheckOperation() to check the completion of the read process. BlockSize must be 512 bytes
+*/
+static void SD_StartBlockTransfert(uint32_t* pBuffer, uint32_t BlockSize, uint32_t NumberOfBlocks, uint8_t dir)
+{
+ SDIO_DATACTL = 0; // Initialize data control register
+ SD_Handle.TransferComplete = 0; // Initialize handle flags
+ SD_Handle.TransferError = SD_OK;
+ SD_Handle.Operation = (NumberOfBlocks > 1) ? SD_MULTIPLE_BLOCK : SD_SINGLE_BLOCK; // Initialize SD Read operation
+ SD_Handle.Operation |= dir << 1;
+ SDIO_INTEN = 0;
+ if (dir == SDIO_DIR_RX) {
+ SDIO_INTEN |= (SDIO_INTEN_DTCRCERRIE | SDIO_INTEN_DTTMOUTIE | // Enable transfer interrupts
+ SDIO_INTEN_DTENDIE | SDIO_INTEN_RXOREIE);
+ } else {
+ SDIO_INTEN |= (SDIO_INTEN_DTCRCERRIE | SDIO_INTEN_DTTMOUTIE | // Enable transfer interrupts
+ SDIO_INTEN_TXUREIE);
+ }
+ if (dir == SDIO_DIR_TX) {
+ SDIO_DATACTL |= SDIO_DATACTL_DMAEN; // Enable SDIO DMA transfer
+ }
+ DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) &= ~DMA_CHXCTL_CHEN; // Disable the Peripheral
+ while (DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) & DMA_CHXCTL_CHEN);
+ DMA_CHCNT(dma_periph_sdio,dma_channel_sdio) = (uint32_t) (BlockSize * NumberOfBlocks) / 4; // Configure DMA Stream data length
+ DMA_CHM0ADDR(dma_periph_sdio,dma_channel_sdio) = (uint32_t) pBuffer; // Configure DMA Stream memory address
+ if (dir == SDIO_DIR_RX) {
+ DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) &= ~(0x01U << 6U); // Sets peripheral to memory
+ } else {
+ DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) |= DMA_MEMORY_TO_PERIPH; // Sets memory to peripheral
+ }
+ if ((uint32_t)dmaStream == DMA1_CH3_BASE) {
+ // clear dma flags
+ dma_flag_clear(DMA1, DMA1_CH3, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF);
+ } else {
+ // clear dma flags
+ dma_flag_clear(DMA1, DMA1_CH6, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF); // Clear the transfer error flag
+ }
+ DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) |= DMA_CHXCTL_FTFIE | DMA_CHXCTL_HTFIE | DMA_CHXCTL_TAEIE | DMA_CHXCTL_SDEIE; // Enable all interrupts
+ DMA_CHFCTL(dma_periph_sdio,dma_channel_sdio) |= DMA_CHXFCTL_FEEIE;
+ DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) |= DMA_CHXCTL_CHEN; // Enable the Peripheral
+ if (dir == SDIO_DIR_RX) {
+ SDIO_DATACTL |= SDIO_DATACTL_DMAEN; // Enable SDIO DMA transfer
+ }
+}
+
+/*!
+ \brief Writes block(s) to a specified address in a card. The Data transfer is managed by DMA mode
+ \param[in] WriteAddress: address from where data is to be read
+ \param[in] buffer: pointer to the buffer that will contain the data to transmit
+ \param[in] BlockSize: the SD card Data block size (must be 512 bytes)
+ \param[in] NumberOfBlocks: number of blocks to write
+ \param[out] none
+ \retval SD Card error state
+ \note This API should be followed by the function SD_CheckOperation() to check the completion of the write process (by SD current status polling). BlockSize must be 512 bytes
+*/
+SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks)
+{
+ SD_Error_t ErrorState = SD_OK;
+ uint32_t CmdIndex;
+ SD_Handle.RXCplt = 1;
+
+ if(SD_CardType != SD_HIGH_CAPACITY)
+ {
+ ReadAddress *= 512;
+ }
+
+ SD_StartBlockTransfert(buffer, BlockSize, NumberOfBlocks, SDIO_DIR_RX);
+
+ // Configure the SD DPSM (Data Path State Machine)
+ SD_DataTransferInit(BlockSize * NumberOfBlocks, SD_DATABLOCK_SIZE_512B, true);
+
+ // Set Block Size for Card
+ ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), BlockSize, 1);
+
+ // Send CMD18 READ_MULT_BLOCK with argument data address
+ // or send CMD17 READ_SINGLE_BLOCK depending on number of block
+ uint8_t retries = 10;
+ CmdIndex = (NumberOfBlocks > 1) ? SD_CMD_READ_MULT_BLOCK : SD_CMD_READ_SINGLE_BLOCK;
+ do {
+ ErrorState = SD_TransmitCommand((CmdIndex | SD_CMD_RESPONSE_SHORT), (uint32_t)ReadAddress, 1);
+ if (ErrorState != SD_OK && retries--) {
+ ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1);
+ }
+ } while (ErrorState != SD_OK && retries);
+
+ if (ErrorState != SD_OK) {
+ SD_Handle.RXCplt = 0;
+ }
+
+ // Update the SD transfer error in SD handle
+ SD_Handle.TransferError = ErrorState;
+
+ return ErrorState;
+}
+
+/*!
+ \brief Writes block(s) to a specified address in a card. The Data transfer is managed by DMA mode
+ \param[in] WriteAddress: address from where data is to be read
+ \param[in] buffer: pointer to the buffer that will contain the data to transmit
+ \param[in] BlockSize: the SD card Data block size (must be 512 bytes)
+ \param[in] NumberOfBlocks: number of blocks to write
+ \param[out] none
+ \retval SD Card error state
+ \note This API should be followed by the function SD_CheckOperation() to check the completion of the write process (by SD current status polling). BlockSize must be 512 bytes
+*/
+SD_Error_t SD_WriteBlocks_DMA(uint64_t WriteAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks)
+{
+ SD_Error_t ErrorState = SD_OK;
+ uint32_t CmdIndex;
+ SD_Handle.TXCplt = 1;
+
+ //printf("Reading at %ld into %p %ld blocks\n", (uint32_t)WriteAddress, (void*)buffer, NumberOfBlocks);
+
+ if(SD_CardType != SD_HIGH_CAPACITY)
+ {
+ WriteAddress *= 512;
+ }
+
+ // Check number of blocks command
+ // Send CMD24 WRITE_SINGLE_BLOCK
+ // Send CMD25 WRITE_MULT_BLOCK with argument data address
+ CmdIndex = (NumberOfBlocks > 1) ? SD_CMD_WRITE_MULT_BLOCK : SD_CMD_WRITE_SINGLE_BLOCK;
+
+ // Set Block Size for Card
+ uint8_t retries = 10;
+ do {
+ ErrorState = SD_TransmitCommand((CmdIndex | SD_CMD_RESPONSE_SHORT), (uint32_t)WriteAddress, 1);
+ if (ErrorState != SD_OK && retries--) {
+ ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1);
+ }
+ } while(ErrorState != SD_OK && retries);
+
+ if (ErrorState != SD_OK) {
+ SD_Handle.TXCplt = 0;
+ return ErrorState;
+ }
+
+ SD_StartBlockTransfert(buffer, BlockSize, NumberOfBlocks, SDIO_DIR_TX);
+
+ // Configure the SD DPSM (Data Path State Machine)
+ SD_DataTransferInit(BlockSize * NumberOfBlocks, SD_DATABLOCK_SIZE_512B, false);
+
+ SD_Handle.TransferError = ErrorState;
+
+ return ErrorState;
+}
+
+SD_Error_t SD_CheckWrite(void)
+{
+ if (SD_Handle.TXCplt != 0) return SD_BUSY;
+ return SD_OK;
+}
+
+SD_Error_t SD_CheckRead(void)
+{
+ if (SD_Handle.RXCplt != 0) return SD_BUSY;
+ return SD_OK;
+}
+
+/*!
+ \brief Returns information about specific card, contains all SD card information
+ \param[in] none
+ \param[out] none
+ \retval SD Card error state
+*/
+SD_Error_t SD_GetCardInfo(void)
+{
+ SD_Error_t ErrorState = SD_OK;
+ uint32_t Temp = 0;
+
+ // Byte 0
+ Temp = (SD_Handle.CSD[0] & 0xFF000000) >> 24;
+ SD_CardInfo.SD_csd.CSDStruct = (uint8_t)((Temp & 0xC0) >> 6);
+ SD_CardInfo.SD_csd.SysSpecVersion = (uint8_t)((Temp & 0x3C) >> 2);
+ SD_CardInfo.SD_csd.Reserved1 = Temp & 0x03;
+
+ // Byte 1
+ Temp = (SD_Handle.CSD[0] & 0x00FF0000) >> 16;
+ SD_CardInfo.SD_csd.TAAC = (uint8_t)Temp;
+
+ // Byte 2
+ Temp = (SD_Handle.CSD[0] & 0x0000FF00) >> 8;
+ SD_CardInfo.SD_csd.NSAC = (uint8_t)Temp;
+
+ // Byte 3
+ Temp = SD_Handle.CSD[0] & 0x000000FF;
+ SD_CardInfo.SD_csd.MaxBusClkFrec = (uint8_t)Temp;
+
+ // Byte 4
+ Temp = (SD_Handle.CSD[1] & 0xFF000000) >> 24;
+ SD_CardInfo.SD_csd.CardComdClasses = (uint16_t)(Temp << 4);
+
+ // Byte 5
+ Temp = (SD_Handle.CSD[1] & 0x00FF0000) >> 16;
+ SD_CardInfo.SD_csd.CardComdClasses |= (uint16_t)((Temp & 0xF0) >> 4);
+ SD_CardInfo.SD_csd.RdBlockLen = (uint8_t)(Temp & 0x0F);
+
+ // Byte 6
+ Temp = (SD_Handle.CSD[1] & 0x0000FF00) >> 8;
+ SD_CardInfo.SD_csd.PartBlockRead = (uint8_t)((Temp & 0x80) >> 7);
+ SD_CardInfo.SD_csd.WrBlockMisalign = (uint8_t)((Temp & 0x40) >> 6);
+ SD_CardInfo.SD_csd.RdBlockMisalign = (uint8_t)((Temp & 0x20) >> 5);
+ SD_CardInfo.SD_csd.DSRImpl = (uint8_t)((Temp & 0x10) >> 4);
+ SD_CardInfo.SD_csd.Reserved2 = 0; /*!< Reserved */
+
+ if((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0)) {
+ SD_CardInfo.SD_csd.DeviceSize = (Temp & 0x03) << 10;
+
+ // Byte 7
+ Temp = (uint8_t)(SD_Handle.CSD[1] & 0x000000FF);
+ SD_CardInfo.SD_csd.DeviceSize |= (Temp) << 2;
+
+ // Byte 8
+ Temp = (uint8_t)((SD_Handle.CSD[2] & 0xFF000000) >> 24);
+ SD_CardInfo.SD_csd.DeviceSize |= (Temp & 0xC0) >> 6;
+
+ SD_CardInfo.SD_csd.MaxRdCurrentVDDMin = (Temp & 0x38) >> 3;
+ SD_CardInfo.SD_csd.MaxRdCurrentVDDMax = (Temp & 0x07);
+
+ // Byte 9
+ Temp = (uint8_t)((SD_Handle.CSD[2] & 0x00FF0000) >> 16);
+ SD_CardInfo.SD_csd.MaxWrCurrentVDDMin = (Temp & 0xE0) >> 5;
+ SD_CardInfo.SD_csd.MaxWrCurrentVDDMax = (Temp & 0x1C) >> 2;
+ SD_CardInfo.SD_csd.DeviceSizeMul = (Temp & 0x03) << 1;
+
+ // Byte 10
+ Temp = (uint8_t)((SD_Handle.CSD[2] & 0x0000FF00) >> 8);
+ SD_CardInfo.SD_csd.DeviceSizeMul |= (Temp & 0x80) >> 7;
+
+ SD_CardInfo.CardCapacity = (SD_CardInfo.SD_csd.DeviceSize + 1) ;
+ SD_CardInfo.CardCapacity *= (1 << (SD_CardInfo.SD_csd.DeviceSizeMul + 2));
+ SD_CardInfo.CardBlockSize = 1 << (SD_CardInfo.SD_csd.RdBlockLen);
+ SD_CardInfo.CardCapacity = SD_CardInfo.CardCapacity * SD_CardInfo.CardBlockSize / 512; // In 512 byte blocks
+ } else if(SD_CardType == SD_HIGH_CAPACITY) {
+ // Byte 7
+ Temp = (uint8_t)(SD_Handle.CSD[1] & 0x000000FF);
+ SD_CardInfo.SD_csd.DeviceSize = (Temp & 0x3F) << 16;
+
+ // Byte 8
+ Temp = (uint8_t)((SD_Handle.CSD[2] & 0xFF000000) >> 24);
+
+ SD_CardInfo.SD_csd.DeviceSize |= (Temp << 8);
+
+ // Byte 9
+ Temp = (uint8_t)((SD_Handle.CSD[2] & 0x00FF0000) >> 16);
+
+ SD_CardInfo.SD_csd.DeviceSize |= (Temp);
+
+ // Byte 10
+ Temp = (uint8_t)((SD_Handle.CSD[2] & 0x0000FF00) >> 8);
+
+ SD_CardInfo.CardCapacity = ((uint64_t)SD_CardInfo.SD_csd.DeviceSize + 1) * 1024;
+ SD_CardInfo.CardBlockSize = 512;
+ } else {
+ // Not supported card type
+ ErrorState = SD_ERROR;
+ }
+
+ SD_CardInfo.SD_csd.EraseGrSize = (Temp & 0x40) >> 6;
+ SD_CardInfo.SD_csd.EraseGrMul = (Temp & 0x3F) << 1;
+
+ // Byte 11
+ Temp = (uint8_t)(SD_Handle.CSD[2] & 0x000000FF);
+ SD_CardInfo.SD_csd.EraseGrMul |= (Temp & 0x80) >> 7;
+ SD_CardInfo.SD_csd.WrProtectGrSize = (Temp & 0x7F);
+
+ // Byte 12
+ Temp = (uint8_t)((SD_Handle.CSD[3] & 0xFF000000) >> 24);
+ SD_CardInfo.SD_csd.WrProtectGrEnable = (Temp & 0x80) >> 7;
+ SD_CardInfo.SD_csd.ManDeflECC = (Temp & 0x60) >> 5;
+ SD_CardInfo.SD_csd.WrSpeedFact = (Temp & 0x1C) >> 2;
+ SD_CardInfo.SD_csd.MaxWrBlockLen = (Temp & 0x03) << 2;
+
+ // Byte 13
+ Temp = (uint8_t)((SD_Handle.CSD[3] & 0x00FF0000) >> 16);
+ SD_CardInfo.SD_csd.MaxWrBlockLen |= (Temp & 0xC0) >> 6;
+ SD_CardInfo.SD_csd.WriteBlockPaPartial = (Temp & 0x20) >> 5;
+ SD_CardInfo.SD_csd.Reserved3 = 0;
+ SD_CardInfo.SD_csd.ContentProtectAppli = (Temp & 0x01);
+
+ // Byte 14
+ Temp = (uint8_t)((SD_Handle.CSD[3] & 0x0000FF00) >> 8);
+ SD_CardInfo.SD_csd.FileFormatGrouop = (Temp & 0x80) >> 7;
+ SD_CardInfo.SD_csd.CopyFlag = (Temp & 0x40) >> 6;
+ SD_CardInfo.SD_csd.PermWrProtect = (Temp & 0x20) >> 5;
+ SD_CardInfo.SD_csd.TempWrProtect = (Temp & 0x10) >> 4;
+ SD_CardInfo.SD_csd.FileFormat = (Temp & 0x0C) >> 2;
+ SD_CardInfo.SD_csd.ECC = (Temp & 0x03);
+
+ // Byte 15
+ Temp = (uint8_t)(SD_Handle.CSD[3] & 0x000000FF);
+ SD_CardInfo.SD_csd.CSD_CRC = (Temp & 0xFE) >> 1;
+ SD_CardInfo.SD_csd.Reserved4 = 1;
+
+ // Byte 0
+ Temp = (uint8_t)((SD_Handle.CID[0] & 0xFF000000) >> 24);
+ SD_CardInfo.SD_cid.ManufacturerID = Temp;
+
+ // Byte 1
+ Temp = (uint8_t)((SD_Handle.CID[0] & 0x00FF0000) >> 16);
+ SD_CardInfo.SD_cid.OEM_AppliID = Temp << 8;
+
+ // Byte 2
+ Temp = (uint8_t)((SD_Handle.CID[0] & 0x000000FF00) >> 8);
+ SD_CardInfo.SD_cid.OEM_AppliID |= Temp;
+
+ // Byte 3
+ Temp = (uint8_t)(SD_Handle.CID[0] & 0x000000FF);
+ SD_CardInfo.SD_cid.ProdName1 = Temp << 24;
+
+ // Byte 4
+ Temp = (uint8_t)((SD_Handle.CID[1] & 0xFF000000) >> 24);
+ SD_CardInfo.SD_cid.ProdName1 |= Temp << 16;
+
+ // Byte 5
+ Temp = (uint8_t)((SD_Handle.CID[1] & 0x00FF0000) >> 16);
+ SD_CardInfo.SD_cid.ProdName1 |= Temp << 8;
+
+ // Byte 6
+ Temp = (uint8_t)((SD_Handle.CID[1] & 0x0000FF00) >> 8);
+ SD_CardInfo.SD_cid.ProdName1 |= Temp;
+
+ // Byte 7
+ Temp = (uint8_t)(SD_Handle.CID[1] & 0x000000FF);
+ SD_CardInfo.SD_cid.ProdName2 = Temp;
+
+ // Byte 8
+ Temp = (uint8_t)((SD_Handle.CID[2] & 0xFF000000) >> 24);
+ SD_CardInfo.SD_cid.ProdRev = Temp;
+
+ // Byte 9
+ Temp = (uint8_t)((SD_Handle.CID[2] & 0x00FF0000) >> 16);
+ SD_CardInfo.SD_cid.ProdSN = Temp << 24;
+
+ // Byte 10
+ Temp = (uint8_t)((SD_Handle.CID[2] & 0x0000FF00) >> 8);
+ SD_CardInfo.SD_cid.ProdSN |= Temp << 16;
+
+ // Byte 11
+ Temp = (uint8_t)(SD_Handle.CID[2] & 0x000000FF);
+ SD_CardInfo.SD_cid.ProdSN |= Temp << 8;
+
+ // Byte 12
+ Temp = (uint8_t)((SD_Handle.CID[3] & 0xFF000000) >> 24);
+ SD_CardInfo.SD_cid.ProdSN |= Temp;
+
+ // Byte 13
+ Temp = (uint8_t)((SD_Handle.CID[3] & 0x00FF0000) >> 16);
+ SD_CardInfo.SD_cid.Reserved1 |= (Temp & 0xF0) >> 4;
+ SD_CardInfo.SD_cid.ManufactDate = (Temp & 0x0F) << 8;
+
+ // Byte 14
+ Temp = (uint8_t)((SD_Handle.CID[3] & 0x0000FF00) >> 8);
+ SD_CardInfo.SD_cid.ManufactDate |= Temp;
+
+ // Byte 15
+ Temp = (uint8_t)(SD_Handle.CID[3] & 0x000000FF);
+ SD_CardInfo.SD_cid.CID_CRC = (Temp & 0xFE) >> 1;
+ SD_CardInfo.SD_cid.Reserved2 = 1;
+
+ return ErrorState;
+}
+
+/*!
+ \brief Enables wide bus operation for the requested card if supported by card
+ \param[in] WideMode: specifies the SD card wide bus mode (SD_BUS_WIDE_8B/4B/1B)
+ \param[out] none
+ \retval SD Card error state
+ \note WideMode can be: SD_BUS_WIDE_8B (8-bit data transfer, only for MMC), SD_BUS_WIDE_4B (4-bit data transfer), SD_BUS_WIDE_1B (1-bit data transfer)
+*/
+static SD_Error_t SD_WideBusOperationConfig(uint32_t WideMode)
+{
+ SD_Error_t ErrorState = SD_OK;
+ uint32_t Temp;
+ uint32_t reg;
+ uint32_t SCR[2] = {0, 0};
+
+ if((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) ||
+ (SD_CardType == SD_HIGH_CAPACITY)) {
+
+ if(WideMode == SD_BUS_WIDE_8B) {
+ ErrorState = SD_UNSUPPORTED_FEATURE;
+ } else if((WideMode == SD_BUS_WIDE_4B) || (WideMode == SD_BUS_WIDE_1B)) {
+ if((SDIO_RESP0 & SD_CARD_LOCKED) != SD_CARD_LOCKED) {
+ // Get SCR Register
+ ErrorState = SD_FindSCR(SCR);
+ if(ErrorState == SD_OK) {
+ Temp = (WideMode == SD_BUS_WIDE_4B) ? SD_WIDE_BUS_SUPPORT : SD_SINGLE_BUS_SUPPORT;
+
+ // If requested card supports wide bus operation
+ if((SCR[1] & Temp) != SD_ALLZERO) {
+ // Send CMD55 APP_CMD with argument as card's RCA.
+ ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1);
+ if(ErrorState == SD_OK) {
+ Temp = (WideMode == SD_BUS_WIDE_4B) ? 2 : 0;
+
+ // Send ACMD6 APP_CMD with argument as 2 for wide bus mode
+ ErrorState = SD_TransmitCommand((SD_CMD_APP_SD_SET_BUSWIDTH | SD_CMD_RESPONSE_SHORT), Temp, 1);
+ }
+ } else {
+ ErrorState = SD_REQUEST_NOT_APPLICABLE;
+ }
+ }
+ } else {
+ ErrorState = SD_LOCK_UNLOCK_FAILED;
+ }
+ } else {
+ ErrorState = SD_INVALID_PARAMETER; // WideMode is not a valid argument
+ }
+
+ if(ErrorState == SD_OK) {
+ // Configure the SDIO peripheral, we need this delay for some reason...
+ while ((SDIO_CLKCTL & 0x800) != WideMode) {
+ reg = SDIO_CLKCTL;
+ reg &= ~CLKCTL_CLEAR_MASK;
+ reg |=(uint32_t) WideMode;
+ SDIO_CLKCTL = reg;
+ }
+ }
+ } else {
+ ErrorState = SD_UNSUPPORTED_FEATURE;
+ }
+
+ return ErrorState;
+}
+
+static SD_Error_t SD_HighSpeed(void)
+{
+ SD_Error_t ErrorState;
+ uint8_t SD_hs[64] = {0};
+ uint32_t SD_scr[2] = {0, 0};
+ uint32_t SD_SPEC = 0;
+ uint32_t Count = 0;
+ uint32_t* Buffer = (uint32_t *)SD_hs;
+
+ // Initialize the Data control register
+ SDIO_DATACTL = 0;
+
+ // Get SCR Register
+ if((ErrorState = SD_FindSCR(SD_scr)) != SD_OK) {
+ return ErrorState;
+ }
+
+ // Test the Version supported by the card
+ SD_SPEC = (SD_scr[1] & 0x01000000) | (SD_scr[1] & 0x02000000);
+
+ if(SD_SPEC != SD_ALLZERO) {
+ // Set Block Size for Card
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), 64, 1)) != SD_OK) {
+ return ErrorState;
+ }
+
+ // Configure the SD DPSM (Data Path State Machine)
+ SD_DataTransferInit(64, SD_DATABLOCK_SIZE_64B, true);
+
+ // Send CMD6 switch mode
+ if((ErrorState =SD_TransmitCommand((SD_CMD_HS_SWITCH | SD_CMD_RESPONSE_SHORT), 0x80FFFF01, 1)) != SD_OK) {
+ return ErrorState;
+ }
+
+ while((SDIO_STAT & (SDIO_STAT_RXORE | SDIO_STAT_DTCRCERR | SDIO_STAT_DTTMOUT | SDIO_STAT_DTBLKEND)) == 0) {
+ if((SDIO_STAT & SDIO_STAT_RFH) != 0) {
+ for(Count = 0; Count < 8; Count++) {
+ *(Buffer + Count) = SDIO_FIFO;
+ }
+
+ Buffer += 8;
+ }
+ }
+
+ if((SDIO_STAT & SDIO_STAT_DTTMOUT) != 0) return SD_DATA_TIMEOUT;
+ else if((SDIO_STAT & SDIO_STAT_DTCRCERR) != 0) return SD_DATA_CRC_FAIL;
+ else if((SDIO_STAT & SDIO_STAT_RXORE) != 0) return SD_RX_OVERRUN;
+
+ Count = SD_DATATIMEOUT;
+
+ while(((SDIO_STAT & SDIO_STAT_RXDTVAL) != 0) && (Count > 0)) {
+ *Buffer = SDIO_FIFO;
+ Buffer++;
+ Count--;
+ }
+
+ // Test if the switch mode HS is ok
+ if((SD_hs[13] & 2) != 2) {
+ ErrorState = SD_UNSUPPORTED_FEATURE;
+ }
+ }
+
+ return ErrorState;
+}
+
+static SD_Error_t SD_GetStatus(void)
+{
+ SD_Error_t ErrorState;
+ uint32_t Response1;
+ SD_CardState_t CardState;
+
+ // Send Status command
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SEND_STATUS | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) == SD_OK) {
+ Response1 = SDIO_RESP0;
+ CardState = (SD_CardState_t)((Response1 >> 9) & 0x0F);
+
+ // Find SD status according to card state
+ if (CardState == SD_CARD_TRANSFER) ErrorState = SD_OK;
+ else if(CardState == SD_CARD_ERROR) ErrorState = SD_ERROR;
+ else ErrorState = SD_BUSY;
+ } else {
+ ErrorState = SD_ERROR;
+ }
+
+ return ErrorState;
+}
+
+/*!
+ \brief Gets the SD card status
+ \param[in] none
+ \param[out] pCardStatus: pointer to SD card status structure
+ \retval SD Card error state
+*/
+SD_Error_t SD_GetCardStatus(SD_CardStatus_t* pCardStatus)
+{
+ SD_Error_t ErrorState;
+ uint32_t Temp = 0;
+ uint32_t Status[16];
+ uint32_t Count;
+
+ // Check SD response
+ if((SDIO_RESP0 & SD_CARD_LOCKED) == SD_CARD_LOCKED) {
+ return SD_LOCK_UNLOCK_FAILED;
+ }
+
+ // Set block size for card if it is not equal to current block size for card
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), 64, 1)) != SD_OK) {
+ return ErrorState;
+ }
+
+ // Send CMD55
+ if((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) != SD_OK) {
+ return ErrorState;
+ }
+
+ // Configure the SD DPSM (Data Path State Machine)
+ SD_DataTransferInit(64, SD_DATABLOCK_SIZE_64B, true);
+
+ // Send ACMD13 (SD_APP_STAUS) with argument as card's RCA
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_STATUS | SD_CMD_RESPONSE_SHORT), 0, 1)) != SD_OK) {
+ return ErrorState;
+ }
+
+ // Get status data
+ while((SDIO_STAT & (SDIO_STAT_RXORE | SDIO_STAT_DTCRCERR | SDIO_STAT_DTTMOUT | SDIO_STAT_DTBLKEND)) == 0) {
+ if((SDIO_STAT & SDIO_STAT_RFH) != 0) {
+ for(Count = 0; Count < 8; Count++) {
+ Status[Count] = SDIO_FIFO;
+ }
+ }
+ }
+
+ if((SDIO_STAT & SDIO_STAT_DTTMOUT) != 0) return SD_DATA_TIMEOUT;
+ else if((SDIO_STAT & SDIO_STAT_DTCRCERR) != 0) return SD_DATA_CRC_FAIL;
+ else if((SDIO_STAT & SDIO_STAT_RXORE) != 0) return SD_RX_OVERRUN;
+ else
+ {
+ }
+
+ // Byte 0
+ Temp = (Status[0] & 0xC0) >> 6;
+ pCardStatus->DAT_BUS_WIDTH = (uint8_t)Temp;
+
+ // Byte 0
+ Temp = (Status[0] & 0x20) >> 5;
+ pCardStatus->SECURED_MODE = (uint8_t)Temp;
+
+ // Byte 2
+ Temp = (Status[2] & 0xFF);
+ pCardStatus->SD_CARD_TYPE = (uint8_t)(Temp << 8);
+
+ // Byte 3
+ Temp = (Status[3] & 0xFF);
+ pCardStatus->SD_CARD_TYPE |= (uint8_t)Temp;
+
+ // Byte 4
+ Temp = (Status[4] & 0xFF);
+ pCardStatus->SIZE_OF_PROTECTED_AREA = (uint8_t)(Temp << 24);
+
+ // Byte 5
+ Temp = (Status[5] & 0xFF);
+ pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)(Temp << 16);
+
+ // Byte 6
+ Temp = (Status[6] & 0xFF);
+ pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)(Temp << 8);
+
+ // Byte 7
+ Temp = (Status[7] & 0xFF);
+ pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)Temp;
+
+ // Byte 8
+ Temp = (Status[8] & 0xFF);
+ pCardStatus->SPEED_CLASS = (uint8_t)Temp;
+
+ // Byte 9
+ Temp = (Status[9] & 0xFF);
+ pCardStatus->PERFORMANCE_MOVE = (uint8_t)Temp;
+
+ // Byte 10
+ Temp = (Status[10] & 0xF0) >> 4;
+ pCardStatus->AU_SIZE = (uint8_t)Temp;
+
+ // Byte 11
+ Temp = (Status[11] & 0xFF);
+ pCardStatus->ERASE_SIZE = (uint8_t)(Temp << 8);
+
+ // Byte 12
+ Temp = (Status[12] & 0xFF);
+ pCardStatus->ERASE_SIZE |= (uint8_t)Temp;
+
+ // Byte 13
+ Temp = (Status[13] & 0xFC) >> 2;
+ pCardStatus->ERASE_TIMEOUT = (uint8_t)Temp;
+
+ // Byte 13
+ Temp = (Status[13] & 0x3);
+ pCardStatus->ERASE_OFFSET = (uint8_t)Temp;
+
+ return SD_OK;
+}
+
+static SD_Error_t SD_PowerON(void)
+{
+ SD_Error_t ErrorState;
+ uint32_t Response = 0;
+ uint32_t Count;
+ uint32_t ValidVoltage;
+ uint32_t SD_Type;
+
+ Count = 0;
+ ValidVoltage = 0;
+ SD_Type = SD_RESP_STD_CAPACITY;
+
+ SDIO_CLKCTL &= ~SDIO_CLKCTL_CLKEN; // Disable SDIO Clock
+ SDIO_PWRCTL = SDIO_PWRCTL_PWRCTL; // Set Power State to ON
+
+ delay(2);
+
+ SDIO_CLKCTL |= SDIO_CLKCTL_CLKEN; // Enable SDIO Clock
+
+ // CMD0: GO_IDLE_STATE -----------------------------------------------------
+ // No CMD response required
+ if((ErrorState = SD_TransmitCommand(SD_CMD_GO_IDLE_STATE, 0, 0)) != SD_OK) {
+ // CMD Response Timeout (wait for CMDSENT flag)
+ return ErrorState;
+ }
+
+ // CMD8: SEND_IF_COND ------------------------------------------------------
+ // Send CMD8 to verify SD card interface operating condition
+ // Argument: - [31:12]: Reserved (shall be set to '0')
+ //- [11:8]: Supply Voltage (VHS) 0x1 (Range: 2.7-3.6 V)
+ //- [7:0]: Check Pattern (recommended 0xAA)
+ // CMD Response: R7 */
+ if((ErrorState = SD_TransmitCommand((SD_SDIO_SEND_IF_COND | SD_CMD_RESPONSE_SHORT), SD_CHECK_PATTERN, 7)) == SD_OK) {
+ // SD Card 2.0
+ SD_CardType = SD_STD_CAPACITY_V2_0;
+ SD_Type = SD_RESP_HIGH_CAPACITY;
+ }
+
+ // Send CMD55
+ // If ErrorState is Command Timeout, it is a MMC card
+ // If ErrorState is SD_OK it is a SD card: SD card 2.0 (voltage range mismatch) or SD card 1.x
+ if((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1)) == SD_OK) {
+ // SD CARD
+ // Send ACMD41 SD_APP_OP_COND with Argument 0x80100000
+ while((ValidVoltage == 0) && (Count < SD_MAX_VOLT_TRIAL)) {
+ // SEND CMD55 APP_CMD with RCA as 0
+ if((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1)) != SD_OK) {
+ return ErrorState;
+ }
+
+ // Send CMD41
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_OP_COND | SD_CMD_RESPONSE_SHORT), SD_VOLTAGE_WINDOW_SD | SD_Type, 3)) != SD_OK) {
+ return ErrorState;
+ }
+
+ Response = SDIO_RESP0; // Get command response
+ ValidVoltage = (((Response >> 31) == 1) ? 1 : 0); // Get operating voltage
+ Count++;
+ }
+
+ if(Count >= SD_MAX_VOLT_TRIAL) {
+ return SD_INVALID_VOLTRANGE;
+ }
+
+ if((Response & SD_RESP_HIGH_CAPACITY) == SD_RESP_HIGH_CAPACITY) {
+ SD_CardType = SD_HIGH_CAPACITY;
+ }
+ } // else MMC Card
+
+ return ErrorState;
+}
+
+static SD_Error_t SD_FindSCR(uint32_t *pSCR)
+{
+ SD_Error_t ErrorState;
+ uint32_t Index = 0;
+ uint32_t tempscr[2] = {0, 0};
+
+ // Set Block Size To 8 Bytes
+ // Send CMD55 APP_CMD with argument as card's RCA
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), 8, 1)) == SD_OK) {
+ // Send CMD55 APP_CMD with argument as card's RCA
+ if((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) == SD_OK) {
+ SD_DataTransferInit(8, SD_DATABLOCK_SIZE_8B, true);
+
+ // Send ACMD51 SD_APP_SEND_SCR with argument as 0
+ if((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_SEND_SCR | SD_CMD_RESPONSE_SHORT), 0, 1)) == SD_OK) {
+ while((SDIO_STAT & (SDIO_STAT_RXORE | SDIO_STAT_DTCRCERR | SDIO_STAT_DTTMOUT | SDIO_STAT_DTBLKEND)) == 0) {
+ if((SDIO_STAT & SDIO_STAT_RXDTVAL) != 0) {
+ *(tempscr + Index) = SDIO_FIFO;
+ Index++;
+ }
+ }
+
+ if ((SDIO_STAT & SDIO_STAT_DTTMOUT) != 0) ErrorState = SD_DATA_TIMEOUT;
+ else if((SDIO_STAT & SDIO_STAT_DTCRCERR) != 0) ErrorState = SD_DATA_CRC_FAIL;
+ else if((SDIO_STAT & SDIO_STAT_RXORE) != 0) ErrorState = SD_RX_OVERRUN;
+ else if((SDIO_STAT & SDIO_STAT_RXDTVAL) != 0) ErrorState = SD_OUT_OF_BOUND;
+ else
+ {
+ *(pSCR + 1) = ((tempscr[0] & SD_0TO7BITS) << 24) | ((tempscr[0] & SD_8TO15BITS) << 8) |
+ ((tempscr[0] & SD_16TO23BITS) >> 8) | ((tempscr[0] & SD_24TO31BITS) >> 24);
+
+ *(pSCR) = ((tempscr[1] & SD_0TO7BITS) << 24) | ((tempscr[1] & SD_8TO15BITS) << 8) |
+ ((tempscr[1] & SD_16TO23BITS) >> 8) | ((tempscr[1] & SD_24TO31BITS) >> 24);
+ }
+
+ }
+ }
+ }
+
+ return ErrorState;
+}
+
+/*!
+ \brief Initialize the SDIO module, DMA, and IO
+ \param[in] dma: DMA stream pointer
+ \param[out] none
+ \retval initialization success status
+*/
+bool SD_Initialize_LL(DMA_Stream_TypeDef *dma)
+{
+ const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier((dmaResource_t *)dma);
+
+ dmaStream = dma;
+
+ if (!((uint32_t)dma == DMA1_CH3_BASE || (uint32_t)dma == DMA1_CH6_BASE) || !dmaAllocate(dmaIdentifier, OWNER_SDCARD, 0)) {
+ return false;
+ }
+
+ // Reset SDIO Module
+ RCU_APB2RST |= RCU_APB2RST_SDIORST;
+ delay(1);
+ RCU_APB2RST &= ~RCU_APB2RST_SDIORST;
+ delay(1);
+
+ // Enable SDIO clock
+ RCU_APB2EN |= RCU_APB2EN_SDIOEN;
+
+ // Enable DMA2 clocks
+ RCU_AHB1EN |= RCU_AHB1EN_DMA1EN;
+
+ //Configure Pins
+ RCU_AHB1EN |= RCU_AHB1EN_PCEN | RCU_AHB1EN_PDEN;
+
+ uint8_t is4BitWidth = sdioConfig()->use4BitWidth;
+
+ const IO_t d0 = IOGetByTag(IO_TAG(PC8));
+ const IO_t d1 = IOGetByTag(IO_TAG(PC9));
+ const IO_t d2 = IOGetByTag(IO_TAG(PC10));
+ const IO_t d3 = IOGetByTag(IO_TAG(PC11));
+ const IO_t clk = IOGetByTag(IO_TAG(PC12));
+ const IO_t cmd = IOGetByTag(IO_TAG(PD2));
+
+ IOInit(d0, OWNER_SDCARD, 0);
+ if (is4BitWidth) {
+ IOInit(d1, OWNER_SDCARD, 0);
+ IOInit(d2, OWNER_SDCARD, 0);
+ IOInit(d3, OWNER_SDCARD, 0);
+ }
+ IOInit(clk, OWNER_SDCARD, 0);
+ IOInit(cmd, OWNER_SDCARD, 0);
+
+#define SDIO_DATA IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_25MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP)
+#define SDIO_CMD IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_25MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP)
+#define SDIO_CLK IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_25MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE)
+
+ IOConfigGPIOAF(d0, SDIO_DATA, GPIO_AF_12);
+
+ if (is4BitWidth) {
+ IOConfigGPIOAF(d1, SDIO_DATA, GPIO_AF_12);
+ IOConfigGPIOAF(d2, SDIO_DATA, GPIO_AF_12);
+ IOConfigGPIOAF(d3, SDIO_DATA, GPIO_AF_12);
+ }
+
+ IOConfigGPIOAF(clk, SDIO_CLK, GPIO_AF_12);
+ IOConfigGPIOAF(cmd, SDIO_CMD, GPIO_AF_12);
+
+ // NVIC configuration for SDIO interrupts
+ nvic_irq_enable(SDIO_IRQn, 1, 0);
+
+ gd32_dma_chbase_parse((uint32_t)dma, &dma_periph_sdio, &dma_channel_sdio);
+
+ RCU_AHB1EN |= RCU_AHB1EN_DMA1EN;
+ // Initialize DMA
+ DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) = 0; // Reset DMA Stream control register
+ DMA_CHPADDR(dma_periph_sdio,dma_channel_sdio) = (uint32_t)&SDIO_FIFO;
+
+ if ((uint32_t)dmaStream == DMA1_CH3_BASE) {
+ DMA_INTC0(dma_periph_sdio) = INTC_CLEAR_MASK_CH3; // Clear all interrupt flags
+ } else {
+ DMA_INTC1(dma_periph_sdio) = INTC_CLEAR_MASK_CH6; // Clear all interrupt flags
+ }
+
+ DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) = (DMA_CHANNEL_4 | DMA_CHXCTL_TFCS | // Prepare the DMA Stream configuration
+ DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register
+ DMA_MDATAALIGN_WORD | DMA_PRIORITY_ULTRA_HIGH |
+ DMA_MBURST_INC4 | DMA_PBURST_INC4 |
+ DMA_MEMORY_TO_PERIPH);
+ DMA_CHFCTL(dma_periph_sdio,dma_channel_sdio) = (DMA_CHXFCTL_MDMEN | DMA_FIFO_4_WORD); // Configuration FIFO control register
+ dmaEnable(dmaIdentifier);
+
+ if ((uint32_t)dmaStream == DMA1_CH3_BASE) {
+ dmaSetHandler(dmaIdentifier, SDIO_DMA_ST3_IRQHandler, 1, 0);
+ } else {
+ dmaSetHandler(dmaIdentifier, SDIO_DMA_ST6_IRQHandler, 1, 0);
+ }
+
+ return true;
+}
+
+bool SD_GetState(void)
+{
+ // Check SDCARD status
+ if(SD_GetStatus() == SD_OK) return true;
+ return false;
+}
+
+static SD_Error_t SD_DoInit(void)
+{
+ SD_Error_t errorState;
+ uint32_t reg;
+
+ // Initialize SDIO peripheral interface with default configuration for SD card initialization.
+ reg = SDIO_CLKCTL;
+ reg &= ~CLKCTL_CLEAR_MASK;
+ reg |= (uint32_t)SDIO_INIT_CLK_DIV;
+ SDIO_CLKCTL = reg;
+
+ // Identify card operating voltage.
+ errorState = SD_PowerON();
+ if (errorState != SD_OK) {
+ return errorState;
+ }
+
+ // Initialize the present card and put them in idle state.
+ errorState = SD_InitializeCard();
+ if (errorState != SD_OK) {
+ return errorState;
+ }
+
+ // Read CSD/CID MSD registers.
+ errorState = SD_GetCardInfo();
+ if (errorState != SD_OK) {
+ return errorState;
+ }
+
+ // Select the Card - Send CMD7 SDIO_SEL_DESEL_CARD.
+ errorState = SD_TransmitCommand((SD_CMD_SEL_DESEL_CARD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1);
+ // Configure SDIO peripheral interface.
+ reg = SDIO_CLKCTL;
+ reg &= ~CLKCTL_CLEAR_MASK;
+ reg |= (uint32_t) SDIO_CLK_DIV;
+ SDIO_CLKCTL = reg;
+
+ // Configure SD Bus width.
+ if (errorState == SD_OK) {
+ // Enable wide operation.
+ if (sdioConfig()->use4BitWidth) {
+ errorState = SD_WideBusOperationConfig(SD_BUS_WIDE_4B);
+ } else {
+ errorState = SD_WideBusOperationConfig(SD_BUS_WIDE_1B);
+ }
+
+ if (errorState == SD_OK && sdioConfig()->clockBypass) {
+ if (SD_HighSpeed()) {
+ SDIO_CLKCTL |= SDIO_CLKCTL_CLKBYP;
+ SDIO_CLKCTL |= SDIO_CLKCTL_CLKEDGE;
+ }
+ }
+ }
+
+ return errorState;
+}
+
+SD_Error_t SD_Init(void)
+{
+ static bool sdInitAttempted = false;
+ static SD_Error_t result = SD_ERROR;
+
+ if (sdInitAttempted) {
+ return result;
+ }
+
+ sdInitAttempted = true;
+
+ result = SD_DoInit();
+
+ return result;
+}
+
+/*!
+ \brief This function handles SD card interrupt request
+ \param[in] none
+ \param[out] none
+ \retval none
+*/
+void SDIO_IRQHandler(void)
+{
+ // Check for SDIO interrupt flags
+ if ((SDIO_STAT & SDIO_STAT_DTEND) != 0) {
+ SDIO_INTC = SDIO_INTC_DTENDC;
+ SDIO_INTC = SDIO_INTC_STATIC_FLAGS;
+ SDIO_INTEN &= ~(SDIO_INTEN_DTENDIE | SDIO_INTEN_DTCRCERRIE | SDIO_INTEN_DTTMOUTIE |
+ SDIO_INTEN_TXUREIE | SDIO_INTEN_RXOREIE | SDIO_INTEN_TFHIE | SDIO_INTEN_RFHIE);
+
+ /* Currently doesn't implement multiple block write handling */
+ if ((SD_Handle.Operation & 0x02) == (SDIO_DIR_TX << 1)) {
+ /* Disable the stream */
+ DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) &= ~DMA_CHXCTL_CHEN;
+ SDIO_DATACTL &= ~(SDIO_DATACTL_DMAEN);
+ /* Transfer is complete */
+ SD_Handle.TXCplt = 0;
+ if ((SD_Handle.Operation & 0x01) == SD_MULTIPLE_BLOCK) {
+ /* Send stop command in multiblock write */
+ SD_TransmitCommand((SD_CMD_STOP_TRANSMISSION | SD_CMD_RESPONSE_SHORT), 0, 1);
+ }
+ }
+
+ SD_Handle.TransferComplete = 1;
+ SD_Handle.TransferError = SD_OK;
+ } else if ((SDIO_STAT & SDIO_STAT_DTCRCERR) != 0)
+ SD_Handle.TransferError = SD_DATA_CRC_FAIL;
+ else if ((SDIO_STAT & SDIO_STAT_DTTMOUT) != 0)
+ SD_Handle.TransferError = SD_DATA_TIMEOUT;
+ else if ((SDIO_STAT & SDIO_STAT_RXORE) != 0)
+ SD_Handle.TransferError = SD_RX_OVERRUN;
+ else if ((SDIO_STAT & SDIO_STAT_TXURE) != 0)
+ SD_Handle.TransferError = SD_TX_UNDERRUN;
+
+ SDIO_INTC = SDIO_INTC_STATIC_FLAGS;
+
+ // Disable all SDIO peripheral interrupt sources
+ SDIO_INTEN &= ~(SDIO_INTEN_DTCRCERRIE | SDIO_INTEN_DTTMOUTIE |
+ SDIO_INTEN_DTENDIE |
+ SDIO_INTEN_TFHIE | SDIO_INTEN_RFHIE | SDIO_INTEN_TXUREIE |
+ SDIO_INTEN_RXOREIE);
+}
+
+/*!
+ \brief Common handler for DMA stream interrupt requests
+ \param[in] dma_periph: DMA peripheral (DMA0 or DMA1)
+ \param[in] dma_channel: DMA channel (DMA_CH0 ~ DMA_CH7)
+ \param[out] none
+ \retval none
+*/
+static void SDIO_DMA_IRQHandler_Common(uint32_t dma_periph, dma_channel_enum dma_channel)
+{
+ // Transfer Error Interrupt management
+ if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_TAE)) {
+ if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXCTL_TAEIE)) {
+ dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXCTL_TAEIE);
+ dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_TAE);
+ }
+ }
+
+ // FIFO Error Interrupt management
+ if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_FEE)) {
+ if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXFCTL_FEEIE)) {
+ dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXFCTL_FEEIE);
+ dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_FEE);
+ }
+ }
+
+ // Single data mode exception flag
+ if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_SDE)) {
+ if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXCTL_SDEIE)) {
+ dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXCTL_SDEIE);
+ dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_SDE);
+ }
+ }
+
+ // Half Transfer Complete Interrupt management
+ if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_HTF)) {
+ if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXCTL_HTFIE)) {
+ if(dma_single_data_mode_get(dma_periph, dma_channel) != RESET) {
+ dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_HTF);
+ } else {
+ if(dma_circulation_get(dma_periph, dma_channel) == RESET) {
+ dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXCTL_HTFIE);
+ }
+
+ dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_HTF);
+ }
+ }
+ }
+
+ // Transfer Complete Interrupt management
+ if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_FTF)) {
+ if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXCTL_FTFIE)) {
+ if(dma_single_data_mode_get(dma_periph, dma_channel) != RESET) {
+ dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_FTF);
+ } else {
+ if(dma_circulation_get(dma_periph, dma_channel) == RESET) {
+ dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXCTL_FTFIE);
+ }
+
+ dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_FTF);
+ SD_DMA_Complete(dma_periph, dma_channel);
+ }
+ }
+ }
+}
+
+/*!
+ \brief This function handles DMA2 Stream 3 interrupt request
+ \param[in] dma: DMA channel descriptor
+ \param[out] none
+ \retval none
+*/
+void SDIO_DMA_ST3_IRQHandler(dmaChannelDescriptor_t *dma)
+{
+ UNUSED(dma);
+ SDIO_DMA_IRQHandler_Common(DMA1, DMA_CH3);
+}
+
+/*!
+ \brief This function handles DMA2 Stream 6 interrupt request
+ \param[in] dma: DMA channel descriptor
+ \param[out] none
+ \retval none
+*/
+void SDIO_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma)
+{
+ UNUSED(dma);
+ SDIO_DMA_IRQHandler_Common(DMA1, DMA_CH6);
+}
+
+#endif
diff --git a/src/platform/GD32/serial_uart_gd32f4xx.c b/src/platform/GD32/serial_uart_gd32f4xx.c
new file mode 100755
index 0000000000..b3c0296a07
--- /dev/null
+++ b/src/platform/GD32/serial_uart_gd32f4xx.c
@@ -0,0 +1,311 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ * jflyper - Refactoring, cleanup and made pin-configurable
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_UART
+
+#include "build/debug.h"
+
+#include "drivers/system.h"
+#include "drivers/io.h"
+#include "drivers/dma.h"
+#include "drivers/nvic.h"
+#include "platform/rcc.h"
+
+#include "drivers/serial.h"
+#include "drivers/serial_uart.h"
+#include "drivers/serial_uart_impl.h"
+
+const uartHardware_t uartHardware[UARTDEV_COUNT] = {
+#ifdef USE_UART0
+ {
+ .identifier = SERIAL_PORT_UART0,
+ .reg = (USART_TypeDef *)USART0,
+ .rxDMAChannel = DMA_SUBPERI4,
+ .txDMAChannel = DMA_SUBPERI4,
+#ifdef USE_UART0_RX_DMA
+ .rxDMAResource = &(dmaResource_t){DMA1, DMA_CH5},
+#endif
+#ifdef USE_UART0_TX_DMA
+ .txDMAResource = &(dmaResource_t){DMA1, DMA_CH7},
+#endif
+ .rxPins = { { DEFIO_TAG_E(PA10) }, { DEFIO_TAG_E(PB3) }, { DEFIO_TAG_E(PB7) }, },
+ .txPins = { { DEFIO_TAG_E(PA9) }, { DEFIO_TAG_E(PA15) }, { DEFIO_TAG_E(PB6) }, },
+ .af = GPIO_AF_7,
+ .rcc = RCC_APB2(USART0),
+ .irqn = USART0_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART0_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART0,
+ .txBuffer = uart0TxBuffer,
+ .rxBuffer = uart0RxBuffer,
+ .txBufferSize = sizeof(uart0TxBuffer),
+ .rxBufferSize = sizeof(uart0RxBuffer),
+ },
+#endif
+
+#ifdef USE_UART1
+ {
+ .identifier = SERIAL_PORT_USART1,
+ .reg = (USART_TypeDef *)USART1,
+ .rxDMAChannel = DMA_SUBPERI4,
+ .txDMAChannel = DMA_SUBPERI4,
+#ifdef USE_UART1_RX_DMA
+ .rxDMAResource = &(dmaResource_t){DMA0, DMA_CH5},
+#endif
+#ifdef USE_UART1_TX_DMA
+ .txDMAResource = &(dmaResource_t){DMA0, DMA_CH6},
+#endif
+ .rxPins = { { DEFIO_TAG_E(PA3) }, { DEFIO_TAG_E(PD6) } },
+ .txPins = { { DEFIO_TAG_E(PA2) }, { DEFIO_TAG_E(PD5) } },
+ .af = GPIO_AF_7,
+ .rcc = RCC_APB1(USART1),
+ .irqn = USART1_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART1_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART1,
+ .txBuffer = uart1TxBuffer,
+ .rxBuffer = uart1RxBuffer,
+ .txBufferSize = sizeof(uart1TxBuffer),
+ .rxBufferSize = sizeof(uart1RxBuffer),
+ },
+#endif
+
+#ifdef USE_UART2
+ {
+ .identifier = SERIAL_PORT_USART2,
+ .reg = (USART_TypeDef *)USART2,
+ .rxDMAChannel = DMA_SUBPERI4,
+ .txDMAChannel = DMA_SUBPERI4,
+#ifdef USE_UART2_RX_DMA
+ .rxDMAResource = &(dmaResource_t){DMA0, DMA_CH1},
+#endif
+#ifdef USE_UART2_TX_DMA
+ .txDMAResource = &(dmaResource_t){DMA0, DMA_CH3},
+#endif
+ .rxPins = { { DEFIO_TAG_E(PB11) }, { DEFIO_TAG_E(PC11) }, { DEFIO_TAG_E(PD9) } },
+ .txPins = { { DEFIO_TAG_E(PB10) }, { DEFIO_TAG_E(PC10) }, { DEFIO_TAG_E(PD8) } },
+ .af = GPIO_AF_7,
+ .rcc = RCC_APB1(USART2),
+ .irqn = USART2_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART2_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART2,
+ .txBuffer = uart2TxBuffer,
+ .rxBuffer = uart2RxBuffer,
+ .txBufferSize = sizeof(uart2TxBuffer),
+ .rxBufferSize = sizeof(uart2RxBuffer),
+ },
+#endif
+
+#ifdef USE_UART3
+ {
+ .identifier = SERIAL_PORT_UART3,
+ .reg = (USART_TypeDef *)UART3,
+ .rxDMAChannel = DMA_SUBPERI4,
+ .txDMAChannel = DMA_SUBPERI4,
+#ifdef USE_UART3_RX_DMA
+ .rxDMAResource = &(dmaResource_t){DMA0, DMA_CH2},
+#endif
+#ifdef USE_UART3_TX_DMA
+ .txDMAResource = &(dmaResource_t){DMA0, DMA_CH4},
+#endif
+ .rxPins = { { DEFIO_TAG_E(PA1) }, { DEFIO_TAG_E(PC11) } },
+ .txPins = { { DEFIO_TAG_E(PA0) }, { DEFIO_TAG_E(PC10) } },
+ .af = GPIO_AF_8,
+ .rcc = RCC_APB1(UART3),
+ .irqn = UART3_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART3_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART3,
+ .txBuffer = uart3TxBuffer,
+ .rxBuffer = uart3RxBuffer,
+ .txBufferSize = sizeof(uart3TxBuffer),
+ .rxBufferSize = sizeof(uart3RxBuffer),
+ },
+#endif
+
+#ifdef USE_UART4
+ {
+ .identifier = SERIAL_PORT_UART4,
+ .reg = (USART_TypeDef *)UART4,
+ .rxDMAChannel = DMA_SUBPERI4,
+ .txDMAChannel = DMA_SUBPERI4,
+#ifdef USE_UART4_RX_DMA
+ .rxDMAResource = &(dmaResource_t){DMA0, DMA_CH0},
+#endif
+#ifdef USE_UART4_TX_DMA
+ .txDMAResource = &(dmaResource_t){DMA0, DMA_CH7},
+#endif
+ .rxPins = { { DEFIO_TAG_E(PD2) } },
+ .txPins = { { DEFIO_TAG_E(PC12) } },
+ .af = GPIO_AF_8,
+ .rcc = RCC_APB1(UART4),
+ .irqn = UART4_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART4_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART4,
+ .txBuffer = uart4TxBuffer,
+ .rxBuffer = uart4RxBuffer,
+ .txBufferSize = sizeof(uart4TxBuffer),
+ .rxBufferSize = sizeof(uart4RxBuffer),
+ },
+#endif
+
+#ifdef USE_UART5
+ {
+ .identifier = SERIAL_PORT_UART5,
+ .reg = (USART_TypeDef *)USART5,
+ .rxDMAChannel = DMA_SUBPERI5,
+ .txDMAChannel = DMA_SUBPERI5,
+#ifdef USE_UART5_RX_DMA
+ .rxDMAResource = &(dmaResource_t){DMA1, DMA_CH1},
+#endif
+#ifdef USE_UART5_TX_DMA
+ .txDMAResource = &(dmaResource_t){DMA1, DMA_CH6},
+#endif
+ .rxPins = { { DEFIO_TAG_E(PC7) }, { DEFIO_TAG_E(PG9) } },
+ .txPins = { { DEFIO_TAG_E(PC6) }, { DEFIO_TAG_E(PG14) } },
+ .af = GPIO_AF_8,
+ .rcc = RCC_APB2(USART5),
+ .irqn = USART5_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART5_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART5,
+ .txBuffer = uart5TxBuffer,
+ .rxBuffer = uart5RxBuffer,
+ .txBufferSize = sizeof(uart5TxBuffer),
+ .rxBufferSize = sizeof(uart5RxBuffer),
+ },
+#endif
+
+};
+
+bool checkUsartTxOutput(uartPort_t *s)
+{
+ uartDevice_t *uart = container_of(s, uartDevice_t, port);
+ IO_t txIO = IOGetByTag(uart->tx.pin);
+
+ if ((uart->txPinState == TX_PIN_MONITOR) && txIO) {
+ if (IORead(txIO)) {
+ uart->txPinState = TX_PIN_ACTIVE;
+ IOConfigGPIOAF(txIO, IOCFG_AF_PP, uart->hardware->af);
+ usart_transmit_config((uint32_t)s->USARTx, USART_TRANSMIT_ENABLE);
+
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+void uartTxMonitor(uartPort_t *s)
+{
+ uartDevice_t *uart = container_of(s, uartDevice_t, port);
+
+ if (uart->txPinState == TX_PIN_ACTIVE) {
+ IO_t txIO = IOGetByTag(uart->tx.pin);
+
+ usart_transmit_config((uint32_t)s->USARTx, USART_TRANSMIT_DISABLE);
+ uart->txPinState = TX_PIN_MONITOR;
+ IOConfigGPIO(txIO, IOCFG_IPU);
+ }
+}
+
+static void handleUsartTxDma(uartPort_t *s)
+{
+ uartDevice_t *uart = container_of(s, uartDevice_t, port);
+
+ uartTryStartTxDMA(s);
+
+ if (s->txDMAEmpty && (uart->txPinState != TX_PIN_IGNORE)) {
+ uartTxMonitor(s);
+ }
+}
+
+void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor)
+{
+ uartPort_t *s = &(((uartDevice_t*)(descriptor->userParam))->port);
+
+ if (dma_interrupt_flag_get((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_FTF)) {
+ dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_FTF);
+ dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_HTF);
+
+ if (dma_interrupt_flag_get((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_FEE)) {
+ dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_FEE);
+ }
+
+ handleUsartTxDma(s);
+ }
+
+ if (dma_interrupt_flag_get((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_TAE)) {
+ dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_TAE);
+ }
+
+ if (dma_interrupt_flag_get((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_SDE)) {
+ dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_SDE);
+ }
+}
+
+void uartIrqHandler(uartPort_t *s)
+{
+ if (!s->rxDMAResource && (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_RBNE) == SET)) {
+ if (s->port.rxCallback) {
+ s->port.rxCallback(usart_data_receive((uint32_t)s->USARTx), s->port.rxCallbackData);
+ } else {
+ s->port.rxBuffer[s->port.rxBufferHead] = (uint8_t)usart_data_receive((uint32_t)s->USARTx);
+ s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
+ }
+ }
+
+ if (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_TC) == SET) {
+ uartTxMonitor(s);
+ usart_interrupt_flag_clear((uint32_t)s->USARTx, USART_INT_FLAG_TC);
+ }
+
+ if (!s->txDMAResource && (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_TBE) == SET)) {
+ if (s->port.txBufferTail != s->port.txBufferHead) {
+ usart_data_transmit((uint32_t)s->USARTx, s->port.txBuffer[s->port.txBufferTail]);
+ s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
+ } else {
+ usart_interrupt_disable((uint32_t)s->USARTx, USART_INT_TBE);
+ }
+ }
+
+ if (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_ERR_ORERR) == SET) {
+ usart_interrupt_flag_clear((uint32_t)s->USARTx, USART_INT_FLAG_ERR_ORERR);
+ }
+
+ if (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_IDLE) == SET) {
+ if (s->port.idleCallback) {
+ s->port.idleCallback();
+ }
+
+ USART_STAT0((uint32_t)s->USARTx);
+ USART_DATA((uint32_t)s->USARTx);
+ }
+
+}
+#endif // USE_UART
diff --git a/src/platform/GD32/serial_uart_stdperiph.c b/src/platform/GD32/serial_uart_stdperiph.c
new file mode 100755
index 0000000000..24a31f0f7b
--- /dev/null
+++ b/src/platform/GD32/serial_uart_stdperiph.c
@@ -0,0 +1,184 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_UART
+
+#include "build/build_config.h"
+#include "build/atomic.h"
+
+#include "common/utils.h"
+#include "drivers/inverter.h"
+#include "drivers/nvic.h"
+#include "platform/rcc.h"
+
+#include "drivers/serial.h"
+#include "drivers/serial_uart.h"
+#include "drivers/serial_uart_impl.h"
+
+void uartReconfigure(uartPort_t *uartPort)
+{
+ usart_deinit((uint32_t)uartPort->USARTx);
+ usart_baudrate_set((uint32_t)uartPort->USARTx, uartPort->port.baudRate);
+
+ if(uartPort->port.options & SERIAL_PARITY_EVEN) {
+ usart_word_length_set((uint32_t)uartPort->USARTx, USART_WL_9BIT);
+ } else {
+ usart_word_length_set((uint32_t)uartPort->USARTx, USART_WL_8BIT);
+ }
+
+ if(uartPort->port.options & SERIAL_STOPBITS_2) {
+ usart_stop_bit_set((uint32_t)uartPort->USARTx, USART_STB_2BIT);
+ } else {
+ usart_stop_bit_set((uint32_t)uartPort->USARTx, USART_STB_1BIT);
+ }
+
+ if(uartPort->port.options & SERIAL_PARITY_EVEN) {
+ usart_parity_config((uint32_t)uartPort->USARTx, USART_PM_EVEN);
+ } else {
+ usart_parity_config((uint32_t)uartPort->USARTx, USART_PM_NONE);
+ }
+
+ usart_hardware_flow_rts_config((uint32_t)uartPort->USARTx, USART_RTS_DISABLE);
+ usart_hardware_flow_cts_config((uint32_t)uartPort->USARTx, USART_CTS_DISABLE);
+
+ if (uartPort->port.mode & MODE_RX)
+ usart_receive_config((uint32_t)uartPort->USARTx, USART_RECEIVE_ENABLE);
+ if (uartPort->port.mode & MODE_TX)
+ usart_transmit_config((uint32_t)uartPort->USARTx, USART_TRANSMIT_ENABLE);
+
+ uartConfigureExternalPinInversion(uartPort);
+
+ if (uartPort->port.options & SERIAL_BIDIR) {
+ usart_halfduplex_enable((uint32_t)uartPort->USARTx);
+ } else {
+ usart_halfduplex_disable((uint32_t)uartPort->USARTx);
+ }
+
+ if (uartPort->port.mode & MODE_RX) {
+#ifdef USE_DMA
+ if (uartPort->rxDMAResource) {
+ dma_single_data_parameter_struct dma_init_struct;
+ dma_single_data_para_struct_init(&dma_init_struct);
+
+ dma_init_struct.periph_addr = uartPort->rxDMAPeripheralBaseAddr;
+ dma_init_struct.memory0_addr = (uint32_t)uartPort->port.rxBuffer;
+ dma_init_struct.number = uartPort->port.rxBufferSize;
+ dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+ dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_8BIT;
+ dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_ENABLE;
+ dma_init_struct.direction = DMA_PERIPH_TO_MEMORY;
+ dma_init_struct.priority = DMA_PRIORITY_MEDIUM;
+
+ xDMA_DeInit(uartPort->rxDMAResource);
+ gd32_dma_init((uint32_t)uartPort->rxDMAResource, &dma_init_struct);
+ xDMA_Cmd(uartPort->rxDMAResource, ENABLE);
+
+ usart_dma_receive_config((uint32_t)uartPort->USARTx, USART_RECEIVE_DMA_ENABLE);
+
+ uartPort->rxDMAPos = xDMA_GetCurrDataCounter(uartPort->rxDMAResource);
+ } else
+#endif
+ {
+ usart_interrupt_flag_clear((uint32_t)uartPort->USARTx, USART_INT_FLAG_RBNE);
+ usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_RBNE);
+ usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_IDLE);
+ }
+ }
+
+ if (uartPort->port.mode & MODE_TX) {
+#ifdef USE_DMA
+ if (uartPort->txDMAResource) {
+ dma_single_data_parameter_struct dma_init_struct;
+ dma_single_data_para_struct_init(&dma_init_struct);
+
+ dma_init_struct.periph_addr = uartPort->txDMAPeripheralBaseAddr;
+ dma_init_struct.memory0_addr = (uint32_t)uartPort->port.txBuffer;
+ dma_init_struct.number = uartPort->port.txBufferSize;
+ dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+ dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+ dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_8BIT;
+ dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_DISABLE;
+ dma_init_struct.direction = DMA_MEMORY_TO_PERIPH;
+ dma_init_struct.priority = DMA_PRIORITY_MEDIUM;
+
+
+ xDMA_DeInit(uartPort->txDMAResource);
+ gd32_dma_init((uint32_t)uartPort->txDMAResource, &dma_init_struct);
+
+ xDMA_ITConfig(uartPort->txDMAResource, DMA_INT_FTF | DMA_INT_FEE | DMA_INT_TAE | DMA_INT_SDE, ENABLE);
+ xDMA_SetCurrDataCounter(uartPort->txDMAResource, 0);
+
+ usart_dma_transmit_config((uint32_t)uartPort->USARTx, USART_TRANSMIT_DMA_ENABLE);
+ } else
+#endif
+ {
+ usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_TBE);
+ }
+
+ usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_TC);
+ }
+
+ usart_enable((uint32_t)uartPort->USARTx);
+}
+
+#ifdef USE_DMA
+void uartTryStartTxDMA(uartPort_t *s)
+{
+
+ ATOMIC_BLOCK(NVIC_PRIO_SERIALUART_TXDMA) {
+ if (IS_DMA_ENABLED(s->txDMAResource)) {
+ return;
+ }
+
+ if (xDMA_GetCurrDataCounter(s->txDMAResource)) {
+ goto reenable;
+ }
+
+ if (s->port.txBufferHead == s->port.txBufferTail) {
+ s->txDMAEmpty = true;
+ return;
+ }
+
+ xDMA_MemoryTargetConfig(s->txDMAResource, (uint32_t)&s->port.txBuffer[s->port.txBufferTail], DMA_MEMORY_0);
+
+ unsigned chunk;
+ if (s->port.txBufferHead > s->port.txBufferTail) {
+ chunk = s->port.txBufferHead - s->port.txBufferTail;
+ s->port.txBufferTail = s->port.txBufferHead;
+ } else {
+ chunk = s->port.txBufferSize - s->port.txBufferTail;
+ s->port.txBufferTail = 0;
+ }
+ s->txDMAEmpty = false;
+ xDMA_SetCurrDataCounter(s->txDMAResource, chunk);
+ reenable:
+ xDMA_Cmd(s->txDMAResource, ENABLE);
+ }
+}
+#endif
+
+#endif // USE_UART
diff --git a/src/platform/GD32/serial_usb_vcp.c b/src/platform/GD32/serial_usb_vcp.c
new file mode 100755
index 0000000000..0619bc9828
--- /dev/null
+++ b/src/platform/GD32/serial_usb_vcp.c
@@ -0,0 +1,247 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include "platform.h"
+
+#ifdef USE_VCP
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "drivers/io.h"
+
+#include "pg/usb.h"
+
+#include "usbd_core.h"
+#include "usbd_cdc_vcp.h"
+#ifdef USE_USB_CDC_HID
+#include "usb_cdc_hid.h"
+#endif
+#include "drivers/usb_io.h"
+
+#include "drivers/time.h"
+
+#include "drivers/serial.h"
+#include "drivers/serial_usb_vcp.h"
+#include "drv_usb_hw.h"
+
+#define USB_TIMEOUT 50
+
+static vcpPort_t vcpPort = {0};
+
+static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
+{
+ UNUSED(instance);
+ UNUSED(baudRate);
+
+ // TODO implement
+}
+
+static void usbVcpSetMode(serialPort_t *instance, portMode_e mode)
+{
+ UNUSED(instance);
+ UNUSED(mode);
+
+ // TODO implement
+}
+
+static void usbVcpSetCtrlLineStateCb(serialPort_t *instance, void (*cb)(void *context, uint16_t ctrlLineState), void *context)
+{
+ UNUSED(instance);
+
+ // Register upper driver control line state callback routine with USB driver
+ CDC_SetCtrlLineStateCb((void (*)(void *context, uint16_t ctrlLineState))cb, context);
+}
+
+static void usbVcpSetBaudRateCb(serialPort_t *instance, void (*cb)(serialPort_t *context, uint32_t baud), serialPort_t *context)
+{
+ UNUSED(instance);
+
+ // Register upper driver baud rate callback routine with USB driver
+ CDC_SetBaudRateCb((void (*)(void *context, uint32_t baud))cb, (void *)context);
+}
+
+static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance)
+{
+ UNUSED(instance);
+ return true;
+}
+
+static uint32_t usbVcpAvailable(const serialPort_t *instance)
+{
+ UNUSED(instance);
+
+ return CDC_Receive_BytesAvailable();
+}
+
+static uint8_t usbVcpRead(serialPort_t *instance)
+{
+ UNUSED(instance);
+
+ uint8_t buf[1]={0};
+
+ while (true) {
+ if (CDC_Receive_DATA(buf, 1))
+ return buf[0];
+ }
+}
+
+static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count)
+{
+ UNUSED(instance);
+
+ if (!(usbIsConnected() && usbIsConfigured())) {
+ return;
+ }
+
+ uint32_t start = millis();
+ const uint8_t *p = data;
+ while (count > 0) {
+ uint32_t txed = CDC_Send_DATA(p, count);
+ count -= txed;
+ p += txed;
+
+ if (millis() - start > USB_TIMEOUT) {
+ break;
+ }
+ }
+}
+
+static bool usbVcpFlush(vcpPort_t *port)
+{
+ uint32_t count = port->txAt;
+ port->txAt = 0;
+
+ if (count == 0) {
+ return true;
+ }
+
+ if (!usbIsConnected() || !usbIsConfigured()) {
+ return false;
+ }
+
+ uint32_t start = millis();
+ uint8_t *p = port->txBuf;
+ while (count > 0) {
+ uint32_t txed = CDC_Send_DATA(p, count);
+ count -= txed;
+ p += txed;
+
+ if (millis() - start > USB_TIMEOUT) {
+ break;
+ }
+ }
+ return count == 0;
+}
+
+static void usbVcpWrite(serialPort_t *instance, uint8_t c)
+{
+ vcpPort_t *port = container_of(instance, vcpPort_t, port);
+
+ port->txBuf[port->txAt++] = c;
+ if (!port->buffering || port->txAt >= ARRAYLEN(port->txBuf)) {
+ usbVcpFlush(port);
+ }
+}
+
+static void usbVcpBeginWrite(serialPort_t *instance)
+{
+ vcpPort_t *port = container_of(instance, vcpPort_t, port);
+ port->buffering = true;
+}
+
+static uint32_t usbTxBytesFree(const serialPort_t *instance)
+{
+ UNUSED(instance);
+ return CDC_Send_FreeBytes();
+}
+
+static void usbVcpEndWrite(serialPort_t *instance)
+{
+ vcpPort_t *port = container_of(instance, vcpPort_t, port);
+ port->buffering = false;
+ usbVcpFlush(port);
+}
+
+static const struct serialPortVTable usbVTable[] = {
+ {
+ .serialWrite = usbVcpWrite,
+ .serialTotalRxWaiting = usbVcpAvailable,
+ .serialTotalTxFree = usbTxBytesFree,
+ .serialRead = usbVcpRead,
+ .serialSetBaudRate = usbVcpSetBaudRate,
+ .isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty,
+ .setMode = usbVcpSetMode,
+ .setCtrlLineStateCb = usbVcpSetCtrlLineStateCb,
+ .setBaudRateCb = usbVcpSetBaudRateCb,
+ .writeBuf = usbVcpWriteBuf,
+ .beginWrite = usbVcpBeginWrite,
+ .endWrite = usbVcpEndWrite
+ }
+};
+
+void usbd_desc_string_update(void);
+serialPort_t *usbVcpOpen(void)
+{
+ IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0);
+ IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0);
+
+ usbGenerateDisconnectPulse();
+
+ switch (usbDevConfig()->type) {
+#ifdef USE_USB_CDC_HID
+ case COMPOSITE:
+ usb_gpio_config();
+ usb_rcu_config();
+ usbd_desc_string_update();
+ usbd_init(&USB_OTG_dev, USB_CORE_ENUM_FS, &bf_cdc_hid_desc, &bf_usbd_cdc_hid_cb);
+ usb_intr_config();
+ break;
+#endif
+ default:
+ usb_gpio_config();
+ usb_rcu_config();
+ usbd_desc_string_update();
+ usbd_init(&USB_OTG_dev, USB_CORE_ENUM_FS, &bf_cdc_desc, &bf_cdc_class);
+ usb_intr_config();
+ break;
+ }
+
+ vcpPort_t *s = &vcpPort;
+ s->port.vTable = usbVTable;
+ return &s->port;
+}
+
+uint32_t usbVcpGetBaudRate(serialPort_t *instance)
+{
+ UNUSED(instance);
+
+ return CDC_BaudRate();
+}
+
+uint8_t usbVcpIsConnected(void)
+{
+ return usbIsConnected();
+}
+#endif
diff --git a/src/platform/GD32/startup/gd32f4xx_libopt.h b/src/platform/GD32/startup/gd32f4xx_libopt.h
new file mode 100755
index 0000000000..f1a43d8980
--- /dev/null
+++ b/src/platform/GD32/startup/gd32f4xx_libopt.h
@@ -0,0 +1,78 @@
+/*!
+ \file gd32f4xx_libopt.h
+ \brief library optional for gd32f4xx
+
+ \version 2024-12-20, V3.3.1, firmware for GD32F4xx
+*/
+
+/*
+ Copyright (c) 2024, GigaDevice Semiconductor Inc.
+
+ Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+ 2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ 3. Neither the name of the copyright holder nor the names of its contributors
+ may be used to endorse or promote products derived from this software without
+ specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+OF SUCH DAMAGE.
+*/
+
+#ifndef GD32F4XX_LIBOPT_H
+#define GD32F4XX_LIBOPT_H
+
+#if defined (GD32F450) || defined (GD32F405) || defined (GD32F407) || defined (GD32F460) || defined (GD32F470) || defined (GD32F425) || defined (GD32F427)
+#include "gd32f4xx_rcu.h"
+#include "gd32f4xx_adc.h"
+#include "gd32f4xx_can.h"
+#include "gd32f4xx_crc.h"
+#include "gd32f4xx_ctc.h"
+#include "gd32f4xx_dac.h"
+#include "gd32f4xx_dbg.h"
+#include "gd32f4xx_dci.h"
+#include "gd32f4xx_dma.h"
+#include "gd32f4xx_exti.h"
+#include "gd32f4xx_fmc.h"
+#include "gd32f4xx_fwdgt.h"
+#include "gd32f4xx_gpio.h"
+#include "gd32f4xx_syscfg.h"
+#include "gd32f4xx_i2c.h"
+#include "gd32f4xx_iref.h"
+#include "gd32f4xx_pmu.h"
+#include "gd32f4xx_rtc.h"
+#include "gd32f4xx_sdio.h"
+#include "gd32f4xx_spi.h"
+#include "gd32f4xx_timer.h"
+#include "gd32f4xx_trng.h"
+#include "gd32f4xx_usart.h"
+#include "gd32f4xx_wwdgt.h"
+#include "gd32f4xx_misc.h"
+#endif
+
+#if defined (GD32F450) || defined (GD32F460) || defined (GD32F470)
+#include "gd32f4xx_enet.h"
+#include "gd32f4xx_exmc.h"
+#include "gd32f4xx_ipa.h"
+#include "gd32f4xx_tli.h"
+#endif
+
+#if defined (GD32F407) || defined (GD32F427)
+#include "gd32f4xx_enet.h"
+#include "gd32f4xx_exmc.h"
+#endif
+
+#endif /* GD32F4XX_LIBOPT_H */
diff --git a/src/platform/GD32/startup/startup_gd32f460.S b/src/platform/GD32/startup/startup_gd32f460.S
new file mode 100755
index 0000000000..55430ff983
--- /dev/null
+++ b/src/platform/GD32/startup/startup_gd32f460.S
@@ -0,0 +1,466 @@
+
+ .syntax unified
+ .cpu cortex-m4
+ .fpu fpv4-sp-d16
+ .thumb
+
+.global Default_Handler
+
+/* necessary symbols defined in linker script to initialize data */
+.word _sidata
+.word _sdata
+.word _edata
+.word _sbss
+.word _ebss
+.word __fastram_bss_start__
+.word __fastram_bss_end__
+
+
+ .section .text.Reset_Handler
+ .weak Reset_Handler
+ .type Reset_Handler, %function
+
+/* reset Handler */
+Reset_Handler:
+ movs r1, #0
+ b DataInit
+
+CopyData:
+ ldr r3, =_sidata
+ ldr r3, [r3, r1]
+ str r3, [r0, r1]
+ adds r1, r1, #4
+
+DataInit:
+ ldr r0, =_sdata
+ ldr r3, =_edata
+ adds r2, r0, r1
+ cmp r2, r3
+ bcc CopyData
+ ldr r2, =_sbss
+ b Zerobss
+
+FillZerobss:
+ movs r3, #0
+ str r3, [r2], #4
+
+Zerobss:
+ ldr r3, = _ebss
+ cmp r2, r3
+ bcc FillZerobss
+
+
+ ldr r2, =__fastram_bss_start__
+ b Zerofastram_bss
+
+FillZerofastram_bss:
+ movs r3, #0
+ str r3, [r2], #4
+
+Zerofastram_bss:
+ ldr r3, = __fastram_bss_end__
+ cmp r2, r3
+ bcc FillZerofastram_bss
+
+
+/* Call SystemInit function */
+ bl SystemInit
+/* Call static constructors */
+ bl __libc_init_array
+/*Call the main function */
+ bl main
+ bx lr
+.size Reset_Handler, .-Reset_Handler
+
+ .section .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+ b Infinite_Loop
+ .size Default_Handler, .-Default_Handler
+
+ .section .vectors,"a",%progbits
+ .global __gVectors
+
+__gVectors:
+ .word _estack /* Top of Stack */
+ .word Reset_Handler /* Reset Handler */
+ .word NMI_Handler /* NMI Handler */
+ .word HardFault_Handler /* Hard Fault Handler */
+ .word MemManage_Handler /* MPU Fault Handler */
+ .word BusFault_Handler /* Bus Fault Handler */
+ .word UsageFault_Handler /* Usage Fault Handler */
+ .word 0 /* Reserved */
+ .word 0 /* Reserved */
+ .word 0 /* Reserved */
+ .word 0 /* Reserved */
+ .word SVC_Handler /* SVCall Handler */
+ .word DebugMon_Handler /* Debug Monitor Handler */
+ .word 0 /* Reserved */
+ .word PendSV_Handler /* PendSV Handler */
+ .word SysTick_Handler /* SysTick Handler */
+
+ /* External interrupts handler */
+ .word WWDGT_IRQHandler /* Vector Number 16,Window Watchdog Timer */
+ .word LVD_IRQHandler /* Vector Number 17,LVD through EXTI Line detect */
+ .word TAMPER_STAMP_IRQHandler /* Vector Number 18,Tamper and TimeStamp through EXTI Line detect */
+ .word RTC_WKUP_IRQHandler /* Vector Number 19,RTC Wakeup through EXTI Line */
+ .word FMC_IRQHandler /* Vector Number 20,FMC */
+ .word RCU_CTC_IRQHandler /* Vector Number 21,RCU and CTC */
+ .word EXTI0_IRQHandler /* Vector Number 22,EXTI Line 0 */
+ .word EXTI1_IRQHandler /* Vector Number 23,EXTI Line 1 */
+ .word EXTI2_IRQHandler /* Vector Number 24,EXTI Line 2 */
+ .word EXTI3_IRQHandler /* Vector Number 25,EXTI Line 3 */
+ .word EXTI4_IRQHandler /* Vector Number 26,EXTI Line 4 */
+ .word DMA0_Channel0_IRQHandler /* Vector Number 27,DMA0 Channel0 */
+ .word DMA0_Channel1_IRQHandler /* Vector Number 28,DMA0 Channel1 */
+ .word DMA0_Channel2_IRQHandler /* Vector Number 29,DMA0 Channel2 */
+ .word DMA0_Channel3_IRQHandler /* Vector Number 30,DMA0 Channel3 */
+ .word DMA0_Channel4_IRQHandler /* Vector Number 31,DMA0 Channel4 */
+ .word DMA0_Channel5_IRQHandler /* Vector Number 32,DMA0 Channel5 */
+ .word DMA0_Channel6_IRQHandler /* Vector Number 33,DMA0 Channel6 */
+ .word ADC_IRQHandler /* Vector Number 34,ADC */
+ .word CAN0_TX_IRQHandler /* Vector Number 35,CAN0 TX */
+ .word CAN0_RX0_IRQHandler /* Vector Number 36,CAN0 RX0 */
+ .word CAN0_RX1_IRQHandler /* Vector Number 37,CAN0 RX1 */
+ .word CAN0_EWMC_IRQHandler /* Vector Number 38,CAN0 EWMC */
+ .word EXTI5_9_IRQHandler /* Vector Number 39,EXTI5 to EXTI9 */
+ .word TIMER0_BRK_TIMER8_IRQHandler /* Vector Number 40,TIMER0 Break and TIMER8 */
+ .word TIMER0_UP_TIMER9_IRQHandler /* Vector Number 41,TIMER0 Update and TIMER9 */
+ .word TIMER0_TRG_CMT_TIMER10_IRQHandler /* Vector Number 42,TIMER0 Trigger and Commutation and TIMER10 */
+ .word TIMER0_Channel_IRQHandler /* Vector Number 43,TIMER0 Channel Capture Compare */
+ .word TIMER1_IRQHandler /* Vector Number 44,TIMER1 */
+ .word TIMER2_IRQHandler /* Vector Number 45,TIMER2 */
+ .word TIMER3_IRQHandler /* Vector Number 46,TIMER3 */
+ .word I2C0_EV_IRQHandler /* Vector Number 47,I2C0 Event */
+ .word I2C0_ER_IRQHandler /* Vector Number 48,I2C0 Error */
+ .word I2C1_EV_IRQHandler /* Vector Number 49,I2C1 Event */
+ .word I2C1_ER_IRQHandler /* Vector Number 50,I2C1 Error */
+ .word SPI0_IRQHandler /* Vector Number 51,SPI0 */
+ .word SPI1_IRQHandler /* Vector Number 52,SPI1 */
+ .word USART0_IRQHandler /* Vector Number 53,USART0 */
+ .word USART1_IRQHandler /* Vector Number 54,USART1 */
+ .word USART2_IRQHandler /* Vector Number 55,USART2 */
+ .word EXTI10_15_IRQHandler /* Vector Number 56,EXTI10 to EXTI15 */
+ .word RTC_Alarm_IRQHandler /* Vector Number 57,RTC Alarm */
+ .word USBFS_WKUP_IRQHandler /* Vector Number 58,USBFS Wakeup */
+ .word TIMER7_BRK_TIMER11_IRQHandler /* Vector Number 59,TIMER7 Break and TIMER11 */
+ .word TIMER7_UP_TIMER12_IRQHandler /* Vector Number 60,TIMER7 Update and TIMER12 */
+ .word TIMER7_TRG_CMT_TIMER13_IRQHandler /* Vector Number 61,TIMER7 Trigger and Commutation and TIMER13 */
+ .word TIMER7_Channel_IRQHandler /* Vector Number 62,TIMER7 Channel Capture Compare */
+ .word DMA0_Channel7_IRQHandler /* Vector Number 63,DMA0 Channel7 */
+ .word 0 /* Vector Number 64,Reserved */
+ .word SDIO_IRQHandler /* Vector Number 65,SDIO */
+ .word TIMER4_IRQHandler /* Vector Number 66,TIMER4 */
+ .word SPI2_IRQHandler /* Vector Number 67,SPI2 */
+ .word UART3_IRQHandler /* Vector Number 68,UART3 */
+ .word UART4_IRQHandler /* Vector Number 69,UART4 */
+ .word TIMER5_DAC_IRQHandler /* Vector Number 70,TIMER5 and DAC0 DAC1 Underrun error */
+ .word TIMER6_IRQHandler /* Vector Number 71,TIMER6 */
+ .word DMA1_Channel0_IRQHandler /* Vector Number 72,DMA1 Channel0 */
+ .word DMA1_Channel1_IRQHandler /* Vector Number 73,DMA1 Channel1 */
+ .word DMA1_Channel2_IRQHandler /* Vector Number 74,DMA1 Channel2 */
+ .word DMA1_Channel3_IRQHandler /* Vector Number 75,DMA1 Channel3 */
+ .word DMA1_Channel4_IRQHandler /* Vector Number 76,DMA1 Channel4 */
+ .word ENET_IRQHandler /* Vector Number 77,Ethernet */
+ .word ENET_WKUP_IRQHandler /* Vector Number 78,Ethernet Wakeup through EXTI Line */
+ .word CAN1_TX_IRQHandler /* Vector Number 79,CAN1 TX */
+ .word CAN1_RX0_IRQHandler /* Vector Number 80,CAN1 RX0 */
+ .word CAN1_RX1_IRQHandler /* Vector Number 81,CAN1 RX1 */
+ .word CAN1_EWMC_IRQHandler /* Vector Number 82,CAN1 EWMC */
+ .word USBFS_IRQHandler /* Vector Number 83,USBFS */
+ .word DMA1_Channel5_IRQHandler /* Vector Number 84,DMA1 Channel5 */
+ .word DMA1_Channel6_IRQHandler /* Vector Number 85,DMA1 Channel6 */
+ .word DMA1_Channel7_IRQHandler /* Vector Number 86,DMA1 Channel7 */
+ .word USART5_IRQHandler /* Vector Number 87,USART5 */
+ .word I2C2_EV_IRQHandler /* Vector Number 88,I2C2 Event */
+ .word I2C2_ER_IRQHandler /* Vector Number 89,I2C2 Error */
+ .word USBHS_EP1_Out_IRQHandler /* Vector Number 90,USBHS Endpoint 1 Out */
+ .word USBHS_EP1_In_IRQHandler /* Vector Number 91,USBHS Endpoint 1 in */
+ .word USBHS_WKUP_IRQHandler /* Vector Number 92,USBHS Wakeup through EXTI Line */
+ .word USBHS_IRQHandler /* Vector Number 93,USBHS */
+ .word DCI_IRQHandler /* Vector Number 94,DCI */
+ .word 0 /* Vector Number 95,Reserved */
+ .word TRNG_IRQHandler /* Vector Number 96,TRNG */
+ .word FPU_IRQHandler /* Vector Number 97,FPU */
+ .word SPI3_IRQHandler /* Vector Number 100,SPI3 */
+ .word SPI4_IRQHandler /* Vector Number 101,SPI4 */
+ .word SPI5_IRQHandler /* Vector Number 102,SPI5 */
+ .word 0 /* Vector Number 103,Reserved */
+
+ .size __gVectors, . - __gVectors
+
+ .weak NMI_Handler
+ .thumb_set NMI_Handler,Default_Handler
+
+ .weak HardFault_Handler
+ .thumb_set HardFault_Handler,Default_Handler
+
+ .weak MemManage_Handler
+ .thumb_set MemManage_Handler,Default_Handler
+
+ .weak BusFault_Handler
+ .thumb_set BusFault_Handler,Default_Handler
+
+ .weak UsageFault_Handler
+ .thumb_set UsageFault_Handler,Default_Handler
+
+ .weak SVC_Handler
+ .thumb_set SVC_Handler,Default_Handler
+
+ .weak DebugMon_Handler
+ .thumb_set DebugMon_Handler,Default_Handler
+
+ .weak PendSV_Handler
+ .thumb_set PendSV_Handler,Default_Handler
+
+ .weak SysTick_Handler
+ .thumb_set SysTick_Handler,Default_Handler
+
+ .weak WWDGT_IRQHandler
+ .thumb_set WWDGT_IRQHandler,Default_Handler
+
+ .weak LVD_IRQHandler
+ .thumb_set LVD_IRQHandler,Default_Handler
+
+ .weak TAMPER_STAMP_IRQHandler
+ .thumb_set TAMPER_STAMP_IRQHandler,Default_Handler
+
+ .weak RTC_WKUP_IRQHandler
+ .thumb_set RTC_WKUP_IRQHandler,Default_Handler
+
+ .weak FMC_IRQHandler
+ .thumb_set FMC_IRQHandler,Default_Handler
+
+ .weak RCU_CTC_IRQHandler
+ .thumb_set RCU_CTC_IRQHandler,Default_Handler
+
+ .weak EXTI0_IRQHandler
+ .thumb_set EXTI0_IRQHandler,Default_Handler
+
+ .weak EXTI1_IRQHandler
+ .thumb_set EXTI1_IRQHandler,Default_Handler
+
+ .weak EXTI2_IRQHandler
+ .thumb_set EXTI2_IRQHandler,Default_Handler
+
+ .weak EXTI3_IRQHandler
+ .thumb_set EXTI3_IRQHandler,Default_Handler
+
+ .weak EXTI4_IRQHandler
+ .thumb_set EXTI4_IRQHandler,Default_Handler
+
+ .weak DMA0_Channel0_IRQHandler
+ .thumb_set DMA0_Channel0_IRQHandler,Default_Handler
+
+ .weak DMA0_Channel1_IRQHandler
+ .thumb_set DMA0_Channel1_IRQHandler,Default_Handler
+
+ .weak DMA0_Channel2_IRQHandler
+ .thumb_set DMA0_Channel2_IRQHandler,Default_Handler
+
+ .weak DMA0_Channel3_IRQHandler
+ .thumb_set DMA0_Channel3_IRQHandler,Default_Handler
+
+ .weak DMA0_Channel4_IRQHandler
+ .thumb_set DMA0_Channel4_IRQHandler,Default_Handler
+
+ .weak DMA0_Channel5_IRQHandler
+ .thumb_set DMA0_Channel5_IRQHandler,Default_Handler
+
+ .weak DMA0_Channel6_IRQHandler
+ .thumb_set DMA0_Channel6_IRQHandler,Default_Handler
+
+ .weak ADC_IRQHandler
+ .thumb_set ADC_IRQHandler,Default_Handler
+
+ .weak CAN0_TX_IRQHandler
+ .thumb_set CAN0_TX_IRQHandler,Default_Handler
+
+ .weak CAN0_RX0_IRQHandler
+ .thumb_set CAN0_RX0_IRQHandler,Default_Handler
+
+ .weak CAN0_RX1_IRQHandler
+ .thumb_set CAN0_RX1_IRQHandler,Default_Handler
+
+ .weak CAN0_EWMC_IRQHandler
+ .thumb_set CAN0_EWMC_IRQHandler,Default_Handler
+
+ .weak EXTI5_9_IRQHandler
+ .thumb_set EXTI5_9_IRQHandler,Default_Handler
+
+ .weak TIMER0_BRK_TIMER8_IRQHandler
+ .thumb_set TIMER0_BRK_TIMER8_IRQHandler,Default_Handler
+
+ .weak TIMER0_UP_TIMER9_IRQHandler
+ .thumb_set TIMER0_UP_TIMER9_IRQHandler,Default_Handler
+
+ .weak TIMER0_TRG_CMT_TIMER10_IRQHandler
+ .thumb_set TIMER0_TRG_CMT_TIMER10_IRQHandler,Default_Handler
+
+ .weak TIMER0_Channel_IRQHandler
+ .thumb_set TIMER0_Channel_IRQHandler,Default_Handler
+
+ .weak TIMER1_IRQHandler
+ .thumb_set TIMER1_IRQHandler,Default_Handler
+
+ .weak TIMER2_IRQHandler
+ .thumb_set TIMER2_IRQHandler,Default_Handler
+
+ .weak TIMER3_IRQHandler
+ .thumb_set TIMER3_IRQHandler,Default_Handler
+
+ .weak I2C0_EV_IRQHandler
+ .thumb_set I2C0_EV_IRQHandler,Default_Handler
+
+ .weak I2C0_ER_IRQHandler
+ .thumb_set I2C0_ER_IRQHandler,Default_Handler
+
+ .weak I2C1_EV_IRQHandler
+ .thumb_set I2C1_EV_IRQHandler,Default_Handler
+
+ .weak I2C1_ER_IRQHandler
+ .thumb_set I2C1_ER_IRQHandler,Default_Handler
+
+ .weak SPI0_IRQHandler
+ .thumb_set SPI0_IRQHandler,Default_Handler
+
+ .weak SPI1_IRQHandler
+ .thumb_set SPI1_IRQHandler,Default_Handler
+
+ .weak USART0_IRQHandler
+ .thumb_set USART0_IRQHandler,Default_Handler
+
+ .weak USART1_IRQHandler
+ .thumb_set USART1_IRQHandler,Default_Handler
+
+ .weak USART2_IRQHandler
+ .thumb_set USART2_IRQHandler,Default_Handler
+
+ .weak EXTI10_15_IRQHandler
+ .thumb_set EXTI10_15_IRQHandler,Default_Handler
+
+ .weak RTC_Alarm_IRQHandler
+ .thumb_set RTC_Alarm_IRQHandler,Default_Handler
+
+ .weak USBFS_WKUP_IRQHandler
+ .thumb_set USBFS_WKUP_IRQHandler,Default_Handler
+
+ .weak TIMER7_BRK_TIMER11_IRQHandler
+ .thumb_set TIMER7_BRK_TIMER11_IRQHandler,Default_Handler
+
+ .weak TIMER7_UP_TIMER12_IRQHandler
+ .thumb_set TIMER7_UP_TIMER12_IRQHandler,Default_Handler
+
+ .weak TIMER7_TRG_CMT_TIMER13_IRQHandler
+ .thumb_set TIMER7_TRG_CMT_TIMER13_IRQHandler,Default_Handler
+
+ .weak TIMER7_Channel_IRQHandler
+ .thumb_set TIMER7_Channel_IRQHandler,Default_Handler
+
+ .weak DMA0_Channel7_IRQHandler
+ .thumb_set DMA0_Channel7_IRQHandler,Default_Handler
+
+ .weak SDIO_IRQHandler
+ .thumb_set SDIO_IRQHandler,Default_Handler
+
+ .weak TIMER4_IRQHandler
+ .thumb_set TIMER4_IRQHandler,Default_Handler
+
+ .weak SPI2_IRQHandler
+ .thumb_set SPI2_IRQHandler,Default_Handler
+
+ .weak UART3_IRQHandler
+ .thumb_set UART3_IRQHandler,Default_Handler
+
+ .weak UART4_IRQHandler
+ .thumb_set UART4_IRQHandler,Default_Handler
+
+ .weak TIMER5_DAC_IRQHandler
+ .thumb_set TIMER5_DAC_IRQHandler,Default_Handler
+
+ .weak TIMER6_IRQHandler
+ .thumb_set TIMER6_IRQHandler,Default_Handler
+
+ .weak DMA1_Channel0_IRQHandler
+ .thumb_set DMA1_Channel0_IRQHandler,Default_Handler
+
+ .weak DMA1_Channel1_IRQHandler
+ .thumb_set DMA1_Channel1_IRQHandler,Default_Handler
+
+ .weak DMA1_Channel2_IRQHandler
+ .thumb_set DMA1_Channel2_IRQHandler,Default_Handler
+
+ .weak DMA1_Channel3_IRQHandler
+ .thumb_set DMA1_Channel3_IRQHandler,Default_Handler
+
+ .weak DMA1_Channel4_IRQHandler
+ .thumb_set DMA1_Channel4_IRQHandler,Default_Handler
+
+ .weak ENET_IRQHandler
+ .thumb_set ENET_IRQHandler,Default_Handler
+
+ .weak ENET_WKUP_IRQHandler
+ .thumb_set ENET_WKUP_IRQHandler,Default_Handler
+
+ .weak CAN1_TX_IRQHandler
+ .thumb_set CAN1_TX_IRQHandler,Default_Handler
+
+ .weak CAN1_RX0_IRQHandler
+ .thumb_set CAN1_RX0_IRQHandler,Default_Handler
+
+ .weak CAN1_RX1_IRQHandler
+ .thumb_set CAN1_RX1_IRQHandler,Default_Handler
+
+ .weak CAN1_EWMC_IRQHandler
+ .thumb_set CAN1_EWMC_IRQHandler,Default_Handler
+
+ .weak USBFS_IRQHandler
+ .thumb_set USBFS_IRQHandler,Default_Handler
+
+ .weak DMA1_Channel5_IRQHandler
+ .thumb_set DMA1_Channel5_IRQHandler,Default_Handler
+
+ .weak DMA1_Channel6_IRQHandler
+ .thumb_set DMA1_Channel6_IRQHandler,Default_Handler
+
+ .weak DMA1_Channel7_IRQHandler
+ .thumb_set DMA1_Channel7_IRQHandler,Default_Handler
+
+ .weak USART5_IRQHandler
+ .thumb_set USART5_IRQHandler,Default_Handler
+
+ .weak I2C2_EV_IRQHandler
+ .thumb_set I2C2_EV_IRQHandler,Default_Handler
+
+ .weak I2C2_ER_IRQHandler
+ .thumb_set I2C2_ER_IRQHandler,Default_Handler
+
+ .weak USBHS_EP1_Out_IRQHandler
+ .thumb_set USBHS_EP1_Out_IRQHandler,Default_Handler
+
+ .weak USBHS_EP1_In_IRQHandler
+ .thumb_set USBHS_EP1_In_IRQHandler,Default_Handler
+
+ .weak USBHS_WKUP_IRQHandler
+ .thumb_set USBHS_WKUP_IRQHandler,Default_Handler
+
+ .weak USBHS_IRQHandler
+ .thumb_set USBHS_IRQHandler,Default_Handler
+
+ .weak DCI_IRQHandler
+ .thumb_set DCI_IRQHandler,Default_Handler
+
+ .weak TRNG_IRQHandler
+ .thumb_set TRNG_IRQHandler,Default_Handler
+
+ .weak FPU_IRQHandler
+ .thumb_set FPU_IRQHandler,Default_Handler
+
+ .weak SPI3_IRQHandler
+ .thumb_set SPI3_IRQHandler,Default_Handler
+
+ .weak SPI4_IRQHandler
+ .thumb_set SPI4_IRQHandler,Default_Handler
+
+ .weak SPI5_IRQHandler
+ .thumb_set SPI5_IRQHandler,Default_Handler
diff --git a/src/platform/GD32/startup/system_gd32f4xx.c b/src/platform/GD32/startup/system_gd32f4xx.c
new file mode 100755
index 0000000000..b9ca5cbb72
--- /dev/null
+++ b/src/platform/GD32/startup/system_gd32f4xx.c
@@ -0,0 +1,1293 @@
+/*!
+ \file system_gd32f4xx.c
+ \brief CMSIS Cortex-M4 Device Peripheral Access Layer Source File for
+ GD32F4xx Device Series
+*/
+
+/* Copyright (c) 2012 ARM LIMITED
+ Copyright (c) 2024, GigaDevice Semiconductor Inc.
+
+ All rights reserved.
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ - Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ - Neither the name of ARM nor the names of its contributors may be used
+ to endorse or promote products derived from this software without
+ specific prior written permission.
+ *
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+ LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGE.
+ ---------------------------------------------------------------------------*/
+
+/* This file refers the CMSIS standard, some adjustments are made according to GigaDevice chips */
+
+#include "gd32f4xx.h"
+
+/* system frequency define */
+#define __IRC16M (IRC16M_VALUE) /* internal 16 MHz RC oscillator frequency */
+#define __HXTAL (HXTAL_VALUE) /* high speed crystal oscillator frequency */
+#define __SYS_OSC_CLK (__IRC16M) /* main oscillator frequency */
+
+/* select a system clock by uncommenting the following line */
+//#define __SYSTEM_CLOCK_IRC16M (uint32_t)(__IRC16M)
+//#define __SYSTEM_CLOCK_HXTAL (uint32_t)(__HXTAL)
+//#define __SYSTEM_CLOCK_120M_PLL_IRC16M (uint32_t)(120000000)
+//#define __SYSTEM_CLOCK_120M_PLL_8M_HXTAL (uint32_t)(120000000)
+//#define __SYSTEM_CLOCK_120M_PLL_25M_HXTAL (uint32_t)(120000000)
+//#define __SYSTEM_CLOCK_168M_PLL_IRC16M (uint32_t)(168000000)
+// #define __SYSTEM_CLOCK_168M_PLL_8M_HXTAL (uint32_t)(168000000)
+// #define __SYSTEM_CLOCK_168M_PLL_25M_HXTAL (uint32_t)(168000000)
+
+#if defined (GD32F450) || defined (GD32F425) || defined (GD32F427)
+#if (HSE_VALUE==25000000)
+#define __SYSTEM_CLOCK_200M_PLL_25M_HXTAL (uint32_t)(200000000)
+#elif (HSE_VALUE==8000000)
+#define __SYSTEM_CLOCK_200M_PLL_8M_HXTAL (uint32_t)(200000000)
+#else
+#define __SYSTEM_CLOCK_200M_PLL_8M_HXTAL (uint32_t)(200000000)
+#endif
+#endif // GD32F450 || GD32F425 || GD32F427
+
+#if defined (GD32F470) || defined (GD32F460)
+#if (HSE_VALUE==25000000)
+#define __SYSTEM_CLOCK_240M_PLL_25M_HXTAL (uint32_t)(240000000)
+#elif (HSE_VALUE==8000000)
+#define __SYSTEM_CLOCK_240M_PLL_8M_HXTAL (uint32_t)(240000000)
+#else
+#define __SYSTEM_CLOCK_240M_PLL_8M_HXTAL (uint32_t)(240000000)
+#endif
+#endif // GD32F470 || GD32F460
+
+/* The following is to prevent Vcore fluctuations caused by frequency switching.
+ It is strongly recommended to include it to avoid issues caused by self-removal. */
+#define RCU_MODIFY_4(__delay) do{ \
+ volatile uint32_t i, reg; \
+ if(0 != __delay){ \
+ /* Insert a software delay */ \
+ for(i=0; i<__delay; i++){ \
+ } \
+ reg = RCU_CFG0; \
+ reg &= ~(RCU_CFG0_AHBPSC); \
+ reg |= RCU_AHB_CKSYS_DIV2; \
+ /* AHB = SYSCLK/2 */ \
+ RCU_CFG0 = reg; \
+ /* Insert a software delay */ \
+ for(i=0; i<__delay; i++){ \
+ } \
+ reg = RCU_CFG0; \
+ reg &= ~(RCU_CFG0_AHBPSC); \
+ reg |= RCU_AHB_CKSYS_DIV4; \
+ /* AHB = SYSCLK/4 */ \
+ RCU_CFG0 = reg; \
+ /* Insert a software delay */ \
+ for(i=0; i<__delay; i++){ \
+ } \
+ reg = RCU_CFG0; \
+ reg &= ~(RCU_CFG0_AHBPSC); \
+ reg |= RCU_AHB_CKSYS_DIV8; \
+ /* AHB = SYSCLK/8 */ \
+ RCU_CFG0 = reg; \
+ /* Insert a software delay */ \
+ for(i=0; i<__delay; i++){ \
+ } \
+ reg = RCU_CFG0; \
+ reg &= ~(RCU_CFG0_AHBPSC); \
+ reg |= RCU_AHB_CKSYS_DIV16; \
+ /* AHB = SYSCLK/16 */ \
+ RCU_CFG0 = reg; \
+ /* Insert a software delay */ \
+ for(i=0; i<__delay; i++){ \
+ } \
+ } \
+ }while(0)
+
+#define SEL_IRC16M 0x00U
+#define SEL_HXTAL 0x01U
+#define SEL_PLLP 0x02U
+
+/* set the system clock frequency and declare the system clock configuration function */
+#ifdef __SYSTEM_CLOCK_IRC16M
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_IRC16M;
+static void system_clock_16m_irc16m(void);
+#elif defined (__SYSTEM_CLOCK_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_HXTAL;
+static void system_clock_hxtal(void);
+#elif defined (__SYSTEM_CLOCK_120M_PLL_IRC16M)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_120M_PLL_IRC16M;
+static void system_clock_120m_irc16m(void);
+#elif defined (__SYSTEM_CLOCK_120M_PLL_8M_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_120M_PLL_8M_HXTAL;
+static void system_clock_120m_8m_hxtal(void);
+#elif defined (__SYSTEM_CLOCK_120M_PLL_25M_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_120M_PLL_25M_HXTAL;
+static void system_clock_120m_25m_hxtal(void);
+#elif defined (__SYSTEM_CLOCK_168M_PLL_IRC16M)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_168M_PLL_IRC16M;
+static void system_clock_168m_irc16m(void);
+#elif defined (__SYSTEM_CLOCK_168M_PLL_8M_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_168M_PLL_8M_HXTAL;
+static void system_clock_168m_8m_hxtal(void);
+#elif defined (__SYSTEM_CLOCK_168M_PLL_25M_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_168M_PLL_25M_HXTAL;
+static void system_clock_168m_25m_hxtal(void);
+#elif defined (__SYSTEM_CLOCK_200M_PLL_IRC16M)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_200M_PLL_IRC16M;
+static void system_clock_200m_irc16m(void);
+#elif defined (__SYSTEM_CLOCK_200M_PLL_8M_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_200M_PLL_8M_HXTAL;
+static void system_clock_200m_8m_hxtal(void);
+#elif defined (__SYSTEM_CLOCK_200M_PLL_25M_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_200M_PLL_25M_HXTAL;
+static void system_clock_200m_25m_hxtal(void);
+#elif defined (__SYSTEM_CLOCK_240M_PLL_IRC16M)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_240M_PLL_IRC16M;
+static void system_clock_240m_irc16m(void);
+#elif defined (__SYSTEM_CLOCK_240M_PLL_8M_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_240M_PLL_8M_HXTAL;
+static void system_clock_240m_8m_hxtal(void);
+#elif defined (__SYSTEM_CLOCK_240M_PLL_25M_HXTAL)
+uint32_t SystemCoreClock = __SYSTEM_CLOCK_240M_PLL_25M_HXTAL;
+static void system_clock_240m_25m_hxtal(void);
+#endif /* __SYSTEM_CLOCK_IRC16M */
+
+/* configure the system clock */
+static void system_clock_config(void);
+
+/* software delay to prevent the impact of Vcore fluctuations.
+ It is strongly recommended to include it to avoid issues caused by self-removal. */
+static void _soft_delay_(uint32_t time)
+{
+ __IO uint32_t i;
+ for(i=0; i