diff --git a/mk/mcu/APM32F4.mk b/mk/mcu/APM32F4.mk
new file mode 100644
index 0000000000..a061c6e2c7
--- /dev/null
+++ b/mk/mcu/APM32F4.mk
@@ -0,0 +1,124 @@
+#
+# APM32F4 Make file include
+#
+
+#CMSIS
+CMSIS_DIR := $(ROOT)/lib/main/APM32F4/Libraries/Device
+STDPERIPH_DIR = $(ROOT)/lib/main/APM32F4/Libraries/APM32F4xx_DAL_Driver
+STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/Source/*.c))
+EXCLUDES = apm32f4xx_dal_timebase_rtc_alarm_template.c \
+ apm32f4xx_dal_timebase_rtc_wakeup_template.c \
+ apm32f4xx_dal_timebase_tmr_template.c \
+ apm32f4xx_device_cfg_template.c
+
+STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
+
+VPATH := $(VPATH):$(STDPERIPH_DIR)/Source
+
+#USB
+USBCORE_DIR = $(ROOT)/lib/main/APM32F4/Middlewares/APM32_USB_Library/Device/Core
+USBCORE_SRC = $(notdir $(wildcard $(USBCORE_DIR)/Src/*.c))
+USBCORE_SRC := $(filter-out ${EXCLUDES}, $(USBCORE_SRC))
+
+USBCDC_DIR = $(ROOT)/lib/main/APM32F4/Middlewares/APM32_USB_Library/Device/Class/CDC
+USBCDC_SRC = $(notdir $(wildcard $(USBCDC_DIR)/Src/*.c))
+USBCDC_SRC := $(filter-out ${EXCLUDES}, $(USBCDC_SRC))
+
+USBMSC_DIR = $(ROOT)/lib/main/APM32F4/Middlewares/APM32_USB_Library/Device/Class/MSC
+USBMSC_SRC = $(notdir $(wildcard $(USBMSC_DIR)/Src/*.c))
+USBMSC_SRC := $(filter-out ${EXCLUDES}, $(USBMSC_SRC))
+
+VPATH := $(VPATH):$(USBCDC_DIR)/Src:$(USBCORE_DIR)/Src:$(USBMSC_DIR)/Src
+
+DEVICE_STDPERIPH_SRC := $(STDPERIPH_SRC) \
+ $(USBCORE_SRC) \
+ $(USBCDC_SRC) \
+ $(USBMSC_SRC)
+#CMSIS
+VPATH := $(VPATH):$(ROOT)/lib/main/APM32F4/Libraries/Device/Geehy/APM32F4xx
+
+INCLUDE_DIRS := $(INCLUDE_DIRS) \
+ $(SRC_DIR)/startup/apm32 \
+ $(SRC_DIR)/drivers/mcu/apm32
+
+CMSIS_SRC :=
+INCLUDE_DIRS := $(INCLUDE_DIRS) \
+ $(STDPERIPH_DIR)/Include \
+ $(USBCORE_DIR)/Inc \
+ $(USBCDC_DIR)/Inc \
+ $(USBMSC_DIR)/Inc \
+ $(CMSIS_DIR)/Geehy/APM32F4xx/Include \
+ $(SRC_DIR)/drivers/mcu/apm32/usb/vcp \
+ $(SRC_DIR)/drivers/mcu/apm32/usb/msc \
+ $(SRC_DIR)/drivers/mcu/apm32/usb \
+ $(ROOT)/lib/main/CMSIS/Core/Include \
+ $(SRC_DIR)/msc
+
+#Flags
+ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant
+
+ifeq ($(TARGET_MCU),APM32F405xx)
+DEVICE_FLAGS = -DUSE_DAL_DRIVER -DAPM32F405xx -DHSE_VALUE=$(HSE_VALUE) -DAPM32
+LD_SCRIPT = $(LINKER_DIR)/apm32_flash_f405.ld
+STARTUP_SRC = apm32/startup_apm32f405xx.S
+MCU_FLASH_SIZE := 1024
+
+else ifeq ($(TARGET_MCU),APM32F407xx)
+DEVICE_FLAGS = -DUSE_DAL_DRIVER -DAPM32F407xx -DHSE_VALUE=$(HSE_VALUE) -DAPM32
+LD_SCRIPT = $(LINKER_DIR)/apm32_flash_f407.ld
+STARTUP_SRC = apm32/startup_apm32f407xx.S
+MCU_FLASH_SIZE := 1024
+else
+$(error TARGET_MCU [$(TARGET_MCU] is not supported)
+endif
+
+MCU_COMMON_SRC = \
+ startup/apm32/system_apm32f4xx.c \
+ drivers/inverter.c \
+ drivers/dshot_bitbang_decode.c \
+ drivers/pwm_output_dshot_shared.c \
+ drivers/mcu/apm32/bus_spi_apm32.c \
+ drivers/mcu/apm32/bus_i2c_apm32.c \
+ drivers/mcu/apm32/bus_i2c_apm32_init.c \
+ drivers/mcu/apm32/camera_control.c \
+ drivers/mcu/apm32/debug.c \
+ drivers/mcu/apm32/dma_reqmap_mcu.c \
+ drivers/mcu/apm32/dshot_bitbang.c \
+ drivers/mcu/apm32/dshot_bitbang_ddl.c \
+ drivers/mcu/apm32/eint_apm32.c \
+ drivers/mcu/apm32/io_apm32.c \
+ drivers/mcu/apm32/light_ws2811strip_apm32.c \
+ drivers/mcu/apm32/persistent_apm32.c \
+ drivers/mcu/apm32/pwm_output_apm32.c \
+ drivers/mcu/apm32/pwm_output_dshot_apm32.c \
+ drivers/mcu/apm32/rcm_apm32.c \
+ drivers/mcu/apm32/serial_uart_apm32.c \
+ drivers/mcu/apm32/timer_apm32.c \
+ drivers/mcu/apm32/transponder_ir_io_apm32.c \
+ drivers/mcu/apm32/timer_apm32f4xx.c \
+ drivers/mcu/apm32/adc_apm32f4xx.c \
+ drivers/mcu/apm32/dma_apm32f4xx.c \
+ drivers/mcu/apm32/serial_uart_apm32f4xx.c \
+ drivers/mcu/apm32/system_apm32f4xx.c
+
+VCP_SRC = \
+ drivers/mcu/apm32/usb/vcp/usbd_cdc_descriptor.c \
+ drivers/mcu/apm32/usb/usbd_board_apm32f4.c \
+ drivers/mcu/apm32/usb/vcp/usbd_cdc_vcp.c \
+ drivers/mcu/apm32/usb/vcp/serial_usb_vcp.c \
+ drivers/usb_io.c
+
+MSC_SRC = \
+ drivers/usb_msc_common.c \
+ drivers/mcu/apm32/usb/msc/usb_msc_apm32f4xx.c \
+ drivers/mcu/apm32/usb/msc/usbd_memory.c \
+ drivers/mcu/apm32/usb/msc/usbd_msc_descriptor.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
+
+DSP_LIB := $(ROOT)/lib/main/CMSIS/DSP
+DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM4 -DUSE_FULL_DDL_DRIVER
diff --git a/src/link/apm32_flash_f405.ld b/src/link/apm32_flash_f405.ld
new file mode 100644
index 0000000000..121c7e8da9
--- /dev/null
+++ b/src/link/apm32_flash_f405.ld
@@ -0,0 +1,40 @@
+/*
+*****************************************************************************
+**
+** File : apm32_flash_f405.ld
+**
+** Abstract : Linker script for APM32F405RG Device with
+** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
+**
+*****************************************************************************
+*/
+
+/*
+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,
+*/
+
+/* Specify the memory areas */
+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 (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+ CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+ BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
+ MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("STACKRAM", CCM)
+REGION_ALIAS("FASTRAM", CCM)
+REGION_ALIAS("VECTAB", RAM)
+
+/* Put various bits and bobs of data into the main chunk of flash as we have enough of it */
+
+REGION_ALIAS("MOVABLE_FLASH", FLASH1)
+
+INCLUDE "stm32_flash_f4_split.ld"
diff --git a/src/link/apm32_flash_f407.ld b/src/link/apm32_flash_f407.ld
new file mode 100644
index 0000000000..3e4ed3b4ca
--- /dev/null
+++ b/src/link/apm32_flash_f407.ld
@@ -0,0 +1,40 @@
+/*
+*****************************************************************************
+**
+** File : apm32_flash_f407.ld
+**
+** Abstract : Linker script for APM32F407RG Device with
+** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM)
+**
+*****************************************************************************
+*/
+
+/*
+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,
+*/
+
+/* Specify the memory areas */
+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 (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+ CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+ BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
+ MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
+}
+
+REGION_ALIAS("STACKRAM", CCM)
+REGION_ALIAS("FASTRAM", CCM)
+REGION_ALIAS("VECTAB", RAM)
+
+/* Put various bits and bobs of data into the main chunk of flash as we have enough of it */
+
+REGION_ALIAS("MOVABLE_FLASH", FLASH1)
+
+INCLUDE "stm32_flash_f4_split.ld"
diff --git a/src/main/build/build_config.c b/src/main/build/build_config.c
index 0faab12b4d..d6d92e65e5 100644
--- a/src/main/build/build_config.c
+++ b/src/main/build/build_config.c
@@ -70,6 +70,8 @@ mcuTypeId_e getMcuTypeId(void)
return MCU_TYPE_G474;
#elif defined(AT32F435)
return MCU_TYPE_AT32;
+#elif defined(APM32F4)
+ return MCU_TYPE_APM32F40X;
#else
return MCU_TYPE_UNKNOWN;
#endif
diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h
index 52811118a5..cd77a4b849 100644
--- a/src/main/build/build_config.h
+++ b/src/main/build/build_config.h
@@ -60,6 +60,7 @@ typedef enum {
MCU_TYPE_G474,
MCU_TYPE_H730,
MCU_TYPE_AT32,
+ MCU_TYPE_APM32F40X,
MCU_TYPE_UNKNOWN = 255,
} mcuTypeId_e;
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 207a021443..b380e2c47c 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -293,7 +293,9 @@ static const char *mcuTypeNames[] = {
"H723/H725",
"G474",
"H730",
- "AT32F435"
+ "AT32F435",
+ "APM32F405",
+ "APM32F407",
};
static const char *configurationStates[] = {
@@ -4630,7 +4632,7 @@ static void cliStatus(const char *cmdName, char *cmdline)
cliPrintf("MCU %s Clock=%dMHz", getMcuTypeById(getMcuTypeId()), (SystemCoreClock / 1000000));
-#if defined(STM32F4) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32G4) || defined(APM32F4)
// Only F4 and G4 is capable of switching between HSE/HSI (for now)
int sysclkSource = SystemSYSCLKSource();
@@ -5344,7 +5346,7 @@ dmaoptEntry_t dmaoptEntryTable[] = {
#define DMA_CHANREQ_STRING "Channel"
#endif
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(APM32F4)
#define DMA_STCH_STRING "Stream"
#else
#define DMA_STCH_STRING "Channel"
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index b9cc50158c..f5079ee9d1 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -367,6 +367,8 @@ static const char * const lookupOverclock[] = {
"108MHZ", "120MHZ"
#elif defined(STM32F7)
"240MHZ"
+#elif defined(APM32F405xx) || defined(APM32F407xx)
+ "192MHZ", "216MHZ", "240MHZ"
#endif
};
#endif
@@ -1561,7 +1563,7 @@ const clivalue_t valueTable[] = {
#endif // end of #ifdef USE_OSD
// PG_SYSTEM_CONFIG
-#if defined(STM32F4) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32G4) || defined(APM32F4)
{ "system_hse_mhz", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, hseMhz) },
#endif
{ "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) },
diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c
index a00fc8aa84..c9c5285766 100644
--- a/src/main/config/config_streamer.c
+++ b/src/main/config/config_streamer.c
@@ -57,6 +57,8 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
#elif defined(CONFIG_IN_FLASH) || defined(CONFIG_IN_FILE)
#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
HAL_FLASH_Unlock();
+#elif defined(APM32F4)
+ DAL_FLASH_Unlock();
#elif defined(AT32F4)
flash_unlock();
#else
@@ -79,6 +81,8 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
// NOP
#elif defined(AT32F4)
flash_flag_clear(FLASH_ODF_FLAG | FLASH_PRGMERR_FLAG | FLASH_EPPERR_FLAG);
+#elif defined(APM32F4)
+ __DAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
#elif defined(UNIT_TEST) || defined(SIMULATOR_BUILD)
// NOP
#else
@@ -319,6 +323,55 @@ static void getFLASHSectorForEEPROM(uint32_t *bank, uint32_t *sector)
}
}
}
+#elif defined(APM32F4)
+/*
+Sector 0 0x08000000 - 0x08003FFF 16 Kbytes
+Sector 1 0x08004000 - 0x08007FFF 16 Kbytes
+Sector 2 0x08008000 - 0x0800BFFF 16 Kbytes
+Sector 3 0x0800C000 - 0x0800FFFF 16 Kbytes
+Sector 4 0x08010000 - 0x0801FFFF 64 Kbytes
+Sector 5 0x08020000 - 0x0803FFFF 128 Kbytes
+Sector 6 0x08040000 - 0x0805FFFF 128 Kbytes
+Sector 7 0x08060000 - 0x0807FFFF 128 Kbytes
+Sector 8 0x08080000 - 0x0809FFFF 128 Kbytes
+Sector 9 0x080A0000 - 0x080BFFFF 128 Kbytes
+Sector 10 0x080C0000 - 0x080DFFFF 128 Kbytes
+Sector 11 0x080E0000 - 0x080FFFFF 128 Kbytes
+*/
+
+static uint32_t getFLASHSectorForEEPROM(void)
+{
+ if ((uint32_t)&__config_start <= 0x08003FFF)
+ return FLASH_SECTOR_0;
+ if ((uint32_t)&__config_start <= 0x08007FFF)
+ return FLASH_SECTOR_1;
+ if ((uint32_t)&__config_start <= 0x0800BFFF)
+ return FLASH_SECTOR_2;
+ if ((uint32_t)&__config_start <= 0x0800FFFF)
+ return FLASH_SECTOR_3;
+ if ((uint32_t)&__config_start <= 0x0801FFFF)
+ return FLASH_SECTOR_4;
+ if ((uint32_t)&__config_start <= 0x0803FFFF)
+ return FLASH_SECTOR_5;
+ if ((uint32_t)&__config_start <= 0x0805FFFF)
+ return FLASH_SECTOR_6;
+ if ((uint32_t)&__config_start <= 0x0807FFFF)
+ return FLASH_SECTOR_7;
+ if ((uint32_t)&__config_start <= 0x0809FFFF)
+ return FLASH_SECTOR_8;
+ if ((uint32_t)&__config_start <= 0x080DFFFF)
+ return FLASH_SECTOR_9;
+ if ((uint32_t)&__config_start <= 0x080BFFFF)
+ return FLASH_SECTOR_10;
+ if ((uint32_t)&__config_start <= 0x080FFFFF)
+ return FLASH_SECTOR_11;
+
+ // Not good
+ while (1) {
+ failureMode(FAILURE_CONFIG_STORE_FAILURE);
+ }
+}
+
#endif
#endif // CONFIG_IN_FLASH
@@ -478,6 +531,26 @@ static int write_word(config_streamer_t *c, config_streamer_buffer_align_type_t
if (status != FLASH_OPERATE_DONE) {
return -2;
}
+#elif defined(APM32F4)
+ if (c->address % FLASH_PAGE_SIZE == 0) {
+ FLASH_EraseInitTypeDef EraseInitStruct = {
+ .TypeErase = FLASH_TYPEERASE_SECTORS,
+ .VoltageRange = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V
+ .NbSectors = 1
+ };
+ EraseInitStruct.Sector = getFLASHSectorForEEPROM();
+ uint32_t SECTORError;
+ const DAL_StatusTypeDef status = DAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
+ if (status != DAL_OK) {
+ return -1;
+ }
+ }
+
+ STATIC_ASSERT(CONFIG_STREAMER_BUFFER_SIZE == sizeof(uint32_t) * 1, "CONFIG_STREAMER_BUFFER_SIZE does not match written size");
+ const DAL_StatusTypeDef status = DAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, (uint64_t)*buffer);
+ if (status != DAL_OK) {
+ return -2;
+ }
#else // !STM32H7 && !STM32F7 && !STM32G4
if (c->address % FLASH_PAGE_SIZE == 0) {
const FLASH_Status status = FLASH_EraseSector(getFLASHSectorForEEPROM(), VoltageRange_3); //0x08080000 to 0x080A0000
@@ -543,6 +616,8 @@ int config_streamer_finish(config_streamer_t *c)
HAL_FLASH_Lock();
#elif defined(AT32F4)
flash_lock();
+#elif defined(APM32F4)
+ DAL_FLASH_Lock();
#else
FLASH_Lock();
#endif
diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h
index ce2f7967b7..930dcbe50b 100644
--- a/src/main/config/config_streamer.h
+++ b/src/main/config/config_streamer.h
@@ -41,6 +41,9 @@ typedef uint64_t config_streamer_buffer_align_type_t;
#elif defined(STM32G4)
#define CONFIG_STREAMER_BUFFER_SIZE 8 // Flash word = 64-bits
typedef uint64_t config_streamer_buffer_align_type_t;
+#elif defined(APM32F4)
+#define CONFIG_STREAMER_BUFFER_SIZE 4 // Flash word = 32-bits
+typedef uint32_t config_streamer_buffer_align_type_t;
#else
#define CONFIG_STREAMER_BUFFER_SIZE 4
typedef uint32_t config_streamer_buffer_align_type_t;
diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h
index 15fdadc165..9e272d9e19 100644
--- a/src/main/drivers/adc.h
+++ b/src/main/drivers/adc.h
@@ -29,7 +29,7 @@
#define ADC_INSTANCE ADC1
#endif
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(APM32F4)
#ifndef ADC1_DMA_STREAM
#define ADC1_DMA_STREAM DMA2_Stream4 // ST0 or ST4
#endif
diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h
index cf0edcc0b2..d9b337b45e 100644
--- a/src/main/drivers/adc_impl.h
+++ b/src/main/drivers/adc_impl.h
@@ -45,6 +45,8 @@
#else
#define ADC_TAG_MAP_COUNT 47
#endif
+#elif defined(APM32F4)
+#define ADC_TAG_MAP_COUNT 16
#else
#define ADC_TAG_MAP_COUNT 10
#endif
@@ -75,11 +77,11 @@ typedef struct adcDevice_s {
rccPeriphTag_t rccADC;
#if !defined(USE_DMA_SPEC)
dmaResource_t* dmaResource;
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
uint32_t channel;
#endif
#endif // !defined(USE_DMA_SPEC)
-#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
ADC_HandleTypeDef ADCHandle;
DMA_HandleTypeDef DmaHandle;
#endif
diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h
index f399491497..74dcfadb3d 100644
--- a/src/main/drivers/bus_i2c.h
+++ b/src/main/drivers/bus_i2c.h
@@ -37,7 +37,7 @@ typedef enum I2CDevice {
I2CDEV_4,
} I2CDevice;
-#if defined(STM32F4)
+#if defined(STM32F4) || defined(APM32F4)
#define I2CDEV_COUNT 3
#elif defined(STM32F7)
#define I2CDEV_COUNT 4
diff --git a/src/main/drivers/bus_i2c_config.c b/src/main/drivers/bus_i2c_config.c
index f4ab7b975d..4cf78c03ba 100644
--- a/src/main/drivers/bus_i2c_config.c
+++ b/src/main/drivers/bus_i2c_config.c
@@ -56,13 +56,13 @@ void i2cHardwareConfigure(const i2cConfig_t *i2cConfig)
for (int pindex = 0 ; pindex < I2C_PIN_SEL_MAX ; pindex++) {
if (i2cConfig[device].ioTagScl == hardware->sclPins[pindex].ioTag) {
pDev->scl = IOGetByTag(i2cConfig[device].ioTagScl);
-#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4)
+#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4)
pDev->sclAF = hardware->sclPins[pindex].af;
#endif
}
if (i2cConfig[device].ioTagSda == hardware->sdaPins[pindex].ioTag) {
pDev->sda = IOGetByTag(i2cConfig[device].ioTagSda);
-#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4)
+#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4)
pDev->sdaAF = hardware->sdaPins[pindex].af;
#endif
}
diff --git a/src/main/drivers/bus_i2c_impl.h b/src/main/drivers/bus_i2c_impl.h
index b286bc18cf..0dd1fb2c99 100644
--- a/src/main/drivers/bus_i2c_impl.h
+++ b/src/main/drivers/bus_i2c_impl.h
@@ -32,12 +32,12 @@
typedef struct i2cPinDef_s {
ioTag_t ioTag;
-#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4)
+#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4)
uint8_t af;
#endif
} i2cPinDef_t;
-#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4)
+#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4)
#define I2CPINDEF(pin, af) { DEFIO_TAG_E(pin), af }
#else
#define I2CPINDEF(pin) { DEFIO_TAG_E(pin) }
@@ -74,7 +74,7 @@ typedef struct i2cDevice_s {
I2C_TypeDef *reg;
IO_t scl;
IO_t sda;
-#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4)
+#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4)
uint8_t sclAF;
uint8_t sdaAF;
#endif
diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c
index 53cf8d2020..f9a9274376 100644
--- a/src/main/drivers/bus_spi.c
+++ b/src/main/drivers/bus_spi.c
@@ -358,7 +358,7 @@ uint8_t spiReadRegMsk(const extDevice_t *dev, uint8_t reg)
uint16_t spiCalculateDivider(uint32_t freq)
{
-#if defined(STM32F4) || defined(STM32G4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32G4) || defined(STM32F7) || defined(APM32F4)
uint32_t spiClk = SystemCoreClock / 2;
#elif defined(STM32H7)
uint32_t spiClk = 100000000;
@@ -383,7 +383,7 @@ uint16_t spiCalculateDivider(uint32_t freq)
uint32_t spiCalculateClock(uint16_t spiClkDivisor)
{
-#if defined(STM32F4) || defined(STM32G4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32G4) || defined(STM32F7) || defined(APM32F4)
uint32_t spiClk = SystemCoreClock / 2;
#elif defined(STM32H7)
uint32_t spiClk = 100000000;
@@ -393,7 +393,6 @@ uint32_t spiCalculateClock(uint16_t spiClkDivisor)
if ((spiClk / spiClkDivisor) > 36000000){
return 36000000;
}
-
#else
#error "Base SPI clock not defined for this architecture"
#endif
@@ -572,7 +571,7 @@ bool spiSetBusInstance(extDevice_t *dev, uint32_t device)
void spiInitBusDMA(void)
{
uint32_t device;
-#if defined(STM32F4) && defined(USE_DSHOT_BITBANG)
+#if (defined(STM32F4) || defined(APM32F4)) && defined(USE_DSHOT_BITBANG)
/* Check https://www.st.com/resource/en/errata_sheet/dm00037591-stm32f405407xx-and-stm32f415417xx-device-limitations-stmicroelectronics.pdf
* section 2.1.10 which reports an errata that corruption may occurs on DMA2 if AHB peripherals (eg GPIO ports) are
* access concurrently with APB peripherals (eg SPI busses). Bitbang DSHOT uses DMA2 to write to GPIO ports. If this
@@ -606,7 +605,7 @@ void spiInitBusDMA(void)
if (dmaTxChannelSpec) {
dmaTxIdentifier = dmaGetIdentifier(dmaTxChannelSpec->ref);
-#if defined(STM32F4) && defined(USE_DSHOT_BITBANG)
+#if (defined(STM32F4) || defined(APM32F4)) && defined(USE_DSHOT_BITBANG)
if (dshotBitbangActive && (DMA_DEVICE_NO(dmaTxIdentifier) == 2)) {
dmaTxIdentifier = DMA_NONE;
break;
@@ -617,7 +616,7 @@ void spiInitBusDMA(void)
continue;
}
bus->dmaTx = dmaGetDescriptorByIdentifier(dmaTxIdentifier);
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4)
bus->dmaTx->stream = DMA_DEVICE_INDEX(dmaTxIdentifier);
bus->dmaTx->channel = dmaTxChannelSpec->channel;
#endif
@@ -644,7 +643,7 @@ void spiInitBusDMA(void)
if (dmaRxChannelSpec) {
dmaRxIdentifier = dmaGetIdentifier(dmaRxChannelSpec->ref);
-#if defined(STM32F4) && defined(USE_DSHOT_BITBANG)
+#if (defined(STM32F4) || defined(APM32F4)) && defined(USE_DSHOT_BITBANG)
if (dshotBitbangActive && (DMA_DEVICE_NO(dmaRxIdentifier) == 2)) {
dmaRxIdentifier = DMA_NONE;
break;
@@ -655,7 +654,7 @@ void spiInitBusDMA(void)
continue;
}
bus->dmaRx = dmaGetDescriptorByIdentifier(dmaRxIdentifier);
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4)
bus->dmaRx->stream = DMA_DEVICE_INDEX(dmaRxIdentifier);
bus->dmaRx->channel = dmaRxChannelSpec->channel;
#endif
diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h
index d8ba416f8a..06e45967ea 100644
--- a/src/main/drivers/bus_spi.h
+++ b/src/main/drivers/bus_spi.h
@@ -33,7 +33,7 @@
#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN)
#define SPI_IO_AF_SDI_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP)
#define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL)
-#elif defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#elif defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
#define SPI_IO_AF_CFG IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
#define SPI_IO_AF_SCK_CFG_HIGH IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
#define SPI_IO_AF_SCK_CFG_LOW IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN)
@@ -78,6 +78,8 @@ typedef enum SPIDevice {
#define SPIDEV_COUNT 4
#elif defined(STM32H7)
#define SPIDEV_COUNT 6
+#elif defined(APM32F4)
+#define SPIDEV_COUNT 3
#else
#define SPIDEV_COUNT 4
#endif
diff --git a/src/main/drivers/bus_spi_impl.h b/src/main/drivers/bus_spi_impl.h
index eadbb4cfbb..11ce5d9f79 100644
--- a/src/main/drivers/bus_spi_impl.h
+++ b/src/main/drivers/bus_spi_impl.h
@@ -28,6 +28,8 @@
#define MAX_SPI_PIN_SEL 4
#elif defined(STM32H7)
#define MAX_SPI_PIN_SEL 5
+#elif defined(APM32F4)
+#define MAX_SPI_PIN_SEL 2
#else
#error Unknown MCU family
#endif
@@ -36,7 +38,7 @@
typedef struct spiPinDef_s {
ioTag_t pin;
-#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4)
+#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4)
uint8_t af;
#endif
} spiPinDef_t;
@@ -63,7 +65,7 @@ typedef struct SPIDevice_s {
ioTag_t sck;
ioTag_t miso;
ioTag_t mosi;
-#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4)
+#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4)
uint8_t sckAF;
uint8_t misoAF;
uint8_t mosiAF;
diff --git a/src/main/drivers/bus_spi_pinconfig.c b/src/main/drivers/bus_spi_pinconfig.c
index 961980601f..10c6d76ae2 100644
--- a/src/main/drivers/bus_spi_pinconfig.c
+++ b/src/main/drivers/bus_spi_pinconfig.c
@@ -430,6 +430,62 @@ const spiHardware_t spiHardware[] = {
.rcc = RCC_APB2(SPI4),
},
#endif
+#ifdef APM32F4
+ {
+ .device = SPIDEV_1,
+ .reg = SPI1,
+ .sckPins = {
+ { DEFIO_TAG_E(PA5), GPIO_AF5_SPI1 },
+ { DEFIO_TAG_E(PB3), GPIO_AF5_SPI1 },
+ },
+ .misoPins = {
+ { DEFIO_TAG_E(PA6), GPIO_AF5_SPI1 },
+ { DEFIO_TAG_E(PB4), GPIO_AF5_SPI1 },
+ },
+ .mosiPins = {
+ { DEFIO_TAG_E(PA7), GPIO_AF5_SPI1 },
+ { DEFIO_TAG_E(PB5), GPIO_AF5_SPI1 },
+ },
+ .rcc = RCC_APB2(SPI1),
+ .dmaIrqHandler = DMA2_ST3_HANDLER,
+ },
+ {
+ .device = SPIDEV_2,
+ .reg = SPI2,
+ .sckPins = {
+ { DEFIO_TAG_E(PB10), GPIO_AF5_SPI2 },
+ { DEFIO_TAG_E(PB13), GPIO_AF5_SPI2 },
+ },
+ .misoPins = {
+ { DEFIO_TAG_E(PC2), GPIO_AF5_SPI2 },
+ { DEFIO_TAG_E(PB14), GPIO_AF5_SPI2 },
+ },
+ .mosiPins = {
+ { DEFIO_TAG_E(PC3), GPIO_AF5_SPI2 },
+ { DEFIO_TAG_E(PB15), GPIO_AF5_SPI2 },
+ },
+ .rcc = RCC_APB1(SPI2),
+ .dmaIrqHandler = DMA1_ST4_HANDLER,
+ },
+ {
+ .device = SPIDEV_3,
+ .reg = SPI3,
+ .sckPins = {
+ { DEFIO_TAG_E(PB3), GPIO_AF6_SPI3 },
+ { DEFIO_TAG_E(PC10), GPIO_AF6_SPI3 },
+ },
+ .misoPins = {
+ { DEFIO_TAG_E(PB4), GPIO_AF6_SPI3 },
+ { DEFIO_TAG_E(PC11), GPIO_AF6_SPI3 },
+ },
+ .mosiPins = {
+ { DEFIO_TAG_E(PB5), GPIO_AF6_SPI3 },
+ { DEFIO_TAG_E(PC12), GPIO_AF6_SPI3 },
+ },
+ .rcc = RCC_APB1(SPI3),
+ .dmaIrqHandler = DMA1_ST7_HANDLER,
+ },
+#endif
};
void spiPinConfigure(const spiPinConfig_t *pConfig)
diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h
index 77c0331298..330126b3bb 100644
--- a/src/main/drivers/dma.h
+++ b/src/main/drivers/dma.h
@@ -26,6 +26,10 @@
#include "drivers/mcu/at32/dma_atbsp.h"
#endif
+#if defined(APM32F4)
+#include "drivers/mcu/apm32/dma_apm32.h"
+#endif
+
#define CACHE_LINE_SIZE 32
#define CACHE_LINE_MASK (CACHE_LINE_SIZE - 1)
@@ -45,6 +49,8 @@ typedef struct dmaResource_s dmaResource_t;
#define DMA_ARCH_TYPE DMA_Stream_TypeDef
#elif defined(AT32F435)
#define DMA_ARCH_TYPE dma_channel_type
+#elif defined(APM32F4)
+#define DMA_ARCH_TYPE DMA_Stream_TypeDef
#else
#define DMA_ARCH_TYPE DMA_Channel_TypeDef
#endif
@@ -55,7 +61,7 @@ typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channel
typedef struct dmaChannelDescriptor_s {
DMA_TypeDef* dma;
dmaResource_t *ref;
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4)
uint8_t stream;
#endif
uint32_t channel;
@@ -75,6 +81,9 @@ typedef struct dmaChannelDescriptor_s {
#if defined(USE_ATBSP_DRIVER)
+#elif defined(APM32F4)
+// dma_apm32.h
+
#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
typedef enum {
@@ -248,6 +257,9 @@ typedef enum {
#elif defined(AT32F4)
#define DMA_CCR_EN 1
#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->ctrl_bit.chen & DMA_CCR_EN)
+#elif defined(APM32F4)
+#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->SCFG & DMA_SCFGx_EN)
+#define REG_NDTR NDATA
#else
#define IS_DMA_ENABLED(reg) (((DMA_ARCH_TYPE *)(reg))->CCR & DMA_CCR_EN)
#define DMAx_SetMemoryAddress(reg, address) ((DMA_ARCH_TYPE *)(reg))->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail]
diff --git a/src/main/drivers/dshot_bitbang_impl.h b/src/main/drivers/dshot_bitbang_impl.h
index 36122f65a9..0139c24257 100644
--- a/src/main/drivers/dshot_bitbang_impl.h
+++ b/src/main/drivers/dshot_bitbang_impl.h
@@ -100,6 +100,12 @@ typedef struct dmaRegCache_s {
uint32_t CNDTR;
uint32_t CPAR;
uint32_t CMAR;
+#elif defined(APM32F4)
+ uint32_t SCFG;
+ uint32_t FCTRL;
+ uint32_t NDATA;
+ uint32_t PADDR;
+ uint32_t M0ADDR;
#else
#error No MCU dependent code here
#endif
diff --git a/src/main/drivers/dshot_dpwm.h b/src/main/drivers/dshot_dpwm.h
index c3b32f2f9d..03f606be46 100644
--- a/src/main/drivers/dshot_dpwm.h
+++ b/src/main/drivers/dshot_dpwm.h
@@ -74,7 +74,7 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1
#define DSHOT_DMA_BUFFER_ATTRIBUTE // None
#endif
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F435)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F435) || defined(APM32F4)
#define DSHOT_DMA_BUFFER_UNIT uint32_t
#else
#define DSHOT_DMA_BUFFER_UNIT uint8_t
diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h
index 574c245d1a..ba4a5f6f1e 100644
--- a/src/main/drivers/io.h
+++ b/src/main/drivers/io.h
@@ -88,6 +88,23 @@
#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_MODE_INPUT , GPIO_DRIVE_STRENGTH_MODERATE, 0, GPIO_PULL_NONE)
#define IOCFG_IPU_25 IO_CONFIG(GPIO_MODE_INPUT , GPIO_DRIVE_STRENGTH_MODERATE , 0, GPIO_PULL_UP)
+#elif defined(APM32F4)
+
+//speed is packed inside modebits 5 and 2,
+#define IO_CONFIG(mode, speed, pupd) ((mode) | ((speed) << 2) | ((pupd) << 5))
+
+#define IOCFG_OUT_PP IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_OUT_PP_UP IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP)
+#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_HIGH, GPIO_NOPULL)
+#define IOCFG_OUT_OD IO_CONFIG(GPIO_MODE_OUTPUT_OD, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_AF_PP IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_AF_PP_PD IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLDOWN)
+#define IOCFG_AF_PP_UP IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP)
+#define IOCFG_AF_OD IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_IPD IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_PULLDOWN)
+#define IOCFG_IPU IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_PULLUP)
+#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_LOW, GPIO_NOPULL)
+#define IOCFG_IPU_25 IO_CONFIG(GPIO_MODE_INPUT, GPIO_SPEED_FREQ_HIGH, GPIO_PULLUP)
#elif defined(UNIT_TEST) || defined(SIMULATOR_BUILD)
@@ -103,7 +120,7 @@
# warning "Unknown TARGET"
#endif
-#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4)
+#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4)
// Expose these for EXTIConfig
#define IO_CONFIG_GET_MODE(cfg) (((cfg) >> 0) & 0x03)
#define IO_CONFIG_GET_SPEED(cfg) (((cfg) >> 2) & 0x03)
diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h
index 9877ee0dc8..d70e410964 100644
--- a/src/main/drivers/io_impl.h
+++ b/src/main/drivers/io_impl.h
@@ -44,7 +44,7 @@ int IO_GPIOPinIdx(IO_t io);
int IO_GPIO_PinSource(IO_t io);
int IO_GPIO_PortSource(IO_t io);
-#if defined(STM32F4)
+#if defined(STM32F4) || defined(APM32F4)
int IO_EXTI_PortSourceGPIO(IO_t io);
int IO_EXTI_PinSource(IO_t io);
#endif
diff --git a/src/main/drivers/mco.c b/src/main/drivers/mco.c
index 692fc8d7b8..74d6a75d31 100644
--- a/src/main/drivers/mco.c
+++ b/src/main/drivers/mco.c
@@ -69,7 +69,7 @@ void mcoConfigure(MCODevice_e device, const mcoConfig_t *config)
IO_t io;
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(APM32F4)
// Only configure MCO2 with PLLI2SCLK as source for now.
// Other MCO1 and other sources can easily be added.
@@ -83,6 +83,9 @@ void mcoConfigure(MCODevice_e device, const mcoConfig_t *config)
#if defined(STM32F7)
HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_PLLI2SCLK, RCC_MCODIV_4);
IOConfigGPIOAF(io, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL), GPIO_AF0_MCO);
+#elif defined(APM32F4)
+ DAL_RCM_MCOConfig(RCM_MCO2, RCM_MCO2SOURCE_PLLI2SCLK, RCM_MCODIV_4);
+ IOConfigGPIOAF(io, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL), GPIO_AF0_MCO);
#else
// All F4s
RCC_MCO2Config(RCC_MCO2Source_PLLI2SCLK, RCC_MCO2Div_4);
diff --git a/src/main/drivers/mcu/apm32/adc_apm32f4xx.c b/src/main/drivers/mcu/apm32/adc_apm32f4xx.c
new file mode 100644
index 0000000000..619a537213
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/adc_apm32f4xx.c
@@ -0,0 +1,360 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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_ADC
+
+#include "build/debug.h"
+
+#include "drivers/dma_reqmap.h"
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "drivers/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 = ADC1,
+ .rccADC = RCC_APB2(ADC1),
+#if !defined(USE_DMA_SPEC)
+ .dmaResource = (dmaResource_t *)ADC1_DMA_STREAM,
+ .channel = DMA_Channel_0
+#endif
+ },
+ {
+ .ADCx = ADC2,
+ .rccADC = RCC_APB2(ADC2),
+#if !defined(USE_DMA_SPEC)
+ .dmaResource = (dmaResource_t *)ADC2_DMA_STREAM,
+ .channel = DMA_Channel_1
+#endif
+ },
+ {
+ .ADCx = ADC3,
+ .rccADC = RCC_APB2(ADC3),
+#if !defined(USE_DMA_SPEC)
+ .dmaResource = (dmaResource_t *)ADC3_DMA_STREAM,
+ .channel = DMA_Channel_2
+#endif
+ }
+};
+
+/* note these could be packed up for saving space */
+const adcTagMap_t adcTagMap[] = {
+/*
+ { DEFIO_TAG_E__PF3, ADC_DEVICES_3, ADC_CHANNEL_9 },
+ { DEFIO_TAG_E__PF4, ADC_DEVICES_3, ADC_CHANNEL_14 },
+ { DEFIO_TAG_E__PF5, ADC_DEVICES_3, ADC_CHANNEL_15 },
+ { DEFIO_TAG_E__PF6, ADC_DEVICES_3, ADC_CHANNEL_4 },
+ { DEFIO_TAG_E__PF7, ADC_DEVICES_3, ADC_CHANNEL_5 },
+ { DEFIO_TAG_E__PF8, ADC_DEVICES_3, ADC_CHANNEL_6 },
+ { DEFIO_TAG_E__PF9, ADC_DEVICES_3, ADC_CHANNEL_7 },
+ { DEFIO_TAG_E__PF10,ADC_DEVICES_3, ADC_CHANNEL_8 },
+*/
+ { DEFIO_TAG_E__PC0, ADC_DEVICES_123, ADC_CHANNEL_10 },
+ { DEFIO_TAG_E__PC1, ADC_DEVICES_123, ADC_CHANNEL_11 },
+ { DEFIO_TAG_E__PC2, ADC_DEVICES_123, ADC_CHANNEL_12 },
+ { DEFIO_TAG_E__PC3, ADC_DEVICES_123, ADC_CHANNEL_13 },
+ { DEFIO_TAG_E__PC4, ADC_DEVICES_12, ADC_CHANNEL_14 },
+ { DEFIO_TAG_E__PC5, ADC_DEVICES_12, ADC_CHANNEL_15 },
+ { DEFIO_TAG_E__PB0, ADC_DEVICES_12, ADC_CHANNEL_8 },
+ { DEFIO_TAG_E__PB1, ADC_DEVICES_12, ADC_CHANNEL_9 },
+ { DEFIO_TAG_E__PA0, ADC_DEVICES_123, ADC_CHANNEL_0 },
+ { DEFIO_TAG_E__PA1, ADC_DEVICES_123, ADC_CHANNEL_1 },
+ { DEFIO_TAG_E__PA2, ADC_DEVICES_123, ADC_CHANNEL_2 },
+ { DEFIO_TAG_E__PA3, ADC_DEVICES_123, ADC_CHANNEL_3 },
+ { DEFIO_TAG_E__PA4, ADC_DEVICES_12, ADC_CHANNEL_4 },
+ { DEFIO_TAG_E__PA5, ADC_DEVICES_12, ADC_CHANNEL_5 },
+ { DEFIO_TAG_E__PA6, ADC_DEVICES_12, ADC_CHANNEL_6 },
+ { DEFIO_TAG_E__PA7, ADC_DEVICES_12, ADC_CHANNEL_7 },
+};
+
+void adcInitDevice(adcDevice_t *adcdev, int channelCount)
+{
+ ADC_HandleTypeDef *hadc = &adcdev->ADCHandle;
+
+ hadc->Instance = adcdev->ADCx;
+
+ hadc->Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8;
+ hadc->Init.Resolution = ADC_RESOLUTION_12B;
+ hadc->Init.DataAlign = ADC_DATAALIGN_RIGHT;
+ hadc->Init.EOCSelection = ADC_EOC_SEQ_CONV;
+ hadc->Init.ContinuousConvMode = ENABLE;
+ hadc->Init.DiscontinuousConvMode = DISABLE;
+ hadc->Init.ExternalTrigConv = ADC_SOFTWARE_START;
+ hadc->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+ hadc->Init.DMAContinuousRequests = ENABLE;
+ hadc->Init.NbrOfConversion = 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
+ hadc->Init.ScanConvMode = ENABLE;
+#else
+ hadc->Init.ScanConvMode = channelCount > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
+#endif
+ if (DAL_ADC_Init(hadc) != DAL_OK) {
+ /* Initialization Error */
+ }
+}
+
+static adcDevice_t adc;
+
+#ifdef USE_ADC_INTERNAL
+
+static adcDevice_t adcInternal;
+static ADC_HandleTypeDef *adcInternalHandle;
+
+void adcInitInternalInjected(adcDevice_t *adcdev)
+{
+ adcInternalHandle = &adcdev->ADCHandle;
+
+ ADC_InjectionConfTypeDef iConfig;
+
+ iConfig.InjectedSamplingTime = ADC_SAMPLETIME_480CYCLES;
+ iConfig.InjectedOffset = 0;
+ iConfig.InjectedNbrOfConversion = 2;
+ iConfig.InjectedDiscontinuousConvMode = DISABLE;
+ iConfig.AutoInjectedConv = DISABLE;
+ iConfig.ExternalTrigInjecConv = 0; // Don't care
+ iConfig.ExternalTrigInjecConvEdge = 0; // Don't care
+
+ iConfig.InjectedChannel = ADC_CHANNEL_VREFINT;
+ iConfig.InjectedRank = 1;
+
+ if (DAL_ADCEx_InjectedConfigChannel(adcInternalHandle, &iConfig) != DAL_OK) {
+ /* Channel Configuration Error */
+ }
+
+ iConfig.InjectedChannel = ADC_CHANNEL_TEMPSENSOR;
+ iConfig.InjectedRank = 2;
+
+ if (DAL_ADCEx_InjectedConfigChannel(adcInternalHandle, &iConfig) != DAL_OK) {
+ /* Channel Configuration Error */
+ }
+
+ adcVREFINTCAL = *(uint16_t *)VREFINT_CAL_ADDR;
+ adcTSCAL1 = *(uint16_t *)TEMPSENSOR_CAL1_ADDR;
+ adcTSCAL2 = *(uint16_t *)TEMPSENSOR_CAL2_ADDR;
+ adcTSSlopeK = (TEMPSENSOR_CAL2_TEMP - TEMPSENSOR_CAL1_TEMP) * 1000 / (adcTSCAL2 - adcTSCAL1);
+}
+
+// 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 (DAL_ADCEx_InjectedPollForConversion(adcInternalHandle, 0) == DAL_OK) {
+ adcInternalConversionInProgress = false;
+ }
+ }
+
+ return adcInternalConversionInProgress;
+}
+
+void adcInternalStartConversion(void)
+{
+ DAL_ADCEx_InjectedStart(adcInternalHandle);
+
+ adcInternalConversionInProgress = true;
+}
+
+uint16_t adcInternalReadVrefint(void)
+{
+ return DAL_ADCEx_InjectedGetValue(adcInternalHandle, ADC_INJECTED_RANK_1);
+}
+
+uint16_t adcInternalReadTempsensor(void)
+{
+ return DAL_ADCEx_InjectedGetValue(adcInternalHandle, ADC_INJECTED_RANK_2);
+}
+#endif // USE_ADC_INTERNAL
+
+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;
+ }
+
+ 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_NOPULL));
+ adcOperatingConfig[i].adcChannel = adcChannelByTag(adcOperatingConfig[i].tag);
+ adcOperatingConfig[i].dmaIndex = configuredAdcChannels++;
+ adcOperatingConfig[i].sampleTime = ADC_SAMPLETIME_480CYCLES;
+ adcOperatingConfig[i].enabled = true;
+ }
+
+#ifndef USE_ADC_INTERNAL
+ if (!adcActive) {
+ return;
+ }
+#endif // USE_ADC_INTERNAL
+
+ RCC_ClockCmd(adc.rccADC, ENABLE);
+
+ adcInitDevice(&adc, configuredAdcChannels);
+
+#ifdef USE_ADC_INTERNAL
+ // If device is not ADC1 or there's no active channel, then initialize ADC1 separately
+ if (device != ADCDEV_1 || !adcActive) {
+ adcInternal = adcHardware[ADCDEV_1];
+ RCC_ClockCmd(adcInternal.rccADC, ENABLE);
+ adcInitDevice(&adcInternal, 2);
+ DDL_ADC_Enable(adcInternal.ADCx);
+ adcInitInternalInjected(&adcInternal);
+ }
+ else {
+ // Initialize for injected conversion
+ adcInitInternalInjected(&adc);
+ }
+
+ if (!adcActive) {
+ return;
+ }
+#endif // USE_ADC_INTERNAL
+
+ uint8_t rank = 1;
+ for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
+ if (!adcOperatingConfig[i].enabled) {
+ continue;
+ }
+ ADC_ChannelConfTypeDef sConfig;
+
+ sConfig.Channel = adcOperatingConfig[i].adcChannel;
+ sConfig.Rank = rank++;
+ sConfig.SamplingTime = adcOperatingConfig[i].sampleTime;
+ sConfig.Offset = 0;
+
+ if (DAL_ADC_ConfigChannel(&adc.ADCHandle, &sConfig) != DAL_OK)
+ {
+ /* Channel Configuration Error */
+ }
+ }
+ DDL_ADC_REG_SetDMATransfer(adc.ADCx, DDL_ADC_REG_DMA_TRANSFER_UNLIMITED);
+ DDL_ADC_Enable(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));
+
+ xLL_EX_DMA_DeInit(dmaSpec->ref);
+#else
+ if (!dmaAllocate(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0)) {
+ return;
+ }
+
+ dmaEnable(dmaGetIdentifier(adc.dmaResource));
+
+ xLL_EX_DMA_DeInit(adc.dmaResource);
+#endif
+
+#ifdef USE_DMA_SPEC
+ adc.DmaHandle.Init.Channel = dmaSpec->channel;
+ adc.DmaHandle.Instance = (DMA_ARCH_TYPE *)dmaSpec->ref;
+#else
+ adc.DmaHandle.Init.Channel = adc.channel;
+ adc.DmaHandle.Instance = (DMA_ARCH_TYPE *)adc.dmaResource;
+#endif
+
+ adc.DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
+ adc.DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+ adc.DmaHandle.Init.MemInc = configuredAdcChannels > 1 ? DMA_MINC_ENABLE : DMA_MINC_DISABLE;
+ adc.DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+ adc.DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+ adc.DmaHandle.Init.Mode = DMA_CIRCULAR;
+ adc.DmaHandle.Init.Priority = DMA_PRIORITY_HIGH;
+ adc.DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ adc.DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+ adc.DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+ adc.DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+
+ if (DAL_DMA_Init(&adc.DmaHandle) != DAL_OK)
+ {
+ /* Initialization Error */
+ }
+
+ __DAL_LINKDMA(&adc.ADCHandle, DMA_Handle, adc.DmaHandle);
+
+ if (DAL_ADC_Start_DMA(&adc.ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != DAL_OK)
+ {
+ /* Start Conversion Error */
+ }
+}
+
+void adcGetChannelValues(void)
+{
+ // Nothing to do
+}
+#endif // USE_ADC
diff --git a/src/main/drivers/mcu/apm32/apm32f4xx_ddl_ex.h b/src/main/drivers/mcu/apm32/apm32f4xx_ddl_ex.h
new file mode 100644
index 0000000000..da6f28a284
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/apm32f4xx_ddl_ex.h
@@ -0,0 +1,96 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "apm32f4xx.h"
+#include "common/utils.h"
+
+// XXX Followings are straight copy of stm32f7xx_ll_ex.h.
+// XXX Consider consolidation when LL-DShot is stable.
+
+__STATIC_INLINE void DDL_EX_TMR_DisableIT(TMR_TypeDef *TMRx, uint32_t Sources)
+{
+ CLEAR_BIT(TMRx->DIEN, Sources);
+}
+__STATIC_INLINE void DDL_EX_TMR_EnableIT(TMR_TypeDef *TMRx, uint32_t Sources)
+{
+ SET_BIT(TMRx->DIEN, Sources);
+}
+__STATIC_INLINE uint32_t DDL_EX_DMA_Stream_to_Stream(DMA_Stream_TypeDef *DMAx_Streamy)
+{
+ STATIC_ASSERT(DMA1_Stream0_BASE - DMA1_BASE == sizeof(DMA_TypeDef), DMA_TypeDef_has_padding);
+ STATIC_ASSERT(DMA1_Stream1_BASE - DMA1_Stream0_BASE == sizeof(DMA_Stream_TypeDef), DMA_Stream_TypeDef_has_padding);
+
+ const size_t firstStreamOffset = sizeof(DMA_TypeDef);
+ const size_t streamSize = sizeof(DMA_Stream_TypeDef);
+
+ return (((uint32_t) DMAx_Streamy & 0XFF) - firstStreamOffset) / streamSize;
+}
+__STATIC_INLINE DMA_TypeDef *DDL_EX_DMA_Stream_to_DMA(DMA_Stream_TypeDef *DMAx_Streamy)
+{
+ // clear stream address
+ return (DMA_TypeDef *) (((uint32_t) DMAx_Streamy) & ((uint32_t) ~0XFF));
+}
+__STATIC_INLINE uint32_t DDL_EX_DMA_DeInit(DMA_Stream_TypeDef *DMAx_Streamy)
+{
+ DMA_TypeDef *DMA = DDL_EX_DMA_Stream_to_DMA(DMAx_Streamy);
+ const uint32_t Stream = DDL_EX_DMA_Stream_to_Stream(DMAx_Streamy);
+
+ return DDL_DMA_DeInit(DMA, Stream);
+}
+__STATIC_INLINE uint32_t DDL_EX_DMA_Init(DMA_Stream_TypeDef *DMAx_Streamy, DDL_DMA_InitTypeDef *DMA_InitStruct)
+{
+ DMA_TypeDef *DMA = DDL_EX_DMA_Stream_to_DMA(DMAx_Streamy);
+ const uint32_t Stream = DDL_EX_DMA_Stream_to_Stream(DMAx_Streamy);
+
+ return DDL_DMA_Init(DMA, Stream, DMA_InitStruct);
+}
+__STATIC_INLINE void DDL_EX_DMA_SetDataLength(DMA_Stream_TypeDef* DMAx_Streamy, uint32_t NbData)
+{
+ MODIFY_REG(DMAx_Streamy->NDATA, DMA_NDATAx, NbData);
+}
+
+__STATIC_INLINE uint32_t DDL_EX_DMA_GetDataLength(DMA_Stream_TypeDef* DMAx_Streamy)
+{
+ DMA_TypeDef *DMA = DDL_EX_DMA_Stream_to_DMA(DMAx_Streamy);
+ const uint32_t Stream = DDL_EX_DMA_Stream_to_Stream(DMAx_Streamy);
+
+ return DDL_DMA_GetDataLength(DMA, Stream);
+}
+
+__STATIC_INLINE void DDL_EX_DMA_EnableResource(DMA_Stream_TypeDef *DMAx_Streamy)
+{
+ SET_BIT(DMAx_Streamy->SCFG, DMA_SCFGx_EN);
+}
+__STATIC_INLINE void DDL_EX_DMA_DisableResource(DMA_Stream_TypeDef *DMAx_Streamy)
+{
+ CLEAR_BIT(DMAx_Streamy->SCFG, DMA_SCFGx_EN);
+}
+
+__STATIC_INLINE void DDL_EX_DMA_EnableIT_TC(DMA_Stream_TypeDef *DMAx_Streamy)
+{
+ SET_BIT(DMAx_Streamy->SCFG, DMA_SCFGx_TXCIEN);
+}
+
+__STATIC_INLINE void DDL_EX_TMR_CC_EnableNChannel(TMR_TypeDef *TMRx, uint32_t Channel)
+{
+ DDL_TMR_CC_EnableChannel(TMRx, 4 * Channel);
+}
\ No newline at end of file
diff --git a/src/main/drivers/mcu/apm32/bus_i2c_apm32.c b/src/main/drivers/mcu/apm32/bus_i2c_apm32.c
new file mode 100644
index 0000000000..338ded478d
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/bus_i2c_apm32.c
@@ -0,0 +1,221 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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"
+
+#if defined(USE_I2C) && !defined(SOFT_I2C)
+
+#include "drivers/io.h"
+#include "drivers/io_impl.h"
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+#include "drivers/rcc.h"
+
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_impl.h"
+
+#ifdef USE_I2C_DEVICE_1
+void I2C1_ER_IRQHandler(void)
+{
+ DAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_1].handle);
+}
+
+void I2C1_EV_IRQHandler(void)
+{
+ DAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_1].handle);
+}
+#endif
+
+#ifdef USE_I2C_DEVICE_2
+void I2C2_ER_IRQHandler(void)
+{
+ DAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_2].handle);
+}
+
+void I2C2_EV_IRQHandler(void)
+{
+ DAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_2].handle);
+}
+#endif
+
+#ifdef USE_I2C_DEVICE_3
+void I2C3_ER_IRQHandler(void)
+{
+ DAL_I2C_ER_IRQHandler(&i2cDevice[I2CDEV_3].handle);
+}
+
+void I2C3_EV_IRQHandler(void)
+{
+ DAL_I2C_EV_IRQHandler(&i2cDevice[I2CDEV_3].handle);
+}
+#endif
+
+static volatile uint16_t i2cErrorCount = 0;
+
+static bool i2cHandleHardwareFailure(I2CDevice device)
+{
+ (void)device;
+ i2cErrorCount++;
+ // reinit peripheral + clock out garbage
+ //i2cInit(device);
+ return false;
+}
+
+uint16_t i2cGetErrorCounter(void)
+{
+ return i2cErrorCount;
+}
+
+// Blocking write
+bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
+{
+ if (device == I2CINVALID || device >= I2CDEV_COUNT) {
+ return false;
+ }
+
+ I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle;
+
+ if (!pHandle->Instance) {
+ return false;
+ }
+
+ HAL_StatusTypeDef status;
+
+ if (reg_ == 0xFF)
+ status = DAL_I2C_Master_Transmit(pHandle ,addr_ << 1, &data, 1, I2C_TIMEOUT_SYS_TICKS);
+ else
+ status = DAL_I2C_Mem_Write(pHandle ,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT, &data, 1, I2C_TIMEOUT_SYS_TICKS);
+
+ if (status != DAL_OK)
+ return i2cHandleHardwareFailure(device);
+
+ return true;
+}
+
+// Non-blocking write
+bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
+{
+ if (device == I2CINVALID || device >= I2CDEV_COUNT) {
+ return false;
+ }
+
+ I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle;
+
+ if (!pHandle->Instance) {
+ return false;
+ }
+
+ DAL_StatusTypeDef status;
+
+ status = DAL_I2C_Mem_Write_IT(pHandle ,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_);
+
+ if (status == DAL_BUSY) {
+ return false;
+ }
+
+ if (status != DAL_OK)
+ {
+ return i2cHandleHardwareFailure(device);
+ }
+
+ return true;
+}
+
+// Blocking read
+bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
+{
+ if (device == I2CINVALID || device >= I2CDEV_COUNT) {
+ return false;
+ }
+
+ I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle;
+
+ if (!pHandle->Instance) {
+ return false;
+ }
+
+ DAL_StatusTypeDef status;
+
+ if (reg_ == 0xFF)
+ status = DAL_I2C_Master_Receive(pHandle ,addr_ << 1, buf, len, I2C_TIMEOUT_SYS_TICKS);
+ else
+ status = DAL_I2C_Mem_Read(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_TIMEOUT_SYS_TICKS);
+
+ if (status != DAL_OK) {
+ return i2cHandleHardwareFailure(device);
+ }
+
+ return true;
+}
+
+// Non-blocking read
+bool i2cReadBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
+{
+ if (device == I2CINVALID || device >= I2CDEV_COUNT) {
+ return false;
+ }
+
+ I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle;
+
+ if (!pHandle->Instance) {
+ return false;
+ }
+
+ DAL_StatusTypeDef status;
+
+ status = DAL_I2C_Mem_Read_IT(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT, buf, len);
+
+ if (status == DAL_BUSY) {
+ return false;
+ }
+
+ if (status != DAL_OK) {
+ return i2cHandleHardwareFailure(device);
+ }
+
+ return true;
+}
+
+bool i2cBusy(I2CDevice device, bool *error)
+{
+ I2C_HandleTypeDef *pHandle = &i2cDevice[device].handle;
+
+ if (error) {
+ *error = pHandle->ErrorCode;
+ }
+
+ if (pHandle->State == DAL_I2C_STATE_READY)
+ {
+ if (__DAL_I2C_GET_FLAG(pHandle, I2C_FLAG_BUSY) == SET)
+ {
+ return true;
+ }
+
+ return false;
+ }
+
+ return true;
+}
+
+#endif
diff --git a/src/main/drivers/mcu/apm32/bus_i2c_apm32_init.c b/src/main/drivers/mcu/apm32/bus_i2c_apm32_init.c
new file mode 100644
index 0000000000..9db60f71ab
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/bus_i2c_apm32_init.c
@@ -0,0 +1,136 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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"
+
+#if defined(USE_I2C) && !defined(SOFT_I2C)
+
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+#include "drivers/rcc.h"
+
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_impl.h"
+#include "drivers/bus_i2c_timing.h"
+#include "drivers/bus_i2c_utils.h"
+
+#define IOCFG_I2C_PU IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
+#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
+
+const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
+#ifdef USE_I2C_DEVICE_1
+ {
+ .device = I2CDEV_1,
+ .reg = I2C1,
+ .sclPins = { I2CPINDEF(PB6, GPIO_AF4_I2C1), I2CPINDEF(PB8, GPIO_AF4_I2C1) },
+ .sdaPins = { I2CPINDEF(PB7, GPIO_AF4_I2C1), I2CPINDEF(PB9, GPIO_AF4_I2C1) },
+ .rcc = RCC_APB1(I2C1),
+ .ev_irq = I2C1_EV_IRQn,
+ .er_irq = I2C1_ER_IRQn,
+ },
+#endif
+#ifdef USE_I2C_DEVICE_2
+ {
+ .device = I2CDEV_2,
+ .reg = I2C2,
+ .sclPins = { I2CPINDEF(PB10, GPIO_AF4_I2C2), I2CPINDEF(PF1, GPIO_AF4_I2C2) },
+ .sdaPins = { I2CPINDEF(PB11, GPIO_AF4_I2C2), I2CPINDEF(PF0, GPIO_AF4_I2C2) },
+ .rcc = RCC_APB1(I2C2),
+ .ev_irq = I2C2_EV_IRQn,
+ .er_irq = I2C2_ER_IRQn,
+ },
+#endif
+#ifdef USE_I2C_DEVICE_3
+ {
+ .device = I2CDEV_3,
+ .reg = I2C3,
+ .sclPins = { I2CPINDEF(PA8, GPIO_AF4_I2C3) },
+ .sdaPins = { I2CPINDEF(PC9, GPIO_AF4_I2C3) },
+ .rcc = RCC_APB1(I2C3),
+ .ev_irq = I2C3_EV_IRQn,
+ .er_irq = I2C3_ER_IRQn,
+ },
+#endif
+};
+
+i2cDevice_t i2cDevice[I2CDEV_COUNT];
+
+void i2cInit(I2CDevice device)
+{
+ if (device == I2CINVALID) {
+ return;
+ }
+
+ i2cDevice_t *pDev = &i2cDevice[device];
+
+ const i2cHardware_t *hardware = pDev->hardware;
+ const IO_t scl = pDev->scl;
+ const IO_t sda = pDev->sda;
+
+ if (!hardware || IOGetOwner(scl) || IOGetOwner(sda)) {
+ return;
+ }
+
+ IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device));
+ IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device));
+
+ // Enable RCC
+ RCC_ClockCmd(hardware->rcc, ENABLE);
+
+ i2cUnstick(scl, sda);
+
+ // Init pins
+ IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sclAF);
+ IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sdaAF);
+
+ // Init I2C peripheral
+
+ I2C_HandleTypeDef *pHandle = &pDev->handle;
+
+ memset(pHandle, 0, sizeof(*pHandle));
+
+ pHandle->Instance = pDev->hardware->reg;
+
+ // Compute TIMINGR value based on peripheral clock for this device instance
+
+ pHandle->Init.ClockSpeed = pDev->clockSpeed * 1000;
+
+ pHandle->Init.OwnAddress1 = 0x0;
+ pHandle->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+ pHandle->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+ pHandle->Init.OwnAddress2 = 0x0;
+ pHandle->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+ pHandle->Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+
+ DAL_I2C_Init(pHandle);
+
+ // Setup interrupt handlers
+ DAL_NVIC_SetPriority(hardware->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
+ DAL_NVIC_EnableIRQ(hardware->er_irq);
+ DAL_NVIC_SetPriority(hardware->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
+ DAL_NVIC_EnableIRQ(hardware->ev_irq);
+}
+
+#endif
diff --git a/src/main/drivers/mcu/apm32/bus_spi_apm32.c b/src/main/drivers/mcu/apm32/bus_spi_apm32.c
new file mode 100644
index 0000000000..82c2a01d6b
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/bus_spi_apm32.c
@@ -0,0 +1,450 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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"
+
+#if defined(USE_SPI)
+
+#include "common/utils.h"
+#include "common/maths.h"
+
+#include "drivers/bus.h"
+#include "drivers/bus_spi.h"
+#include "drivers/bus_spi_impl.h"
+#include "drivers/dma.h"
+#include "drivers/io.h"
+#include "drivers/rcc.h"
+
+// Use DMA if possible if this many bytes are to be transferred
+#define SPI_DMA_THRESHOLD 8
+
+// APM32F405 can't DMA to/from FASTRAM (CCM SRAM)
+#define IS_CCM(p) (((uint32_t)p & 0xffff0000) == 0x10000000)
+
+static DDL_SPI_InitTypeDef defaultInit =
+{
+ .TransferDirection = DDL_SPI_FULL_DUPLEX,
+ .Mode = DDL_SPI_MODE_MASTER,
+ .DataWidth = DDL_SPI_DATAWIDTH_8BIT,
+ .NSS = DDL_SPI_NSS_SOFT,
+ .BaudRate = DDL_SPI_BAUDRATEPRESCALER_DIV8,
+ .BitOrder = DDL_SPI_MSB_FIRST,
+ .CRCCalculation = DDL_SPI_CRCCALCULATION_DISABLE,
+ .ClockPolarity = DDL_SPI_POLARITY_HIGH,
+ .ClockPhase = DDL_SPI_PHASE_2EDGE,
+};
+
+static uint32_t spiDivisorToBRbits(const SPI_TypeDef *instance, uint16_t divisor)
+{
+ // SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2.
+ if (instance == SPI2 || instance == SPI3) {
+ divisor /= 2; // Safe for divisor == 0 or 1
+ }
+
+ divisor = constrain(divisor, 2, 256);
+
+ return (ffs(divisor) - 2) << 3;// SPI_CR1_BR_Pos
+}
+
+void spiInitDevice(SPIDevice device)
+{
+ spiDevice_t *spi = &spiDevice[device];
+
+ if (!spi->dev) {
+ return;
+ }
+
+ // Enable SPI clock
+ RCC_ClockCmd(spi->rcc, ENABLE);
+ RCC_ResetCmd(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->miso), SPI_IO_AF_SDI_CFG, spi->misoAF);
+ IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF);
+ IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
+
+ DDL_SPI_Disable(spi->dev);
+ DDL_SPI_DeInit(spi->dev);
+
+ DDL_SPI_DisableDMAReq_RX(spi->dev);
+ DDL_SPI_DisableDMAReq_TX(spi->dev);
+
+ DDL_SPI_Init(spi->dev, &defaultInit);
+ DDL_SPI_Enable(spi->dev);
+}
+
+void spiInternalResetDescriptors(busDevice_t *bus)
+{
+ DDL_DMA_InitTypeDef *initTx = bus->initTx;
+
+ DDL_DMA_StructInit(initTx);
+
+ initTx->Channel = bus->dmaTx->channel;
+ initTx->Mode = DDL_DMA_MODE_NORMAL;
+ initTx->Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH;
+ initTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DATA;
+ initTx->Priority = DDL_DMA_PRIORITY_LOW;
+ initTx->PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT;
+ initTx->PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_BYTE;
+ initTx->MemoryOrM2MDstDataSize = DDL_DMA_MDATAALIGN_BYTE;
+
+ if (bus->dmaRx) {
+ DDL_DMA_InitTypeDef *initRx = bus->initRx;
+
+ DDL_DMA_StructInit(initRx);
+
+ initRx->Channel = bus->dmaRx->channel;
+ initRx->Mode = DDL_DMA_MODE_NORMAL;
+ initRx->Direction = DDL_DMA_DIRECTION_PERIPH_TO_MEMORY;
+ initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DATA;
+ initRx->Priority = DDL_DMA_PRIORITY_LOW;
+ initRx->PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT;
+ initRx->PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_BYTE;
+ }
+}
+
+void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
+{
+ // Disable the stream
+ DDL_DMA_DisableStream(descriptor->dma, descriptor->stream);
+ while (DDL_DMA_IsEnabledStream(descriptor->dma, descriptor->stream));
+
+ // Clear any pending interrupt flags
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+}
+
+
+FAST_CODE static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
+{
+ while (len) {
+ while (!DDL_SPI_IsActiveFlag_TXE(instance));
+ uint8_t b = txData ? *(txData++) : 0xFF;
+ DDL_SPI_TransmitData8(instance, b);
+
+ while (!DDL_SPI_IsActiveFlag_RXNE(instance));
+ b = DDL_SPI_ReceiveData8(instance);
+ if (rxData) {
+ *(rxData++) = b;
+ }
+ --len;
+ }
+
+ 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;
+
+ busSegment_t *segment = (busSegment_t *)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;
+ DDL_DMA_InitTypeDef *initTx = bus->initTx;
+
+ if (txData) {
+ initTx->MemoryOrM2MDstAddress = (uint32_t)txData;
+ initTx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT;
+ } else {
+ dummyTxByte = 0xff;
+ initTx->MemoryOrM2MDstAddress = (uint32_t)&dummyTxByte;
+ initTx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_NOINCREMENT;
+ }
+ initTx->NbData = len;
+
+ if (dev->bus->dmaRx) {
+ uint8_t *rxData = segment->u.buffers.rxData;
+ DDL_DMA_InitTypeDef *initRx = bus->initRx;
+
+ if (rxData) {
+ /* Flush the D cache for the start and end of the receive buffer as
+ * the cache will be invalidated after the transfer and any valid data
+ * just before/after must be in memory at that point
+ */
+ initRx->MemoryOrM2MDstAddress = (uint32_t)rxData;
+ initRx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT;
+ } else {
+ initRx->MemoryOrM2MDstAddress = (uint32_t)&dummyRxByte;
+ initRx->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_NOINCREMENT;
+ }
+ initRx->NbData = len;
+ }
+}
+
+void spiInternalStartDMA(const extDevice_t *dev)
+{
+ busDevice_t *bus = dev->bus;
+
+ dmaChannelDescriptor_t *dmaTx = bus->dmaTx;
+ dmaChannelDescriptor_t *dmaRx = 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_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+ DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+
+ // Disable streams to enable update
+ DDL_DMA_WriteReg(streamRegsTx, SCFG, 0U);
+ DDL_DMA_WriteReg(streamRegsRx, SCFG, 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
+ */
+ DDL_EX_DMA_EnableIT_TC(streamRegsRx);
+
+ // Update streams
+ DDL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
+ DDL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx);
+
+ /* Note from AN4031
+ *
+ * If the user enables the used peripheral before the corresponding DMA stream, a FEIF
+ * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
+ * the first required data to the peripheral (in case of memory-to-peripheral transfer).
+ */
+
+ // Enable the SPI DMA Tx & Rx requests
+
+ // Enable streams
+ DDL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
+ DDL_DMA_EnableStream(dmaRx->dma, dmaRx->stream);
+
+ SET_BIT(dev->bus->busType_u.spi.instance->CTRL2, SPI_CTRL2_TXDEN | SPI_CTRL2_RXDEN);
+ } else {
+ // Use the correct callback argument
+ dmaTx->userParam = (uint32_t)dev;
+
+ // Clear transfer flags
+ DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+
+ // Disable streams to enable update
+ DDL_DMA_WriteReg(streamRegsTx, SCFG, 0U);
+
+ DDL_EX_DMA_EnableIT_TC(streamRegsTx);
+
+ // Update streams
+ DDL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
+
+ /* Note from AN4031
+ *
+ * If the user enables the used peripheral before the corresponding DMA stream, a FEIF
+ * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
+ * the first required data to the peripheral (in case of memory-to-peripheral transfer).
+ */
+
+ // Enable the SPI DMA Tx request
+ // Enable streams
+ DDL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
+
+ SET_BIT(dev->bus->busType_u.spi.instance->CTRL2, SPI_CTRL2_TXDEN);
+ }
+}
+
+void spiInternalStopDMA (const extDevice_t *dev)
+{
+ busDevice_t *bus = dev->bus;
+
+ dmaChannelDescriptor_t *dmaTx = bus->dmaTx;
+ dmaChannelDescriptor_t *dmaRx = bus->dmaRx;
+ SPI_TypeDef *instance = bus->busType_u.spi.instance;
+
+ if (dmaRx) {
+ // Disable the DMA engine and SPI interface
+ DDL_DMA_DisableStream(dmaRx->dma, dmaRx->stream);
+ DDL_DMA_DisableStream(dmaTx->dma, dmaTx->stream);
+
+ // Clear transfer flags
+ DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+
+ DDL_SPI_DisableDMAReq_TX(instance);
+ DDL_SPI_DisableDMAReq_RX(instance);
+ } else {
+ SPI_TypeDef *instance = bus->busType_u.spi.instance;
+
+ // Ensure the current transmission is complete
+ while (DDL_SPI_IsActiveFlag_BSY(instance));
+
+ // Drain the RX buffer
+ while (DDL_SPI_IsActiveFlag_RXNE(instance)) {
+ instance->DATA;
+ }
+
+ // Disable the DMA engine and SPI interface
+ DDL_DMA_DisableStream(dmaTx->dma, dmaTx->stream);
+
+ DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+
+ DDL_SPI_DisableDMAReq_TX(instance);
+ }
+}
+
+// DMA transfer setup and start
+FAST_CODE void spiSequenceStart(const extDevice_t *dev)
+{
+ busDevice_t *bus = dev->bus;
+ SPI_TypeDef *instance = bus->busType_u.spi.instance;
+ spiDevice_t *spi = &spiDevice[spiDeviceByInstance(instance)];
+ bool dmaSafe = dev->useDMA;
+ uint32_t xferLen = 0;
+ uint32_t segmentCount = 0;
+
+ bus->initSegment = true;
+
+ DDL_SPI_Disable(instance);
+
+ // Switch bus speed
+ if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) {
+ DDL_SPI_SetBaudRatePrescaler(instance, spiDivisorToBRbits(instance, dev->busType_u.spi.speed));
+ bus->busType_u.spi.speed = dev->busType_u.spi.speed;
+ }
+
+ // Switch SPI clock polarity/phase if necessary
+ if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) {
+ if (dev->busType_u.spi.leadingEdge) {
+ IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF);
+ DDL_SPI_SetClockPhase(instance, DDL_SPI_PHASE_1EDGE);
+ DDL_SPI_SetClockPolarity(instance, DDL_SPI_POLARITY_LOW);
+ }
+ else {
+ IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
+ DDL_SPI_SetClockPhase(instance, DDL_SPI_PHASE_2EDGE);
+ DDL_SPI_SetClockPolarity(instance, DDL_SPI_POLARITY_HIGH);
+ }
+
+ bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge;
+ }
+
+ DDL_SPI_Enable(instance);
+
+ // Check that any reads are cache aligned and of multiple cache lines in length
+ 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_CCM(checkSegment->u.buffers.rxData) || (bus->dmaRx == (dmaChannelDescriptor_t *)NULL))) ||
+ ((checkSegment->u.buffers.txData) && IS_CCM(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)) {
+ // Intialise the init structures for the first transfer
+ spiInternalInitStream(dev, false);
+
+ // Assert Chip Select
+ IOLo(dev->busType_u.spi.csnPin);
+
+ // Start the transfers
+ spiInternalStartDMA(dev);
+ } else {
+ busSegment_t *lastSegment = NULL;
+ bool segmentComplete;
+
+ // Manually work through the segment list performing a transfer for each
+ while (bus->curSegment->len) {
+ if (!lastSegment || lastSegment->negateCS) {
+ // Assert Chip Select if necessary - it's costly so only do so if necessary
+ IOLo(dev->busType_u.spi.csnPin);
+ }
+
+ spiInternalReadWriteBufPolled(
+ bus->busType_u.spi.instance,
+ bus->curSegment->u.buffers.txData,
+ bus->curSegment->u.buffers.rxData,
+ bus->curSegment->len);
+
+ if (bus->curSegment->negateCS) {
+ // Negate Chip Select
+ IOHi(dev->busType_u.spi.csnPin);
+ }
+
+ segmentComplete = true;
+ if (bus->curSegment->callback) {
+ switch(bus->curSegment->callback(dev->callbackArg)) {
+ case BUS_BUSY:
+ // Repeat the last DMA segment
+ segmentComplete = false;
+ break;
+
+ case BUS_ABORT:
+ bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
+ segmentComplete = false;
+ return;
+
+ case BUS_READY:
+ default:
+ // Advance to the next DMA segment
+ break;
+ }
+ }
+ if (segmentComplete) {
+ lastSegment = (busSegment_t *)bus->curSegment;
+ bus->curSegment++;
+ }
+ }
+
+ // If a following transaction has been linked, start it
+ if (bus->curSegment->u.link.dev) {
+ busSegment_t *endSegment = (busSegment_t *)bus->curSegment;
+ const extDevice_t *nextDev = endSegment->u.link.dev;
+ busSegment_t *nextSegments = (busSegment_t *)endSegment->u.link.segments;
+ bus->curSegment = nextSegments;
+ endSegment->u.link.dev = NULL;
+ endSegment->u.link.segments = NULL;
+ spiSequenceStart(nextDev);
+ } else {
+ // The end of the segment list has been reached, so mark transactions as complete
+ bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
+ }
+ }
+}
+#endif
diff --git a/src/main/drivers/mcu/apm32/camera_control.c b/src/main/drivers/mcu/apm32/camera_control.c
new file mode 100644
index 0000000000..45e9de23d6
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/camera_control.c
@@ -0,0 +1,268 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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
+
+#ifndef CAMERA_CONTROL_PIN
+#define CAMERA_CONTROL_PIN NONE
+#endif
+
+#include
+
+#include "drivers/camera_control_impl.h"
+#include "drivers/rcc.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/pwm_output.h"
+#include "drivers/time.h"
+#include "pg/pg_ids.h"
+
+#define CAMERA_CONTROL_PWM_RESOLUTION 128
+#define CAMERA_CONTROL_SOFT_PWM_RESOLUTION 448
+
+#ifdef CURRENT_TARGET_CPU_VOLTAGE
+#define ADC_VOLTAGE CURRENT_TARGET_CPU_VOLTAGE
+#else
+#define ADC_VOLTAGE 3.3f
+#endif
+
+#if !defined(STM32F411xE) && !defined(STM32F7) && !defined(STM32H7) && !defined(STM32G4)
+#define CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+#include "build/atomic.h"
+#endif
+
+#define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+#include "drivers/timer.h"
+
+#ifdef USE_OSD
+#include "osd/osd.h"
+#endif
+
+PG_REGISTER_WITH_RESET_FN(cameraControlConfig_t, cameraControlConfig, PG_CAMERA_CONTROL_CONFIG, 0);
+
+void pgResetFn_cameraControlConfig(cameraControlConfig_t *cameraControlConfig)
+{
+ cameraControlConfig->mode = CAMERA_CONTROL_MODE_HARDWARE_PWM;
+ cameraControlConfig->refVoltage = 330;
+ cameraControlConfig->keyDelayMs = 180;
+ cameraControlConfig->internalResistance = 470;
+ cameraControlConfig->ioTag = IO_TAG(CAMERA_CONTROL_PIN);
+ cameraControlConfig->inverted = 0; // Output is inverted externally
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_ENTER] = 450;
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_LEFT] = 270;
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_UP] = 150;
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_RIGHT] = 68;
+ cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_DOWN] = 0;
+}
+
+static struct {
+ bool enabled;
+ IO_t io;
+ timerChannel_t channel;
+ uint32_t period;
+ uint8_t inverted;
+} cameraControlRuntime;
+
+static uint32_t endTimeMillis;
+
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+static void cameraControlHi(void)
+{
+ if (cameraControlRuntime.inverted) {
+ IOLo(cameraControlRuntime.io);
+ } else {
+ IOHi(cameraControlRuntime.io);
+ }
+}
+
+static void cameraControlLo(void)
+{
+ if (cameraControlRuntime.inverted) {
+ IOHi(cameraControlRuntime.io);
+ } else {
+ IOLo(cameraControlRuntime.io);
+ }
+}
+
+void TMR6_DAC_IRQHandler(void)
+{
+ cameraControlHi();
+ TMR6->STS = 0;
+}
+
+void TMR7_IRQHandler(void)
+{
+ cameraControlLo();
+
+ TMR7->STS = 0;
+}
+#endif
+
+void cameraControlInit(void)
+{
+ if (cameraControlConfig()->ioTag == IO_TAG_NONE)
+ return;
+
+ cameraControlRuntime.inverted = cameraControlConfig()->inverted;
+ cameraControlRuntime.io = IOGetByTag(cameraControlConfig()->ioTag);
+ IOInit(cameraControlRuntime.io, OWNER_CAMERA_CONTROL, 0);
+
+ if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
+#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+ const timerHardware_t *timerHardware = timerAllocate(cameraControlConfig()->ioTag, OWNER_CAMERA_CONTROL, 0);
+
+ if (!timerHardware) {
+ return;
+ }
+
+ IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
+
+ pwmOutConfig(&cameraControlRuntime.channel, timerHardware, timerClock(TMR6), CAMERA_CONTROL_PWM_RESOLUTION, 0, cameraControlRuntime.inverted);
+
+ cameraControlRuntime.period = CAMERA_CONTROL_PWM_RESOLUTION;
+ *cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
+ cameraControlRuntime.enabled = true;
+#endif
+ } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+
+ IOConfigGPIO(cameraControlRuntime.io, IOCFG_OUT_PP);
+ cameraControlHi();
+
+ cameraControlRuntime.period = CAMERA_CONTROL_SOFT_PWM_RESOLUTION;
+ cameraControlRuntime.enabled = true;
+
+ DAL_NVIC_SetPriority(TMR6_DAC_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ DAL_NVIC_EnableIRQ(TMR6_DAC_IRQn);
+
+ DAL_NVIC_SetPriority(TMR7_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ DAL_NVIC_EnableIRQ(TMR7_IRQn);
+
+ __DAL_RCM_TMR6_CLK_ENABLE();
+ __DAL_RCM_TMR7_CLK_ENABLE();
+ DDL_TMR_SetPrescaler(TMR6, 0);
+ DDL_TMR_SetPrescaler(TMR7, 0);
+#endif
+ } else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
+ // @todo not yet implemented
+ }
+}
+
+void cameraControlProcess(uint32_t currentTimeUs)
+{
+ if (endTimeMillis && currentTimeUs >= 1000 * endTimeMillis) {
+ if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
+ *cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
+ } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
+
+ }
+
+ endTimeMillis = 0;
+ }
+}
+
+static float calculateKeyPressVoltage(const cameraControlKey_e key)
+{
+ const int buttonResistance = cameraControlConfig()->buttonResistanceValues[key] * 100;
+ return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance);
+}
+
+#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
+static float calculatePWMDutyCycle(const cameraControlKey_e key)
+{
+ const float voltage = calculateKeyPressVoltage(key);
+
+ return voltage / ADC_VOLTAGE;
+}
+#endif
+
+void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
+{
+ if (!cameraControlRuntime.enabled)
+ return;
+
+ if (key >= CAMERA_CONTROL_KEYS_COUNT)
+ return;
+
+#if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
+ const float dutyCycle = calculatePWMDutyCycle(key);
+#else
+ (void) holdDurationMs;
+#endif
+
+#ifdef USE_OSD
+ // Force OSD timeout so we are alone on the display.
+ resumeRefreshAt = 0;
+#endif
+
+ if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
+#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
+ *cameraControlRuntime.channel.ccr = lrintf(dutyCycle * cameraControlRuntime.period);
+ endTimeMillis = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
+#endif
+ } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
+#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
+ const uint32_t hiTime = lrintf(dutyCycle * cameraControlRuntime.period);
+
+ if (0 == hiTime) {
+ cameraControlLo();
+ delay(cameraControlConfig()->keyDelayMs + holdDurationMs);
+ cameraControlHi();
+ } else {
+ DDL_TMR_SetCounter(TMR6, hiTime);
+ DDL_TMR_SetAutoReload(TMR6, cameraControlRuntime.period);
+
+ DDL_TMR_SetCounter(TMR7, 0);
+ DDL_TMR_SetAutoReload(TMR7, cameraControlRuntime.period);
+
+ // Start two timers as simultaneously as possible
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ DDL_TMR_EnableCounter(TMR6);
+ DDL_TMR_EnableCounter(TMR7);
+ }
+
+ // Enable interrupt generation
+ DDL_TMR_EnableIT_UPDATE(TMR6);
+ DDL_TMR_EnableIT_UPDATE(TMR7);
+
+ const uint32_t endTime = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
+
+ // Wait to give the camera a chance at registering the key press
+ while (millis() < endTime);
+
+ // Disable timers and interrupt generation
+ DDL_TMR_DisableCounter(TMR6);
+ DDL_TMR_DisableCounter(TMR7);
+
+ TMR6->DIEN = 0;
+ TMR7->DIEN = 0;
+
+ // Reset to idle state
+ IOHi(cameraControlRuntime.io);
+ }
+#endif
+ } else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
+ // @todo not yet implemented
+ }
+}
+
+#endif // USE_CAMERA_CONTROL
diff --git a/src/main/drivers/mcu/apm32/debug.c b/src/main/drivers/mcu/apm32/debug.c
new file mode 100644
index 0000000000..038ed5a196
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/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/main/drivers/mcu/apm32/dma_apm32.h b/src/main/drivers/mcu/apm32/dma_apm32.h
new file mode 100644
index 0000000000..accfb132f3
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/dma_apm32.h
@@ -0,0 +1,89 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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"
+
+typedef enum {
+ DMA_NONE = 0,
+ DMA1_ST0_HANDLER = 1,
+ DMA1_ST1_HANDLER,
+ DMA1_ST2_HANDLER,
+ DMA1_ST3_HANDLER,
+ DMA1_ST4_HANDLER,
+ DMA1_ST5_HANDLER,
+ DMA1_ST6_HANDLER,
+ DMA1_ST7_HANDLER,
+ DMA2_ST0_HANDLER,
+ DMA2_ST1_HANDLER,
+ DMA2_ST2_HANDLER,
+ DMA2_ST3_HANDLER,
+ DMA2_ST4_HANDLER,
+ DMA2_ST5_HANDLER,
+ DMA2_ST6_HANDLER,
+ DMA2_ST7_HANDLER,
+ DMA_LAST_HANDLER = DMA2_ST7_HANDLER
+} dmaIdentifier_e;
+
+#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 Stream %d:"
+#define DMA_INPUT_STRING "DMA%d_ST%d"
+
+#define DEFINE_DMA_CHANNEL(d, s, f) { \
+ .dma = d, \
+ .ref = (dmaResource_t *)d ## _Stream ## s, \
+ .stream = s, \
+ .irqHandlerCallback = NULL, \
+ .flagsShift = f, \
+ .irqN = d ## _Stream ## s ## _IRQn, \
+ .userParam = 0, \
+ .owner.owner = 0, \
+ .owner.resourceIndex = 0 \
+ }
+
+#define DEFINE_DMA_IRQ_HANDLER(d, s, i) FAST_IRQ_HANDLER void DMA ## d ## _STR ## 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) d->dma->HIFCLR = (flag << (d->flagsShift - 32)); else d->dma->LIFCLR = (flag << d->flagsShift)
+#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HINTSTS & (flag << (d->flagsShift - 32)): d->dma->LINTSTS & (flag << d->flagsShift))
+
+#define xDDL_EX_DMA_DeInit(dmaResource) DDL_EX_DMA_DeInit((DMA_ARCH_TYPE *)(dmaResource))
+#define xDDL_EX_DMA_Init(dmaResource, initstruct) DDL_EX_DMA_Init((DMA_ARCH_TYPE *)(dmaResource), initstruct)
+#define xDDL_EX_DMA_DisableResource(dmaResource) DDL_EX_DMA_DisableResource((DMA_ARCH_TYPE *)(dmaResource))
+#define xDDL_EX_DMA_EnableResource(dmaResource) DDL_EX_DMA_EnableResource((DMA_ARCH_TYPE *)(dmaResource))
+#define xDDL_EX_DMA_GetDataLength(dmaResource) DDL_EX_DMA_GetDataLength((DMA_ARCH_TYPE *)(dmaResource))
+#define xDDL_EX_DMA_SetDataLength(dmaResource, length) DDL_EX_DMA_SetDataLength((DMA_ARCH_TYPE *)(dmaResource), length)
+#define xDDL_EX_DMA_EnableIT_TC(dmaResource) DDL_EX_DMA_EnableIT_TC((DMA_ARCH_TYPE *)(dmaResource))
+
+#define DMA_IT_TCIF ((uint32_t)0x00000020)
+#define DMA_IT_HTIF ((uint32_t)0x00000010)
+#define DMA_IT_TEIF ((uint32_t)0x00000008)
+#define DMA_IT_DMEIF ((uint32_t)0x00000004)
+#define DMA_IT_FEIF ((uint32_t)0x00000001)
+
+void dmaMuxEnable(dmaIdentifier_e identifier, uint32_t dmaMuxId);
diff --git a/src/main/drivers/mcu/apm32/dma_apm32f4xx.c b/src/main/drivers/mcu/apm32/dma_apm32f4xx.c
new file mode 100644
index 0000000000..723838c06b
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/dma_apm32f4xx.c
@@ -0,0 +1,100 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "drivers/rcc.h"
+#include "drivers/resource.h"
+
+/*
+ * DMA descriptors.
+ */
+dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
+ 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),
+
+ DEFINE_DMA_CHANNEL(DMA2, 0, 0),
+ DEFINE_DMA_CHANNEL(DMA2, 1, 6),
+ DEFINE_DMA_CHANNEL(DMA2, 2, 16),
+ DEFINE_DMA_CHANNEL(DMA2, 3, 22),
+ DEFINE_DMA_CHANNEL(DMA2, 4, 32),
+ DEFINE_DMA_CHANNEL(DMA2, 5, 38),
+ DEFINE_DMA_CHANNEL(DMA2, 6, 48),
+ DEFINE_DMA_CHANNEL(DMA2, 7, 54),
+};
+
+/*
+ * DMA IRQ Handlers
+ */
+DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_ST0_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_ST1_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_ST2_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_ST3_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER)
+DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER)
+
+static void enableDmaClock(int index)
+{
+ RCC_ClockCmd(dmaDescriptors[index].dma == DMA1 ? RCC_AHB1(DMA1) : RCC_AHB1(DMA2), ENABLE);
+}
+
+void dmaEnable(dmaIdentifier_e identifier)
+{
+ const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
+
+ enableDmaClock(index);
+}
+
+void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
+{
+ const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
+
+ enableDmaClock(index);
+ dmaDescriptors[index].irqHandlerCallback = callback;
+ dmaDescriptors[index].userParam = userParam;
+
+ DAL_NVIC_SetPriority(dmaDescriptors[index].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
+ DAL_NVIC_EnableIRQ(dmaDescriptors[index].irqN);
+}
+#endif
diff --git a/src/main/drivers/mcu/apm32/dma_reqmap_mcu.c b/src/main/drivers/mcu/apm32/dma_reqmap_mcu.c
new file mode 100644
index 0000000000..57b0132fda
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/dma_reqmap_mcu.c
@@ -0,0 +1,207 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "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;
+
+#if defined(APM32F4)
+
+#define DMA(d, s, c) { DMA_CODE(d, s, c), (dmaResource_t *)DMA ## d ## _Stream ## s, DMA_CHANNEL_ ## c }
+
+static const dmaPeripheralMapping_t dmaPeripheralMapping[] = {
+#ifdef USE_SPI
+ { DMA_PERIPH_SPI_SDO, SPIDEV_1, { DMA(2, 3, 3), DMA(2, 5, 3) } },
+ { DMA_PERIPH_SPI_SDI, SPIDEV_1, { DMA(2, 0, 3), DMA(2, 2, 3) } },
+ { DMA_PERIPH_SPI_SDO, SPIDEV_2, { DMA(1, 4, 0) } },
+ { DMA_PERIPH_SPI_SDI, SPIDEV_2, { DMA(1, 3, 0) } },
+ { DMA_PERIPH_SPI_SDO, SPIDEV_3, { DMA(1, 5, 0), DMA(1, 7, 0) } },
+ { DMA_PERIPH_SPI_SDI, SPIDEV_3, { DMA(1, 0, 0), DMA(1, 2, 0) } },
+#endif // USE_SPI
+
+#ifdef USE_ADC
+ { DMA_PERIPH_ADC, ADCDEV_1, { DMA(2, 0, 0), DMA(2, 4, 0) } },
+ { DMA_PERIPH_ADC, ADCDEV_2, { DMA(2, 2, 1), DMA(2, 3, 1) } },
+ { DMA_PERIPH_ADC, ADCDEV_3, { DMA(2, 0, 2), DMA(2, 1, 2) } },
+#endif // USE_ADC
+
+#ifdef USE_SDCARD_SDIO
+ { DMA_PERIPH_SDIO, 0, { DMA(2, 3, 4), DMA(2, 6, 4) } },
+#endif // USE_SDCARD_SDIO
+
+#ifdef USE_UART
+ { DMA_PERIPH_UART_TX, UARTDEV_1, { DMA(2, 7, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_1, { DMA(2, 5, 4), DMA(2, 2, 4) } },
+ { DMA_PERIPH_UART_TX, UARTDEV_2, { DMA(1, 6, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_2, { DMA(1, 5, 4) } },
+ { DMA_PERIPH_UART_TX, UARTDEV_3, { DMA(1, 3, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_3, { DMA(1, 1, 4) } },
+ { DMA_PERIPH_UART_TX, UARTDEV_4, { DMA(1, 4, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_4, { DMA(1, 2, 4) } },
+ { DMA_PERIPH_UART_TX, UARTDEV_5, { DMA(1, 7, 4) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_5, { DMA(1, 0, 4) } },
+ { DMA_PERIPH_UART_TX, UARTDEV_6, { DMA(2, 6, 5), DMA(2, 7, 5) } },
+ { DMA_PERIPH_UART_RX, UARTDEV_6, { DMA(2, 1, 5), DMA(2, 2, 5) } },
+#endif // USE_UART
+};
+
+#define TC(chan) DEF_TIM_CHANNEL(CH_ ## chan)
+
+static const dmaTimerMapping_t dmaTimerMapping[] = {
+ // Generated from 'timer_def.h'
+ { TMR1, TC(CH1), { DMA(2, 6, 0), DMA(2, 1, 6), DMA(2, 3, 6) } },
+ { TMR1, TC(CH2), { DMA(2, 6, 0), DMA(2, 2, 6) } },
+ { TMR1, TC(CH3), { DMA(2, 6, 0), DMA(2, 6, 6) } },
+ { TMR1, TC(CH4), { DMA(2, 4, 6) } },
+
+ { TMR2, TC(CH1), { DMA(1, 5, 3) } },
+ { TMR2, TC(CH2), { DMA(1, 6, 3) } },
+ { TMR2, TC(CH3), { DMA(1, 1, 3) } },
+ { TMR2, TC(CH4), { DMA(1, 7, 3), DMA(1, 6, 3) } },
+
+ { TMR3, TC(CH1), { DMA(1, 4, 5) } },
+ { TMR3, TC(CH2), { DMA(1, 5, 5) } },
+ { TMR3, TC(CH3), { DMA(1, 7, 5) } },
+ { TMR3, TC(CH4), { DMA(1, 2, 5) } },
+
+ { TMR4, TC(CH1), { DMA(1, 0, 2) } },
+ { TMR4, TC(CH2), { DMA(1, 3, 2) } },
+ { TMR4, TC(CH3), { DMA(1, 7, 2) } },
+
+ { TMR5, TC(CH1), { DMA(1, 2, 6) } },
+ { TMR5, TC(CH2), { DMA(1, 4, 6) } },
+ { TMR5, TC(CH3), { DMA(1, 0, 6) } },
+ { TMR5, TC(CH4), { DMA(1, 1, 6), DMA(1, 3, 6) } },
+
+ { TMR8, TC(CH1), { DMA(2, 2, 0), DMA(2, 2, 7) } },
+ { TMR8, TC(CH2), { DMA(2, 2, 0), DMA(2, 3, 7) } },
+ { TMR8, TC(CH3), { DMA(2, 2, 0), DMA(2, 4, 7) } },
+ { TMR8, TC(CH4), { DMA(2, 7, 7) } },
+};
+#undef TC
+#undef DMA
+#endif // APM32F4
+
+const dmaChannelSpec_t *dmaGetChannelSpecByPeripheral(dmaPeripheral_e device, uint8_t index, int8_t opt)
+{
+ if (opt < 0 || opt >= MAX_PERIPHERAL_DMA_OPTIONS) {
+ return NULL;
+ }
+
+ for (unsigned i = 0 ; i < ARRAYLEN(dmaPeripheralMapping) ; i++) {
+ const dmaPeripheralMapping_t *periph = &dmaPeripheralMapping[i];
+
+ 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
+#if defined(APM32F4)
+ && dma->channel == timer->dmaChannelConfigured
+#endif
+ ) {
+ return j;
+ }
+ }
+ }
+ }
+
+ return DMA_OPT_UNUSED;
+}
+
+#endif // USE_DMA_SPEC
diff --git a/src/main/drivers/mcu/apm32/dma_reqmap_mcu.h b/src/main/drivers/mcu/apm32/dma_reqmap_mcu.h
new file mode 100644
index 0000000000..1e11e27c00
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/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/main/drivers/mcu/apm32/dshot_bitbang.c b/src/main/drivers/mcu/apm32/dshot_bitbang.c
new file mode 100644
index 0000000000..3e20081e99
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/dshot_bitbang.c
@@ -0,0 +1,778 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "drivers/dshot_bitbang_impl.h"
+#include "drivers/dshot_command.h"
+#include "drivers/motor.h"
+#include "drivers/nvic.h"
+#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring
+#include "drivers/dshot_dpwm.h" // XXX for motorDmaOutput_t *getMotorDmaOutput(uint8_t index); should go away with refactoring
+#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
+
+FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS]; // TIM1 or TIM8
+FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
+
+FAST_DATA_ZERO_INIT bbPort_t bbPorts[MAX_SUPPORTED_MOTOR_PORTS];
+FAST_DATA_ZERO_INIT int usedMotorPorts;
+
+FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS];
+
+static FAST_DATA_ZERO_INIT int motorCount;
+dshotBitbangStatus_e bbStatus;
+
+// 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(APM32F4)
+#define BB_OUTPUT_BUFFER_ATTRIBUTE
+#define BB_INPUT_BUFFER_ATTRIBUTE
+#endif // APM32F4
+#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(APM32F4)
+ DEF_TIM(TMR8, CH1, NONE, 0, 1),
+ DEF_TIM(TMR8, CH2, NONE, 0, 1),
+ DEF_TIM(TMR8, CH3, NONE, 0, 1),
+ DEF_TIM(TMR8, CH4, NONE, 0, 0),
+ DEF_TIM(TMR1, CH1, NONE, 0, 1),
+ DEF_TIM(TMR1, CH1, NONE, 0, 2),
+ DEF_TIM(TMR1, CH2, NONE, 0, 1),
+ DEF_TIM(TMR1, CH3, NONE, 0, 1),
+ DEF_TIM(TMR1, CH4, NONE, 0, 0),
+
+#else
+#error MCU dependent code required
+#endif
+};
+
+static FAST_DATA_ZERO_INIT motorDevice_t bbDevice;
+static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
+
+static motorPwmProtocolTypes_e motorPwmProtocol;
+
+// 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(motorPwmProtocolTypes_e pwmProtocolType)
+{
+ switch (pwmProtocolType) {
+ case(PWM_TYPE_DSHOT600):
+ return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
+ case(PWM_TYPE_DSHOT300):
+ return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
+ default:
+ case(PWM_TYPE_DSHOT150):
+ return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
+ }
+}
+
+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_IT_TEIF)) {
+ while (1) {};
+ }
+
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
+
+#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, motorPwmProtocolTypes_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, motorPwmProtocolTypes_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)) {
+ bbDevice.vTable.write = motorWriteNull;
+ bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
+ bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
+
+ 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 < motorCount; motorIndex++) {
+#ifdef APM32F4
+ uint32_t rawValue = decode_bb_bitband(
+ 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;
+ }
+
+ // fetch requestTelemetry from motors. Needs to be refactored.
+ motorDmaOutput_t * const motor = getMotorDmaOutput(motorIndex);
+ bbmotor->protocolControl.requestTelemetry = motor->protocolControl.requestTelemetry;
+ motor->protocolControl.requestTelemetry = false;
+
+ // 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(bbDevice.count)) {
+ 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 < motorCount; 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(uint8_t index)
+{
+ return bbMotors[index].enabled;
+}
+
+static void bbPostInit(void)
+{
+ bbFindPacerTimer();
+
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
+
+ if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) {
+ return;
+ }
+
+
+ bbMotors[motorIndex].enabled = true;
+
+ // Fill in motors structure for 4way access (XXX Should be refactored)
+
+ motors[motorIndex].enabled = true;
+ }
+}
+
+static 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,
+};
+
+dshotBitbangStatus_e dshotBitbangGetStatus(void)
+{
+ return bbStatus;
+}
+
+motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count)
+{
+ dbgPinLo(0);
+ dbgPinLo(1);
+
+ motorPwmProtocol = motorConfig->motorPwmProtocol;
+ bbDevice.vTable = bbVTable;
+ motorCount = 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 < motorCount; 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->motorPwmInversion ? 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 */
+ bbDevice.vTable.write = motorWriteNull;
+ bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
+ bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
+ bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
+ return NULL;
+ }
+
+ int pinIndex = IO_GPIOPinIdx(io);
+
+ bbMotors[motorIndex].pinIndex = pinIndex;
+ bbMotors[motorIndex].io = io;
+ bbMotors[motorIndex].output = output;
+#if defined(APM32F4)
+ bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_LOW, bbPuPdMode);
+#endif
+
+ IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+ IOConfigGPIO(io, bbMotors[motorIndex].iocfg);
+ if (output & TIMER_OUTPUT_INVERTED) {
+ IOLo(io);
+ } else {
+ IOHi(io);
+ }
+
+ // Fill in motors structure for 4way access (XXX Should be refactored)
+ motors[motorIndex].io = bbMotors[motorIndex].io;
+ }
+
+ return &bbDevice;
+}
+
+#endif // USE_DSHOT_BITBANG
diff --git a/src/main/drivers/mcu/apm32/dshot_bitbang_ddl.c b/src/main/drivers/mcu/apm32/dshot_bitbang_ddl.c
new file mode 100644
index 0000000000..f26a21966f
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/dshot_bitbang_ddl.c
@@ -0,0 +1,287 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "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_impl.h"
+#include "drivers/dshot_command.h"
+#include "drivers/motor.h"
+#include "drivers/nvic.h"
+#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring
+#include "drivers/time.h"
+#include "drivers/timer.h"
+
+#include "pg/motor.h"
+
+// Setup GPIO_MODER and GPIO_ODR register manipulation values
+
+void bbGpioSetup(bbMotor_t *bbMotor)
+{
+ bbPort_t *bbPort = bbMotor->bbPort;
+ int pinIndex = bbMotor->pinIndex;
+
+ bbPort->gpioModeMask |= (GPIO_MODE_MODE0 << (pinIndex * 2));
+ bbPort->gpioModeInput |= (DDL_GPIO_MODE_INPUT << (pinIndex * 2));
+ bbPort->gpioModeOutput |= (DDL_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;
+
+ switch (bbPort->timhw->channel) {
+ case TMR_CHANNEL_1: bbPort->llChannel = DDL_TMR_CHANNEL_CH1; break;
+ case TMR_CHANNEL_2: bbPort->llChannel = DDL_TMR_CHANNEL_CH2; break;
+ case TMR_CHANNEL_3: bbPort->llChannel = DDL_TMR_CHANNEL_CH3; break;
+ case TMR_CHANNEL_4: bbPort->llChannel = DDL_TMR_CHANNEL_CH4; break;
+ }
+
+ DDL_TMR_OC_InitTypeDef ocInit;
+ DDL_TMR_OC_StructInit(&ocInit);
+ ocInit.OCMode = DDL_TMR_OCMODE_PWM1;
+ ocInit.OCIdleState = DDL_TMR_OCIDLESTATE_HIGH;
+ ocInit.OCState = DDL_TMR_OCSTATE_ENABLE;
+ ocInit.OCPolarity = DDL_TMR_OCPOLARITY_LOW;
+ ocInit.CompareValue = 10; // Duty doesn't matter, but too value small would make monitor output invalid
+
+ DDL_TMR_DisableCounter(bbPort->timhw->tim);
+
+ DDL_TMR_OC_Init(timhw->tim, bbPort->llChannel, &ocInit);
+
+ DDL_TMR_OC_EnablePreload(timhw->tim, bbPort->llChannel);
+
+#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);
+ DDL_TMR_EnableAllOutputs(timhw->tim);
+ }
+#endif
+
+ // Enable and keep it running
+ DDL_TMR_EnableCounter(bbPort->timhw->tim);
+}
+
+#ifdef USE_DMA_REGISTER_CACHE
+void bbLoadDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache)
+{
+ ((DMA_ARCH_TYPE *)dmaResource)->SCFG = dmaRegCache->SCFG;
+ ((DMA_ARCH_TYPE *)dmaResource)->FCTRL = dmaRegCache->FCTRL;
+ ((DMA_ARCH_TYPE *)dmaResource)->NDATA = dmaRegCache->NDATA;
+ ((DMA_ARCH_TYPE *)dmaResource)->PADDR = dmaRegCache->PADDR;
+ ((DMA_ARCH_TYPE *)dmaResource)->M0ADDR = dmaRegCache->M0ADDR;
+}
+
+static void bbSaveDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache)
+{
+ dmaRegCache->SCFG = ((DMA_ARCH_TYPE *)dmaResource)->SCFG;
+ dmaRegCache->FCTRL = ((DMA_ARCH_TYPE *)dmaResource)->FCTRL;
+ dmaRegCache->NDATA = ((DMA_ARCH_TYPE *)dmaResource)->NDATA;
+ dmaRegCache->PADDR = ((DMA_ARCH_TYPE *)dmaResource)->PADDR;
+ dmaRegCache->M0ADDR = ((DMA_ARCH_TYPE *)dmaResource)->M0ADDR;
+}
+#endif
+
+void bbSwitchToOutput(bbPort_t * bbPort)
+{
+ // Output idle level before switching to output
+ // Use BSC register for this
+ // Normal: Use BC (higher half)
+ // Inverted: Use BS (lower half)
+
+ WRITE_REG(bbPort->gpio->BSC, bbPort->gpioIdleBSRR);
+
+ // Set GPIO to output
+
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ MODIFY_REG(bbPort->gpio->MODE, bbPort->gpioModeMask, bbPort->gpioModeOutput);
+ }
+
+ // Reinitialize port group DMA for output
+
+ dmaResource_t *dmaResource = bbPort->dmaResource;
+#ifdef USE_DMA_REGISTER_CACHE
+ bbDMA_Cmd(bbPort, DISABLE);
+ bbLoadDMARegs(dmaResource, &bbPort->dmaRegOutput);
+#else
+ xDDL_EX_DMA_Deinit(dmaResource);
+ xDDL_EX_DMA_Init(dmaResource, &bbPort->outputDmaInit);
+ // Needs this, as it is DeInit'ed above...
+ xDDL_EX_DMA_EnableIT_TC(dmaResource);
+#endif
+
+ // Reinitialize pacer timer for output
+
+ bbPort->timhw->tim->AUTORLD = bbPort->outputARR;
+
+ bbPort->direction = DSHOT_BITBANG_DIRECTION_OUTPUT;
+}
+
+#ifdef USE_DSHOT_TELEMETRY
+void bbSwitchToInput(bbPort_t *bbPort)
+{
+ // Set GPIO to input
+
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ MODIFY_REG(bbPort->gpio->MODE, 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
+ xDDL_EX_DMA_Deinit(dmaResource);
+ xDDL_EX_DMA_Init(dmaResource, &bbPort->inputDmaInit);
+
+ // Needs this, as it is DeInit'ed above...
+ xDDL_EX_DMA_EnableIT_TC(dmaResource);
+#endif
+
+ // Reinitialize pacer timer for input
+
+ bbPort->timhw->tim->AUTORLD = bbPort->inputARR;
+
+ bbDMA_Cmd(bbPort, ENABLE);
+
+ bbPort->direction = DSHOT_BITBANG_DIRECTION_INPUT;
+}
+#endif
+
+void bbDMAPreconfigure(bbPort_t *bbPort, uint8_t direction)
+{
+ DDL_DMA_InitTypeDef *dmainit = (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) ? &bbPort->outputDmaInit : &bbPort->inputDmaInit;
+
+ DDL_DMA_StructInit(dmainit);
+
+ dmainit->Mode = DDL_DMA_MODE_NORMAL;
+ dmainit->Channel = bbPort->dmaChannel;
+ dmainit->FIFOMode = DDL_DMA_FIFOMODE_ENABLE ;
+
+ dmainit->PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT;
+ dmainit->MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT;
+
+ if (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) {
+ dmainit->Priority = DDL_DMA_PRIORITY_VERYHIGH;
+ dmainit->Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH;
+ dmainit->NbData = bbPort->portOutputCount;
+ dmainit->PeriphOrM2MSrcAddress = (uint32_t)&bbPort->gpio->BSC;
+ dmainit->PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_WORD;
+ dmainit->MemoryOrM2MDstAddress = (uint32_t)bbPort->portOutputBuffer;
+ dmainit->MemoryOrM2MDstDataSize = DDL_DMA_MDATAALIGN_WORD;
+
+#ifdef USE_DMA_REGISTER_CACHE
+ xDDL_EX_DMA_Init(bbPort->dmaResource, dmainit);
+ bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegOutput);
+#endif
+ } else {
+ dmainit->Priority = DDL_DMA_PRIORITY_HIGH;
+ dmainit->Direction = DDL_DMA_DIRECTION_PERIPH_TO_MEMORY;
+ dmainit->NbData = bbPort->portInputCount;
+
+ dmainit->PeriphOrM2MSrcAddress = (uint32_t)&bbPort->gpio->IDATA;
+ dmainit->PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_HALFWORD;
+ dmainit->MemoryOrM2MDstAddress = (uint32_t)bbPort->portInputBuffer;
+ dmainit->MemoryOrM2MDstDataSize = DDL_DMA_MDATAALIGN_WORD;
+
+#ifdef USE_DMA_REGISTER_CACHE
+ xDDL_EX_DMA_Init(bbPort->dmaResource, dmainit);
+ bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegInput);
+#endif
+ }
+}
+
+void bbTIM_TimeBaseInit(bbPort_t *bbPort, uint16_t period)
+{
+ DDL_TMR_InitTypeDef *init = &bbPort->timeBaseInit;
+
+ init->Prescaler = 0; // Feed raw timerClock
+ init->ClockDivision = DDL_TMR_CLOCKDIVISION_DIV1;
+ init->CounterMode = DDL_TMR_COUNTERMODE_UP;
+ init->Autoreload = period;
+ //TIM_TimeBaseInit(bbPort->timhw->tim, &bbPort->timeBaseInit);
+ DDL_TMR_Init(bbPort->timhw->tim, init);
+ MODIFY_REG(bbPort->timhw->tim->CTRL1, TMR_CTRL1_ARPEN, TMR_AUTORELOAD_PRELOAD_ENABLE);
+}
+
+void bbTIM_DMACmd(TMR_TypeDef* TMRx, uint16_t TIM_DMASource, FunctionalState NewState)
+{
+ if (NewState == ENABLE) {
+ SET_BIT(TMRx->DIEN, TIM_DMASource);
+ } else {
+ CLEAR_BIT(TMRx->DIEN, TIM_DMASource);
+ }
+}
+
+void bbDMA_ITConfig(bbPort_t *bbPort)
+{
+ xDDL_EX_DMA_EnableIT_TC(bbPort->dmaResource);
+
+ SET_BIT(((DMA_Stream_TypeDef *)(bbPort->dmaResource))->SCFG, DMA_SCFGx_TXCIEN|DMA_SCFGx_TXEIEN);
+}
+
+void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState)
+{
+ if (NewState == ENABLE) {
+ xDDL_EX_DMA_EnableResource(bbPort->dmaResource);
+ } else {
+ xDDL_EX_DMA_DisableResource(bbPort->dmaResource);
+ }
+}
+
+int bbDMA_Count(bbPort_t *bbPort)
+{
+ return xDDL_EX_DMA_GetDataLength(bbPort->dmaResource);
+}
+#endif // USE_DSHOT_BITBANG
diff --git a/src/main/drivers/mcu/apm32/eint_apm32.c b/src/main/drivers/mcu/apm32/eint_apm32.c
new file mode 100644
index 0000000000..8d8543d821
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/eint_apm32.c
@@ -0,0 +1,202 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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"
+
+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(APM32F4)
+static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
+ EINT0_IRQn,
+ EINT1_IRQn,
+ EINT2_IRQn,
+ EINT3_IRQn,
+ EINT4_IRQn,
+ EINT9_5_IRQn,
+ EINT15_10_IRQn
+};
+#else
+# warning "Unknown CPU"
+#endif
+
+static uint32_t triggerLookupTable[] = {
+#if defined(APM32F4)
+ [BETAFLIGHT_EXTI_TRIGGER_RISING] = GPIO_MODE_IT_RISING,
+ [BETAFLIGHT_EXTI_TRIGGER_FALLING] = GPIO_MODE_IT_FALLING,
+ [BETAFLIGHT_EXTI_TRIGGER_BOTH] = GPIO_MODE_IT_RISING_FALLING
+#else
+# warning "Unknown CPU"
+#endif
+};
+
+// Absorb the difference in IMR and PR assignments to registers
+
+#define EXTI_REG_IMR (EINT->IMASK)
+#define EXTI_REG_PR (EINT->IPEND)
+
+void EXTIInit(void)
+{
+#if defined(APM32F4)
+ /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
+ __DAL_RCM_SYSCFG_CLK_ENABLE();
+#endif
+ 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);
+
+ GPIO_InitTypeDef init = {
+ .Pin = IO_Pin(io),
+ .Mode = GPIO_MODE_INPUT | IO_CONFIG_GET_MODE(config) | triggerLookupTable[trigger],
+ .Speed = IO_CONFIG_GET_SPEED(config),
+ .Pull = IO_CONFIG_GET_PULL(config),
+ };
+ DAL_GPIO_Init(IO_GPIO(io), &init);
+
+ if (extiGroupPriority[group] > irqPriority) {
+ extiGroupPriority[group] = irqPriority;
+ DAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+ DAL_NVIC_EnableIRQ(extiGroupIRQn[group]);
+ }
+}
+
+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(APM32F4)
+ uint32_t extiLine = IO_EXTI_Line(io);
+
+ if (!extiLine) {
+ return;
+ }
+
+ EXTI_REG_IMR |= extiLine;
+#else
+# error "Unknown CPU"
+#endif
+}
+
+
+void EXTIDisable(IO_t io)
+{
+#if defined(APM32F4)
+ uint32_t extiLine = IO_EXTI_Line(io);
+
+ if (!extiLine) {
+ return;
+ }
+
+ EXTI_REG_IMR &= ~extiLine;
+ EXTI_REG_PR = extiLine;
+#else
+# error "Unknown CPU"
+#endif
+}
+
+#define EXTI_EVENT_MASK 0xFFFF // first 16 bits only, see also definition of extiChannelRecs.
+
+void EXTI_IRQHandler(uint32_t mask)
+{
+ uint32_t exti_active = (EXTI_REG_IMR & EXTI_REG_PR) & mask;
+
+ EXTI_REG_PR = 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(EINT0_IRQHandler, 0x0001);
+_EXTI_IRQ_HANDLER(EINT1_IRQHandler, 0x0002);
+#if defined(APM32F4)
+_EXTI_IRQ_HANDLER(EINT2_IRQHandler, 0x0004);
+#else
+# warning "Unknown CPU"
+#endif
+_EXTI_IRQ_HANDLER(EINT3_IRQHandler, 0x0008);
+_EXTI_IRQ_HANDLER(EINT4_IRQHandler, 0x0010);
+_EXTI_IRQ_HANDLER(EINT9_5_IRQHandler, 0x03e0);
+_EXTI_IRQ_HANDLER(EINT15_10_IRQHandler, 0xfc00);
+
+#endif
diff --git a/src/main/drivers/mcu/apm32/io_apm32.c b/src/main/drivers/mcu/apm32/io_apm32.c
new file mode 100644
index 0000000000..1bfdefc8a7
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/io_apm32.c
@@ -0,0 +1,183 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "drivers/rcc.h"
+
+#include "common/utils.h"
+
+// io ports defs are stored in array by index now
+struct ioPortDef_s {
+ rccPeriphTag_t rcc;
+};
+
+const struct ioPortDef_s ioPortDefs[] = {
+ { RCC_AHB1(PA) },
+ { RCC_AHB1(PB) },
+ { RCC_AHB1(PC) },
+ { RCC_AHB1(PD) },
+ { RCC_AHB1(PE) },
+ { RCC_AHB1(PF) },
+};
+
+// mask on stm32f103, bit index on stm32f303
+uint32_t IO_EXTI_Line(IO_t io)
+{
+ if (!io) {
+ return 0;
+ }
+#if defined(APM32F4)
+ return 1 << IO_GPIOPinIdx(io);
+#elif defined(SIMULATOR_BUILD)
+ return 0;
+#else
+# error "Unknown target type"
+#endif
+}
+
+bool IORead(IO_t io)
+{
+ if (!io) {
+ return false;
+ }
+#if defined(USE_FULL_DDL_DRIVER)
+ return (DDL_GPIO_ReadInputPort(IO_GPIO(io)) & IO_Pin(io));
+#elif defined(USE_DAL_DRIVER)
+ return !! DAL_GPIO_ReadPin(IO_GPIO(io), IO_Pin(io));
+#endif
+}
+
+void IOWrite(IO_t io, bool hi)
+{
+ if (!io) {
+ return;
+ }
+#if defined(USE_FULL_DDL_DRIVER)
+ DDL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io) << (hi ? 0 : 16));
+#elif defined(USE_DAL_DRIVER)
+ DAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), hi ? GPIO_PIN_SET : GPIO_PIN_RESET);
+#endif
+}
+
+void IOHi(IO_t io)
+{
+ if (!io) {
+ return;
+ }
+#if defined(USE_FULL_DDL_DRIVER)
+ DDL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io));
+#elif defined(USE_DAL_DRIVER)
+ DAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), GPIO_PIN_SET);
+#endif
+}
+
+void IOLo(IO_t io)
+{
+ if (!io) {
+ return;
+ }
+#if defined(USE_FULL_DDL_DRIVER)
+ DDL_GPIO_ResetOutputPin(IO_GPIO(io), IO_Pin(io));
+#elif defined(USE_DAL_DRIVER)
+ DAL_GPIO_WritePin(IO_GPIO(io), IO_Pin(io), GPIO_PIN_RESET);
+#endif
+}
+
+void IOToggle(IO_t io)
+{
+ if (!io) {
+ return;
+ }
+
+ uint32_t mask = IO_Pin(io);
+ // Read pin state from ODR but write to BSRR because it only changes the pins
+ // high in the mask value rather than all pins. XORing ODR directly risks
+ // setting other pins incorrectly because it change all pins' state.
+#if defined(USE_FULL_DDL_DRIVER)
+ if (DDL_GPIO_ReadOutputPort(IO_GPIO(io)) & mask) {
+ mask <<= 16; // bit is set, shift mask to reset half
+ }
+ DDL_GPIO_SetOutputPin(IO_GPIO(io), mask);
+#elif defined(USE_DAL_DRIVER)
+ UNUSED(mask);
+ DAL_GPIO_TogglePin(IO_GPIO(io), IO_Pin(io));
+#endif
+}
+
+#if defined(USE_DAL_DRIVER)
+
+void IOConfigGPIO(IO_t io, ioConfig_t cfg)
+{
+ IOConfigGPIOAF(io, cfg, 0);
+}
+
+void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
+{
+ if (!io) {
+ return;
+ }
+
+ rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
+ RCC_ClockCmd(rcc, ENABLE);
+
+ GPIO_InitTypeDef init = {
+ .Pin = IO_Pin(io),
+ .Mode = (cfg >> 0) & 0x13,
+ .Speed = (cfg >> 2) & 0x03,
+ .Pull = (cfg >> 5) & 0x03,
+ .Alternate = af
+ };
+
+ DAL_GPIO_Init(IO_GPIO(io), &init);
+}
+
+#elif defined(USE_FULL_DDL_DRIVER)
+
+void IOConfigGPIO(IO_t io, ioConfig_t cfg)
+{
+ IOConfigGPIOAF(io, cfg, 0);
+}
+
+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);
+
+ DDL_GPIO_InitTypeDef init = {
+ .Pin = IO_Pin(io),
+ .Mode = (cfg >> 0) & 0x03,
+ .Speed = (cfg >> 2) & 0x03,
+ .OutputType = (cfg >> 4) & 0x01,
+ .Pull = (cfg >> 5) & 0x03,
+ .Alternate = af
+ };
+
+ DDL_GPIO_Init(IO_GPIO(io), &init);
+}
+#else
+# warning MCU not set
+#endif
diff --git a/src/main/drivers/mcu/apm32/light_ws2811strip_apm32.c b/src/main/drivers/mcu/apm32/light_ws2811strip_apm32.c
new file mode 100644
index 0000000000..a96d3a9bc4
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/light_ws2811strip_apm32.c
@@ -0,0 +1,187 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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_LED_STRIP
+
+#include "common/color.h"
+
+#include "drivers/dma.h"
+#include "drivers/dma_reqmap.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/rcc.h"
+#include "drivers/system.h"
+#include "drivers/timer.h"
+
+#include "drivers/light_ws2811strip.h"
+
+static IO_t ws2811IO = IO_NONE;
+
+static TMR_HandleTypeDef TmrHandle;
+static uint16_t timerChannel = 0;
+
+FAST_IRQ_HANDLER void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
+{
+ DAL_DMA_IRQHandler(TmrHandle.hdma[descriptor->userParam]);
+ TIM_DMACmd(&TmrHandle, timerChannel, DISABLE);
+ ws2811LedDataTransferInProgress = false;
+}
+
+bool ws2811LedStripHardwareInit(ioTag_t ioTag)
+{
+ if (!ioTag) {
+ return false;
+ }
+
+ const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_LED_STRIP, 0);
+
+ if (timerHardware == NULL) {
+ return false;
+ }
+
+ TMR_TypeDef *timer = timerHardware->tim;
+ timerChannel = timerHardware->channel;
+
+ dmaResource_t *dmaRef;
+
+#if defined(USE_DMA_SPEC)
+ const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
+
+ if (dmaSpec == NULL) {
+ return false;
+ }
+
+ dmaRef = dmaSpec->ref;
+ uint32_t dmaChannel = dmaSpec->channel;
+#else
+ dmaRef = timerHardware->dmaRef;
+ uint32_t dmaChannel = timerHardware->dmaChannel;
+#endif
+
+ if (dmaRef == NULL || !dmaAllocate(dmaGetIdentifier(dmaRef), OWNER_LED_STRIP, 0)) {
+ return false;
+ }
+ TmrHandle.Instance = 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;
+
+ TmrHandle.Init.Prescaler = prescaler;
+ TmrHandle.Init.Period = period; // 800kHz
+ TmrHandle.Init.ClockDivision = TMR_CLOCKDIVISION_DIV1;
+ TmrHandle.Init.CounterMode = TMR_COUNTERMODE_UP;
+ TmrHandle.Init.AutoReloadPreload = TMR_AUTORELOAD_PRELOAD_ENABLE;
+ if (DAL_TMR_PWM_Init(&TmrHandle) != DAL_OK) {
+ /* Initialization Error */
+ return false;
+ }
+
+ static DMA_HandleTypeDef hdma_tim;
+
+ ws2811IO = IOGetByTag(ioTag);
+ IOInit(ws2811IO, OWNER_LED_STRIP, 0);
+ IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction);
+
+ __DAL_RCM_DMA1_CLK_ENABLE();
+ __DAL_RCM_DMA2_CLK_ENABLE();
+
+ /* Set the parameters to be configured */
+ hdma_tim.Init.Channel = dmaChannel;
+ hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE;
+ hdma_tim.Init.MemInc = DMA_MINC_ENABLE;
+ hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
+ hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
+ hdma_tim.Init.Mode = DMA_NORMAL;
+ hdma_tim.Init.Priority = DMA_PRIORITY_HIGH;
+ hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+ hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE;
+ hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE;
+
+ /* Set hdma_tim instance */
+ hdma_tim.Instance = (DMA_ARCH_TYPE *)dmaRef;
+
+ uint16_t dmaIndex = timerDmaIndex(timerChannel);
+
+ /* Link hdma_tim to hdma[x] (channelx) */
+ __DAL_LINKDMA(&TmrHandle, hdma[dmaIndex], hdma_tim);
+
+ dmaEnable(dmaGetIdentifier(dmaRef));
+ dmaSetHandler(dmaGetIdentifier(dmaRef), WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaIndex);
+
+ /* Initialize TIMx DMA handle */
+ if (DAL_DMA_Init(TmrHandle.hdma[dmaIndex]) != DAL_OK) {
+ /* Initialization Error */
+ return false;
+ }
+
+ TMR_OC_InitTypeDef TMR_OCInitStructure;
+
+ /* PWM1 Mode configuration: Channel1 */
+ TMR_OCInitStructure.OCMode = TMR_OCMODE_PWM1;
+ TMR_OCInitStructure.OCIdleState = TMR_OCIDLESTATE_SET;
+ TMR_OCInitStructure.OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TMR_OCPOLARITY_LOW : TMR_OCPOLARITY_HIGH;
+ TMR_OCInitStructure.OCNIdleState = TMR_OCNIDLESTATE_RESET;
+ TMR_OCInitStructure.OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TMR_OCNPOLARITY_LOW : TMR_OCNPOLARITY_HIGH;
+ TMR_OCInitStructure.Pulse = 0;
+ TMR_OCInitStructure.OCFastMode = TMR_OCFAST_DISABLE;
+ if (DAL_TMR_PWM_ConfigChannel(&TmrHandle, &TMR_OCInitStructure, timerChannel) != DAL_OK) {
+ /* Configuration Error */
+ return false;
+ }
+ if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+ if (DAL_TMREx_PWMN_Start(&TmrHandle, timerChannel) != DAL_OK) {
+ /* Starting PWM generation Error */
+ return false;
+ }
+ } else {
+ if (DAL_TMR_PWM_Start(&TmrHandle, timerChannel) != DAL_OK) {
+ /* Starting PWM generation Error */
+ return false;
+ }
+ }
+
+ return true;
+}
+
+void ws2811LedStripDMAEnable(void)
+{
+ if (DMA_SetCurrDataCounter(&TmrHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != DAL_OK) {
+ /* DMA set error */
+ ws2811LedDataTransferInProgress = false;
+ return;
+ }
+ /* Reset timer counter */
+ __DAL_TMR_SET_COUNTER(&TmrHandle,0);
+ /* Enable channel DMA requests */
+ TIM_DMACmd(&TmrHandle,timerChannel,ENABLE);
+}
+
+#endif // USE_LED_STRIP
diff --git a/src/main/drivers/mcu/apm32/persistent_apm32.c b/src/main/drivers/mcu/apm32/persistent_apm32.c
new file mode 100644
index 0000000000..4d5caeecbd
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/persistent_apm32.c
@@ -0,0 +1,93 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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)
+{
+ RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+ uint32_t value = DAL_RTCEx_BKUPRead(&rtcHandle, id);
+
+ return value;
+}
+
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value)
+{
+ RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+ DAL_RTCEx_BKUPWrite(&rtcHandle, id, value);
+
+#ifdef USE_SPRACING_PERSISTENT_RTC_WORKAROUND
+ // Also write the persistent location used by the bootloader to support DFU etc.
+ if (id == PERSISTENT_OBJECT_RESET_REASON) {
+ // SPRACING firmware sometimes enters DFU mode when MSC mode is requested
+ if (value == RESET_MSC_REQUEST) {
+ value = RESET_NONE;
+ }
+ DAL_RTCEx_BKUPWrite(&rtcHandle, PERSISTENT_OBJECT_RESET_REASON_FWONLY, value);
+ }
+#endif
+}
+
+void persistentObjectRTCEnable(void)
+{
+ RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+ __DAL_RCM_PMU_CLK_ENABLE(); // Enable Access to PMU
+
+ DAL_PMU_EnableBkUpAccess(); // Disable backup domain protection
+
+ // We don't need a clock source for RTC itself. Skip it.
+
+ __DAL_RTC_WRITEPROTECTION_ENABLE(&rtcHandle); // Reset sequence
+ __DAL_RTC_WRITEPROTECTION_DISABLE(&rtcHandle); // Apply sequence
+}
+
+void persistentObjectInit(void)
+{
+ // Configure and enable RTC for backup register access
+
+ persistentObjectRTCEnable();
+
+ // XXX Magic value checking may be sufficient
+
+ uint32_t wasSoftReset;
+
+ wasSoftReset = RCM->CSTS & RCM_CSTS_SWRSTFLG;
+
+ if (!wasSoftReset || (persistentObjectRead(PERSISTENT_OBJECT_MAGIC) != PERSISTENT_OBJECT_MAGIC_VALUE)) {
+ for (int i = 1; i < PERSISTENT_OBJECT_COUNT; i++) {
+ persistentObjectWrite(i, 0);
+ }
+ persistentObjectWrite(PERSISTENT_OBJECT_MAGIC, PERSISTENT_OBJECT_MAGIC_VALUE);
+ }
+}
diff --git a/src/main/drivers/mcu/apm32/platform_mcu.h b/src/main/drivers/mcu/apm32/platform_mcu.h
new file mode 100644
index 0000000000..048975daef
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/platform_mcu.h
@@ -0,0 +1,149 @@
+/*
+ * 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 they
+ * 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
+
+#ifdef APM32F4
+
+#include "apm32f4xx.h"
+#include "apm32f4xx_dal.h"
+#include "system_apm32f4xx.h"
+
+#include "apm32f4xx_ddl_spi.h"
+#include "apm32f4xx_ddl_gpio.h"
+#include "apm32f4xx_ddl_dma.h"
+#include "apm32f4xx_ddl_rcm.h"
+#include "apm32f4xx_ddl_bus.h"
+#include "apm32f4xx_ddl_tmr.h"
+#include "apm32f4xx_ddl_system.h"
+#include "apm32f4xx_ddl_adc.h"
+
+#include "apm32f4xx_ddl_ex.h"
+
+// Aliases
+#define HAL_StatusTypeDef DAL_StatusTypeDef
+#define HAL_RCC_GetSysClockFreq DAL_RCM_GetSysClockFreq
+#define HAL_IncTick DAL_IncTick
+#define HAL_TIM_IC_Start_IT DAL_TMR_IC_Start_IT
+#define HAL_TIM_IC_ConfigChannel DAL_TMR_IC_ConfigChannel
+#define HAL_NVIC_SetPriority DAL_NVIC_SetPriority
+#define HAL_NVIC_EnableIRQ DAL_NVIC_EnableIRQ
+
+#define __HAL_TIM_GetAutoreload __DAL_TMR_GET_AUTORELOAD
+#define __HAL_TIM_SetCounter __DAL_TMR_SET_COUNTER
+#define __HAL_DMA_GET_COUNTER __DAL_DMA_GET_COUNTER
+#define __HAL_UART_ENABLE_IT __DAL_UART_ENABLE_IT
+
+#define LL_TIM_InitTypeDef DDL_TMR_InitTypeDef
+#define LL_TIM_DeInit DDL_TMR_DeInit
+#define LL_TIM_OC_InitTypeDef DDL_TMR_OC_InitTypeDef
+#define LL_TIM_IC_InitTypeDef DDL_TMR_IC_InitTypeDef
+#define LL_TIM_SetAutoReload DDL_TMR_SetAutoReload
+#define LL_TIM_DisableIT_UPDATE DDL_TMR_DisableIT_UPDATE
+#define LL_TIM_DisableCounter DDL_TMR_DisableCounter
+#define LL_TIM_SetCounter DDL_TMR_SetCounter
+#define LL_TIM_ClearFlag_UPDATE DDL_TMR_ClearFlag_UPDATE
+#define LL_TIM_EnableIT_UPDATE DDL_TMR_EnableIT_UPDATE
+#define LL_TIM_EnableCounter DDL_TMR_EnableCounter
+#define LL_TIM_GenerateEvent_UPDATE DDL_TMR_GenerateEvent_UPDATE
+#define LL_EX_TIM_DisableIT DDL_EX_TMR_DisableIT
+
+#define LL_DMA_InitTypeDef DDL_DMA_InitTypeDef
+#define LL_EX_DMA_DeInit DDL_EX_DMA_DeInit
+#define LL_EX_DMA_Init DDL_EX_DMA_Init
+#define LL_EX_DMA_DisableResource DDL_EX_DMA_DisableResource
+#define LL_EX_DMA_EnableResource DDL_EX_DMA_EnableResource
+#define LL_EX_DMA_GetDataLength DDL_EX_DMA_GetDataLength
+#define LL_EX_DMA_SetDataLength DDL_EX_DMA_SetDataLength
+#define LL_EX_DMA_EnableIT_TC DDL_EX_DMA_EnableIT_TC
+
+#define TIM_TypeDef TMR_TypeDef
+#define TIM_HandleTypeDef TMR_HandleTypeDef
+#define TIM_ICPOLARITY_RISING TMR_ICPOLARITY_RISING
+#define TIM_CCxChannelCmd TMR_CCxChannelCmd
+#define TIM_CCx_DISABLE TMR_CCx_DISABLE
+#define TIM_CCx_ENABLE TMR_CCx_ENABLE
+#define TIM_CCxChannelCmd TMR_CCxChannelCmd
+#define TIM_IC_InitTypeDef TMR_IC_InitTypeDef
+#define TIM_ICPOLARITY_FALLING TMR_ICPOLARITY_FALLING
+#define TIM_ICSELECTION_DIRECTTI TMR_ICSELECTION_DIRECTTI
+#define TIM_ICPSC_DIV1 TMR_ICPSC_DIV1
+
+#ifdef USE_DAL_DRIVER
+#define USE_HAL_DRIVER
+#endif
+
+#ifdef USE_FULL_DDL_DRIVER
+#define USE_FULL_LL_DRIVER
+#endif
+
+#endif // APM32F4
+
+#if defined(APM32F405xx) || defined(APM32F407xx) || defined(APM32F415xx) || defined(APM32F417xx)
+#define USE_FAST_DATA
+
+// Chip Unique ID on APM32F405
+#define U_ID_0 (*(uint32_t*)0x1fff7a10)
+#define U_ID_1 (*(uint32_t*)0x1fff7a14)
+#define U_ID_2 (*(uint32_t*)0x1fff7a18)
+
+#define USE_PIN_AF
+
+#ifndef APM32F4
+#define APM32F4
+#endif
+
+#endif
+
+#define USE_RPM_FILTER
+#define USE_DYN_IDLE
+#define USE_DYN_NOTCH_FILTER
+#define USE_ADC_INTERNAL
+#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 USE_OVERCLOCK
+
+#define TASK_GYROPID_DESIRED_PERIOD 125 // 125us = 8kHz
+#define SCHEDULER_DELAY_LIMIT 10
+
+#define DEFAULT_CPU_OVERCLOCK 0
+
+#define FAST_IRQ_HANDLER
+
+#define DMA_DATA_ZERO_INIT
+#define DMA_DATA
+#define STATIC_DMA_DATA_AUTO static
+
+// Data in RAM which is guaranteed to not be reset on hot reboot
+#define PERSISTENT __attribute__ ((section(".persistent_data"), aligned(4)))
+
+#define DMA_RAM
+#define DMA_RW_AXI
+#define DMA_RAM_R
+#define DMA_RAM_W
+#define DMA_RAM_RW
+
+#define USE_TIMER_MGMT
+#define USE_TIMER_AF
diff --git a/src/main/drivers/mcu/apm32/pwm_output_apm32.c b/src/main/drivers/mcu/apm32/pwm_output_apm32.c
new file mode 100644
index 0000000000..35a6af2716
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/pwm_output_apm32.c
@@ -0,0 +1,289 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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/motor.h"
+#include "drivers/pwm_output.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+
+#include "pg/motor.h"
+
+
+FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
+
+static void pwmOCConfig(TMR_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
+{
+ TMR_HandleTypeDef* Handle = timerFindTimerHandle(tim);
+ if (Handle == NULL) return;
+
+ TMR_OC_InitTypeDef TMR_OCInitStructure;
+
+ TMR_OCInitStructure.OCMode = TMR_OCMODE_PWM1;
+ TMR_OCInitStructure.OCIdleState = TMR_OCIDLESTATE_SET;
+ TMR_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TMR_OCPOLARITY_LOW : TMR_OCPOLARITY_HIGH;
+ TMR_OCInitStructure.OCNIdleState = TMR_OCNIDLESTATE_SET;
+ TMR_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TMR_OCNPOLARITY_LOW : TMR_OCNPOLARITY_HIGH;
+ TMR_OCInitStructure.Pulse = value;
+ TMR_OCInitStructure.OCFastMode = TMR_OCFAST_DISABLE;
+
+ DAL_TMR_PWM_ConfigChannel(Handle, &TMR_OCInitStructure, channel);
+}
+
+void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
+{
+ TMR_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
+ if (Handle == NULL) return;
+
+ configTimeBase(timerHardware->tim, period, hz);
+ pwmOCConfig(timerHardware->tim,
+ timerHardware->channel,
+ value,
+ inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
+ );
+
+ if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
+ DAL_TMREx_PWMN_Start(Handle, timerHardware->channel);
+ else
+ DAL_TMR_PWM_Start(Handle, timerHardware->channel);
+ DAL_TMR_Base_Start(Handle);
+
+ channel->ccr = timerChCCR(timerHardware);
+
+ channel->tim = timerHardware->tim;
+
+ *channel->ccr = 0;
+}
+
+static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice;
+
+static void pwmWriteUnused(uint8_t index, float value)
+{
+ UNUSED(index);
+ UNUSED(value);
+}
+
+static void pwmWriteStandard(uint8_t index, float value)
+{
+ /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
+ *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
+}
+
+void pwmShutdownPulsesForAllMotors(void)
+{
+ for (int index = 0; index < motorPwmDevice.count; index++) {
+ // Set the compare register to 0, which stops the output pulsing if the timer overflows
+ if (motors[index].channel.ccr) {
+ *motors[index].channel.ccr = 0;
+ }
+ }
+}
+
+void pwmDisableMotors(void)
+{
+ pwmShutdownPulsesForAllMotors();
+}
+
+static motorVTable_t motorPwmVTable;
+bool pwmEnableMotors(void)
+{
+ /* check motors can be enabled */
+ return (motorPwmVTable.write != &pwmWriteUnused);
+}
+
+bool pwmIsMotorEnabled(uint8_t index)
+{
+ return motors[index].enabled;
+}
+
+static void pwmCompleteOneshotMotorUpdate(void)
+{
+ for (int index = 0; index < motorPwmDevice.count; index++) {
+ if (motors[index].forceOverflow) {
+ timerForceOverflow(motors[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.
+ *motors[index].channel.ccr = 0;
+ }
+}
+
+static float pwmConvertFromExternal(uint16_t externalValue)
+{
+ return (float)externalValue;
+}
+
+static uint16_t pwmConvertToExternal(float motorValue)
+{
+ return (uint16_t)motorValue;
+}
+
+static motorVTable_t motorPwmVTable = {
+ .postInit = motorPostInitNull,
+ .enable = pwmEnableMotors,
+ .disable = pwmDisableMotors,
+ .isMotorEnabled = pwmIsMotorEnabled,
+ .shutdown = pwmShutdownPulsesForAllMotors,
+ .convertExternalToMotor = pwmConvertFromExternal,
+ .convertMotorToExternal = pwmConvertToExternal,
+};
+
+motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
+{
+ motorPwmDevice.vTable = motorPwmVTable;
+
+ float sMin = 0;
+ float sLen = 0;
+ switch (motorConfig->motorPwmProtocol) {
+ default:
+ case PWM_TYPE_ONESHOT125:
+ sMin = 125e-6f;
+ sLen = 125e-6f;
+ break;
+ case PWM_TYPE_ONESHOT42:
+ sMin = 42e-6f;
+ sLen = 42e-6f;
+ break;
+ case PWM_TYPE_MULTISHOT:
+ sMin = 5e-6f;
+ sLen = 20e-6f;
+ break;
+ case PWM_TYPE_BRUSHED:
+ sMin = 0;
+ useUnsyncedPwm = true;
+ idlePulse = 0;
+ break;
+ case PWM_TYPE_STANDARD:
+ sMin = 1e-3f;
+ sLen = 1e-3f;
+ useUnsyncedPwm = true;
+ idlePulse = 0;
+ break;
+ }
+
+ motorPwmDevice.vTable.write = pwmWriteStandard;
+ motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
+ motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
+
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; 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 */
+ motorPwmDevice.vTable.write = &pwmWriteUnused;
+ motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
+ /* TODO: block arming and add reason system cannot arm */
+ return NULL;
+ }
+
+ motors[motorIndex].io = IOGetByTag(tag);
+ IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
+
+ IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
+
+ /* standard PWM outputs */
+ // margin of safety is 4 periods when unsynced
+ const unsigned pwmRateHz = useUnsyncedPwm ? 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 = useUnsyncedPwm ? 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.
+ */
+ motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
+ motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
+
+ pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
+
+ bool timerAlreadyUsed = false;
+ for (int i = 0; i < motorIndex; i++) {
+ if (motors[i].channel.tim == motors[motorIndex].channel.tim) {
+ timerAlreadyUsed = true;
+ break;
+ }
+ }
+ motors[motorIndex].forceOverflow = !timerAlreadyUsed;
+ motors[motorIndex].enabled = true;
+ }
+
+ return &motorPwmDevice;
+}
+
+pwmOutputPort_t *pwmGetMotors(void)
+{
+ return motors;
+}
+
+#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) {
+ break;
+ }
+
+ 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/main/drivers/mcu/apm32/pwm_output_dshot_apm32.c b/src/main/drivers/mcu/apm32/pwm_output_dshot_apm32.c
new file mode 100644
index 0000000000..67aaa2ab51
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/pwm_output_dshot_apm32.c
@@ -0,0 +1,420 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "common/time.h"
+
+#include "drivers/dma.h"
+#include "drivers/dma_reqmap.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h"
+#include "drivers/dshot_command.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/motor.h"
+#include "drivers/pwm_output.h"
+#include "drivers/pwm_output_dshot_shared.h"
+#include "drivers/rcc.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+#include "drivers/system.h"
+
+#ifdef USE_DSHOT_TELEMETRY
+
+void dshotEnableChannels(uint8_t motorCount)
+{
+ for (int i = 0; i < motorCount; i++) {
+ if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) {
+ DDL_TMR_CC_EnableChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel << 4);
+ } else {
+ DDL_TMR_CC_EnableChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel);
+ }
+ }
+}
+
+#endif
+
+void pwmDshotSetDirectionOutput(
+ motorDmaOutput_t * const motor
+#ifndef USE_DSHOT_TELEMETRY
+ , DDL_TMR_OC_InitTypeDef* pOcInit, DDL_DMA_InitTypeDef* pDmaInit
+#endif
+)
+{
+#ifdef USE_DSHOT_TELEMETRY
+ DDL_TMR_OC_InitTypeDef* pOcInit = &motor->ocInitStruct;
+ DDL_DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
+#endif
+
+ const timerHardware_t * const timerHardware = motor->timerHardware;
+ TMR_TypeDef *timer = timerHardware->tim;
+
+// dmaResource_t *dmaRef = motor->dmaRef;
+
+// #if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT_TELEMETRY)
+// if (useBurstDshot) {
+// dmaRef = timerHardware->dmaTimUPRef;
+// }
+// #endif
+
+ xDDL_EX_DMA_DeInit(motor->dmaRef);
+
+#ifdef USE_DSHOT_TELEMETRY
+ motor->isInput = false;
+#endif
+ DDL_TMR_OC_DisablePreload(timer, motor->llChannel);
+ DDL_TMR_OC_Init(timer, motor->llChannel, pOcInit);
+ DDL_TMR_OC_EnablePreload(timer, motor->llChannel);
+
+ motor->dmaInitStruct.Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH;
+
+ xDDL_EX_DMA_Init(motor->dmaRef, pDmaInit);
+ xDDL_EX_DMA_EnableIT_TC(motor->dmaRef);
+}
+
+#ifdef USE_DSHOT_TELEMETRY
+FAST_CODE static void pwmDshotSetDirectionInput(
+ motorDmaOutput_t * const motor
+)
+{
+ DDL_DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
+
+ const timerHardware_t * const timerHardware = motor->timerHardware;
+ TMR_TypeDef *timer = timerHardware->tim;
+
+ xDDL_EX_DMA_DeInit(motor->dmaRef);
+
+ motor->isInput = true;
+ if (!inputStampUs) {
+ inputStampUs = micros();
+ }
+ DDL_TMR_EnableARRPreload(timer); // Only update the period once all channels are done
+ timer->AUTORLD = 0xffffffff;
+
+ DDL_TMR_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
+
+ motor->dmaInitStruct.Direction = DDL_DMA_DIRECTION_PERIPH_TO_MEMORY;
+ xDDL_EX_DMA_Init(motor->dmaRef, pDmaInit);
+}
+#endif
+
+
+FAST_CODE void pwmCompleteDshotMotorUpdate(void)
+{
+ /* If there is a dshot command loaded up, time it correctly with motor update*/
+ if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
+ return;
+ }
+
+ for (int i = 0; i < dmaMotorTimerCount; i++) {
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ xDDL_EX_DMA_SetDataLength(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength);
+ xDDL_EX_DMA_EnableResource(dmaMotorTimers[i].dmaBurstRef);
+
+ /* configure the DMA Burst Mode */
+ DDL_TMR_ConfigDMABurst(dmaMotorTimers[i].timer, DDL_TMR_DMABURST_BASEADDR_CC1, DDL_TMR_DMABURST_LENGTH_4TRANSFERS);
+ /* Enable the TIM DMA Request */
+ DDL_TMR_EnableDMAReq_UPDATE(dmaMotorTimers[i].timer);
+ } else
+#endif
+ {
+ DDL_TMR_DisableARRPreload(dmaMotorTimers[i].timer);
+ dmaMotorTimers[i].timer->AUTORLD = dmaMotorTimers[i].outputPeriod;
+
+ /* Reset timer counter */
+ DDL_TMR_SetCounter(dmaMotorTimers[i].timer, 0);
+ /* Enable channel DMA requests */
+ DDL_EX_TMR_EnableIT(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_IT_TCIF)) {
+ motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
+#ifdef USE_DSHOT_TELEMETRY
+ dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
+#endif
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ xDDL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
+ DDL_TMR_DisableDMAReq_UPDATE(motor->timerHardware->tim);
+ } else
+#endif
+ {
+ xDDL_EX_DMA_DisableResource(motor->dmaRef);
+ DDL_EX_TMR_DisableIT(motor->timerHardware->tim, motor->timerDmaSource);
+ }
+
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ pwmDshotSetDirectionInput(motor);
+ xDDL_EX_DMA_SetDataLength(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
+ xDDL_EX_DMA_EnableResource(motor->dmaRef);
+ DDL_EX_TMR_EnableIT(motor->timerHardware->tim, motor->timerDmaSource);
+ dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
+ }
+#endif
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
+ }
+}
+
+bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
+{
+#ifdef USE_DSHOT_TELEMETRY
+#define OCINIT motor->ocInitStruct
+#define DMAINIT motor->dmaInitStruct
+#else
+ DDL_TMR_OC_InitTypeDef ocInitStruct;
+ DDL_DMA_InitTypeDef dmaInitStruct;
+#define OCINIT ocInitStruct
+#define DMAINIT dmaInitStruct
+#endif
+
+ dmaResource_t *dmaRef = NULL;
+ uint32_t dmaChannel = 0;
+#if defined(USE_DMA_SPEC)
+ const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
+
+ if (dmaSpec != NULL) {
+ dmaRef = dmaSpec->ref;
+ dmaChannel = dmaSpec->channel;
+ }
+#else
+ dmaRef = timerHardware->dmaRef;
+ dmaChannel = timerHardware->dmaChannel;
+#endif
+
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ dmaRef = timerHardware->dmaTimUPRef;
+ dmaChannel = timerHardware->dmaTimUPChannel;
+ }
+#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;
+
+ TMR_TypeDef *timer = timerHardware->tim;
+
+ const uint8_t timerIndex = getTimerIndex(timer);
+ const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1);
+
+ motor->timer = &dmaMotorTimers[timerIndex];
+ motor->index = motorIndex;
+
+ const IO_t motorIO = IOGetByTag(timerHardware->tag);
+ uint8_t pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PULLDOWN : GPIO_PULLUP;
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ output ^= TIMER_OUTPUT_INVERTED;
+ }
+#endif
+ motor->timerHardware = timerHardware;
+
+ motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, pupMode);
+
+ IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
+
+ if (configureTimer) {
+ DDL_TMR_InitTypeDef init;
+ DDL_TMR_StructInit(&init);
+
+ RCC_ClockCmd(timerRCC(timer), ENABLE);
+ DDL_TMR_DisableCounter(timer);
+
+ init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
+ init.Autoreload = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1;
+ init.ClockDivision = DDL_TMR_CLOCKDIVISION_DIV1;
+ init.RepetitionCounter = 0;
+ init.CounterMode = DDL_TMR_COUNTERMODE_UP;
+ DDL_TMR_Init(timer, &init);
+ }
+
+ DDL_TMR_OC_StructInit(&OCINIT);
+ OCINIT.OCMode = DDL_TMR_OCMODE_PWM1;
+ if (output & TIMER_OUTPUT_N_CHANNEL) {
+ OCINIT.OCNState = DDL_TMR_OCSTATE_ENABLE;
+ OCINIT.OCNIdleState = DDL_TMR_OCIDLESTATE_LOW;
+ OCINIT.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? DDL_TMR_OCPOLARITY_LOW : DDL_TMR_OCPOLARITY_HIGH;
+ } else {
+ OCINIT.OCState = DDL_TMR_OCSTATE_ENABLE;
+ OCINIT.OCIdleState = DDL_TMR_OCIDLESTATE_HIGH;
+ OCINIT.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? DDL_TMR_OCPOLARITY_LOW : DDL_TMR_OCPOLARITY_HIGH;
+ }
+ OCINIT.CompareValue = 0;
+
+#ifdef USE_DSHOT_TELEMETRY
+ DDL_TMR_IC_StructInit(&motor->icInitStruct);
+ motor->icInitStruct.ICPolarity = DDL_TMR_IC_POLARITY_BOTHEDGE;
+ motor->icInitStruct.ICPrescaler = DDL_TMR_ICPSC_DIV1;
+ motor->icInitStruct.ICFilter = 2;
+#endif
+
+ uint32_t channel = 0;
+ switch (timerHardware->channel) {
+ case TMR_CHANNEL_1: channel = DDL_TMR_CHANNEL_CH1; break;
+ case TMR_CHANNEL_2: channel = DDL_TMR_CHANNEL_CH2; break;
+ case TMR_CHANNEL_3: channel = DDL_TMR_CHANNEL_CH3; break;
+ case TMR_CHANNEL_4: channel = DDL_TMR_CHANNEL_CH4; break;
+ }
+ motor->llChannel = channel;
+
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ motor->timer->dmaBurstRef = dmaRef;
+#ifdef USE_DSHOT_TELEMETRY
+ motor->dmaRef = dmaRef;
+#endif
+ } else
+#endif
+ {
+ motor->timerDmaSource = timerDmaSource(timerHardware->channel);
+ motor->timer->timerDmaSources &= ~motor->timerDmaSource;
+ }
+
+ if (!dmaIsConfigured) {
+ xDDL_EX_DMA_DisableResource(dmaRef);
+ xDDL_EX_DMA_DeInit(dmaRef);
+
+ dmaEnable(dmaIdentifier);
+ }
+
+ DDL_DMA_StructInit(&DMAINIT);
+#ifdef USE_DSHOT_DMAR
+ if (useBurstDshot) {
+ motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
+
+ DMAINIT.Channel = dmaChannel;
+ DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer;
+ DMAINIT.FIFOThreshold = DDL_DMA_FIFOTHRESHOLD_FULL;
+ DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)&timerHardware->tim->DMAR;
+ } else
+#endif
+ {
+ motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
+
+ DMAINIT.Channel = dmaChannel;
+ DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer;
+ DMAINIT.FIFOThreshold = DDL_DMA_FIFOTHRESHOLD_1_4;
+ DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)timerChCCR(timerHardware);
+ }
+
+ DMAINIT.Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH;
+ DMAINIT.FIFOMode = DDL_DMA_FIFOMODE_ENABLE;
+ DMAINIT.MemBurst = DDL_DMA_MBURST_SINGLE;
+ DMAINIT.PeriphBurst = DDL_DMA_PBURST_SINGLE;
+ DMAINIT.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
+ DMAINIT.PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT;
+ DMAINIT.MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT;
+ DMAINIT.PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_WORD;
+ DMAINIT.MemoryOrM2MDstDataSize = DDL_DMA_MDATAALIGN_WORD;
+ DMAINIT.Mode = DDL_DMA_MODE_NORMAL;
+ DMAINIT.Priority = DDL_DMA_PRIORITY_HIGH;
+
+ if (!dmaIsConfigured) {
+ xDDL_EX_DMA_Init(dmaRef, &DMAINIT);
+ xDDL_EX_DMA_EnableIT_TC(dmaRef);
+ }
+
+ motor->dmaRef = dmaRef;
+
+#ifdef USE_DSHOT_TELEMETRY
+ motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
+ ( 16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
+ motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
+ pwmDshotSetDirectionOutput(motor);
+#else
+ pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
+#endif
+
+#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);
+ }
+
+ DDL_TMR_OC_Init(timer, channel, &OCINIT);
+ DDL_TMR_OC_EnablePreload(timer, channel);
+ DDL_TMR_OC_DisableFast(timer, channel);
+
+ DDL_TMR_EnableCounter(timer);
+ if (output & TIMER_OUTPUT_N_CHANNEL) {
+ DDL_EX_TMR_CC_EnableNChannel(timer, channel);
+ } else {
+ DDL_TMR_CC_EnableChannel(timer, channel);
+ }
+
+ if (configureTimer) {
+ DDL_TMR_EnableAllOutputs(timer);
+ DDL_TMR_EnableARRPreload(timer);
+ DDL_TMR_EnableCounter(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/main/drivers/mcu/apm32/rcm_apm32.c b/src/main/drivers/mcu/apm32/rcm_apm32.c
new file mode 100644
index 0000000000..9f86cf594c
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/rcm_apm32.c
@@ -0,0 +1,114 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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/rcc.h"
+
+void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
+{
+ int tag = periphTag >> 5;
+ uint32_t mask = 1 << (periphTag & 0x1f);
+
+// Note on "suffix" macro parameter:
+// ENR and RSTR naming conventions for buses with multiple registers per bus differs among MCU types.
+// ST decided to use AxBn{L,H}ENR convention for H7 which can be handled with simple "ENR" (or "RSTR") contatenation,
+// while use AxBnENR{1,2} convention for G4 which requires extra "suffix" to be concatenated.
+// Here, we use "suffix" for all MCU types and leave it as empty where not applicable.
+
+#define NOSUFFIX // Empty
+
+#define __DAL_RCM_CLK_ENABLE(bus, suffix, enbit) do { \
+ __IO uint32_t tmpreg; \
+ SET_BIT(RCM->bus ## CLKEN ## suffix, enbit); \
+ /* Delay after an RCM peripheral clock enabling */ \
+ tmpreg = READ_BIT(RCM->bus ## CLKEN ## suffix, enbit); \
+ UNUSED(tmpreg); \
+ } while(0)
+
+#define __DAL_RCM_CLK_DISABLE(bus, suffix, enbit) (RCM->bus ## CLKEN ## suffix &= ~(enbit))
+
+#define __DAL_RCM_CLK(bus, suffix, enbit, newState) \
+ if (newState == ENABLE) { \
+ __DAL_RCM_CLK_ENABLE(bus, suffix, enbit); \
+ } else { \
+ __DAL_RCM_CLK_DISABLE(bus, suffix, enbit); \
+ }
+
+ switch (tag) {
+ case RCC_AHB1:
+ __DAL_RCM_CLK(AHB1, NOSUFFIX, mask, NewState);
+ break;
+
+ case RCC_AHB2:
+ __DAL_RCM_CLK(AHB2, NOSUFFIX, mask, NewState);
+ break;
+
+ case RCC_APB1:
+ __DAL_RCM_CLK(APB1, NOSUFFIX, mask, NewState);
+ break;
+
+ case RCC_APB2:
+ __DAL_RCM_CLK(APB2, NOSUFFIX, mask, NewState);
+ break;
+
+ case RCC_AHB3:
+ __DAL_RCM_CLK(AHB3, NOSUFFIX, mask, NewState);
+ break;
+ }
+}
+
+void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState)
+{
+ int tag = periphTag >> 5;
+ uint32_t mask = 1 << (periphTag & 0x1f);
+
+// Peripheral reset control relies on RST bits are identical to ENR bits where applicable
+
+#define __DAL_RCM_FORCE_RESET(bus, suffix, enbit) (RCM->bus ## RST ## suffix |= (enbit))
+#define __DAL_RCM_RELEASE_RESET(bus, suffix, enbit) (RCM->bus ## RST ## suffix &= ~(enbit))
+#define __DAL_RCM_RESET(bus, suffix, enbit, NewState) \
+ if (NewState == ENABLE) { \
+ __DAL_RCM_RELEASE_RESET(bus, suffix, enbit); \
+ } else { \
+ __DAL_RCM_FORCE_RESET(bus, suffix, enbit); \
+ }
+
+ switch (tag) {
+ case RCC_AHB1:
+ __DAL_RCM_RESET(AHB1, NOSUFFIX, mask, NewState);
+ break;
+
+ case RCC_AHB2:
+ __DAL_RCM_RESET(AHB2, NOSUFFIX, mask, NewState);
+ break;
+
+ case RCC_AHB3:
+ __DAL_RCM_RESET(AHB3, NOSUFFIX, mask, NewState);
+ break;
+
+ case RCC_APB1:
+ __DAL_RCM_RESET(APB1, NOSUFFIX, mask, NewState);
+ break;
+
+ case RCC_APB2:
+ __DAL_RCM_RESET(APB2, NOSUFFIX, mask, NewState);
+ break;
+ }
+}
diff --git a/src/main/drivers/mcu/apm32/serial_uart_apm32.c b/src/main/drivers/mcu/apm32/serial_uart_apm32.c
new file mode 100644
index 0000000000..a04b5de27a
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/serial_uart_apm32.c
@@ -0,0 +1,359 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 .
+ */
+
+/*
+ * Authors:
+ * jflyper - Refactoring, cleanup and made pin-configurable
+ * Dominic Clifton - Serial port abstraction, Separation of common STM32 code for cleanflight, various cleanups.
+ * Hamasaki/Timecop - Initial baseflight code
+*/
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#ifdef USE_UART
+
+#include "build/build_config.h"
+#include "build/atomic.h"
+
+#include "common/utils.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/inverter.h"
+#include "drivers/dma.h"
+#include "drivers/rcc.h"
+
+#include "drivers/serial.h"
+#include "drivers/serial_uart.h"
+#include "drivers/serial_uart_impl.h"
+
+static void usartConfigurePinInversion(uartPort_t *uartPort)
+{
+#if !defined(USE_INVERTER)
+ UNUSED(uartPort);
+#else
+ bool inverted = uartPort->port.options & SERIAL_INVERTED;
+
+#ifdef USE_INVERTER
+ if (inverted) {
+ // Enable hardware inverter if available.
+ enableInverter(uartPort->USARTx, true);
+ }
+#endif
+#endif
+}
+
+// XXX uartReconfigure does not handle resource management properly.
+
+void uartReconfigure(uartPort_t *uartPort)
+{
+ DAL_UART_DeInit(&uartPort->Handle);
+ uartPort->Handle.Init.BaudRate = uartPort->port.baudRate;
+ // according to the stm32 documentation wordlen has to be 9 for parity bits
+ // this does not seem to matter for rx but will give bad data on tx!
+ uartPort->Handle.Init.WordLength = (uartPort->port.options & SERIAL_PARITY_EVEN) ? UART_WORDLENGTH_9B : UART_WORDLENGTH_8B;
+ uartPort->Handle.Init.StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_STOPBITS_2 : USART_STOPBITS_1;
+ uartPort->Handle.Init.Parity = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_PARITY_EVEN : USART_PARITY_NONE;
+ uartPort->Handle.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+ uartPort->Handle.Init.OverSampling = UART_OVERSAMPLING_16;
+ uartPort->Handle.Init.Mode = 0;
+
+ if (uartPort->port.mode & MODE_RX)
+ uartPort->Handle.Init.Mode |= UART_MODE_RX;
+ if (uartPort->port.mode & MODE_TX)
+ uartPort->Handle.Init.Mode |= UART_MODE_TX;
+
+
+ usartConfigurePinInversion(uartPort);
+
+#ifdef TARGET_USART_CONFIG
+ void usartTargetConfigure(uartPort_t *);
+ usartTargetConfigure(uartPort);
+#endif
+
+ if (uartPort->port.options & SERIAL_BIDIR)
+ {
+ DAL_HalfDuplex_Init(&uartPort->Handle);
+ }
+ else
+ {
+ DAL_UART_Init(&uartPort->Handle);
+ }
+
+ // Receive DMA or IRQ
+ if (uartPort->port.mode & MODE_RX)
+ {
+#ifdef USE_DMA
+ if (uartPort->rxDMAResource)
+ {
+ uartPort->rxDMAHandle.Instance = (DMA_ARCH_TYPE *)uartPort->rxDMAResource;
+ uartPort->txDMAHandle.Init.Channel = uartPort->rxDMAChannel;
+ uartPort->rxDMAHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
+ uartPort->rxDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+ uartPort->rxDMAHandle.Init.MemInc = DMA_MINC_ENABLE;
+ uartPort->rxDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ uartPort->rxDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ uartPort->rxDMAHandle.Init.Mode = DMA_CIRCULAR;
+#if defined(APM32F4)
+ uartPort->rxDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ uartPort->rxDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
+ uartPort->rxDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+ uartPort->rxDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+#endif
+ uartPort->rxDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM;
+
+
+ DAL_DMA_DeInit(&uartPort->rxDMAHandle);
+ DAL_DMA_Init(&uartPort->rxDMAHandle);
+ /* Associate the initialized DMA handle to the UART handle */
+ __DAL_LINKDMA(&uartPort->Handle, hdmarx, uartPort->rxDMAHandle);
+
+ DAL_UART_Receive_DMA(&uartPort->Handle, (uint8_t*)uartPort->port.rxBuffer, uartPort->port.rxBufferSize);
+
+ uartPort->rxDMAPos = __DAL_DMA_GET_COUNTER(&uartPort->rxDMAHandle);
+ } else
+#endif
+ {
+ /* Enable the UART Parity Error Interrupt */
+ SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_PEIEN);
+
+ /* Enable the UART Error Interrupt: (Frame error, noise error, overrun error) */
+ SET_BIT(uartPort->USARTx->CTRL3, USART_CTRL3_ERRIEN);
+
+ /* Enable the UART Data Register not empty Interrupt */
+ SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_RXBNEIEN);
+
+ /* Enable Idle Line detection */
+ SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_IDLEIEN);
+ }
+ }
+
+ // Transmit DMA or IRQ
+ if (uartPort->port.mode & MODE_TX) {
+#ifdef USE_DMA
+ if (uartPort->txDMAResource) {
+ uartPort->txDMAHandle.Instance = (DMA_ARCH_TYPE *)uartPort->txDMAResource;
+ uartPort->txDMAHandle.Init.Channel = uartPort->txDMAChannel;
+ uartPort->txDMAHandle.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ uartPort->txDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+ uartPort->txDMAHandle.Init.MemInc = DMA_MINC_ENABLE;
+ uartPort->txDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+ uartPort->txDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+ uartPort->txDMAHandle.Init.Mode = DMA_NORMAL;
+ uartPort->txDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ uartPort->txDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
+ uartPort->txDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+ uartPort->txDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+ uartPort->txDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM;
+
+
+ DAL_DMA_DeInit(&uartPort->txDMAHandle);
+ DAL_StatusTypeDef status = DAL_DMA_Init(&uartPort->txDMAHandle);
+ if (status != DAL_OK)
+ {
+ while (1);
+ }
+ /* Associate the initialized DMA handle to the UART handle */
+ __DAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle);
+
+ __DAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0);
+ } else
+#endif
+ {
+
+ /* Enable the UART Transmit Data Register Empty Interrupt */
+ SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_TXBEIEN);
+ SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_TXCIEN);
+ }
+ }
+ return;
+}
+
+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)) {
+ // TX is high so we're good to transmit
+
+ // Enable USART TX output
+ uart->txPinState = TX_PIN_ACTIVE;
+ IOConfigGPIOAF(txIO, IOCFG_AF_PP, uart->tx.af);
+
+ // Enable the UART transmitter
+ SET_BIT(s->Handle.Instance->CTRL1, USART_CTRL1_TXEN);
+
+ return true;
+ } else {
+ // TX line is pulled low so don't enable USART TX
+ 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);
+
+ // Disable the UART transmitter
+ CLEAR_BIT(s->Handle.Instance->CTRL1, USART_CTRL1_TXEN);
+
+ // Switch TX to an input with pullup so it's state can be monitored
+ uart->txPinState = TX_PIN_MONITOR;
+ IOConfigGPIO(txIO, IOCFG_IPU);
+ }
+}
+
+#ifdef USE_DMA
+
+void uartTryStartTxDMA(uartPort_t *s)
+{
+ ATOMIC_BLOCK(NVIC_PRIO_SERIALUART_TXDMA) {
+ if (IS_DMA_ENABLED(s->txDMAResource)) {
+ // DMA is already in progress
+ return;
+ }
+
+ DAL_UART_StateTypeDef state = DAL_UART_GetState(&s->Handle);
+ if ((state & DAL_UART_STATE_BUSY_TX) == DAL_UART_STATE_BUSY_TX) {
+ // UART is still transmitting
+ return;
+ }
+
+ if (s->port.txBufferHead == s->port.txBufferTail) {
+ // No more data to transmit
+ s->txDMAEmpty = true;
+ return;
+ }
+
+ uint16_t size;
+ uint32_t fromwhere = s->port.txBufferTail;
+
+ if (s->port.txBufferHead > s->port.txBufferTail) {
+ size = s->port.txBufferHead - s->port.txBufferTail;
+ s->port.txBufferTail = s->port.txBufferHead;
+ } else {
+ size = s->port.txBufferSize - s->port.txBufferTail;
+ s->port.txBufferTail = 0;
+ }
+ s->txDMAEmpty = false;
+
+ DAL_UART_Transmit_DMA(&s->Handle, (uint8_t *)&s->port.txBuffer[fromwhere], size);
+ }
+}
+
+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)) {
+ // Switch TX to an input with pullup so it's state can be monitored
+ uartTxMonitor(s);
+ }
+}
+
+void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor)
+{
+ UNUSED(descriptor);
+ uartPort_t *s = &(((uartDevice_t*)(descriptor->userParam))->port);
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF))
+ {
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF);
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_FEIF))
+ {
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_FEIF);
+ }
+ handleUsartTxDma(s);
+ }
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF))
+ {
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF);
+ }
+ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_DMEIF))
+ {
+ DMA_CLEAR_FLAG(descriptor, DMA_IT_DMEIF);
+ }
+}
+#endif // USE_DMA
+
+FAST_IRQ_HANDLER void uartIrqHandler(uartPort_t *s)
+{
+ UART_HandleTypeDef *huart = &s->Handle;
+ uint32_t isrflags = READ_REG(huart->Instance->STS);
+ uint32_t cr1its = READ_REG(huart->Instance->CTRL1);
+ uint32_t cr3its = READ_REG(huart->Instance->CTRL3);
+ /* UART in mode Receiver ---------------------------------------------------*/
+ if (!s->rxDMAResource && (((isrflags & USART_STS_RXBNEFLG) != RESET) && ((cr1its & USART_CTRL1_RXBNEIEN) != RESET))) {
+ if (s->port.rxCallback) {
+ s->port.rxCallback(huart->Instance->DATA, s->port.rxCallbackData);
+ } else {
+ s->port.rxBuffer[s->port.rxBufferHead] = huart->Instance->DATA;
+ s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
+ }
+ }
+
+ // Detect completion of transmission
+ if (((isrflags & USART_STS_TXCFLG) != RESET) && ((cr1its & USART_CTRL1_TXCIEN) != RESET)) {
+ // Switch TX to an input with pullup so it's state can be monitored
+ uartTxMonitor(s);
+
+ __DAL_UART_CLEAR_FLAG(huart, UART_IT_TC);
+ }
+
+ if (!s->txDMAResource && (((isrflags & USART_STS_TXBEFLG) != RESET) && ((cr1its & USART_CTRL1_TXBEIEN) != RESET))) {
+ if (s->port.txBufferTail != s->port.txBufferHead) {
+ huart->Instance->DATA = (((uint16_t) s->port.txBuffer[s->port.txBufferTail]) & (uint16_t) 0x01FFU);
+ s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
+ } else {
+ __DAL_UART_DISABLE_IT(huart, UART_IT_TXE);
+ }
+ }
+
+ if (((isrflags & USART_STS_OVREFLG) != RESET) && (((cr1its & USART_CTRL1_RXBNEIEN) != RESET)
+ || ((cr3its & USART_CTRL3_ERRIEN) != RESET))) {
+ __DAL_UART_CLEAR_OREFLAG(huart);
+ }
+
+ if (((isrflags & USART_STS_IDLEFLG) != RESET) && ((cr1its & USART_STS_IDLEFLG) != RESET)) {
+ if (s->port.idleCallback) {
+ s->port.idleCallback();
+ }
+
+ // clear
+ (void) huart->Instance->STS;
+ (void) huart->Instance->DATA;
+ }
+}
+
+#endif // USE_UART
diff --git a/src/main/drivers/mcu/apm32/serial_uart_apm32f4xx.c b/src/main/drivers/mcu/apm32/serial_uart_apm32f4xx.c
new file mode 100644
index 0000000000..7e9153fc03
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/serial_uart_apm32f4xx.c
@@ -0,0 +1,318 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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"
+#include "build/debug.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 "drivers/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_UART1
+ {
+ .device = UARTDEV_1,
+ .reg = USART1,
+ .rxDMAChannel = DMA_CHANNEL_4,
+ .txDMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART1_RX_DMA
+ .rxDMAResource = (dmaResource_t *)DMA2_Stream5,
+#endif
+#ifdef USE_UART1_TX_DMA
+ .txDMAResource = (dmaResource_t *)DMA2_Stream7,
+#endif
+ .rxPins = {
+ { DEFIO_TAG_E(PA10), GPIO_AF7_USART1 },
+ { DEFIO_TAG_E(PB7), GPIO_AF7_USART1 },
+ },
+ .txPins = {
+ { DEFIO_TAG_E(PA9), GPIO_AF7_USART1 },
+ { DEFIO_TAG_E(PB6), GPIO_AF7_USART1 },
+ },
+ .af = GPIO_AF7_USART1,
+ .rcc = RCC_APB2(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
+ {
+ .device = UARTDEV_2,
+ .reg = USART2,
+ .rxDMAChannel = DMA_CHANNEL_4,
+ .txDMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART2_RX_DMA
+ .rxDMAResource = (dmaResource_t *)DMA1_Stream5,
+#endif
+#ifdef USE_UART2_TX_DMA
+ .txDMAResource = (dmaResource_t *)DMA1_Stream6,
+#endif
+ .rxPins = {
+ { DEFIO_TAG_E(PA3), GPIO_AF7_USART2 },
+ { DEFIO_TAG_E(PD6), GPIO_AF7_USART2 }
+ },
+ .txPins = {
+ { DEFIO_TAG_E(PA2), GPIO_AF7_USART2 },
+ { DEFIO_TAG_E(PD5), GPIO_AF7_USART2 }
+ },
+ .af = GPIO_AF7_USART2,
+ .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
+ {
+ .device = UARTDEV_3,
+ .reg = USART3,
+ .rxDMAChannel = DMA_CHANNEL_4,
+ .txDMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART3_RX_DMA
+ .rxDMAResource = (dmaResource_t *)DMA1_Stream1,
+#endif
+#ifdef USE_UART3_TX_DMA
+ .txDMAResource = (dmaResource_t *)DMA1_Stream3,
+#endif
+ .rxPins = {
+ { DEFIO_TAG_E(PB11), GPIO_AF7_USART3 },
+ { DEFIO_TAG_E(PC11), GPIO_AF7_USART3 },
+ { DEFIO_TAG_E(PD9), GPIO_AF7_USART3 }
+ },
+ .txPins = {
+ { DEFIO_TAG_E(PB10), GPIO_AF7_USART3 },
+ { DEFIO_TAG_E(PC10), GPIO_AF7_USART3 },
+ { DEFIO_TAG_E(PD8), GPIO_AF7_USART3 }
+ },
+ .af = GPIO_AF7_USART3,
+ .rcc = RCC_APB1(USART3),
+ .irqn = USART3_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART3_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART3,
+ .txBuffer = uart3TxBuffer,
+ .rxBuffer = uart3RxBuffer,
+ .txBufferSize = sizeof(uart3TxBuffer),
+ .rxBufferSize = sizeof(uart3RxBuffer),
+ },
+#endif
+
+#ifdef USE_UART4
+ {
+ .device = UARTDEV_4,
+ .reg = UART4,
+ .rxDMAChannel = DMA_CHANNEL_4,
+ .txDMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART4_RX_DMA
+ .rxDMAResource = (dmaResource_t *)DMA1_Stream2,
+#endif
+#ifdef USE_UART4_TX_DMA
+ .txDMAResource = (dmaResource_t *)DMA1_Stream4,
+#endif
+ .rxPins = {
+ { DEFIO_TAG_E(PA1), GPIO_AF8_UART4 },
+ { DEFIO_TAG_E(PC11), GPIO_AF8_UART4 }
+ },
+ .txPins = {
+ { DEFIO_TAG_E(PA0), GPIO_AF8_UART4 },
+ { DEFIO_TAG_E(PC10), GPIO_AF8_UART4 }
+ },
+ .af = GPIO_AF8_UART4,
+ .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
+ {
+ .device = UARTDEV_5,
+ .reg = UART5,
+ .rxDMAChannel = DMA_CHANNEL_4,
+ .txDMAChannel = DMA_CHANNEL_4,
+#ifdef USE_UART5_RX_DMA
+ .rxDMAResource = (dmaResource_t *)DMA1_Stream0,
+#endif
+#ifdef USE_UART5_TX_DMA
+ .txDMAResource = (dmaResource_t *)DMA1_Stream7,
+#endif
+ .rxPins = {
+ { DEFIO_TAG_E(PD2), GPIO_AF8_UART5 }
+ },
+ .txPins = {
+ { DEFIO_TAG_E(PC12), GPIO_AF8_UART5 }
+ },
+ .af = GPIO_AF8_UART5,
+ .rcc = RCC_APB1(UART5),
+ .irqn = UART5_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART5_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART5,
+ .txBuffer = uart5TxBuffer,
+ .rxBuffer = uart5RxBuffer,
+ .txBufferSize = sizeof(uart5TxBuffer),
+ .rxBufferSize = sizeof(uart5RxBuffer),
+ },
+#endif
+
+#ifdef USE_UART6
+ {
+ .device = UARTDEV_6,
+ .reg = USART6,
+ .rxDMAChannel = DMA_CHANNEL_5,
+ .txDMAChannel = DMA_CHANNEL_5,
+#ifdef USE_UART6_RX_DMA
+ .rxDMAResource = (dmaResource_t *)DMA2_Stream1,
+#endif
+#ifdef USE_UART6_TX_DMA
+ .txDMAResource = (dmaResource_t *)DMA2_Stream6,
+#endif
+ .rxPins = {
+ { DEFIO_TAG_E(PC7), GPIO_AF8_USART6 },
+ { DEFIO_TAG_E(PG9), GPIO_AF8_USART6 }
+ },
+ .txPins = {
+ { DEFIO_TAG_E(PC6), GPIO_AF8_USART6 },
+ { DEFIO_TAG_E(PG14), GPIO_AF8_USART6 }
+ },
+ .af = GPIO_AF8_USART6,
+ .rcc = RCC_APB2(USART6),
+ .irqn = USART6_IRQn,
+ .txPriority = NVIC_PRIO_SERIALUART6_TXDMA,
+ .rxPriority = NVIC_PRIO_SERIALUART6,
+ .txBuffer = uart6TxBuffer,
+ .rxBuffer = uart6RxBuffer,
+ .txBufferSize = sizeof(uart6TxBuffer),
+ .rxBufferSize = sizeof(uart6RxBuffer),
+ },
+#endif
+};
+
+// XXX Should serialUART be consolidated?
+
+uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, portOptions_e options)
+{
+ uartDevice_t *uartdev = uartDevmap[device];
+ if (!uartdev) {
+ return NULL;
+ }
+
+ uartPort_t *s = &(uartdev->port);
+
+ s->port.vTable = uartVTable;
+
+ s->port.baudRate = baudRate;
+
+ const uartHardware_t *hardware = uartdev->hardware;
+
+ s->USARTx = hardware->reg;
+
+ s->port.rxBuffer = hardware->rxBuffer;
+ s->port.txBuffer = hardware->txBuffer;
+ s->port.rxBufferSize = hardware->rxBufferSize;
+ s->port.txBufferSize = hardware->txBufferSize;
+
+ s->checkUsartTxOutput = checkUsartTxOutput;
+
+#ifdef USE_DMA
+ uartConfigureDma(uartdev);
+#endif
+
+ s->Handle.Instance = hardware->reg;
+
+ if (hardware->rcc) {
+ RCC_ClockCmd(hardware->rcc, ENABLE);
+ }
+
+ IO_t txIO = IOGetByTag(uartdev->tx.pin);
+ IO_t rxIO = IOGetByTag(uartdev->rx.pin);
+
+ uartdev->txPinState = TX_PIN_IGNORE;
+
+ if ((options & SERIAL_BIDIR) && txIO) {
+ ioConfig_t ioCfg = IO_CONFIG(
+ ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_MODE_AF_PP : GPIO_MODE_AF_OD,
+ GPIO_SPEED_FREQ_HIGH,
+ ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_PULLDOWN : GPIO_PULLUP
+ );
+
+ IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
+ IOConfigGPIOAF(txIO, ioCfg, uartdev->tx.af);
+ }
+ else {
+ if ((mode & MODE_TX) && txIO) {
+ IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
+
+ if (options & SERIAL_CHECK_TX) {
+ uartdev->txPinState = TX_PIN_MONITOR;
+ // Switch TX to UART output whilst UART sends idle preamble
+ checkUsartTxOutput(s);
+ } else {
+ IOConfigGPIOAF(txIO, IOCFG_AF_PP, uartdev->tx.af);
+ }
+ }
+
+ if ((mode & MODE_RX) && rxIO) {
+ IOInit(rxIO, OWNER_SERIAL_RX, RESOURCE_INDEX(device));
+ IOConfigGPIOAF(rxIO, IOCFG_AF_PP, uartdev->rx.af);
+ }
+ }
+
+#ifdef USE_DMA
+ if (!s->rxDMAResource) {
+ DAL_NVIC_SetPriority(hardware->irqn, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority));
+ DAL_NVIC_EnableIRQ(hardware->irqn);
+ }
+#endif
+
+ return s;
+}
+#endif // USE_UART
diff --git a/src/main/drivers/mcu/apm32/system_apm32f4xx.c b/src/main/drivers/mcu/apm32/system_apm32f4xx.c
new file mode 100644
index 0000000000..25bdeff99b
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/system_apm32f4xx.c
@@ -0,0 +1,169 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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"
+
+#include "drivers/accgyro/accgyro_mpu.h"
+#include "drivers/exti.h"
+#include "drivers/nvic.h"
+#include "drivers/system.h"
+#include "drivers/persistent.h"
+
+void systemReset(void)
+{
+ __disable_irq();
+ NVIC_SystemReset();
+}
+
+void systemResetToBootloader(bootloaderRequestType_e requestType)
+{
+ switch (requestType) {
+ case BOOTLOADER_REQUEST_ROM:
+ default:
+ persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
+
+ break;
+ }
+
+ __disable_irq();
+ NVIC_SystemReset();
+}
+
+typedef void resetHandler_t(void);
+
+typedef struct isrVector_s {
+ __I uint32_t stackEnd;
+ resetHandler_t *resetHandler;
+} isrVector_t;
+
+// Used in the startup files for F4
+void checkForBootLoaderRequest(void)
+{
+ uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON);
+
+ if (bootloaderRequest != RESET_BOOTLOADER_REQUEST_ROM) {
+ return;
+ }
+ persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
+ extern isrVector_t system_isr_vector_table_base;
+
+ __set_MSP(system_isr_vector_table_base.stackEnd);
+ system_isr_vector_table_base.resetHandler();
+ while (1);
+}
+
+void enableGPIOPowerUsageAndNoiseReductions(void)
+{
+ __DAL_RCM_BKPSRAM_CLK_ENABLE();
+ __DAL_RCM_DMA1_CLK_ENABLE();
+ __DAL_RCM_DMA2_CLK_ENABLE();
+
+ __DAL_RCM_TMR2_CLK_ENABLE();
+ __DAL_RCM_TMR3_CLK_ENABLE();
+ __DAL_RCM_TMR4_CLK_ENABLE();
+ __DAL_RCM_TMR5_CLK_ENABLE();
+ __DAL_RCM_TMR6_CLK_ENABLE();
+ __DAL_RCM_TMR7_CLK_ENABLE();
+ __DAL_RCM_TMR12_CLK_ENABLE();
+ __DAL_RCM_TMR13_CLK_ENABLE();
+ __DAL_RCM_TMR14_CLK_ENABLE();
+ __DAL_RCM_WWDT_CLK_ENABLE();
+ __DAL_RCM_SPI2_CLK_ENABLE();
+ __DAL_RCM_SPI3_CLK_ENABLE();
+ __DAL_RCM_USART2_CLK_ENABLE();
+ __DAL_RCM_USART3_CLK_ENABLE();
+ __DAL_RCM_UART4_CLK_ENABLE();
+ __DAL_RCM_UART5_CLK_ENABLE();
+ __DAL_RCM_I2C1_CLK_ENABLE();
+ __DAL_RCM_I2C2_CLK_ENABLE();
+ __DAL_RCM_I2C3_CLK_ENABLE();
+ __DAL_RCM_CAN1_CLK_ENABLE();
+ __DAL_RCM_CAN2_CLK_ENABLE();
+ __DAL_RCM_PMU_CLK_ENABLE();
+ __DAL_RCM_DAC_CLK_ENABLE();
+
+ __DAL_RCM_TMR1_CLK_ENABLE();
+ __DAL_RCM_TMR8_CLK_ENABLE();
+ __DAL_RCM_USART1_CLK_ENABLE();
+ __DAL_RCM_USART6_CLK_ENABLE();
+ __DAL_RCM_ADC1_CLK_ENABLE();
+ __DAL_RCM_ADC2_CLK_ENABLE();
+ __DAL_RCM_ADC3_CLK_ENABLE();
+ __DAL_RCM_SDIO_CLK_ENABLE();
+ __DAL_RCM_SPI1_CLK_ENABLE();
+ __DAL_RCM_SYSCFG_CLK_ENABLE();
+ __DAL_RCM_TMR9_CLK_ENABLE();
+ __DAL_RCM_TMR10_CLK_ENABLE();
+ __DAL_RCM_TMR11_CLK_ENABLE();
+}
+
+bool isMPUSoftReset(void)
+{
+ if (cachedRccCsrValue & RCM_CSTS_SWRSTFLG)
+ return true;
+ else
+ return false;
+}
+
+void systemInit(void)
+{
+ persistentObjectInit();
+
+ checkForBootLoaderRequest();
+
+#ifdef USE_DAL_DRIVER
+ DAL_Init();
+#endif
+
+ /* Configure the System clock source, PLL1 and HCLK */
+ DAL_SysClkConfig();
+
+ /* Update SystemCoreClock variable */
+ SystemCoreClockUpdate();
+
+ // Configure NVIC preempt/priority groups
+ DAL_NVIC_SetPriorityGrouping(NVIC_PRIORITY_GROUPING);
+
+ // cache RCM->CSTS value to use it in isMPUSoftReset() and others
+ cachedRccCsrValue = READ_REG(RCM->CSTS);
+
+ // Although VTOR is already loaded with a possible vector table in RAM,
+ // removing the call to NVIC_SetVectorTable causes USB not to become active,
+
+ extern uint8_t isr_vector_table_base;
+ SCB->VTOR = (uint32_t)&isr_vector_table_base | (0x0 & (uint32_t)0x1FFFFF80);
+
+ // Disable USB OTG FS clock
+ __DAL_RCM_USB_OTG_FS_CLK_DISABLE();
+
+ // Clear reset flags
+ SET_BIT(RCM->CSTS, RCM_CSTS_RSTFLGCLR);
+
+ enableGPIOPowerUsageAndNoiseReductions();
+
+ // Init cycle counter
+ cycleCounterInit();
+
+ //--Todo: Set systick here ? // SysTick is updated whenever DAL_RCM_ClockConfig is called.
+}
diff --git a/src/main/drivers/mcu/apm32/timer_apm32.c b/src/main/drivers/mcu/apm32/timer_apm32.c
new file mode 100644
index 0000000000..e733b8ec45
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/timer_apm32.c
@@ -0,0 +1,1198 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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_TIMER
+
+#include "build/atomic.h"
+
+#include "common/utils.h"
+
+#include "drivers/nvic.h"
+
+#include "drivers/io.h"
+#include "drivers/dma.h"
+
+#include "drivers/rcc.h"
+
+#include "drivers/timer.h"
+#include "drivers/timer_impl.h"
+
+#define TIM_N(n) (1 << (n))
+
+/*
+ Groups that allow running different period (ex 50Hz servos + 400Hz throttle + etc):
+ TIM1 2 channels
+ TIM2 4 channels
+ TIM3 4 channels
+ TIM4 4 channels
+*/
+
+/// TODO: DAL in a lot af calls lookupTimerIndex is used. Instead of passing the timer instance the index should be passed.
+#define USED_TIMER_COUNT BITCOUNT(USED_TIMERS)
+
+#define TIM_IT_CCx(ch) (TMR_IT_CC1 << ((ch) / 4))
+
+typedef struct timerConfig_s {
+ // per-timer
+ timerOvrHandlerRec_t *updateCallback;
+
+ // per-channel
+ timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER];
+ timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER];
+
+ // state
+ timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks
+ uint32_t forcedOverflowTimerValue;
+} timerConfig_t;
+timerConfig_t timerConfig[USED_TIMER_COUNT + 1];
+
+typedef struct {
+ channelType_t type;
+} timerChannelInfo_t;
+timerChannelInfo_t timerChannelInfo[TIMER_CHANNEL_COUNT];
+
+typedef struct {
+ uint8_t priority;
+} timerInfo_t;
+timerInfo_t timerInfo[USED_TIMER_COUNT + 1];
+
+typedef struct {
+ TMR_HandleTypeDef Handle;
+} timerHandle_t;
+timerHandle_t timerHandle[USED_TIMER_COUNT + 1];
+
+// return index of timer in timer table. Lowest timer has index 0
+#define TIMER_INDEX(i) BITCOUNT((TIM_N(i) - 1) & USED_TIMERS)
+
+static uint8_t lookupTimerIndex(const TMR_TypeDef *tim)
+{
+#define _CASE_SHF 10 // amount we can safely shift timer address to the right. gcc will throw error if some timers overlap
+#define _CASE_(tim, index) case ((unsigned)tim >> _CASE_SHF): return index; break
+#define _CASE(i) _CASE_(TMR##i##_BASE, TIMER_INDEX(i))
+
+// let gcc do the work, switch should be quite optimized
+ switch ((unsigned)tim >> _CASE_SHF) {
+#if USED_TIMERS & TIM_N(1)
+ _CASE(1);
+#endif
+#if USED_TIMERS & TIM_N(2)
+ _CASE(2);
+#endif
+#if USED_TIMERS & TIM_N(3)
+ _CASE(3);
+#endif
+#if USED_TIMERS & TIM_N(4)
+ _CASE(4);
+#endif
+#if USED_TIMERS & TIM_N(5)
+ _CASE(5);
+#endif
+#if USED_TIMERS & TIM_N(6)
+ _CASE(6);
+#endif
+#if USED_TIMERS & TIM_N(7)
+ _CASE(7);
+#endif
+#if USED_TIMERS & TIM_N(8)
+ _CASE(8);
+#endif
+#if USED_TIMERS & TIM_N(9)
+ _CASE(9);
+#endif
+#if USED_TIMERS & TIM_N(10)
+ _CASE(10);
+#endif
+#if USED_TIMERS & TIM_N(11)
+ _CASE(11);
+#endif
+#if USED_TIMERS & TIM_N(12)
+ _CASE(12);
+#endif
+#if USED_TIMERS & TIM_N(13)
+ _CASE(13);
+#endif
+#if USED_TIMERS & TIM_N(14)
+ _CASE(14);
+#endif
+#if USED_TIMERS & TIM_N(15)
+ _CASE(15);
+#endif
+#if USED_TIMERS & TIM_N(16)
+ _CASE(16);
+#endif
+#if USED_TIMERS & TIM_N(17)
+ _CASE(17);
+#endif
+#if USED_TIMERS & TIM_N(20)
+ _CASE(20);
+#endif
+ default: return ~1; // make sure final index is out of range
+ }
+#undef _CASE
+#undef _CASE_
+}
+
+TMR_TypeDef * const usedTimers[USED_TIMER_COUNT] = {
+#define _DEF(i) TMR##i
+
+#if USED_TIMERS & TIM_N(1)
+ _DEF(1),
+#endif
+#if USED_TIMERS & TIM_N(2)
+ _DEF(2),
+#endif
+#if USED_TIMERS & TIM_N(3)
+ _DEF(3),
+#endif
+#if USED_TIMERS & TIM_N(4)
+ _DEF(4),
+#endif
+#if USED_TIMERS & TIM_N(5)
+ _DEF(5),
+#endif
+#if USED_TIMERS & TIM_N(6)
+ _DEF(6),
+#endif
+#if USED_TIMERS & TIM_N(7)
+ _DEF(7),
+#endif
+#if USED_TIMERS & TIM_N(8)
+ _DEF(8),
+#endif
+#if USED_TIMERS & TIM_N(9)
+ _DEF(9),
+#endif
+#if USED_TIMERS & TIM_N(10)
+ _DEF(10),
+#endif
+#if USED_TIMERS & TIM_N(11)
+ _DEF(11),
+#endif
+#if USED_TIMERS & TIM_N(12)
+ _DEF(12),
+#endif
+#if USED_TIMERS & TIM_N(13)
+ _DEF(13),
+#endif
+#if USED_TIMERS & TIM_N(14)
+ _DEF(14),
+#endif
+#if USED_TIMERS & TIM_N(15)
+ _DEF(15),
+#endif
+#if USED_TIMERS & TIM_N(16)
+ _DEF(16),
+#endif
+#if USED_TIMERS & TIM_N(17)
+ _DEF(17),
+#endif
+#undef _DEF
+};
+
+// Map timer index to timer number (Straight copy of usedTimers array)
+const int8_t timerNumbers[USED_TIMER_COUNT] = {
+#define _DEF(i) i
+
+#if USED_TIMERS & TIM_N(1)
+ _DEF(1),
+#endif
+#if USED_TIMERS & TIM_N(2)
+ _DEF(2),
+#endif
+#if USED_TIMERS & TIM_N(3)
+ _DEF(3),
+#endif
+#if USED_TIMERS & TIM_N(4)
+ _DEF(4),
+#endif
+#if USED_TIMERS & TIM_N(5)
+ _DEF(5),
+#endif
+#if USED_TIMERS & TIM_N(6)
+ _DEF(6),
+#endif
+#if USED_TIMERS & TIM_N(7)
+ _DEF(7),
+#endif
+#if USED_TIMERS & TIM_N(8)
+ _DEF(8),
+#endif
+#if USED_TIMERS & TIM_N(9)
+ _DEF(9),
+#endif
+#if USED_TIMERS & TIM_N(10)
+ _DEF(10),
+#endif
+#if USED_TIMERS & TIM_N(11)
+ _DEF(11),
+#endif
+#if USED_TIMERS & TIM_N(12)
+ _DEF(12),
+#endif
+#if USED_TIMERS & TIM_N(13)
+ _DEF(13),
+#endif
+#if USED_TIMERS & TIM_N(14)
+ _DEF(14),
+#endif
+#if USED_TIMERS & TIM_N(15)
+ _DEF(15),
+#endif
+#if USED_TIMERS & TIM_N(16)
+ _DEF(16),
+#endif
+#if USED_TIMERS & TIM_N(17)
+ _DEF(17),
+#endif
+#if USED_TIMERS & TIM_N(20)
+ _DEF(20),
+#endif
+#undef _DEF
+};
+
+int8_t timerGetNumberByIndex(uint8_t index)
+{
+ if (index < USED_TIMER_COUNT) {
+ return timerNumbers[index];
+ } else {
+ return 0;
+ }
+}
+
+int8_t timerGetTIMNumber(const TMR_TypeDef *tim)
+{
+ uint8_t index = lookupTimerIndex(tim);
+
+ return timerGetNumberByIndex(index);
+}
+
+static inline uint8_t lookupChannelIndex(const uint16_t channel)
+{
+ return channel >> 2;
+}
+
+uint8_t timerLookupChannelIndex(const uint16_t channel)
+{
+ return lookupChannelIndex(channel);
+}
+
+rccPeriphTag_t timerRCC(const TMR_TypeDef *tim)
+{
+ for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
+ if (timerDefinitions[i].TIMx == tim) {
+ return timerDefinitions[i].rcc;
+ }
+ }
+ return 0;
+}
+
+uint8_t timerInputIrq(const TMR_TypeDef *tim)
+{
+ for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
+ if (timerDefinitions[i].TIMx == tim) {
+ return timerDefinitions[i].inputIrq;
+ }
+ }
+ return 0;
+}
+
+void timerNVICConfigure(uint8_t irq)
+{
+ DAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER));
+ DAL_NVIC_EnableIRQ(irq);
+}
+
+TMR_HandleTypeDef* timerFindTimerHandle(TMR_TypeDef *tim)
+{
+ uint8_t timerIndex = lookupTimerIndex(tim);
+ if (timerIndex >= USED_TIMER_COUNT)
+ return NULL;
+
+ return &timerHandle[timerIndex].Handle;
+}
+
+void timerReconfigureTimeBase(TMR_TypeDef *tim, uint16_t period, uint32_t hz)
+{
+ TMR_HandleTypeDef* handle = timerFindTimerHandle(tim);
+ if (handle == NULL) return;
+
+ handle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
+ handle->Init.Prescaler = (timerClock(tim) / hz) - 1;
+
+ TMR_Base_SetConfig(handle->Instance, &handle->Init);
+}
+
+void configTimeBase(TMR_TypeDef *tim, uint16_t period, uint32_t hz)
+{
+ TMR_HandleTypeDef* handle = timerFindTimerHandle(tim);
+ if (handle == NULL) return;
+
+ if (handle->Instance == tim) {
+ // already configured
+ return;
+ }
+
+ handle->Instance = tim;
+
+ handle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
+ handle->Init.Prescaler = (timerClock(tim) / hz) - 1;
+
+ handle->Init.ClockDivision = TMR_CLOCKDIVISION_DIV1;
+ handle->Init.CounterMode = TMR_COUNTERMODE_UP;
+ handle->Init.RepetitionCounter = 0x0000;
+
+ DAL_TMR_Base_Init(handle);
+ if (tim == TMR1 || tim == TMR2 || tim == TMR3 || tim == TMR4 || tim == TMR5 || tim == TMR8 || tim == TMR9)
+ {
+ TMR_ClockConfigTypeDef sClockSourceConfig;
+ memset(&sClockSourceConfig, 0, sizeof(sClockSourceConfig));
+ sClockSourceConfig.ClockSource = TMR_CLOCKSOURCE_INTERNAL;
+ if (DAL_TMR_ConfigClockSource(handle, &sClockSourceConfig) != DAL_OK) {
+ return;
+ }
+ }
+ if (tim == TMR1 || tim == TMR2 || tim == TMR3 || tim == TMR4 || tim == TMR5 || tim == TMR8) {
+ TMR_MasterConfigTypeDef sMasterConfig;
+ memset(&sMasterConfig, 0, sizeof(sMasterConfig));
+ sMasterConfig.MasterSlaveMode = TMR_MASTERSLAVEMODE_DISABLE;
+ if (DAL_TMREx_MasterConfigSynchronization(handle, &sMasterConfig) != DAL_OK) {
+ return;
+ }
+ }
+}
+
+// old interface for PWM inputs. It should be replaced
+void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz)
+{
+ uint8_t timerIndex = lookupTimerIndex(timerHardwarePtr->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ configTimeBase(timerHardwarePtr->tim, period, hz);
+ DAL_TMR_Base_Start(&timerHandle[timerIndex].Handle);
+
+ uint8_t irq = timerInputIrq(timerHardwarePtr->tim);
+ timerNVICConfigure(irq);
+ // HACK - enable second IRQ on timers that need it
+ switch (irq) {
+
+ case TMR1_CC_IRQn:
+ timerNVICConfigure(TMR1_UP_TMR10_IRQn);
+ break;
+ case TMR8_CC_IRQn:
+ timerNVICConfigure(TMR8_UP_TMR13_IRQn);
+ break;
+
+ }
+}
+
+// allocate and configure timer channel. Timer priority is set to highest priority of its channels
+void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriority, uint8_t irq)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ unsigned channel = timHw - TIMER_HARDWARE;
+ if (channel >= TIMER_CHANNEL_COUNT)
+ return;
+
+ timerChannelInfo[channel].type = type;
+ unsigned timer = lookupTimerIndex(timHw->tim);
+ if (timer >= USED_TIMER_COUNT)
+ return;
+ if (irqPriority < timerInfo[timer].priority) {
+ // it would be better to set priority in the end, but current startup sequence is not ready
+ configTimeBase(usedTimers[timer], 0, 1);
+ DAL_TMR_Base_Start(&timerHandle[timerIndex].Handle);
+
+ DAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+ DAL_NVIC_EnableIRQ(irq);
+
+ timerInfo[timer].priority = irqPriority;
+ }
+}
+
+void timerChCCHandlerInit(timerCCHandlerRec_t *self, timerCCHandlerCallback *fn)
+{
+ self->fn = fn;
+}
+
+void timerChOvrHandlerInit(timerOvrHandlerRec_t *self, timerOvrHandlerCallback *fn)
+{
+ self->fn = fn;
+ self->next = NULL;
+}
+
+// update overflow callback list
+// some synchronization mechanism is neccesary to avoid disturbing other channels (BASEPRI used now)
+static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, const TMR_TypeDef *tim)
+{
+ uint8_t timerIndex = lookupTimerIndex(tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+
+ if (cfg->updateCallback) {
+ *chain = cfg->updateCallback;
+ chain = &cfg->updateCallback->next;
+ }
+
+ for (int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
+ if (cfg->overflowCallback[i]) {
+ *chain = cfg->overflowCallback[i];
+ chain = &cfg->overflowCallback[i]->next;
+ }
+ *chain = NULL;
+ }
+ // enable or disable IRQ
+ if (cfg->overflowCallbackActive)
+ __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TMR_IT_UPDATE);
+ else
+ __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TMR_IT_UPDATE);
+}
+
+// config edge and overflow callback for channel. Try to avoid overflowCallback, it is a bit expensive
+void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *edgeCallback, timerOvrHandlerRec_t *overflowCallback)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ uint8_t channelIndex = lookupChannelIndex(timHw->channel);
+ if (edgeCallback == NULL) // disable irq before changing callback to NULL
+ __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+ // setup callback info
+ timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback;
+ timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
+ // enable channel IRQ
+ if (edgeCallback)
+ __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+
+ timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
+}
+
+void timerConfigUpdateCallback(const TMR_TypeDef *tim, timerOvrHandlerRec_t *updateCallback)
+{
+ uint8_t timerIndex = lookupTimerIndex(tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ timerConfig[timerIndex].updateCallback = updateCallback;
+ timerChConfig_UpdateOverflow(&timerConfig[timerIndex], tim);
+}
+
+// configure callbacks for pair of channels (1+2 or 3+4).
+// Hi(2,4) and Lo(1,3) callbacks are specified, it is not important which timHw channel is used.
+// This is intended for dual capture mode (each channel handles one transition)
+void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_t *edgeCallbackLo, timerCCHandlerRec_t *edgeCallbackHi, timerOvrHandlerRec_t *overflowCallback)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+ uint16_t chLo = timHw->channel & ~TMR_CHANNEL_2; // lower channel
+ uint16_t chHi = timHw->channel | TMR_CHANNEL_2; // upper channel
+ uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel
+
+ if (edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL
+ __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
+ if (edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL
+ __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
+
+ // setup callback info
+ timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallbackLo;
+ timerConfig[timerIndex].edgeCallback[channelIndex + 1] = edgeCallbackHi;
+ timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
+ timerConfig[timerIndex].overflowCallback[channelIndex + 1] = NULL;
+
+ // enable channel IRQs
+ if (edgeCallbackLo) {
+ __DAL_TMR_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
+ __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
+ }
+ if (edgeCallbackHi) {
+ __DAL_TMR_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
+ __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
+ }
+
+ timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
+}
+
+// enable/disable IRQ for low channel in dual configuration
+//void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newState) {
+// TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel&~TIM_Channel_2), newState);
+//}
+// enable/disable IRQ for low channel in dual configuration
+void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newState)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ if (newState)
+ __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel & ~TMR_CHANNEL_2));
+ else
+ __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel & ~TMR_CHANNEL_2));
+}
+
+//// enable or disable IRQ
+//void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState)
+//{
+// TIM_ITConfig(timHw->tim, TIM_IT_CCx(timHw->channel), newState);
+//}
+// enable or disable IRQ
+void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ if (newState)
+ __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+ else
+ __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+}
+
+// clear Compare/Capture flag for channel
+//void timerChClearCCFlag(const timerHardware_t *timHw)
+//{
+// TIM_ClearFlag(timHw->tim, TIM_IT_CCx(timHw->channel));
+//}
+// clear Compare/Capture flag for channel
+void timerChClearCCFlag(const timerHardware_t *timHw)
+{
+ uint8_t timerIndex = lookupTimerIndex(timHw->tim);
+ if (timerIndex >= USED_TIMER_COUNT) {
+ return;
+ }
+
+ __DAL_TMR_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
+}
+
+// configure timer channel GPIO mode
+void timerChConfigGPIO(const timerHardware_t* timHw, ioConfig_t mode)
+{
+ IOInit(IOGetByTag(timHw->tag), OWNER_TIMER, 0);
+ IOConfigGPIO(IOGetByTag(timHw->tag), mode);
+}
+
+// calculate input filter constant
+// TODO - we should probably setup DTS to higher value to allow reasonable input filtering
+// - notice that prescaler[0] does use DTS for sampling - the sequence won't be monotonous anymore
+static unsigned getFilter(unsigned ticks)
+{
+ static const unsigned ftab[16] = {
+ 1*1, // fDTS !
+ 1*2, 1*4, 1*8, // fCK_INT
+ 2*6, 2*8, // fDTS/2
+ 4*6, 4*8,
+ 8*6, 8*8,
+ 16*5, 16*6, 16*8,
+ 32*5, 32*6, 32*8
+ };
+ for (unsigned i = 1; i < ARRAYLEN(ftab); i++)
+ if (ftab[i] > ticks)
+ return i - 1;
+ return 0x0f;
+}
+
+// Configure input captupre
+void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks)
+{
+ unsigned timer = lookupTimerIndex(timHw->tim);
+ if (timer >= USED_TIMER_COUNT)
+ return;
+
+ TMR_IC_InitTypeDef TIM_ICInitStructure;
+
+ TIM_ICInitStructure.ICPolarity = polarityRising ? TMR_ICPOLARITY_RISING : TMR_ICPOLARITY_FALLING;
+ TIM_ICInitStructure.ICSelection = TMR_ICSELECTION_DIRECTTI;
+ TIM_ICInitStructure.ICPrescaler = TMR_ICPSC_DIV1;
+ TIM_ICInitStructure.ICFilter = getFilter(inputFilterTicks);
+ DAL_TMR_IC_ConfigChannel(&timerHandle[timer].Handle, &TIM_ICInitStructure, timHw->channel);
+}
+
+// configure dual channel input channel for capture
+// polarity is for Low channel (capture order is always Lo - Hi)
+void timerChConfigICDual(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks)
+{
+ unsigned timer = lookupTimerIndex(timHw->tim);
+ if (timer >= USED_TIMER_COUNT)
+ return;
+
+ TMR_IC_InitTypeDef TIM_ICInitStructure;
+ bool directRising = (timHw->channel & TMR_CHANNEL_2) ? !polarityRising : polarityRising;
+
+ // configure direct channel
+ TIM_ICInitStructure.ICPolarity = directRising ? TMR_ICPOLARITY_RISING : TMR_ICPOLARITY_FALLING;
+ TIM_ICInitStructure.ICSelection = TMR_ICSELECTION_DIRECTTI;
+ TIM_ICInitStructure.ICPrescaler = TMR_ICPSC_DIV1;
+ TIM_ICInitStructure.ICFilter = getFilter(inputFilterTicks);
+ DAL_TMR_IC_ConfigChannel(&timerHandle[timer].Handle, &TIM_ICInitStructure, timHw->channel);
+
+ // configure indirect channel
+ TIM_ICInitStructure.ICPolarity = directRising ? TMR_ICPOLARITY_FALLING : TMR_ICPOLARITY_RISING;
+ TIM_ICInitStructure.ICSelection = TMR_ICSELECTION_INDIRECTTI;
+ DAL_TMR_IC_ConfigChannel(&timerHandle[timer].Handle, &TIM_ICInitStructure, timHw->channel ^ TMR_CHANNEL_2);
+}
+
+void timerChICPolarity(const timerHardware_t *timHw, bool polarityRising)
+{
+ timCCER_t tmpccer = timHw->tim->CCEN;
+ tmpccer &= ~(TMR_CCEN_CC1POL << timHw->channel);
+ tmpccer |= polarityRising ? (TMR_ICPOLARITY_RISING << timHw->channel) : (TMR_ICPOLARITY_FALLING << timHw->channel);
+ timHw->tim->CCEN = tmpccer;
+}
+
+volatile timCCR_t* timerChCCRHi(const timerHardware_t *timHw)
+{
+ return (volatile timCCR_t*)((volatile char*)&timHw->tim->CC1 + (timHw->channel | TMR_CHANNEL_2));
+}
+
+volatile timCCR_t* timerChCCRLo(const timerHardware_t *timHw)
+{
+ return (volatile timCCR_t*)((volatile char*)&timHw->tim->CC1 + (timHw->channel & ~TMR_CHANNEL_2));
+}
+
+volatile timCCR_t* timerChCCR(const timerHardware_t *timHw)
+{
+ return (volatile timCCR_t*)((volatile char*)&timHw->tim->CC1 + timHw->channel);
+}
+
+void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh)
+{
+ unsigned timer = lookupTimerIndex(timHw->tim);
+ if (timer >= USED_TIMER_COUNT)
+ return;
+
+ TMR_OC_InitTypeDef TIM_OCInitStructure;
+
+ TIM_OCInitStructure.OCMode = TMR_OCMODE_INACTIVE;
+ TIM_OCInitStructure.Pulse = 0x00000000;
+ TIM_OCInitStructure.OCPolarity = stateHigh ? TMR_OCPOLARITY_HIGH : TMR_OCPOLARITY_LOW;
+ TIM_OCInitStructure.OCNPolarity = TMR_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCIdleState = TMR_OCIDLESTATE_RESET;
+ TIM_OCInitStructure.OCNIdleState = TMR_OCNIDLESTATE_RESET;
+
+ DAL_TMR_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
+
+ if (outEnable) {
+ TIM_OCInitStructure.OCMode = TMR_OCMODE_INACTIVE;
+ DAL_TMR_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
+ DAL_TMR_OC_Start(&timerHandle[timer].Handle, timHw->channel);
+ } else {
+ TIM_OCInitStructure.OCMode = TMR_OCMODE_TIMING;
+ DAL_TMR_OC_ConfigChannel(&timerHandle[timer].Handle, &TIM_OCInitStructure, timHw->channel);
+ DAL_TMR_OC_Start_IT(&timerHandle[timer].Handle, timHw->channel);
+ }
+}
+
+static void timCCxHandler(TMR_TypeDef *tim, timerConfig_t *timerConfig)
+{
+ uint16_t capture;
+ unsigned tim_status;
+ tim_status = tim->STS & tim->DIEN;
+#if 1
+ while (tim_status) {
+ // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly
+ // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway)
+ unsigned bit = __builtin_clz(tim_status);
+ unsigned mask = ~(0x80000000 >> bit);
+ tim->STS = mask;
+ tim_status &= mask;
+ switch (bit) {
+ case __builtin_clz(TMR_IT_UPDATE): {
+
+ if (timerConfig->forcedOverflowTimerValue != 0) {
+ capture = timerConfig->forcedOverflowTimerValue - 1;
+ timerConfig->forcedOverflowTimerValue = 0;
+ } else {
+ capture = tim->AUTORLD;
+ }
+
+ timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+ while (cb) {
+ cb->fn(cb, capture);
+ cb = cb->next;
+ }
+ break;
+ }
+ case __builtin_clz(TMR_IT_CC1):
+ if (timerConfig->edgeCallback[0]) {
+ timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CC1);
+ }
+ break;
+ case __builtin_clz(TMR_IT_CC2):
+ if (timerConfig->edgeCallback[1]) {
+ timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CC2);
+ }
+ break;
+ case __builtin_clz(TMR_IT_CC3):
+ if (timerConfig->edgeCallback[2]) {
+ timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CC3);
+ }
+ break;
+ case __builtin_clz(TMR_IT_CC4):
+ if (timerConfig->edgeCallback[3]) {
+ timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CC4);
+ }
+ break;
+ }
+ }
+#else
+ if (tim_status & (int)TIM_IT_Update) {
+ tim->SR = ~TIM_IT_Update;
+ capture = tim->ARR;
+ timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+ while (cb) {
+ cb->fn(cb, capture);
+ cb = cb->next;
+ }
+ }
+ if (tim_status & (int)TIM_IT_CC1) {
+ tim->SR = ~TIM_IT_CC1;
+ timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], tim->CCR1);
+ }
+ if (tim_status & (int)TIM_IT_CC2) {
+ tim->SR = ~TIM_IT_CC2;
+ timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], tim->CCR2);
+ }
+ if (tim_status & (int)TIM_IT_CC3) {
+ tim->SR = ~TIM_IT_CC3;
+ timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], tim->CCR3);
+ }
+ if (tim_status & (int)TIM_IT_CC4) {
+ tim->SR = ~TIM_IT_CC4;
+ timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], tim->CCR4);
+ }
+#endif
+}
+
+static inline void timUpdateHandler(TMR_TypeDef *tim, timerConfig_t *timerConfig)
+{
+ uint16_t capture;
+ unsigned tim_status;
+ tim_status = tim->STS & tim->DIEN;
+ while (tim_status) {
+ // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly
+ // currrent order is highest bit first. Code should not rely on specific order (it will introduce race conditions anyway)
+ unsigned bit = __builtin_clz(tim_status);
+ unsigned mask = ~(0x80000000 >> bit);
+ tim->STS = mask;
+ tim_status &= mask;
+ switch (bit) {
+ case __builtin_clz(TMR_IT_UPDATE): {
+
+ if (timerConfig->forcedOverflowTimerValue != 0) {
+ capture = timerConfig->forcedOverflowTimerValue - 1;
+ timerConfig->forcedOverflowTimerValue = 0;
+ } else {
+ capture = tim->AUTORLD;
+ }
+
+ timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
+ while (cb) {
+ cb->fn(cb, capture);
+ cb = cb->next;
+ }
+ break;
+ }
+ }
+ }
+}
+
+// handler for shared interrupts when both timers need to check status bits
+#define _TIM_IRQ_HANDLER2(name, i, j) \
+ void name(void) \
+ { \
+ timCCxHandler(TMR ## i, &timerConfig[TIMER_INDEX(i)]); \
+ timCCxHandler(TMR ## j, &timerConfig[TIMER_INDEX(j)]); \
+ } struct dummy
+
+#define _TIM_IRQ_HANDLER(name, i) \
+ void name(void) \
+ { \
+ timCCxHandler(TMR ## i, &timerConfig[TIMER_INDEX(i)]); \
+ } struct dummy
+
+#define _TIM_IRQ_HANDLER_UPDATE_ONLY(name, i) \
+ void name(void) \
+ { \
+ timUpdateHandler(TMR ## i, &timerConfig[TIMER_INDEX(i)]); \
+ } struct dummy
+
+#if USED_TIMERS & TIM_N(1)
+_TIM_IRQ_HANDLER(TMR1_CC_IRQHandler, 1);
+# if USED_TIMERS & TIM_N(10)
+_TIM_IRQ_HANDLER2(TMR1_UP_TMR10_IRQHandler, 1, 10); // both timers are in use
+# else
+_TIM_IRQ_HANDLER(TMR1_UP_TMR10_IRQHandler, 1); // timer10 is not used
+# endif
+#endif
+
+#if USED_TIMERS & TIM_N(2)
+_TIM_IRQ_HANDLER(TMR2_IRQHandler, 2);
+#endif
+#if USED_TIMERS & TIM_N(3)
+_TIM_IRQ_HANDLER(TMR3_IRQHandler, 3);
+#endif
+#if USED_TIMERS & TIM_N(4)
+_TIM_IRQ_HANDLER(TMR4_IRQHandler, 4);
+#endif
+#if USED_TIMERS & TIM_N(5)
+_TIM_IRQ_HANDLER(TMR5_IRQHandler, 5);
+#endif
+
+#if USED_TIMERS & TIM_N(6)
+# if !(defined(USE_PID_AUDIO))
+//not for APM32F4
+//_TIM_IRQ_HANDLER_UPDATE_ONLY(TMR6_DAC_IRQHandler, 6);
+# endif
+#endif
+#if USED_TIMERS & TIM_N(7)
+// The USB VCP_DAL driver conflicts with TIM7, see TIMx_IRQHandler in usbd_cdc_interface.h
+# if !(defined(USE_VCP) && (defined(APM32F4)))
+_TIM_IRQ_HANDLER_UPDATE_ONLY(TIM7_IRQHandler, 7);
+# endif
+#endif
+
+#if USED_TIMERS & TIM_N(8)
+_TIM_IRQ_HANDLER(TMR8_CC_IRQHandler, 8);
+
+# if USED_TIMERS & TIM_N(13)
+_TIM_IRQ_HANDLER2(TMR8_UP_TMR13_IRQHandler, 8, 13); // both timers are in use
+# else
+_TIM_IRQ_HANDLER(TMR8_UP_TMR13_IRQHandler, 8); // timer13 is not used
+# endif
+#endif
+
+#if USED_TIMERS & TIM_N(9)
+_TIM_IRQ_HANDLER(TMR1_BRK_TMR9_IRQHandler, 9);
+#endif
+# if USED_TIMERS & TIM_N(11)
+_TIM_IRQ_HANDLER(TMR1_TRG_COM_TMR11_IRQHandler, 11);
+# endif
+#if USED_TIMERS & TIM_N(12)
+_TIM_IRQ_HANDLER(TMR8_BRK_TMR12_IRQHandler, 12);
+#endif
+#if USED_TIMERS & TIM_N(14)
+_TIM_IRQ_HANDLER(TMR8_TRG_COM_TMR14_IRQHandler, 14);
+#endif
+#if USED_TIMERS & TIM_N(15)
+_TIM_IRQ_HANDLER(TMR1_BRK_TMR15_IRQHandler, 15);
+#endif
+#if USED_TIMERS & TIM_N(17)
+_TIM_IRQ_HANDLER(TMR1_TRG_COM_TMR17_IRQHandler, 17);
+#endif
+
+void timerInit(void)
+{
+ memset(timerConfig, 0, sizeof(timerConfig));
+
+#if USED_TIMERS & TIM_N(1)
+ __DAL_RCM_TMR1_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(2)
+ __DAL_RCM_TMR2_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(3)
+ __DAL_RCM_TMR3_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(4)
+ __DAL_RCM_TMR4_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(5)
+ __DAL_RCM_TMR5_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(6)
+ __DAL_RCM_TMR6_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(7)
+ __DAL_RCM_TMR7_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(8)
+ __DAL_RCM_TMR8_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(9)
+ __DAL_RCM_TMR9_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(10)
+ __DAL_RCM_TMR10_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(11)
+ __DAL_RCM_TMR11_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(12)
+ __DAL_RCM_TMR12_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(13)
+ __DAL_RCM_TMR13_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(14)
+ __DAL_RCM_TMR14_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(15)
+ __DAL_RCM_TMR15_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(16)
+ __DAL_RCM_TMR16_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(17)
+ __DAL_RCM_TMR17_CLK_ENABLE();
+#endif
+#if USED_TIMERS & TIM_N(20)
+ __DAL_RCM_TMR20_CLK_ENABLE();
+#endif
+
+ /* enable the timer peripherals */
+ for (int i = 0; i < TIMER_CHANNEL_COUNT; i++) {
+ RCC_ClockCmd(timerRCC(TIMER_HARDWARE[i].tim), ENABLE);
+ }
+
+ /* enable the timer peripherals */
+ for (unsigned i = 0; i < TIMER_CHANNEL_COUNT; i++) {
+ RCC_ClockCmd(timerRCC(TIMER_HARDWARE[i].tim), ENABLE);
+ }
+
+ // initialize timer channel structures
+ for (unsigned i = 0; i < TIMER_CHANNEL_COUNT; i++) {
+ timerChannelInfo[i].type = TYPE_FREE;
+ }
+
+ for (unsigned i = 0; i < USED_TIMER_COUNT; i++) {
+ timerInfo[i].priority = ~0;
+ }
+}
+
+void timerStart(TMR_TypeDef *tim)
+{
+ TMR_HandleTypeDef* handle = timerFindTimerHandle(tim);
+ if (handle == NULL) return;
+
+ __DAL_TMR_ENABLE(handle);
+}
+
+/**
+ * Force an overflow for a given timer.
+ * Saves the current value of the counter in the relevant timerConfig's forcedOverflowTimerValue variable.
+ * @param TMR_Typedef *tim The timer to overflow
+ * @return void
+ **/
+void timerForceOverflow(TMR_TypeDef *tim)
+{
+ uint8_t timerIndex = lookupTimerIndex((const TMR_TypeDef *)tim);
+
+ ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
+ // Save the current count so that PPM reading will work on the same timer that was forced to overflow
+ timerConfig[timerIndex].forcedOverflowTimerValue = tim->CNT + 1;
+
+ // Force an overflow by setting the UG bit
+ tim->CEG |= TMR_CEG_UEG;
+ }
+}
+
+// DMA_Handle_index --TODO STDå¯¹åº”æ–‡ä»¶æ— è¯¥å‡½æ•°
+uint16_t timerDmaIndex(uint8_t channel)
+{
+ switch (channel) {
+ case TMR_CHANNEL_1:
+ return TMR_DMA_ID_CC1;
+ case TMR_CHANNEL_2:
+ return TMR_DMA_ID_CC2;
+ case TMR_CHANNEL_3:
+ return TMR_DMA_ID_CC3;
+ case TMR_CHANNEL_4:
+ return TMR_DMA_ID_CC4;
+ }
+ return 0;
+}
+
+// TIM_DMA_sources
+uint16_t timerDmaSource(uint8_t channel)
+{
+ switch (channel) {
+ case TMR_CHANNEL_1:
+ return TMR_DMA_CC1;
+ case TMR_CHANNEL_2:
+ return TMR_DMA_CC2;
+ case TMR_CHANNEL_3:
+ return TMR_DMA_CC3;
+ case TMR_CHANNEL_4:
+ return TMR_DMA_CC4;
+ }
+ return 0;
+}
+
+uint16_t timerGetPrescalerByDesiredMhz(TMR_TypeDef *tim, uint16_t mhz)
+{
+ return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
+}
+
+uint16_t timerGetPeriodByPrescaler(TMR_TypeDef *tim, uint16_t prescaler, uint32_t hz)
+{
+ return (uint16_t)((timerClock(tim) / (prescaler + 1)) / hz);
+}
+
+uint16_t timerGetPrescalerByDesiredHertz(TMR_TypeDef *tim, uint32_t hz)
+{
+ // protection here for desired hertz > SystemCoreClock???
+ if (hz > timerClock(tim)) {
+ return 0;
+ }
+ return (uint16_t)((timerClock(tim) + hz / 2) / hz) - 1;
+}
+
+DAL_StatusTypeDef TIM_DMACmd(TMR_HandleTypeDef *htim, uint32_t Channel, FunctionalState NewState)
+{
+ switch (Channel) {
+ case TMR_CHANNEL_1: {
+ if (NewState != DISABLE) {
+ /* Enable the TIM Capture/Compare 1 DMA request */
+ __DAL_TMR_ENABLE_DMA(htim, TMR_DMA_CC1);
+ } else {
+ /* Disable the TIM Capture/Compare 1 DMA request */
+ __DAL_TMR_DISABLE_DMA(htim, TMR_DMA_CC1);
+ }
+ }
+ break;
+
+ case TMR_CHANNEL_2: {
+ if (NewState != DISABLE) {
+ /* Enable the TIM Capture/Compare 2 DMA request */
+ __DAL_TMR_ENABLE_DMA(htim, TMR_DMA_CC2);
+ } else {
+ /* Disable the TIM Capture/Compare 2 DMA request */
+ __DAL_TMR_DISABLE_DMA(htim, TMR_DMA_CC2);
+ }
+ }
+ break;
+
+ case TMR_CHANNEL_3: {
+ if (NewState != DISABLE) {
+ /* Enable the TIM Capture/Compare 3 DMA request */
+ __DAL_TMR_ENABLE_DMA(htim, TMR_DMA_CC3);
+ } else {
+ /* Disable the TIM Capture/Compare 3 DMA request */
+ __DAL_TMR_DISABLE_DMA(htim, TMR_DMA_CC3);
+ }
+ }
+ break;
+
+ case TMR_CHANNEL_4: {
+ if (NewState != DISABLE) {
+ /* Enable the TIM Capture/Compare 4 DMA request */
+ __DAL_TMR_ENABLE_DMA(htim, TMR_DMA_CC4);
+ } else {
+ /* Disable the TIM Capture/Compare 4 DMA request */
+ __DAL_TMR_DISABLE_DMA(htim, TMR_DMA_CC4);
+ }
+ }
+ break;
+
+ default:
+ break;
+ }
+ /* Change the htim state */
+ htim->State = DAL_TMR_STATE_READY;
+ /* Return function status */
+ return DAL_OK;
+}
+
+DAL_StatusTypeDef DMA_SetCurrDataCounter(TMR_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length)
+{
+ if ((htim->State == DAL_TMR_STATE_BUSY)) {
+ return DAL_BUSY;
+ } else if ((htim->State == DAL_TMR_STATE_READY)) {
+ if (((uint32_t) pData == 0) && (Length > 0)) {
+ return DAL_ERROR;
+ } else {
+ htim->State = DAL_TMR_STATE_BUSY;
+ }
+ }
+ switch (Channel) {
+ case TMR_CHANNEL_1: {
+ /* Set the DMA Period elapsed callback */
+ htim->hdma[TMR_DMA_ID_CC1]->XferCpltCallback = TMR_DMADelayPulseCplt;
+
+ /* Set the DMA error callback */
+ htim->hdma[TMR_DMA_ID_CC1]->XferErrorCallback = TMR_DMAError;
+
+ /* Enable the DMA Stream */
+ DAL_DMA_Start_IT(htim->hdma[TMR_DMA_ID_CC1], (uint32_t) pData, (uint32_t) & htim->Instance->CC1, Length);
+ }
+ break;
+
+ case TMR_CHANNEL_2: {
+ /* Set the DMA Period elapsed callback */
+ htim->hdma[TMR_DMA_ID_CC2]->XferCpltCallback = TMR_DMADelayPulseCplt;
+
+ /* Set the DMA error callback */
+ htim->hdma[TMR_DMA_ID_CC2]->XferErrorCallback = TMR_DMAError;
+
+ /* Enable the DMA Stream */
+ DAL_DMA_Start_IT(htim->hdma[TMR_DMA_ID_CC2], (uint32_t) pData, (uint32_t) & htim->Instance->CC2, Length);
+ }
+ break;
+
+ case TMR_CHANNEL_3: {
+ /* Set the DMA Period elapsed callback */
+ htim->hdma[TMR_DMA_ID_CC3]->XferCpltCallback = TMR_DMADelayPulseCplt;
+
+ /* Set the DMA error callback */
+ htim->hdma[TMR_DMA_ID_CC3]->XferErrorCallback = TMR_DMAError;
+
+ /* Enable the DMA Stream */
+ DAL_DMA_Start_IT(htim->hdma[TMR_DMA_ID_CC3], (uint32_t) pData, (uint32_t) & htim->Instance->CC3, Length);
+ }
+ break;
+
+ case TMR_CHANNEL_4: {
+ /* Set the DMA Period elapsed callback */
+ htim->hdma[TMR_DMA_ID_CC4]->XferCpltCallback = TMR_DMADelayPulseCplt;
+
+ /* Set the DMA error callback */
+ htim->hdma[TMR_DMA_ID_CC4]->XferErrorCallback = TMR_DMAError;
+
+ /* Enable the DMA Stream */
+ DAL_DMA_Start_IT(htim->hdma[TMR_DMA_ID_CC4], (uint32_t) pData, (uint32_t) & htim->Instance->CC4, Length);
+ }
+ break;
+
+ default:
+ break;
+ }
+ /* Return function status */
+ return DAL_OK;
+}
+#endif
diff --git a/src/main/drivers/mcu/apm32/timer_apm32f4xx.c b/src/main/drivers/mcu/apm32/timer_apm32f4xx.c
new file mode 100644
index 0000000000..7ab390fce3
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/timer_apm32f4xx.c
@@ -0,0 +1,222 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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_TIMER
+
+#include "common/utils.h"
+
+#include "drivers/dma.h"
+#include "drivers/io.h"
+#include "timer_def.h"
+
+#include "apm32f4xx.h"
+#include "drivers/rcc.h"
+#include "drivers/timer.h"
+
+
+const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
+ { .TIMx = TMR1, .rcc = RCC_APB2(TMR1), .inputIrq = TMR1_CC_IRQn},
+ { .TIMx = TMR2, .rcc = RCC_APB1(TMR2), .inputIrq = TMR2_IRQn},
+ { .TIMx = TMR3, .rcc = RCC_APB1(TMR3), .inputIrq = TMR3_IRQn},
+ { .TIMx = TMR4, .rcc = RCC_APB1(TMR4), .inputIrq = TMR4_IRQn},
+ { .TIMx = TMR5, .rcc = RCC_APB1(TMR5), .inputIrq = TMR5_IRQn},
+ { .TIMx = TMR6, .rcc = RCC_APB1(TMR6), .inputIrq = 0},
+ { .TIMx = TMR7, .rcc = RCC_APB1(TMR7), .inputIrq = 0},
+ { .TIMx = TMR8, .rcc = RCC_APB2(TMR8), .inputIrq = TMR8_CC_IRQn},
+ { .TIMx = TMR9, .rcc = RCC_APB2(TMR9), .inputIrq = TMR1_BRK_TMR9_IRQn},
+ { .TIMx = TMR10, .rcc = RCC_APB2(TMR10), .inputIrq = TMR1_UP_TMR10_IRQn},
+ { .TIMx = TMR11, .rcc = RCC_APB2(TMR11), .inputIrq = TMR1_TRG_COM_TMR11_IRQn},
+ { .TIMx = TMR12, .rcc = RCC_APB1(TMR12), .inputIrq = TMR8_BRK_TMR12_IRQn},
+ { .TIMx = TMR13, .rcc = RCC_APB1(TMR13), .inputIrq = TMR8_UP_TMR13_IRQn},
+ { .TIMx = TMR14, .rcc = RCC_APB1(TMR14), .inputIrq = TMR8_TRG_COM_TMR14_IRQn},
+};
+
+#if defined(USE_TIMER_MGMT)
+const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = {
+ // Auto-generated from 'timer_def.h'
+//PORTA
+ DEF_TIM(TMR2, CH1, PA0, 0, 0),
+ DEF_TIM(TMR2, CH2, PA1, 0, 0),
+ DEF_TIM(TMR2, CH3, PA2, 0, 0),
+ DEF_TIM(TMR2, CH4, PA3, 0, 0),
+ DEF_TIM(TMR2, CH1, PA5, 0, 0),
+ DEF_TIM(TMR1, CH1N, PA7, 0, 0),
+ DEF_TIM(TMR1, CH1, PA8, 0, 0),
+ DEF_TIM(TMR1, CH2, PA9, 0, 0),
+ DEF_TIM(TMR1, CH3, PA10, 0, 0),
+ DEF_TIM(TMR1, CH1N, PA11, 0, 0),
+ DEF_TIM(TMR2, CH1, PA15, 0, 0),
+
+ DEF_TIM(TMR5, CH1, PA0, 0, 0),
+ DEF_TIM(TMR5, CH2, PA1, 0, 0),
+ DEF_TIM(TMR5, CH3, PA2, 0, 0),
+ DEF_TIM(TMR5, CH4, PA3, 0, 0),
+ DEF_TIM(TMR3, CH1, PA6, 0, 0),
+ DEF_TIM(TMR3, CH2, PA7, 0, 0),
+
+ DEF_TIM(TMR9, CH1, PA2, 0, 0),
+ DEF_TIM(TMR9, CH2, PA3, 0, 0),
+
+ DEF_TIM(TMR8, CH1N, PA5, 0, 0),
+ DEF_TIM(TMR8, CH1N, PA7, 0, 0),
+
+ DEF_TIM(TMR13, CH1, PA6, 0, 0),
+ DEF_TIM(TMR14, CH1, PA7, 0, 0),
+
+//PORTB
+ DEF_TIM(TMR1, CH2N, PB0, 0, 0),
+ DEF_TIM(TMR1, CH3N, PB1, 0, 0),
+ DEF_TIM(TMR2, CH2, PB3, 0, 0),
+ DEF_TIM(TMR2, CH3, PB10, 0, 0),
+ DEF_TIM(TMR2, CH4, PB11, 0, 0),
+ DEF_TIM(TMR1, CH1N, PB13, 0, 0),
+ DEF_TIM(TMR1, CH2N, PB14, 0, 0),
+ DEF_TIM(TMR1, CH3N, PB15, 0, 0),
+
+ DEF_TIM(TMR3, CH3, PB0, 0, 0),
+ DEF_TIM(TMR3, CH4, PB1, 0, 0),
+ DEF_TIM(TMR3, CH1, PB4, 0, 0),
+ DEF_TIM(TMR3, CH2, PB5, 0, 0),
+ DEF_TIM(TMR4, CH1, PB6, 0, 0),
+ DEF_TIM(TMR4, CH2, PB7, 0, 0),
+ DEF_TIM(TMR4, CH3, PB8, 0, 0),
+ DEF_TIM(TMR4, CH4, PB9, 0, 0),
+
+ DEF_TIM(TMR8, CH2N, PB0, 0, 0),
+ DEF_TIM(TMR8, CH3N, PB1, 0, 0),
+
+ DEF_TIM(TMR10, CH1, PB8, 0, 0),
+ DEF_TIM(TMR11, CH1, PB9, 0, 0),
+
+ DEF_TIM(TMR8, CH2N, PB14, 0, 0),
+ DEF_TIM(TMR8, CH3N, PB15, 0, 0),
+
+ DEF_TIM(TMR12, CH1, PB14, 0, 0),
+ DEF_TIM(TMR12, CH2, PB15, 0, 0),
+
+//PORTC
+ DEF_TIM(TMR3, CH1, PC6, 0, 0),
+ DEF_TIM(TMR3, CH2, PC7, 0, 0),
+ DEF_TIM(TMR3, CH3, PC8, 0, 0),
+ DEF_TIM(TMR3, CH4, PC9, 0, 0),
+
+ DEF_TIM(TMR8, CH1, PC6, 0, 0),
+ DEF_TIM(TMR8, CH2, PC7, 0, 0),
+ DEF_TIM(TMR8, CH3, PC8, 0, 0),
+ DEF_TIM(TMR8, CH4, PC9, 0, 0),
+
+//PORTD
+ DEF_TIM(TMR4, CH1, PD12, 0, 0),
+ DEF_TIM(TMR4, CH2, PD13, 0, 0),
+ DEF_TIM(TMR4, CH3, PD14, 0, 0),
+ DEF_TIM(TMR4, CH4, PD15, 0, 0),
+
+//PORTE
+ DEF_TIM(TMR1, CH1N, PE8, 0, 0),
+ DEF_TIM(TMR1, CH1, PE9, 0, 0),
+ DEF_TIM(TMR1, CH2N, PE10, 0, 0),
+ DEF_TIM(TMR1, CH2, PE11, 0, 0),
+ DEF_TIM(TMR1, CH3N, PE12, 0, 0),
+ DEF_TIM(TMR1, CH3, PE13, 0, 0),
+ DEF_TIM(TMR1, CH4, PE14, 0, 0),
+
+ DEF_TIM(TMR9, CH1, PE5, 0, 0),
+ DEF_TIM(TMR9, CH2, PE6, 0, 0),
+
+//PORTF
+ DEF_TIM(TMR10, CH1, PF6, 0, 0),
+ DEF_TIM(TMR11, CH1, PF7, 0, 0),
+
+//PORTH
+// Port H is not available for LPQFP-100 or 144 package
+// DEF_TIM(TMR5, CH1, PH10, 0, 0),
+// DEF_TIM(TMR5, CH2, PH11, 0, 0),
+// DEF_TIM(TMR5, CH3, PH12, 0, 0),
+//
+//#if !defined(STM32F411xE)
+// DEF_TIM(TMR8, CH1N, PH13, 0, 0),
+// DEF_TIM(TMR8, CH2N, PH14, 0, 0),
+// DEF_TIM(TMR8, CH3N, PH15, 0, 0),
+//
+// DEF_TIM(TMR12, CH1, PH6, 0, 0),
+// DEF_TIM(TMR12, CH2, PH9, 0, 0),
+//#endif
+
+//PORTI
+// Port I is not available for LPQFP-100 or 144 package
+// DEF_TIM(TMR5, CH4, PI0, 0, 0),
+//
+//#if !defined(STM32F411xE)
+// DEF_TIM(TMR8, CH4, PI2, 0, 0),
+// DEF_TIM(TMR8, CH1, PI5, 0, 0),
+// DEF_TIM(TMR8, CH2, PI6, 0, 0),
+// DEF_TIM(TMR8, CH3, PI7, 0, 0),
+//#endif
+};
+#endif
+
+
+/*
+ need a mapping from dma and timers to pins, and the values should all be set here to the dmaMotors array.
+ this mapping could be used for both these motors and for led strip.
+
+ only certain pins have OC output (already used in normal PWM et al) but then
+ there are only certain DMA streams/channels available for certain timers and channels.
+ *** (this may highlight some hardware limitations on some targets) ***
+
+ DMA1
+
+ Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
+ 0
+ 1
+ 2 TMR4_CH1 TMR4_CH2 TMR4_CH3
+ 3 TMR2_CH3 TMR2_CH1 TMR2_CH1 TMR2_CH4
+ TMR2_CH4
+ 4
+ 5 TMR3_CH4 TMR3_CH1 TMR3_CH2 TMR3_CH3
+ 6 TMR5_CH3 TMR5_CH4 TMR5_CH1 TMR5_CH4 TMR5_CH2
+ 7
+
+ DMA2
+
+ Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
+ 0 TMR8_CH1 TMR1_CH1
+ TMR8_CH2 TMR1_CH2
+ TMR8_CH3 TMR1_CH3
+ 1
+ 2
+ 3
+ 4
+ 5
+ 6 TMR1_CH1 TMR1_CH2 TMR1_CH1 TMR1_CH4 TMR1_CH3
+ 7 TMR8_CH1 TMR8_CH2 TMR8_CH3 TMR8_CH4
+*/
+
+uint32_t timerClock(const TIM_TypeDef *tim)
+{
+ if (tim == TMR8 || tim == TMR1 || tim == TMR9 || tim == TMR10 || tim == TMR11) {
+ return SystemCoreClock;
+ } else {
+ return SystemCoreClock / 2;
+ }
+}
+#endif
diff --git a/src/main/drivers/mcu/apm32/timer_def.h b/src/main/drivers/mcu/apm32/timer_def.h
new file mode 100644
index 0000000000..54949a4de8
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/timer_def.h
@@ -0,0 +1,339 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 "common/utils.h"
+
+// allow conditional definition of DMA related members
+#if defined(USE_TIMER_DMA)
+# define DEF_TIM_DMA_COND(...) __VA_ARGS__
+#else
+# define DEF_TIM_DMA_COND(...)
+#endif
+
+#if defined(USE_TIMER_MGMT)
+#define TIMER_GET_IO_TAG(pin) DEFIO_TAG_E(pin)
+#else
+#define TIMER_GET_IO_TAG(pin) DEFIO_TAG(pin)
+#endif
+
+// map to base channel (strip N from channel); works only when channel N exists
+#define DEF_TIM_TCH2BTCH(timch) CONCAT(B, timch)
+#define BTCH_TMR1_CH1N BTCH_TMR1_CH1
+#define BTCH_TMR1_CH2N BTCH_TMR1_CH2
+#define BTCH_TMR1_CH3N BTCH_TMR1_CH3
+
+#define BTCH_TMR8_CH1N BTCH_TMR8_CH1
+#define BTCH_TMR8_CH2N BTCH_TMR8_CH2
+#define BTCH_TMR8_CH3N BTCH_TMR8_CH3
+
+#define BTCH_TMR20_CH1N BTCH_TMR20_CH1
+#define BTCH_TMR20_CH2N BTCH_TMR20_CH2
+#define BTCH_TMR20_CH3N BTCH_TMR20_CH3
+
+#define BTCH_TMR13_CH1N BTCH_TMR13_CH1
+#define BTCH_TMR14_CH1N BTCH_TMR14_CH1
+#define BTCH_TMR15_CH1N BTCH_TMR15_CH1
+#define BTCH_TMR16_CH1N BTCH_TMR16_CH1
+#define BTCH_TMR17_CH1N BTCH_TMR17_CH1
+
+// channel table D(chan_n, n_type)
+#define DEF_TIM_CH_GET(ch) CONCAT2(DEF_TIM_CH__, ch)
+#define DEF_TIM_CH__CH_CH1 D(1, 0)
+#define DEF_TIM_CH__CH_CH2 D(2, 0)
+#define DEF_TIM_CH__CH_CH3 D(3, 0)
+#define DEF_TIM_CH__CH_CH4 D(4, 0)
+#define DEF_TIM_CH__CH_CH1N D(1, 1)
+#define DEF_TIM_CH__CH_CH2N D(2, 1)
+#define DEF_TIM_CH__CH_CH3N D(3, 1)
+
+// get record from DMA table
+// DMA table is identical for all targets for consistency, only variant 0 is defined on F1,F3
+// DMA table entry for TIMx Channel y, with two variants:
+// #define DEF_TIM_DMA__BTCH_TIMx_CHy D(var0),D(var1)
+// Parameters in D(...) are target-specific
+// DMA table for channel without DMA
+// #define DEF_TIM_DMA__BTCH_TIMx_CHy NONE
+// N channels are converted to corresponding base channel first
+
+// Create accessor macro and call it with entry from table
+// DMA_VARIANT_MISSING are used to satisfy variable arguments (-Wpedantic) and to get better error message (undefined symbol instead of preprocessor error)
+#define DEF_TIM_DMA_GET(variant, timch) PP_CALL(CONCAT(DEF_TIM_DMA_GET_VARIANT__, variant), CONCAT(DEF_TIM_DMA__, DEF_TIM_TCH2BTCH(timch)), DMA_VARIANT_MISSING, DMA_VARIANT_MISSING, ERROR)
+
+#define DEF_TIM_DMA_GET_VARIANT__0(_0, ...) _0
+#define DEF_TIM_DMA_GET_VARIANT__1(_0, _1, ...) _1
+#define DEF_TIM_DMA_GET_VARIANT__2(_0, _1, _2, ...) _2
+#define DEF_TIM_DMA_GET_VARIANT__3(_0, _1, _2, _3, ...) _3
+#define DEF_TIM_DMA_GET_VARIANT__4(_0, _1, _2, _3, _4, ...) _4
+#define DEF_TIM_DMA_GET_VARIANT__5(_0, _1, _2, _3, _4, _5, ...) _5
+#define DEF_TIM_DMA_GET_VARIANT__6(_0, _1, _2, _3, _4, _5, _6, ...) _6
+#define DEF_TIM_DMA_GET_VARIANT__7(_0, _1, _2, _3, _4, _5, _6, _7, ...) _7
+#define DEF_TIM_DMA_GET_VARIANT__8(_0, _1, _2, _3, _4, _5, _6, _7, _8, ...) _8
+#define DEF_TIM_DMA_GET_VARIANT__9(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, ...) _9
+#define DEF_TIM_DMA_GET_VARIANT__10(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, ...) _10
+#define DEF_TIM_DMA_GET_VARIANT__11(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, ...) _11
+#define DEF_TIM_DMA_GET_VARIANT__12(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, _12, ...) _12
+#define DEF_TIM_DMA_GET_VARIANT__13(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, _12, _13, ...) _13
+#define DEF_TIM_DMA_GET_VARIANT__14(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, _12, _13, _14, ...) _14
+#define DEF_TIM_DMA_GET_VARIANT__15(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, _11, _12, _13, _14, _15, ...) _15
+
+// symbolic names for DMA variants
+#define DMA_VAR0 0
+#define DMA_VAR1 1
+#define DMA_VAR2 2
+
+// get record from AF table
+// Parameters in D(...) are target-specific
+#define DEF_TIM_AF_GET(timch, pin) CONCAT4(DEF_TIM_AF__, pin, __, timch)
+
+// define output type (N-channel)
+#define DEF_TIM_OUTPUT(ch) CONCAT(DEF_TIM_OUTPUT__, DEF_TIM_CH_GET(ch))
+#define DEF_TIM_OUTPUT__D(chan_n, n_channel) PP_IIF(n_channel, TIMER_OUTPUT_N_CHANNEL, TIMER_OUTPUT_NONE)
+
+#define DEF_TIM(tim, chan, pin, out, dmaopt) { \
+ tim, \
+ TIMER_GET_IO_TAG(pin), \
+ DEF_TIM_CHANNEL(CH_ ## chan), \
+ (DEF_TIM_OUTPUT(CH_ ## chan) | out), \
+ DEF_TIM_AF(TCH_## tim ## _ ## chan, pin) \
+ DEF_TIM_DMA_COND(/* add comma */ , \
+ DEF_TIM_DMA_STREAM(dmaopt, TCH_## tim ## _ ## chan), \
+ DEF_TIM_DMA_CHANNEL(dmaopt, TCH_## tim ## _ ## chan) \
+ ) \
+ DEF_TIM_DMA_COND(/* add comma */ , \
+ DEF_TIM_DMA_STREAM(0, TCH_## tim ## _UP), \
+ DEF_TIM_DMA_CHANNEL(0, TCH_## tim ## _UP), \
+ DEF_TIM_DMA_HANDLER(0, TCH_## tim ## _UP) \
+ ) \
+} \
+/**/
+
+#define DEF_TIM_CHANNEL(ch) CONCAT(DEF_TIM_CHANNEL__, DEF_TIM_CH_GET(ch))
+#define DEF_TIM_CHANNEL__D(chan_n, n_channel) TMR_CHANNEL_ ## chan_n
+
+#define DEF_TIM_AF(timch, pin) CONCAT(DEF_TIM_AF__, DEF_TIM_AF_GET(timch, pin))
+#define DEF_TIM_AF__D(af_n, tim_n) GPIO_AF ## af_n ## _TMR ## tim_n
+
+#define DEF_TIM_DMA_CHANNEL(variant, timch) \
+ CONCAT(DEF_TIM_DMA_CHANNEL__, DEF_TIM_DMA_GET(variant, timch))
+#define DEF_TIM_DMA_CHANNEL__D(dma_n, stream_n, chan_n) DMA_CHANNEL_ ## chan_n
+#define DEF_TIM_DMA_CHANNEL__NONE DMA_CHANNEL_0
+
+#define DEF_TIM_DMA_STREAM(variant, timch) \
+ CONCAT(DEF_TIM_DMA_STREAM__, DEF_TIM_DMA_GET(variant, timch))
+#define DEF_TIM_DMA_STREAM__D(dma_n, stream_n, chan_n) (dmaResource_t *)DMA ## dma_n ## _Stream ## stream_n
+#define DEF_TIM_DMA_STREAM__NONE NULL
+
+#define DEF_TIM_DMA_HANDLER(variant, timch) \
+ CONCAT(DEF_TIM_DMA_HANDLER__, DEF_TIM_DMA_GET(variant, timch))
+#define DEF_TIM_DMA_HANDLER__D(dma_n, stream_n, chan_n) DMA ## dma_n ## _ST ## stream_n ## _HANDLER
+#define DEF_TIM_DMA_HANDLER__NONE 0
+
+/* F4 Stream Mappings */
+// D(DMAx, Stream, Channel)
+#define DEF_TIM_DMA__BTCH_TMR1_CH1 D(2, 6, 0),D(2, 1, 6),D(2, 3, 6)
+#define DEF_TIM_DMA__BTCH_TMR1_CH2 D(2, 6, 0),D(2, 2, 6)
+#define DEF_TIM_DMA__BTCH_TMR1_CH3 D(2, 6, 0),D(2, 6, 6)
+#define DEF_TIM_DMA__BTCH_TMR1_CH4 D(2, 4, 6)
+
+#define DEF_TIM_DMA__BTCH_TMR2_CH1 D(1, 5, 3)
+#define DEF_TIM_DMA__BTCH_TMR2_CH2 D(1, 6, 3)
+#define DEF_TIM_DMA__BTCH_TMR2_CH3 D(1, 1, 3)
+#define DEF_TIM_DMA__BTCH_TMR2_CH4 D(1, 7, 3),D(1, 6, 3)
+
+#define DEF_TIM_DMA__BTCH_TMR3_CH1 D(1, 4, 5)
+#define DEF_TIM_DMA__BTCH_TMR3_CH2 D(1, 5, 5)
+#define DEF_TIM_DMA__BTCH_TMR3_CH3 D(1, 7, 5)
+#define DEF_TIM_DMA__BTCH_TMR3_CH4 D(1, 2, 5)
+
+#define DEF_TIM_DMA__BTCH_TMR4_CH1 D(1, 0, 2)
+#define DEF_TIM_DMA__BTCH_TMR4_CH2 D(1, 3, 2)
+#define DEF_TIM_DMA__BTCH_TMR4_CH3 D(1, 7, 2)
+
+#define DEF_TIM_DMA__BTCH_TMR5_CH1 D(1, 2, 6)
+#define DEF_TIM_DMA__BTCH_TMR5_CH2 D(1, 4, 6)
+#define DEF_TIM_DMA__BTCH_TMR5_CH3 D(1, 0, 6)
+#define DEF_TIM_DMA__BTCH_TMR5_CH4 D(1, 1, 6),D(1, 3, 6)
+
+#define DEF_TIM_DMA__BTCH_TMR8_CH1 D(2, 2, 0),D(2, 2, 7)
+#define DEF_TIM_DMA__BTCH_TMR8_CH2 D(2, 2, 0),D(2, 3, 7)
+#define DEF_TIM_DMA__BTCH_TMR8_CH3 D(2, 2, 0),D(2, 4, 7)
+#define DEF_TIM_DMA__BTCH_TMR8_CH4 D(2, 7, 7)
+
+#define DEF_TIM_DMA__BTCH_TMR4_CH4 NONE
+
+#define DEF_TIM_DMA__BTCH_TMR9_CH1 NONE
+#define DEF_TIM_DMA__BTCH_TMR9_CH2 NONE
+
+#define DEF_TIM_DMA__BTCH_TMR10_CH1 NONE
+
+#define DEF_TIM_DMA__BTCH_TMR11_CH1 NONE
+
+#define DEF_TIM_DMA__BTCH_TMR12_CH1 NONE
+#define DEF_TIM_DMA__BTCH_TMR12_CH2 NONE
+
+#define DEF_TIM_DMA__BTCH_TMR13_CH1 NONE
+
+#define DEF_TIM_DMA__BTCH_TMR14_CH1 NONE
+
+// TIM_UP table
+#define DEF_TIM_DMA__BTCH_TMR1_UP D(2, 5, 6)
+#define DEF_TIM_DMA__BTCH_TMR2_UP D(1, 7, 3)
+#define DEF_TIM_DMA__BTCH_TMR3_UP D(1, 2, 5)
+#define DEF_TIM_DMA__BTCH_TMR4_UP D(1, 6, 2)
+#define DEF_TIM_DMA__BTCH_TMR5_UP D(1, 0, 6)
+#define DEF_TIM_DMA__BTCH_TMR6_UP D(1, 1, 7)
+#define DEF_TIM_DMA__BTCH_TMR7_UP D(1, 4, 1)
+#define DEF_TIM_DMA__BTCH_TMR8_UP D(2, 1, 7)
+#define DEF_TIM_DMA__BTCH_TMR9_UP NONE
+#define DEF_TIM_DMA__BTCH_TMR10_UP NONE
+#define DEF_TIM_DMA__BTCH_TMR11_UP NONE
+#define DEF_TIM_DMA__BTCH_TMR12_UP NONE
+#define DEF_TIM_DMA__BTCH_TMR13_UP NONE
+#define DEF_TIM_DMA__BTCH_TMR14_UP NONE
+
+// AF table
+
+// NONE
+#define DEF_TIM_AF__NONE__TCH_TMR1_CH1 D(1, 1)
+#define DEF_TIM_AF__NONE__TCH_TMR1_CH2 D(1, 1)
+#define DEF_TIM_AF__NONE__TCH_TMR1_CH3 D(1, 1)
+#define DEF_TIM_AF__NONE__TCH_TMR1_CH4 D(1, 1)
+#define DEF_TIM_AF__NONE__TCH_TMR8_CH1 D(3, 8)
+#define DEF_TIM_AF__NONE__TCH_TMR8_CH2 D(3, 8)
+#define DEF_TIM_AF__NONE__TCH_TMR8_CH3 D(3, 8)
+#define DEF_TIM_AF__NONE__TCH_TMR8_CH4 D(3, 8)
+
+//PORTA
+#define DEF_TIM_AF__PA0__TCH_TMR2_CH1 D(1, 2)
+#define DEF_TIM_AF__PA1__TCH_TMR2_CH2 D(1, 2)
+#define DEF_TIM_AF__PA2__TCH_TMR2_CH3 D(1, 2)
+#define DEF_TIM_AF__PA3__TCH_TMR2_CH4 D(1, 2)
+#define DEF_TIM_AF__PA5__TCH_TMR2_CH1 D(1, 2)
+#define DEF_TIM_AF__PA7__TCH_TMR1_CH1N D(1, 1)
+#define DEF_TIM_AF__PA8__TCH_TMR1_CH1 D(1, 1)
+#define DEF_TIM_AF__PA9__TCH_TMR1_CH2 D(1, 1)
+#define DEF_TIM_AF__PA10__TCH_TMR1_CH3 D(1, 1)
+#define DEF_TIM_AF__PA11__TCH_TMR1_CH1N D(1, 1)
+#define DEF_TIM_AF__PA15__TCH_TMR2_CH1 D(1, 2)
+
+#define DEF_TIM_AF__PA0__TCH_TMR5_CH1 D(2, 5)
+#define DEF_TIM_AF__PA1__TCH_TMR5_CH2 D(2, 5)
+#define DEF_TIM_AF__PA2__TCH_TMR5_CH3 D(2, 5)
+#define DEF_TIM_AF__PA3__TCH_TMR5_CH4 D(2, 5)
+#define DEF_TIM_AF__PA6__TCH_TMR3_CH1 D(2, 3)
+#define DEF_TIM_AF__PA7__TCH_TMR3_CH2 D(2, 3)
+
+#define DEF_TIM_AF__PA2__TCH_TMR9_CH1 D(3, 9)
+#define DEF_TIM_AF__PA3__TCH_TMR9_CH2 D(3, 9)
+#define DEF_TIM_AF__PA5__TCH_TMR8_CH1N D(3, 8)
+#define DEF_TIM_AF__PA7__TCH_TMR8_CH1N D(3, 8)
+
+#define DEF_TIM_AF__PA6__TCH_TMR13_CH1 D(9, 13)
+#define DEF_TIM_AF__PA7__TCH_TMR14_CH1 D(9, 14)
+
+//PORTB
+#define DEF_TIM_AF__PB0__TCH_TMR1_CH2N D(1, 1)
+#define DEF_TIM_AF__PB1__TCH_TMR1_CH3N D(1, 1)
+#define DEF_TIM_AF__PB3__TCH_TMR2_CH2 D(1, 2)
+#define DEF_TIM_AF__PB10__TCH_TMR2_CH3 D(1, 2)
+#define DEF_TIM_AF__PB11__TCH_TMR2_CH4 D(1, 2)
+#define DEF_TIM_AF__PB13__TCH_TMR1_CH1N D(1, 1)
+#define DEF_TIM_AF__PB14__TCH_TMR1_CH2N D(1, 1)
+#define DEF_TIM_AF__PB15__TCH_TMR1_CH3N D(1, 1)
+
+#define DEF_TIM_AF__PB0__TCH_TMR3_CH3 D(2, 3)
+#define DEF_TIM_AF__PB1__TCH_TMR3_CH4 D(2, 3)
+#define DEF_TIM_AF__PB4__TCH_TMR3_CH1 D(2, 3)
+#define DEF_TIM_AF__PB5__TCH_TMR3_CH2 D(2, 3)
+#define DEF_TIM_AF__PB6__TCH_TMR4_CH1 D(2, 4)
+#define DEF_TIM_AF__PB7__TCH_TMR4_CH2 D(2, 4)
+#define DEF_TIM_AF__PB8__TCH_TMR4_CH3 D(2, 4)
+#define DEF_TIM_AF__PB9__TCH_TMR4_CH4 D(2, 4)
+
+#define DEF_TIM_AF__PB0__TCH_TMR8_CH2N D(3, 8)
+#define DEF_TIM_AF__PB1__TCH_TMR8_CH3N D(3, 8)
+#define DEF_TIM_AF__PB8__TCH_TMR10_CH1 D(3, 10)
+#define DEF_TIM_AF__PB9__TCH_TMR11_CH1 D(3, 11)
+#define DEF_TIM_AF__PB14__TCH_TMR8_CH2N D(3, 8)
+#define DEF_TIM_AF__PB15__TCH_TMR8_CH3N D(3, 8)
+
+#define DEF_TIM_AF__PB14__TCH_TMR12_CH1 D(9, 12)
+#define DEF_TIM_AF__PB15__TCH_TMR12_CH2 D(9, 12)
+
+//PORTC
+#define DEF_TIM_AF__PC6__TCH_TMR3_CH1 D(2, 3)
+#define DEF_TIM_AF__PC7__TCH_TMR3_CH2 D(2, 3)
+#define DEF_TIM_AF__PC8__TCH_TMR3_CH3 D(2, 3)
+#define DEF_TIM_AF__PC9__TCH_TMR3_CH4 D(2, 3)
+
+#define DEF_TIM_AF__PC6__TCH_TMR8_CH1 D(3, 8)
+#define DEF_TIM_AF__PC7__TCH_TMR8_CH2 D(3, 8)
+#define DEF_TIM_AF__PC8__TCH_TMR8_CH3 D(3, 8)
+#define DEF_TIM_AF__PC9__TCH_TMR8_CH4 D(3, 8)
+
+//PORTD
+#define DEF_TIM_AF__PD12__TCH_TMR4_CH1 D(2, 4)
+#define DEF_TIM_AF__PD13__TCH_TMR4_CH2 D(2, 4)
+#define DEF_TIM_AF__PD14__TCH_TMR4_CH3 D(2, 4)
+#define DEF_TIM_AF__PD15__TCH_TMR4_CH4 D(2, 4)
+
+//PORTE
+#define DEF_TIM_AF__PE8__TCH_TMR1_CH1N D(1, 1)
+#define DEF_TIM_AF__PE9__TCH_TMR1_CH1 D(1, 1)
+#define DEF_TIM_AF__PE10__TCH_TMR1_CH2N D(1, 1)
+#define DEF_TIM_AF__PE11__TCH_TMR1_CH2 D(1, 1)
+#define DEF_TIM_AF__PE12__TCH_TMR1_CH3N D(1, 1)
+#define DEF_TIM_AF__PE13__TCH_TMR1_CH3 D(1, 1)
+#define DEF_TIM_AF__PE14__TCH_TMR1_CH4 D(1, 1)
+
+#define DEF_TIM_AF__PE5__TCH_TMR9_CH1 D(3, 9)
+#define DEF_TIM_AF__PE6__TCH_TMR9_CH2 D(3, 9)
+
+//PORTF
+#define DEF_TIM_AF__PF6__TCH_TMR10_CH1 D(3, 10)
+#define DEF_TIM_AF__PF7__TCH_TMR11_CH1 D(3, 11)
+
+//PORTH
+#define DEF_TIM_AF__PH10__TCH_TMR5_CH1 D(2, 5)
+#define DEF_TIM_AF__PH11__TCH_TMR5_CH2 D(2, 5)
+#define DEF_TIM_AF__PH12__TCH_TMR5_CH3 D(2, 5)
+
+#define DEF_TIM_AF__PH13__TCH_TMR8_CH1N D(3, 8)
+#define DEF_TIM_AF__PH14__TCH_TMR8_CH2N D(3, 8)
+#define DEF_TIM_AF__PH15__TCH_TMR8_CH3N D(3, 8)
+
+#define DEF_TIM_AF__PH6__TCH_TMR12_CH1 D(9, 12)
+#define DEF_TIM_AF__PH9__TCH_TMR12_CH2 D(9, 12)
+
+//PORTI
+#define DEF_TIM_AF__PI0__TCH_TMR5_CH4 D(2, 5)
+
+#define DEF_TIM_AF__PI2__TCH_TMR8_CH4 D(3, 8)
+#define DEF_TIM_AF__PI5__TCH_TMR8_CH1 D(3, 8)
+#define DEF_TIM_AF__PI6__TCH_TMR8_CH2 D(3, 8)
+#define DEF_TIM_AF__PI7__TCH_TMR8_CH3 D(3, 8)
+
+#define FULL_TIMER_CHANNEL_COUNT 78
+#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(6) | TIM_N(7) | TIM_N(8) | TIM_N(9) | TIM_N(10) | TIM_N(11) | TIM_N(12) | TIM_N(13) | TIM_N(14) )
+#define HARDWARE_TIMER_DEFINITION_COUNT 14
+
diff --git a/src/main/drivers/mcu/apm32/transponder_ir_io_apm32.c b/src/main/drivers/mcu/apm32/transponder_ir_io_apm32.c
new file mode 100644
index 0000000000..09d48e7dc3
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/transponder_ir_io_apm32.c
@@ -0,0 +1,282 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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_TRANSPONDER
+
+#include "drivers/dma.h"
+#include "drivers/dma_reqmap.h"
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/rcc.h"
+#include "drivers/timer.h"
+#include "drivers/transponder_ir_arcitimer.h"
+#include "drivers/transponder_ir_erlt.h"
+#include "drivers/transponder_ir_ilap.h"
+
+#include "drivers/transponder_ir.h"
+
+volatile uint8_t transponderIrDataTransferInProgress = 0;
+
+static IO_t transponderIO = IO_NONE;
+static TMR_HandleTypeDef TmrHandle;
+static uint16_t timerChannel = 0;
+static uint8_t output;
+static uint8_t alternateFunction;
+
+transponder_t transponder;
+
+bool transponderInitialised = false;
+
+FAST_IRQ_HANDLER static void TRANSPONDER_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
+{
+ DAL_DMA_IRQHandler(TmrHandle.hdma[descriptor->userParam]);
+ TIM_DMACmd(&TmrHandle, timerChannel, DISABLE);
+ transponderIrDataTransferInProgress = 0;
+}
+
+void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
+{
+ if (!ioTag) {
+ return;
+ }
+
+ const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_TRANSPONDER, 0);
+ TMR_TypeDef *timer = timerHardware->tim;
+ timerChannel = timerHardware->channel;
+ output = timerHardware->output;
+ alternateFunction = timerHardware->alternateFunction;
+
+#if defined(USE_DMA_SPEC)
+ const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
+
+ if (dmaSpec == NULL) {
+ return;
+ }
+
+ dmaResource_t *dmaRef = dmaSpec->ref;
+ uint32_t dmaChannel = dmaSpec->channel;
+#else
+ dmaResource_t *dmaRef = timerHardware->dmaRef;
+ uint32_t dmaChannel = timerHardware->dmaChannel;
+#endif
+
+ dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
+ if (dmaRef == NULL || !dmaAllocate(dmaIdentifier, OWNER_TRANSPONDER, 0)) {
+ return;
+ }
+
+ /* Time base configuration */
+
+ TmrHandle.Instance = timer;
+
+ uint16_t prescaler = timerGetPrescalerByDesiredMhz(timer, transponder->timer_hz);
+ uint16_t period = timerGetPeriodByPrescaler(timer, prescaler, transponder->timer_carrier_hz);
+
+ transponder->bitToggleOne = period / 2;
+
+ TmrHandle.Init.Prescaler = prescaler;
+ TmrHandle.Init.Period = period; // 800kHz
+ TmrHandle.Init.ClockDivision = TMR_CLOCKDIVISION_DIV1;
+ TmrHandle.Init.CounterMode = TMR_COUNTERMODE_UP;
+ if (DAL_TMR_PWM_Init(&TmrHandle) != DAL_OK) {
+ /* Initialization Error */
+ return;
+ }
+
+ /* IO configuration */
+
+ static DMA_HandleTypeDef hdma_tim;
+
+ transponderIO = IOGetByTag(ioTag);
+ IOInit(transponderIO, OWNER_TRANSPONDER, 0);
+ IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction);
+
+ __DAL_RCM_DMA1_CLK_ENABLE();
+ __DAL_RCM_DMA2_CLK_ENABLE();
+
+ /* Set the parameters to be configured */
+ hdma_tim.Init.Channel = dmaChannel;
+ hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH;
+ hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE;
+ hdma_tim.Init.MemInc = DMA_MINC_ENABLE;
+ hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
+ hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
+ hdma_tim.Init.Mode = DMA_NORMAL;
+ hdma_tim.Init.Priority = DMA_PRIORITY_HIGH;
+ hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+ hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+ hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE;
+ hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE;
+
+ /* Set hdma_tim instance */
+ hdma_tim.Instance = (DMA_ARCH_TYPE *)dmaRef;
+
+ uint16_t dmaIndex = timerDmaIndex(timerChannel);
+
+ /* Link hdma_tim to hdma[x] (channelx) */
+ __DAL_LINKDMA(&TmrHandle, hdma[dmaIndex], hdma_tim);
+
+ dmaEnable(dmaIdentifier);
+ dmaSetHandler(dmaIdentifier, TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, dmaIndex);
+
+ /* Initialize TIMx DMA handle */
+ if (DAL_DMA_Init(TmrHandle.hdma[dmaIndex]) != DAL_OK) {
+ /* Initialization Error */
+ return;
+ }
+
+
+ RCC_ClockCmd(timerRCC(timer), ENABLE);
+
+ /* PWM1 Mode configuration: Channel1 */
+ TMR_OC_InitTypeDef TMR_OCInitStructure;
+
+ TMR_OCInitStructure.OCMode = TMR_OCMODE_PWM1;
+ TMR_OCInitStructure.OCIdleState = TMR_OCIDLESTATE_RESET;
+ TMR_OCInitStructure.OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TMR_OCPOLARITY_LOW : TMR_OCPOLARITY_HIGH;
+ TMR_OCInitStructure.OCNIdleState = TMR_OCNIDLESTATE_RESET;
+ TMR_OCInitStructure.OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TMR_OCNPOLARITY_LOW : TMR_OCNPOLARITY_HIGH;
+ TMR_OCInitStructure.Pulse = 0;
+ TMR_OCInitStructure.OCFastMode = TMR_OCFAST_DISABLE;
+ if (DAL_TMR_PWM_ConfigChannel(&TmrHandle, &TMR_OCInitStructure, timerChannel) != DAL_OK) {
+ /* Configuration Error */
+ return;
+ }
+ if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
+ if (DAL_TMREx_PWMN_Start(&TmrHandle, timerChannel) != DAL_OK) {
+ /* Starting PWM generation Error */
+ return;
+ }
+ } else {
+ if (DAL_TMR_PWM_Start(&TmrHandle, timerChannel) != DAL_OK) {
+ /* Starting PWM generation Error */
+ return;
+ }
+ }
+
+ transponderInitialised = true;
+}
+
+bool transponderIrInit(const ioTag_t ioTag, const transponderProvider_e provider)
+{
+ if (!ioTag) {
+ return false;
+ }
+
+ switch (provider) {
+ case TRANSPONDER_ARCITIMER:
+ transponderIrInitArcitimer(&transponder);
+ break;
+ case TRANSPONDER_ILAP:
+ transponderIrInitIlap(&transponder);
+ break;
+ case TRANSPONDER_ERLT:
+ transponderIrInitERLT(&transponder);
+ break;
+ default:
+ return false;
+ }
+
+ transponderIrHardwareInit(ioTag, &transponder);
+
+ return true;
+}
+
+bool isTransponderIrReady(void)
+{
+ return !transponderIrDataTransferInProgress;
+}
+
+void transponderIrWaitForTransmitComplete(void)
+{
+#ifdef DEBUG
+ static uint32_t waitCounter = 0;
+#endif
+
+ while (transponderIrDataTransferInProgress) {
+#ifdef DEBUG
+ waitCounter++;
+#endif
+ }
+}
+
+void transponderIrUpdateData(const uint8_t* transponderData)
+{
+ transponderIrWaitForTransmitComplete();
+ transponder.vTable->updateTransponderDMABuffer(&transponder, transponderData);
+}
+
+void transponderIrDMAEnable(transponder_t *transponder)
+{
+ if (!transponderInitialised) {
+ return;
+ }
+
+ if (DMA_SetCurrDataCounter(&TmrHandle, timerChannel, transponder->transponderIrDMABuffer.ilap, transponder->dma_buffer_size) != DAL_OK) {
+ /* DMA set error */
+ transponderIrDataTransferInProgress = 0;
+ return;
+ }
+
+ /* Reset timer counter */
+ __DAL_TMR_SET_COUNTER(&TmrHandle, 0);
+ /* Enable channel DMA requests */
+ TIM_DMACmd(&TmrHandle, timerChannel, ENABLE);
+}
+
+void transponderIrDisable(void)
+{
+ if (!transponderInitialised) {
+ return;
+ }
+
+ TIM_DMACmd(&TmrHandle, timerChannel, DISABLE);
+ if (output & TIMER_OUTPUT_N_CHANNEL) {
+ DAL_TMREx_PWMN_Stop(&TmrHandle, timerChannel);
+ } else {
+ DAL_TMR_PWM_Stop(&TmrHandle, timerChannel);
+ }
+
+
+ IOInit(transponderIO, OWNER_TRANSPONDER, 0);
+
+#ifdef TRANSPONDER_INVERTED
+ IOHi(transponderIO);
+#else
+ IOLo(transponderIO);
+#endif
+
+ IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), alternateFunction);
+}
+
+void transponderIrTransmit(void)
+{
+ transponderIrWaitForTransmitComplete();
+
+ transponderIrDataTransferInProgress = 1;
+ transponderIrDMAEnable(&transponder);
+}
+#endif // USE_TRANSPONDER
diff --git a/src/main/drivers/mcu/apm32/usb/msc/usb_msc_apm32f4xx.c b/src/main/drivers/mcu/apm32/usb/msc/usb_msc_apm32f4xx.c
new file mode 100644
index 0000000000..8d0227572b
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/msc/usb_msc_apm32f4xx.c
@@ -0,0 +1,114 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 .
+ */
+
+/*
+ * Author: Chris Hockuba (https://github.com/conkerkh)
+ *
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#if defined(USE_USB_MSC)
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "blackbox/blackbox.h"
+
+#include "drivers/io.h"
+#include "drivers/nvic.h"
+#include "drivers/sdmmc_sdio.h"
+#include "drivers/system.h"
+#include "drivers/time.h"
+#include "drivers/usb_msc.h"
+
+#include "msc/usbd_storage.h"
+
+#include "pg/sdcard.h"
+#include "pg/usb.h"
+
+#include "usbd_core.h"
+#include "usbd_cdc_vcp.h"
+#include "drivers/usb_io.h"
+
+extern USBD_INFO_T gUsbDevice;
+
+static void USB_DevUserHandler(USBD_INFO_T* usbInfo, uint8_t userStatus);
+
+extern USBD_MSC_MEMORY_T USBD_MEMORY_INTERFACE;
+extern USBD_DESC_T USBD_DESC_MSC;
+uint8_t mscStart(void)
+{
+ //Start USB
+ usbGenerateDisconnectPulse();
+
+ IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0);
+ IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0);
+
+ switch (blackboxConfig()->device) {
+#ifdef USE_SDCARD
+ case BLACKBOX_DEVICE_SDCARD:
+ switch (sdcardConfig()->mode) {
+#ifdef USE_SDCARD_SDIO
+ case SDCARD_MODE_SDIO:
+ USBD_MSC_RegisterMemory(&gUsbDevice, &USBD_MEMORY_INTERFACE);
+ break;
+#endif
+#ifdef USE_SDCARD_SPI
+ case SDCARD_MODE_SPI:
+ USBD_MSC_RegisterMemory(&gUsbDevice, &USBD_MEMORY_INTERFACE);
+ break;
+#endif
+ default:
+ return 1;
+ }
+ break;
+#endif
+
+#ifdef USE_FLASHFS
+ case BLACKBOX_DEVICE_FLASH:
+ USBD_MSC_RegisterMemory(&gUsbDevice, &USBD_MEMORY_INTERFACE);
+ break;
+#endif
+ default:
+ return 1;
+ }
+
+ USBD_Init(&gUsbDevice, USBD_SPEED_FS, &USBD_DESC_MSC, &USBD_MSC_CLASS, USB_DevUserHandler);
+
+ // NVIC configuration for SYSTick
+ NVIC_DisableIRQ(SysTick_IRQn);
+ NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0));
+ NVIC_EnableIRQ(SysTick_IRQn);
+
+ return 0;
+}
+
+static void USB_DevUserHandler(USBD_INFO_T* usbInfo, uint8_t userStatus)
+{
+ UNUSED(usbInfo);
+ UNUSED(userStatus);
+}
+#endif
diff --git a/src/main/drivers/mcu/apm32/usb/msc/usbd_memory.c b/src/main/drivers/mcu/apm32/usb/msc/usbd_memory.c
new file mode 100644
index 0000000000..b0b04fb27b
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/msc/usbd_memory.c
@@ -0,0 +1,268 @@
+/**
+ * @file usbd_memory.c
+ *
+ * @brief USB device memory management program body
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Includes ***************************************************************/
+#include "usbd_memory.h"
+
+/* Private includes *******************************************************/
+#include "usbd_storage.h"
+#include "platform.h"
+
+/* Private macro **********************************************************/
+#define MEMORY_LUN_NUM 1
+#define MEMORY_BLOCK_NUM 80
+#define MEMORY_BLOCK_SIZE 512
+
+/* Private variables ******************************************************/
+
+/* USB Mass storage Standard Inquiry Data */
+const uint8_t memoryInquiryData[] =
+{
+ /* lun 0 */
+ 0x00,
+ 0x80,
+ 0x02,
+ 0x02,
+ (USBD_LEN_STD_INQUIRY - 5),
+ 0x00,
+ 0x00,
+ 0x00,
+ /* Manufacturer : 8 bytes */
+ 'G', 'e', 'e', 'h', 'y', ' ', ' ', ' ',
+ /* Product : 16 Bytes */
+ 'S', 't', 'o', 'r', 'a', 'g', 'e', ' ',
+ 'D', 'i', 's', 'k', ' ', ' ', ' ', ' ',
+ /* Version : 4 Bytes */
+ '1', '.', '0', '0',
+};
+
+/* Private typedef ********************************************************/
+
+/* USB FS MSC memory management handler */
+USBD_MSC_MEMORY_T USBD_MEMORY_INTERFACE =
+{
+ "MSC Memory",
+ (uint8_t*)memoryInquiryData,
+ USBD_MSC_MemoryReadMaxLun,
+ USBD_MSC_MemoryInit,
+ USBD_MSC_MemoryReadCapacity,
+ USBD_MSC_MemoryCheckReady,
+ USBD_MSC_MemoryCheckWPR,
+ USBD_MSC_MemoryReadData,
+ USBD_MSC_MemoryWriteData,
+};
+
+/* Private function prototypes ********************************************/
+
+/* External variables *****************************************************/
+
+/* External functions *****************************************************/
+
+/**
+ * @brief USB device MSC memory unit init handler
+ *
+ * @param lun: lun number
+ *
+ * @retval USB device operation status
+ */
+USBD_STA_T USBD_MSC_MemoryInit(uint8_t lun)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+#ifdef USE_FLASHFS
+ USBD_MSC_EMFAT_fops.Init(lun);
+#elif defined(USE_SDCARD_SDIO)
+ USBD_MSC_MICRO_SDIO_fops.Init(lun);
+#elif defined(USE_SDCARD_SPI)
+ USBD_MSC_MICRO_SD_SPI_fops.Init(lun);
+#else
+ UNUSED(lun);
+#endif
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device MSC memory unit read capacity handler
+ *
+ * @param lun: lun number
+ *
+ * @param blockNum: block number
+ *
+ * @param blockSize: block size
+ *
+ * @retval USB device operation status
+ */
+USBD_STA_T USBD_MSC_MemoryReadCapacity(uint8_t lun, uint32_t* blockNum, \
+ uint16_t* blockSize)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+#ifdef USE_FLASHFS
+ USBD_MSC_EMFAT_fops.GetCapacity(lun, blockNum, blockSize);
+#elif defined(USE_SDCARD_SDIO)
+ USBD_MSC_MICRO_SDIO_fops.GetCapacity(lun, blockNum, blockSize);
+#elif defined(USE_SDCARD_SPI)
+ USBD_MSC_MICRO_SD_SPI_fops.GetCapacity(lun, blockNum, blockSize);
+#else
+ UNUSED(lun);
+ UNUSED(blockNum);
+ UNUSED(blockSize);
+#endif
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device MSC memory unit check read status handler
+ *
+ * @param lun: lun number
+ *
+ * @retval USB device operation status
+ */
+USBD_STA_T USBD_MSC_MemoryCheckReady(uint8_t lun)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+#ifdef USE_FLASHFS
+ USBD_MSC_EMFAT_fops.IsReady(lun);
+#elif defined(USE_SDCARD_SDIO)
+ USBD_MSC_MICRO_SDIO_fops.IsReady(lun);
+#elif defined(USE_SDCARD_SPI)
+ USBD_MSC_MICRO_SD_SPI_fops.IsReady(lun);
+#else
+ UNUSED(lun);
+#endif
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device MSC memory unit check write protected status handler
+ *
+ * @param lun: lun number
+ *
+ * @retval USB device operation status
+ */
+USBD_STA_T USBD_MSC_MemoryCheckWPR(uint8_t lun)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+#ifdef USE_FLASHFS
+ USBD_MSC_EMFAT_fops.IsWriteProtected(lun);
+#elif defined(USE_SDCARD_SDIO)
+ USBD_MSC_MICRO_SDIO_fops.IsWriteProtected(lun);
+#elif defined(USE_SDCARD_SPI)
+ USBD_MSC_MICRO_SD_SPI_fops.IsWriteProtected(lun);
+#else
+ UNUSED(lun);
+#endif
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device MSC memory read max LUN handler
+ *
+ * @param None
+ *
+ * @retval Max LUN number
+ */
+uint8_t USBD_MSC_MemoryReadMaxLun(void)
+{
+#ifdef USE_FLASHFS
+ return USBD_MSC_EMFAT_fops.GetMaxLun();
+#elif defined(USE_SDCARD_SDIO)
+ return USBD_MSC_MICRO_SDIO_fops.GetMaxLun();
+#elif defined(USE_SDCARD_SPI)
+ return USBD_MSC_MICRO_SD_SPI_fops.GetMaxLun();
+#else
+ return 0;
+#endif
+}
+
+/**
+ * @brief USB device MSC memory unit read data handler
+ *
+ * @param lun: lun number
+ *
+ * @param buffer: data buffer
+ *
+ * @param blockAddr: block address
+ *
+ * @param blockLength: block number
+ *
+ * @retval USB device operation status
+ */
+USBD_STA_T USBD_MSC_MemoryReadData(uint8_t lun, uint8_t* buffer, uint32_t blockAddr, \
+ uint16_t blockLength)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+#ifdef USE_FLASHFS
+ USBD_MSC_EMFAT_fops.Read(lun, buffer, blockAddr, blockLength);
+#elif defined(USE_SDCARD_SDIO)
+ USBD_MSC_MICRO_SDIO_fops.Read(lun, buffer, blockAddr, blockLength);
+#elif defined(USE_SDCARD_SPI)
+ USBD_MSC_MICRO_SD_SPI_fops.Read(lun, buffer, blockAddr, blockLength);
+#else
+ UNUSED(lun);
+ UNUSED(buffer);
+ UNUSED(blockAddr);
+ UNUSED(blockLength);
+#endif
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device MSC memory unit write data handler
+ *
+ * @param lun: lun number
+ *
+ * @param buffer: data buffer
+ *
+ * @param blockAddr: block address
+ *
+ * @param blockLength: block number
+ *
+ * @retval USB device operation status
+ */
+USBD_STA_T USBD_MSC_MemoryWriteData(uint8_t lun, uint8_t* buffer, uint32_t blockAddr, \
+ uint16_t blockLength)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+#ifdef USE_FLASHFS
+ USBD_MSC_EMFAT_fops.Write(lun, buffer, blockAddr, blockLength);
+#elif defined(USE_SDCARD_SDIO)
+ USBD_MSC_MICRO_SDIO_fops.Write(lun, buffer, blockAddr, blockLength);
+#elif defined(USE_SDCARD_SPI)
+ USBD_MSC_MICRO_SD_SPI_fops.Write(lun, buffer, blockAddr, blockLength);
+#else
+ UNUSED(lun);
+ UNUSED(buffer);
+ UNUSED(blockAddr);
+ UNUSED(blockLength);
+#endif
+
+ return usbStatus;
+}
diff --git a/src/main/drivers/mcu/apm32/usb/msc/usbd_memory.h b/src/main/drivers/mcu/apm32/usb/msc/usbd_memory.h
new file mode 100644
index 0000000000..0cf061205a
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/msc/usbd_memory.h
@@ -0,0 +1,53 @@
+/*!
+ * @file usbd_memory.h
+ *
+ * @brief usb device memory management header file
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Define to prevent recursive inclusion */
+#ifndef _USBD_MEMORY_H_
+#define _USBD_MEMORY_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Includes ***************************************************************/
+#include "usbd_msc.h"
+
+/* Exported macro *********************************************************/
+
+/* Exported typedef *******************************************************/
+
+/* Exported function prototypes *******************************************/
+uint8_t USBD_MSC_MemoryReadMaxLun(void);
+USBD_STA_T USBD_MSC_MemoryCheckWPR(uint8_t lun);
+USBD_STA_T USBD_MSC_MemoryCheckReady(uint8_t lun);
+USBD_STA_T USBD_MSC_MemoryInit(uint8_t lun);
+USBD_STA_T USBD_MSC_MemoryReadCapacity(uint8_t lun, uint32_t* blockNum, \
+ uint16_t* blockSize);
+USBD_STA_T USBD_MSC_MemoryReadData(uint8_t lun, uint8_t* buffer, uint32_t blockAddr, \
+ uint16_t blockLength);
+USBD_STA_T USBD_MSC_MemoryWriteData(uint8_t lun, uint8_t* buffer, uint32_t blockAddr, \
+ uint16_t blockLength);
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#endif
diff --git a/src/main/drivers/mcu/apm32/usb/msc/usbd_msc_descriptor.c b/src/main/drivers/mcu/apm32/usb/msc/usbd_msc_descriptor.c
new file mode 100644
index 0000000000..1a1bc7d4ca
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/msc/usbd_msc_descriptor.c
@@ -0,0 +1,679 @@
+/**
+ * @file usbd_descriptor.c
+ *
+ * @brief USB device descriptor configuration
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Includes ***************************************************************/
+#include "usbd_msc_descriptor.h"
+
+/* Private includes *******************************************************/
+#include "usbd_msc.h"
+#include "platform.h"
+
+#include
+
+/* Private macro **********************************************************/
+#define USBD_GEEHY_VID 0x314B
+#define USBD_FS_PID 0x5720
+#define USBD_LANGID_STR 0x0409
+#define USBD_MANUFACTURER_STR "Geehy"
+#define USBD_PRODUCT_HS_STR "Betaflight FC Mass Storage (HS Mode)"
+#define USBD_PRODUCT_FS_STR "Betaflight FC Mass Storage (FS Mode)"
+#define USBD_CONFIGURATION_HS_STR "MSC Config"
+#define USBD_CONFIGURATION_FS_STR "MSC Config"
+#define USBD_INTERFACE_HS_STR "MSC Interface"
+#define USBD_INTERFACE_FS_STR "MSC Interface"
+
+/* Private function prototypes ********************************************/
+static USBD_DESC_INFO_T USBD_MSC_DeviceDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_MSC_ConfigDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_MSC_ConfigStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_MSC_InterfaceStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_MSC_LangIdStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_MSC_ManufacturerStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_MSC_ProductStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_MSC_SerialStrDescHandler(uint8_t usbSpeed);
+#if USBD_SUP_LPM
+static USBD_DESC_INFO_T USBD_MSC_BosDescHandler(uint8_t usbSpeed);
+#endif
+static USBD_DESC_INFO_T USBD_OtherSpeedConfigDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_DevQualifierDescHandler(uint8_t usbSpeed);
+
+static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len);
+static void Get_SerialNum(void);
+
+/* Private typedef ********************************************************/
+
+/* USB device descripotr handler */
+USBD_DESC_T USBD_DESC_MSC =
+{
+ "MSC Descriptor",
+ USBD_MSC_DeviceDescHandler,
+ USBD_MSC_ConfigDescHandler,
+ USBD_MSC_ConfigStrDescHandler,
+ USBD_MSC_InterfaceStrDescHandler,
+ USBD_MSC_LangIdStrDescHandler,
+ USBD_MSC_ManufacturerStrDescHandler,
+ USBD_MSC_ProductStrDescHandler,
+ USBD_MSC_SerialStrDescHandler,
+#if USBD_SUP_LPM
+ USBD_MSC_BosDescHandler,
+#endif
+ NULL,
+ USBD_OtherSpeedConfigDescHandler,
+ USBD_DevQualifierDescHandler,
+};
+
+/* Private variables ******************************************************/
+
+/**
+ * @brief Device descriptor
+ */
+static uint8_t USBD_DeviceDesc[USBD_DEVICE_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ 0x12,
+ /* bDescriptorType */
+ USBD_DESC_DEVICE,
+ /* bcdUSB */
+#if USBD_SUP_LPM
+ 0x01, /*> 8,
+ /* idProduct */
+ USBD_FS_PID & 0xFF, USBD_FS_PID >> 8,
+ /* bcdDevice = 2.00 */
+ 0x00, 0x02,
+ /* Index of string descriptor describing manufacturer */
+ USBD_DESC_STR_MFC,
+ /* Index of string descriptor describing product */
+ USBD_DESC_STR_PRODUCT,
+ /* Index of string descriptor describing the device serial number */
+ USBD_DESC_STR_SERIAL,
+ /* bNumConfigurations */
+ USBD_SUP_CONFIGURATION_MAX_NUM,
+};
+
+/**
+ * @brief Configuration descriptor
+ */
+static uint8_t USBD_ConfigDesc[USBD_CONFIG_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_CONFIGURATION,
+ /* wTotalLength */
+ USBD_CONFIG_DESCRIPTOR_SIZE & 0xFF,
+ USBD_CONFIG_DESCRIPTOR_SIZE >> 8,
+
+ /* bNumInterfaces */
+ 0x01,
+ /* bConfigurationValue */
+ 0x01,
+ /* iConfiguration */
+ 0x01,
+ /* bmAttributes */
+#if USBD_SUP_SELF_PWR
+ 0xC0,
+#else
+ 0x80,
+#endif
+ /* MaxPower */
+ 0x32,
+
+ /* Mass Storage interface */
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_INTERFACE,
+ /* bInterfaceNumber */
+ 0x00,
+ /* bAlternateSetting */
+ 0x00,
+ /* bNumEndpoints */
+ 0x02,
+ /* bInterfaceClass */
+ USBD_MSC_ITF_CLASS_ID,
+ /* bInterfaceSubClass */
+ USBD_MSC_ITF_SUB_CLASS,
+ /* bInterfaceProtocol */
+ USBD_MSC_ITF_PROTOCOL,
+ /* iInterface */
+ 0x05,
+
+ /* Mass Storage Endpoints */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_MSC_IN_EP_ADDR,
+ /* bmAttributes */
+ 0x02,
+ /* wMaxPacketSize: */
+ USBD_MSC_FS_MP_SIZE & 0xFF,
+ USBD_MSC_FS_MP_SIZE >> 8,
+ /* bInterval: */
+ 0x00,
+
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_MSC_OUT_EP_ADDR,
+ /* bmAttributes */
+ 0x02,
+ /* wMaxPacketSize: */
+ USBD_MSC_FS_MP_SIZE & 0xFF,
+ USBD_MSC_FS_MP_SIZE >> 8,
+ /* bInterval: */
+ 0x00,
+};
+
+/**
+ * @brief Other speed configuration descriptor
+ */
+static uint8_t USBD_OtherSpeedCfgDesc[USBD_CONFIG_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_OTHER_SPEED,
+ /* wTotalLength */
+ USBD_CONFIG_DESCRIPTOR_SIZE & 0xFF,
+ USBD_CONFIG_DESCRIPTOR_SIZE >> 8,
+
+ /* bNumInterfaces */
+ 0x01,
+ /* bConfigurationValue */
+ 0x01,
+ /* iConfiguration */
+ 0x01,
+ /* bmAttributes */
+#if USBD_SUP_SELF_PWR
+ 0xC0,
+#else
+ 0x80,
+#endif
+ /* MaxPower */
+ 0x32,
+
+ /* Mass Storage interface */
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_INTERFACE,
+ /* bInterfaceNumber */
+ 0x00,
+ /* bAlternateSetting */
+ 0x00,
+ /* bNumEndpoints */
+ 0x02,
+ /* bInterfaceClass */
+ USBD_MSC_ITF_CLASS_ID,
+ /* bInterfaceSubClass */
+ USBD_MSC_ITF_SUB_CLASS,
+ /* bInterfaceProtocol */
+ USBD_MSC_ITF_PROTOCOL,
+ /* iInterface */
+ 0x05,
+
+ /* Mass Storage Endpoints */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_MSC_IN_EP_ADDR,
+ /* bmAttributes */
+ 0x02,
+ /* wMaxPacketSize: */
+ USBD_MSC_FS_MP_SIZE & 0xFF,
+ USBD_MSC_FS_MP_SIZE >> 8,
+ /* bInterval: */
+ 0x00,
+
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_MSC_OUT_EP_ADDR,
+ /* bmAttributes */
+ 0x02,
+ /* wMaxPacketSize: */
+ USBD_MSC_FS_MP_SIZE & 0xFF,
+ USBD_MSC_FS_MP_SIZE >> 8,
+ /* bInterval: */
+ 0x00,
+};
+
+#if USBD_SUP_LPM
+/**
+ * @brief BOS descriptor
+ */
+static uint8_t USBD_BosDesc[USBD_BOS_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ 0x05,
+ /* bDescriptorType */
+ USBD_DESC_BOS,
+ /* wtotalLength */
+ 0x0C, 0x00,
+ /* bNumDeviceCaps */
+ 0x01,
+
+ /* Device Capability */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType */
+ USBD_DEVICE_CAPABILITY_TYPE,
+ /* bDevCapabilityType */
+ USBD_20_EXTENSION_TYPE,
+ /* bmAttributes */
+ 0x02, 0x00, 0x00, 0x00,
+};
+#endif
+
+/**
+ * @brief Serial string descriptor
+ */
+static uint8_t USBD_SerialStrDesc[USBD_SERIAL_STRING_SIZE] =
+{
+ /* bLength */
+ USBD_SERIAL_STRING_SIZE,
+ /* bDescriptorType */
+ USBD_DESC_STRING,
+};
+
+/**
+ * @brief Language ID string descriptor
+ */
+static uint8_t USBD_LandIDStrDesc[USBD_LANGID_STRING_SIZE] =
+{
+ /* bLength */
+ USBD_LANGID_STRING_SIZE,
+ /* bDescriptorType */
+ USBD_DESC_STRING,
+ USBD_LANGID_STR & 0xFF, USBD_LANGID_STR >> 8
+};
+
+/**
+ * @brief Device qualifier descriptor
+ */
+static uint8_t USBD_DevQualifierDesc[USBD_DEVICE_QUALIFIER_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ USBD_DEVICE_QUALIFIER_DESCRIPTOR_SIZE,
+ /* bDescriptorType */
+ USBD_DESC_DEVICE_QUALIFIER,
+ 0x00,
+ 0x02,
+ 0x00,
+ 0x00,
+ 0x00,
+ USBD_MSC_FS_MP_SIZE, /* In FS device*/
+ 0x01,
+ 0x00,
+};
+
+/* Private functions ******************************************************/
+
+/**
+ * @brief USB device convert ascii string descriptor to unicode format
+ *
+ * @param desc : descriptor string
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_DESC_Ascii2Unicode(uint8_t* desc)
+{
+ USBD_DESC_INFO_T descInfo;
+ uint8_t* buffer;
+ uint8_t str[USBD_SUP_STR_DESC_MAX_NUM];
+
+ uint8_t* unicode = str;
+ uint16_t length = 0;
+ __IO uint8_t index = 0;
+
+ if (desc == NULL)
+ {
+ descInfo.desc = NULL;
+ descInfo.size = 0;
+ }
+ else
+ {
+ buffer = desc;
+ length = (strlen((char*)buffer) * 2) + 2;
+ /* Get unicode descriptor */
+ unicode[index] = length;
+
+ index++;
+ unicode[index] = USBD_DESC_STRING;
+ index++;
+
+ while (*buffer != '\0')
+ {
+ unicode[index] = *buffer;
+ buffer++;
+ index++;
+
+ unicode[index] = 0x00;
+ index++;
+ }
+ }
+
+ descInfo.desc = unicode;
+ descInfo.size = length;
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device FS device descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_DeviceDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_DeviceDesc;
+ descInfo.size = sizeof(USBD_DeviceDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device FS configuration descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_ConfigDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_ConfigDesc;
+ descInfo.size = sizeof(USBD_ConfigDesc);
+
+ return descInfo;
+}
+
+#if USBD_SUP_LPM
+/**
+ * @brief USB device FS BOS descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_BosDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_BosDesc;
+ descInfo.size = sizeof(USBD_BosDesc);
+
+ return descInfo;
+}
+#endif
+
+/**
+ * @brief USB device FS configuration string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_ConfigStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ if (usbSpeed == USBD_SPEED_HS)
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_CONFIGURATION_HS_STR);
+ }
+ else
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_CONFIGURATION_FS_STR);
+ }
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device FS interface string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_InterfaceStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ if (usbSpeed == USBD_SPEED_HS)
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_INTERFACE_HS_STR);
+ }
+ else
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_INTERFACE_FS_STR);
+ }
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device FS LANG ID string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_LangIdStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_LandIDStrDesc;
+ descInfo.size = sizeof(USBD_LandIDStrDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device FS manufacturer string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_ManufacturerStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_MANUFACTURER_STR);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device FS product string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_ProductStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ if (usbSpeed == USBD_SPEED_HS)
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_PRODUCT_HS_STR);
+ }
+ else
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_PRODUCT_FS_STR);
+ }
+
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device FS serial string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_MSC_SerialStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ /* Update the serial number string descriptor with the data from the unique ID*/
+ Get_SerialNum();
+
+ descInfo.desc = USBD_SerialStrDesc;
+ descInfo.size = sizeof(USBD_SerialStrDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device other speed configuration descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_OtherSpeedConfigDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ /* Use FS configuration */
+ descInfo.desc = USBD_OtherSpeedCfgDesc;
+ descInfo.size = sizeof(USBD_OtherSpeedCfgDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device device qualifier descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_DevQualifierDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_DevQualifierDesc;
+ descInfo.size = sizeof(USBD_DevQualifierDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief Create the serial number string descriptor
+ * @param None
+ * @retval None
+ */
+static void Get_SerialNum(void)
+{
+ uint32_t deviceserial0, deviceserial1, deviceserial2;
+
+ deviceserial0 = U_ID_0;
+ deviceserial1 = U_ID_1;
+ deviceserial2 = U_ID_2;
+
+ deviceserial0 += deviceserial2;
+
+ if (deviceserial0 != 0)
+ {
+ IntToUnicode (deviceserial0, &USBD_SerialStrDesc[2] ,8);
+ IntToUnicode (deviceserial1, &USBD_SerialStrDesc[18] ,4);
+ }
+}
+
+/**
+ * @brief Convert Hex 32Bits value into char
+ * @param value: value to convert
+ * @param pbuf: pointer to the buffer
+ * @param len: buffer length
+ * @retval None
+ */
+static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len)
+{
+ uint8_t idx = 0;
+
+ for ( idx = 0; idx < len; idx ++)
+ {
+ if ( ((value >> 28)) < 0xA )
+ {
+ pbuf[ 2* idx] = (value >> 28) + '0';
+ }
+ else
+ {
+ pbuf[2* idx] = (value >> 28) + 'A' - 10;
+ }
+
+ value = value << 4;
+
+ pbuf[ 2* idx + 1] = 0;
+ }
+}
diff --git a/src/main/drivers/mcu/apm32/usb/msc/usbd_msc_descriptor.h b/src/main/drivers/mcu/apm32/usb/msc/usbd_msc_descriptor.h
new file mode 100644
index 0000000000..b76b4d0eeb
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/msc/usbd_msc_descriptor.h
@@ -0,0 +1,56 @@
+/*!
+ * @file usbd_descriptor.h
+ *
+ * @brief usb device descriptor
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Define to prevent recursive inclusion */
+#ifndef _USBD_DESCRIPTOR_H_
+#define _USBD_DESCRIPTOR_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Includes ***************************************************************/
+#include "usbd_core.h"
+
+/* Exported macro *********************************************************/
+#define USBD_DEVICE_DESCRIPTOR_SIZE 18
+#define USBD_CONFIG_DESCRIPTOR_SIZE 32
+#define USBD_SERIAL_STRING_SIZE 26
+#define USBD_LANGID_STRING_SIZE 4
+#define USBD_DEVICE_QUALIFIER_DESCRIPTOR_SIZE 10
+#define USBD_BOS_DESCRIPTOR_SIZE 12
+
+#define USBD_DEVICE_CAPABILITY_TYPE 0x10
+#define USBD_20_EXTENSION_TYPE 0x02
+
+#define USBD_MSC_ITF_CLASS_ID 0x08
+#define USBD_MSC_ITF_PROTOCOL 0x50 /* BOT */
+#define USBD_MSC_ITF_SUB_CLASS 0x06 /* SCSI */
+
+/* Exported typedef *******************************************************/
+
+/* Exported function prototypes *******************************************/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/main/drivers/mcu/apm32/usb/usbd_board.h b/src/main/drivers/mcu/apm32/usb/usbd_board.h
new file mode 100644
index 0000000000..58546ab8df
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/usbd_board.h
@@ -0,0 +1,74 @@
+/**
+ * @file usbd_board.h
+ *
+ * @brief Header file for USB Board
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Define to prevent recursive inclusion */
+#ifndef _USBD_BOARD_H_
+#define _USBD_BOARD_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Includes ***************************************************************/
+#include "platform.h"
+
+/* Exported macro *********************************************************/
+#define USBD_SUP_CLASS_MAX_NUM 1U
+#define USBD_SUP_INTERFACE_MAX_NUM 1U
+#define USBD_SUP_CONFIGURATION_MAX_NUM 1U
+#define USBD_SUP_STR_DESC_MAX_NUM 512U
+#define USBD_SUP_MSC_MEDIA_PACKET 512U
+
+/* Only support LPM USB device */
+#define USBD_SUP_LPM 0U
+#define USBD_SUP_SELF_PWR 1U
+#define USBD_DEBUG_LEVEL 0U
+#define USE_USB_FS
+// #define USE_USB_HS
+
+#if (USBD_DEBUG_LEVEL > 0U)
+#define USBD_USR_LOG(...) do { \
+ printf(__VA_ARGS__); \
+ printf("\r\n"); \
+} while(0)
+#else
+#define USBD_USR_LOG(...) do {} while (0)
+#endif
+
+#if (USBD_DEBUG_LEVEL > 1U)
+#define USBD_USR_Debug(...) do { \
+ printf("Debug:"); \
+ printf(__VA_ARGS__); \
+ printf("\r\n"); \
+} while(0)
+#else
+#define USBD_USR_Debug(...) do {} while (0)
+#endif
+
+/* Exported typedef *******************************************************/
+
+/* Exported function prototypes *******************************************/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* USBD_BOARD_H */
diff --git a/src/main/drivers/mcu/apm32/usb/usbd_board_apm32f4.c b/src/main/drivers/mcu/apm32/usb/usbd_board_apm32f4.c
new file mode 100644
index 0000000000..570fde93da
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/usbd_board_apm32f4.c
@@ -0,0 +1,737 @@
+/**
+ * @file usbd_board.c
+ *
+ * @brief This file provides firmware functions to USB board
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Includes ***************************************************************/
+#include "usbd_board.h"
+
+/* Private includes *******************************************************/
+#include "usbd_core.h"
+#include "platform.h"
+
+/* Private macro **********************************************************/
+#define USBD_FS_RX_FIFO_SIZE 128
+#define USBD_FS_TX_FIFO_0_SIZE 64
+#define USBD_FS_TX_FIFO_1_SIZE 128
+#define USBD_FS_TX_FIFO_2_SIZE 0
+#define USBD_FS_TX_FIFO_3_SIZE 64
+
+#define USBD_HS_RX_FIFO_SIZE 512
+#define USBD_HS_TX_FIFO_0_SIZE 128
+#define USBD_HS_TX_FIFO_1_SIZE 372
+
+/* Private typedef ********************************************************/
+
+/* Private variables ******************************************************/
+PCD_HandleTypeDef husbDevice;
+
+/* Private function prototypes ********************************************/
+
+/* External variables *****************************************************/
+
+/* Private functions ******************************************************/
+
+/**
+ * @brief This function handles USB Handler
+ *
+ * @param None
+ *
+ * @retval None
+ *
+ */
+#ifdef USE_USB_FS
+void OTG_FS_IRQHandler(void)
+#else
+void OTG_HS1_IRQHandler(void)
+#endif /* USE_USB_FS */
+{
+ DAL_PCD_IRQHandler(&husbDevice);
+}
+
+#ifdef USE_USB_FS
+void OTG_FS_WKUP_IRQHandler(void)
+#else
+void OTG_HS1_WKUP_IRQHandler(void)
+#endif /* USE_USB_FS */
+{
+ if((&husbDevice)->Init.low_power_enable)
+ {
+ /* Reset SLEEPDEEP bit of Cortex System Control Register */
+ SCB->SCR &= (uint32_t)~((uint32_t)(SCB_SCR_SLEEPDEEP_Msk | SCB_SCR_SLEEPONEXIT_Msk));
+
+ /* Configures system clock after wake-up from STOP: enable HSE, PLL and select
+ PLL as system clock source (HSE and PLL are disabled in STOP mode) */
+
+ __DAL_RCM_HSE_CONFIG(RCM_HSE_ON);
+
+ /* Wait till HSE is ready */
+ while(__DAL_RCM_GET_FLAG(RCM_FLAG_HSERDY) == RESET)
+ {}
+
+ /* Enable the main PLL. */
+ __DAL_RCM_PLL_ENABLE();
+
+ /* Wait till PLL is ready */
+ while(__DAL_RCM_GET_FLAG(RCM_FLAG_PLLRDY) == RESET)
+ {}
+
+ /* Select PLL as SYSCLK */
+ MODIFY_REG(RCM->CFG, RCM_CFG_SCLKSEL, RCM_SYSCLKSOURCE_PLLCLK);
+
+ while (__DAL_RCM_GET_SYSCLK_SOURCE() != RCM_CFG_SCLKSEL_PLL)
+ {}
+
+ /* ungate PHY clock */
+ __DAL_PCD_UNGATE_PHYCLOCK((&husbDevice));
+ }
+#ifdef USE_USB_FS
+ /* Clear EINT pending Bit*/
+ __DAL_USB_OTG_FS_WAKEUP_EINT_CLEAR_FLAG();
+#else
+ /* Clear EINT pending Bit*/
+ __DAL_USB_OTG_HS_WAKEUP_EINT_CLEAR_FLAG();
+#endif
+
+}
+
+
+/**
+ * @brief Initializes the PCD MSP
+ *
+ * @param hpcd PCD handle
+ *
+ * @retval None
+ */
+void DAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
+{
+ GPIO_InitTypeDef GPIO_InitStruct = {0};
+
+ if(hpcd->Instance == USB_OTG_FS)
+ {
+ /* Configure USB OTG GPIO */
+ __DAL_RCM_GPIOA_CLK_ENABLE();
+
+ /* USB DM, DP pin configuration */
+ GPIO_InitStruct.Pin = GPIO_PIN_11 | GPIO_PIN_12;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF10_OTG_FS;
+ DAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+ /* Configure USB OTG */
+ __DAL_RCM_USB_OTG_FS_CLK_ENABLE();
+
+ /* Configure interrupt */
+ DAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0);
+ DAL_NVIC_EnableIRQ(OTG_FS_IRQn);
+ }
+ else if(hpcd->Instance == USB_OTG_HS)
+ {
+ /* Configure USB OTG GPIO */
+ __DAL_RCM_GPIOB_CLK_ENABLE();
+
+ /* USB DM, DP pin configuration */
+ GPIO_InitStruct.Pin = GPIO_PIN_14 | GPIO_PIN_15;
+ GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStruct.Pull = GPIO_NOPULL;
+ GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
+ GPIO_InitStruct.Alternate = GPIO_AF12_OTG_HS_FS;
+ DAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+ /* Configure USB OTG */
+ __DAL_RCM_USB_OTG_HS_CLK_ENABLE();
+
+ /* Configure interrupt */
+ DAL_NVIC_SetPriority(OTG_HS_IRQn, 6, 0);
+ DAL_NVIC_EnableIRQ(OTG_HS_IRQn);
+ }
+}
+
+/**
+ * @brief DeInitializes PCD MSP
+ *
+ * @param hpcd PCD handle
+ *
+ * @retval None
+ */
+void DAL_PCD_MspDeInit(PCD_HandleTypeDef *hpcd)
+{
+ if(hpcd->Instance == USB_OTG_FS)
+ {
+ /* Disable peripheral clock */
+ __DAL_RCM_USB_OTG_FS_CLK_DISABLE();
+
+ /* USB DM, DP pin configuration */
+ DAL_GPIO_DeInit(GPIOA, GPIO_PIN_11 | GPIO_PIN_12);
+
+ /* Disable peripheral interrupt */
+ DAL_NVIC_DisableIRQ(OTG_FS_IRQn);
+ }
+ else if(hpcd->Instance == USB_OTG_HS)
+ {
+ /* Disable peripheral clock */
+ __DAL_RCM_USB_OTG_HS_CLK_DISABLE();
+
+ /* USB DM, DP pin configuration */
+ DAL_GPIO_DeInit(GPIOB, GPIO_PIN_14 | GPIO_PIN_15);
+
+ /* Disable peripheral interrupt */
+ DAL_NVIC_DisableIRQ(OTG_HS_IRQn);
+ }
+}
+
+/**
+ * @brief Setup stage callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd)
+#else
+void DAL_PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_SetupStage((USBD_INFO_T*)hpcd->pData, (uint8_t *)hpcd->Setup);
+}
+
+/**
+ * @brief Data Out stage callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @param epnum: Endpoint number
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+#else
+void DAL_PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_DataOutStage((USBD_INFO_T*)hpcd->pData, epnum, hpcd->OUT_ep[epnum].xfer_buff);
+}
+
+/**
+ * @brief Data In stage callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @param epnum: Endpoint number
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+#else
+void DAL_PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_DataInStage((USBD_INFO_T*)hpcd->pData, epnum, hpcd->IN_ep[epnum].xfer_buff);
+}
+
+/**
+ * @brief SOF callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_SOFCallback(PCD_HandleTypeDef *hpcd)
+#else
+void DAL_PCD_SOFCallback(PCD_HandleTypeDef *hpcd)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_HandleSOF((USBD_INFO_T*)hpcd->pData);
+}
+
+/**
+ * @brief Reset callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_ResetCallback(PCD_HandleTypeDef *hpcd)
+#else
+void DAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_DEVICE_SPEED_T speed = USBD_DEVICE_SPEED_FS;
+
+ if (hpcd->Init.speed == PCD_SPEED_HIGH)
+ {
+ speed = USBD_DEVICE_SPEED_HS;
+ }
+ else if (hpcd->Init.speed == PCD_SPEED_FULL)
+ {
+ speed = USBD_DEVICE_SPEED_FS;
+ }
+ else
+ {
+ DAL_ErrorHandler();
+ }
+
+ /* Set USB core speed */
+ USBD_SetSpeed((USBD_INFO_T*)hpcd->pData, speed);
+
+ /* Reset Device. */
+ USBD_Reset((USBD_INFO_T*)hpcd->pData);
+}
+
+/**
+ * @brief Suspend callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_SuspendCallback(PCD_HandleTypeDef *hpcd)
+#else
+void DAL_PCD_SuspendCallback(PCD_HandleTypeDef *hpcd)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ /* USB core enters in suspend mode */
+ USBD_Suspend((USBD_INFO_T*)hpcd->pData);
+
+ if ((hpcd->Init.phy_itface == USB_OTG_EMBEDDED_PHY) && \
+ (hpcd->Init.speed == PCD_SPEED_HIGH))
+ {
+ /* Embedded HS PHY can not stop clock */
+ }
+ else
+ {
+ __DAL_PCD_GATE_PHYCLOCK(hpcd);
+ }
+
+ /* Enter in STOP mode. */
+ if (hpcd->Init.low_power_enable)
+ {
+ /* Set SLEEPDEEP bit and SleepOnExit of Cortex System Control Register. */
+ SCB->SCR |= (uint32_t)((uint32_t)(SCB_SCR_SLEEPDEEP_Msk | SCB_SCR_SLEEPONEXIT_Msk));
+ }
+}
+
+/**
+ * @brief Resume callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_ResumeCallback(PCD_HandleTypeDef *hpcd)
+#else
+void DAL_PCD_ResumeCallback(PCD_HandleTypeDef *hpcd)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_Resume((USBD_INFO_T*)hpcd->pData);
+}
+
+/**
+ * @brief ISOOUTIncomplete callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @param epnum: Endpoint number
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_ISOOUTIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+#else
+void DAL_PCD_ISOOUTIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_IsoOutInComplete((USBD_INFO_T*)hpcd->pData, epnum);
+}
+
+/**
+ * @brief ISOINIncomplete callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @param epnum: Endpoint number
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_ISOINIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+#else
+void DAL_PCD_ISOINIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_IsoInInComplete((USBD_INFO_T*)hpcd->pData, epnum);
+}
+
+/**
+ * @brief Connect callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_ConnectCallback(PCD_HandleTypeDef *hpcd)
+#else
+void DAL_PCD_ConnectCallback(PCD_HandleTypeDef *hpcd)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_Connect((USBD_INFO_T*)hpcd->pData);
+}
+
+/**
+ * @brief Disconnect callback
+ *
+ * @param hpcd: PCD handle
+ *
+ * @retval None
+ */
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+static void PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd)
+#else
+void DAL_PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd)
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+{
+ USBD_Disconnect((USBD_INFO_T*)hpcd->pData);
+}
+
+/* External functions *****************************************************/
+
+/**
+ * @brief Init USB hardware
+ *
+ * @param usbInfo: USB core information
+ *
+ * @retval None
+ */
+void USBD_HardwareInit(USBD_INFO_T* usbInfo)
+{
+ if (usbInfo->devSpeed == USBD_SPEED_FS)
+ {
+ /* Link data */
+ husbDevice.pData = usbInfo;
+ usbInfo->dataPoint = &husbDevice;
+
+ husbDevice.Instance = USB_OTG_FS;
+ husbDevice.Init.dev_endpoints = 4;
+ husbDevice.Init.speed = PCD_SPEED_FULL;
+ husbDevice.Init.dma_enable = DISABLE;
+ husbDevice.Init.phy_itface = PCD_PHY_EMBEDDED;
+ husbDevice.Init.Sof_enable = ENABLE;
+ husbDevice.Init.low_power_enable = DISABLE;
+ husbDevice.Init.lpm_enable = DISABLE;
+ husbDevice.Init.vbus_sensing_enable = DISABLE;
+ husbDevice.Init.use_dedicated_ep1 = DISABLE;
+ if (DAL_PCD_Init(&husbDevice) != DAL_OK)
+ {
+ DAL_ErrorHandler();
+ }
+
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+ /* Register USB PCD CallBacks */
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_SOF_CB_ID, PCD_SOFCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_SETUPSTAGE_CB_ID, PCD_SetupStageCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_RESET_CB_ID, PCD_ResetCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_SUSPEND_CB_ID, PCD_SuspendCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_RESUME_CB_ID, PCD_ResumeCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_CONNECT_CB_ID, PCD_ConnectCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_DISCONNECT_CB_ID, PCD_DisconnectCallback);
+
+ DAL_PCD_RegisterDataOutStageCallback(&husbDevice, PCD_DataOutStageCallback);
+ DAL_PCD_RegisterDataInStageCallback(&husbDevice, PCD_DataInStageCallback);
+ DAL_PCD_RegisterIsoOutIncpltCallback(&husbDevice, PCD_ISOOUTIncompleteCallback);
+ DAL_PCD_RegisterIsoInIncpltCallback(&husbDevice, PCD_ISOINIncompleteCallback);
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+
+ DAL_PCDEx_SetRxFiFo(&husbDevice, USBD_FS_RX_FIFO_SIZE);
+ DAL_PCDEx_SetTxFiFo(&husbDevice, 0, USBD_FS_TX_FIFO_0_SIZE);
+ DAL_PCDEx_SetTxFiFo(&husbDevice, 1, USBD_FS_TX_FIFO_1_SIZE);
+ DAL_PCDEx_SetTxFiFo(&husbDevice, 2, USBD_FS_TX_FIFO_2_SIZE);
+ DAL_PCDEx_SetTxFiFo(&husbDevice, 3, USBD_FS_TX_FIFO_3_SIZE);
+
+ /* Start USB device core */
+ USBD_StartCallback(usbInfo);
+ }
+ else if (usbInfo->devSpeed == USBD_SPEED_HS)
+ {
+ husbDevice.pData = usbInfo;
+ usbInfo->dataPoint = &husbDevice;
+
+ husbDevice.Instance = USB_OTG_HS;
+ husbDevice.Init.dev_endpoints = 6;
+ husbDevice.Init.speed = PCD_SPEED_HIGH;
+ husbDevice.Init.dma_enable = DISABLE;
+ husbDevice.Init.phy_itface = USB_OTG_EMBEDDED_PHY;
+ husbDevice.Init.Sof_enable = DISABLE;
+ husbDevice.Init.low_power_enable = DISABLE;
+ husbDevice.Init.lpm_enable = DISABLE;
+ husbDevice.Init.vbus_sensing_enable = DISABLE;
+ husbDevice.Init.use_dedicated_ep1 = DISABLE;
+ husbDevice.Init.use_external_vbus = DISABLE;
+ if (DAL_PCD_Init(&husbDevice) != DAL_OK)
+ {
+ DAL_ErrorHandler( );
+ }
+
+#if (USE_DAL_PCD_REGISTER_CALLBACKS == 1U)
+ /* Register USB PCD CallBacks */
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_SOF_CB_ID, PCD_SOFCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_SETUPSTAGE_CB_ID, PCD_SetupStageCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_RESET_CB_ID, PCD_ResetCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_SUSPEND_CB_ID, PCD_SuspendCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_RESUME_CB_ID, PCD_ResumeCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_CONNECT_CB_ID, PCD_ConnectCallback);
+ DAL_PCD_RegisterCallback(&husbDevice, DAL_PCD_DISCONNECT_CB_ID, PCD_DisconnectCallback);
+
+ DAL_PCD_RegisterDataOutStageCallback(&husbDevice, PCD_DataOutStageCallback);
+ DAL_PCD_RegisterDataInStageCallback(&husbDevice, PCD_DataInStageCallback);
+ DAL_PCD_RegisterIsoOutIncpltCallback(&husbDevice, PCD_ISOOUTIncompleteCallback);
+ DAL_PCD_RegisterIsoInIncpltCallback(&husbDevice, PCD_ISOINIncompleteCallback);
+#endif /* USE_DAL_PCD_REGISTER_CALLBACKS */
+ DAL_PCDEx_SetRxFiFo(&husbDevice, USBD_HS_RX_FIFO_SIZE);
+ DAL_PCDEx_SetTxFiFo(&husbDevice, 0, USBD_HS_TX_FIFO_0_SIZE);
+ DAL_PCDEx_SetTxFiFo(&husbDevice, 1, USBD_HS_TX_FIFO_1_SIZE);
+
+ /* Start USB device core */
+ USBD_StartCallback(usbInfo);
+ }
+}
+
+/**
+ * @brief Reset USB hardware
+ *
+ * @param usbInfo:usb handler information
+ *
+ * @retval None
+ */
+void USBD_HardwareReset(USBD_INFO_T* usbInfo)
+{
+ DAL_PCD_DeInit(usbInfo->dataPoint);
+}
+
+/**
+ * @brief USB device start event callback
+ *
+ * @param usbInfo: USB core information
+ *
+ * @retval None
+ */
+void USBD_StartCallback(USBD_INFO_T* usbInfo)
+{
+ DAL_PCD_Start(usbInfo->dataPoint);
+}
+
+/**
+ * @brief USB device stop event callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @retval None
+ */
+void USBD_StopCallback(USBD_INFO_T* usbInfo)
+{
+ DAL_PCD_Stop(usbInfo->dataPoint);
+}
+
+/**
+ * @brief USB device open EP callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr: endpoint address
+ *
+ * @param epType: endpoint type
+ *
+ * @param epMps: endpoint maxinum of packet size
+ *
+ * @retval None
+ */
+void USBD_EP_OpenCallback(USBD_INFO_T* usbInfo, uint8_t epAddr, \
+ uint8_t epType, uint16_t epMps)
+{
+ DAL_PCD_EP_Open(usbInfo->dataPoint, epAddr, epMps, epType);
+}
+
+/**
+ * @brief USB device close EP callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr: endpoint address
+ *
+ * @retval None
+ */
+void USBD_EP_CloseCallback(USBD_INFO_T* usbInfo, uint8_t epAddr)
+{
+ DAL_PCD_EP_Close(usbInfo->dataPoint, epAddr);
+}
+
+/**
+ * @brief USB device flush EP handler callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr : endpoint address
+ *
+ * @retval usb device status
+ */
+USBD_STA_T USBD_EP_FlushCallback(USBD_INFO_T* usbInfo, uint8_t epAddr)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ DAL_PCD_EP_Flush(usbInfo->dataPoint, epAddr);
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device set EP on stall status callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr: endpoint address
+ *
+ * @retval usb device status
+ */
+USBD_STA_T USBD_EP_StallCallback(USBD_INFO_T* usbInfo, uint8_t epAddr)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ DAL_PCD_EP_SetStall(usbInfo->dataPoint, epAddr);
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device clear EP stall status callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr: endpoint address
+ *
+ * @retval usb device status
+ */
+USBD_STA_T USBD_EP_ClearStallCallback(USBD_INFO_T* usbInfo, uint8_t epAddr)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ DAL_PCD_EP_ClrStall(usbInfo->dataPoint, epAddr);
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device read EP stall status callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr: endpoint address
+ *
+ * @retval Stall status
+ */
+uint8_t USBD_EP_ReadStallStatusCallback(USBD_INFO_T* usbInfo, uint8_t epAddr)
+{
+ return (DAL_PCD_EP_ReadStallStatus(usbInfo->dataPoint, epAddr));
+}
+
+/**
+ * @brief USB device set device address handler callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param address : address
+ *
+ * @retval usb device status
+ */
+USBD_STA_T USBD_SetDevAddressCallback(USBD_INFO_T* usbInfo, uint8_t address)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ DAL_PCD_SetAddress(usbInfo->dataPoint, address);
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device read EP last receive data size callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr: endpoint address
+ *
+ * @retval size of last receive data
+ */
+uint32_t USBD_EP_ReadRxDataLenCallback(USBD_INFO_T* usbInfo, uint8_t epAddr)
+{
+ return DAL_PCD_EP_GetRxCount(usbInfo->dataPoint, epAddr);
+}
+
+/**
+ * @brief USB device EP transfer handler callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr : endpoint address
+ *
+ * @param buffer : data buffer
+ *
+ * @param length : length of data
+ *
+ * @retval usb device status
+ */
+USBD_STA_T USBD_EP_TransferCallback(USBD_INFO_T* usbInfo, uint8_t epAddr, \
+ uint8_t* buffer, uint32_t length)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ DAL_PCD_EP_Transmit(usbInfo->dataPoint, epAddr, buffer, length);
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device EP receive handler callback
+ *
+ * @param usbInfo : USB core information
+ *
+ * @param epAddr : endpoint address
+ *
+ * @param buffer : data buffer
+ *
+ * @param length : length of data
+ *
+ * @retval usb device status
+ */
+USBD_STA_T USBD_EP_ReceiveCallback(USBD_INFO_T* usbInfo, uint8_t epAddr, \
+ uint8_t* buffer, uint32_t length)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ DAL_PCD_EP_Receive(usbInfo->dataPoint, epAddr, buffer, length);
+
+ return usbStatus;
+}
diff --git a/src/main/drivers/mcu/apm32/usb/vcp/serial_usb_vcp.c b/src/main/drivers/mcu/apm32/usb/vcp/serial_usb_vcp.c
new file mode 100644
index 0000000000..d90eb4592b
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/vcp/serial_usb_vcp.c
@@ -0,0 +1,251 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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_descriptor.h"
+#include "usbd_cdc.h"
+#include "usbd_cdc_vcp.h"
+
+#include "drivers/usb_io.h"
+#include "drivers/time.h"
+#include "drivers/serial.h"
+#include "drivers/serial_usb_vcp.h"
+
+static void USB_DevUserHandler(USBD_INFO_T* usbInfo, uint8_t userStatus);
+
+extern USBD_CDC_INTERFACE_T USBD_CDC_INTERFACE;
+extern USBD_DESC_T USBD_DESC_VCP;
+
+USBD_INFO_T gUsbDevice;
+
+#define USB_TIMEOUT 50
+
+static vcpPort_t vcpPort;
+
+static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
+{
+ UNUSED(instance);
+ UNUSED(baudRate);
+}
+
+static void usbVcpSetMode(serialPort_t *instance, portMode_e mode)
+{
+ UNUSED(instance);
+ UNUSED(mode);
+}
+
+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];
+
+ 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
+ }
+};
+
+serialPort_t *usbVcpOpen(void)
+{
+ vcpPort_t *s;
+
+ IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0);
+ IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0);
+
+ usbGenerateDisconnectPulse();
+
+ /* USB CDC register storage handler */
+ USBD_CDC_RegisterItf(&gUsbDevice, &USBD_CDC_INTERFACE);
+
+ /* USB device and class init */
+ USBD_Init(&gUsbDevice, USBD_SPEED_FS, &USBD_DESC_VCP, &USBD_CDC_CLASS, USB_DevUserHandler);
+
+ s = &vcpPort;
+ s->port.vTable = usbVTable;
+
+ return (serialPort_t *)s;
+}
+
+uint32_t usbVcpGetBaudRate(serialPort_t *instance)
+{
+ UNUSED(instance);
+
+ return CDC_BaudRate();
+}
+
+uint8_t usbVcpIsConnected(void)
+{
+ return usbIsConnected();
+}
+
+/**
+ * @brief USB device user handler
+ *
+ * @param usbInfo
+ *
+ * @param userStatus
+ *
+ * @retval None
+ */
+static void USB_DevUserHandler(USBD_INFO_T* usbInfo, uint8_t userStatus)
+{
+ UNUSED(usbInfo);
+ UNUSED(userStatus);
+}
+#endif // USE_VCP
diff --git a/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_descriptor.c b/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_descriptor.c
new file mode 100644
index 0000000000..a34e9ac6e7
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_descriptor.c
@@ -0,0 +1,846 @@
+/**
+ * @file usbd_descriptor.c
+ *
+ * @brief usb device descriptor configuration
+ *
+ * @version V1.0.0
+ *
+ * @date 2023-07-31
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Includes ***************************************************************/
+#include "usbd_cdc_descriptor.h"
+
+/* Private includes *******************************************************/
+#include "usbd_cdc.h"
+#include "platform.h"
+
+#include
+
+/* Private macro **********************************************************/
+#define USBD_GEEHY_VID 0x314B
+#define USBD_PID 0x5740
+#define USBD_LANGID_STR 0x0409
+#define USBD_MANUFACTURER_STR FC_FIRMWARE_NAME
+
+#ifdef USBD_PRODUCT_STRING
+#define USBD_PRODUCT_HS_STR USBD_PRODUCT_STRING
+#define USBD_PRODUCT_FS_STR USBD_PRODUCT_STRING
+#else
+#define USBD_PRODUCT_HS_STR "APM32 Virtual ComPort in HS Mode"
+#define USBD_PRODUCT_FS_STR "APM32 Virtual ComPort in FS Mode"
+#endif /* USBD_PRODUCT_STRING */
+
+#define USBD_CONFIGURATION_HS_STR "VCP Config"
+#define USBD_CONFIGURATION_FS_STR "VCP Config"
+#define USBD_INTERFACE_HS_STR "VCP Interface"
+#define USBD_INTERFACE_FS_STR "VCP Interface"
+
+/* Private function prototypes ********************************************/
+static USBD_DESC_INFO_T USBD_VCP_DeviceDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_VCP_ConfigDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_VCP_ConfigStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_VCP_InterfaceStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_VCP_LangIdStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_VCP_ManufacturerStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_VCP_ProductStrDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_VCP_SerialStrDescHandler(uint8_t usbSpeed);
+#if USBD_SUP_LPM
+static USBD_DESC_INFO_T USBD_VCP_BosDescHandler(uint8_t usbSpeed);
+#endif
+static USBD_DESC_INFO_T USBD_OtherSpeedConfigDescHandler(uint8_t usbSpeed);
+static USBD_DESC_INFO_T USBD_DevQualifierDescHandler(uint8_t usbSpeed);
+
+static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len);
+static void Get_SerialNum(void);
+
+/* Private typedef ********************************************************/
+
+/* USB device descripotr handler */
+USBD_DESC_T USBD_DESC_VCP =
+{
+ "CDC VCP Descriptor",
+ USBD_VCP_DeviceDescHandler,
+ USBD_VCP_ConfigDescHandler,
+ USBD_VCP_ConfigStrDescHandler,
+ USBD_VCP_InterfaceStrDescHandler,
+ USBD_VCP_LangIdStrDescHandler,
+ USBD_VCP_ManufacturerStrDescHandler,
+ USBD_VCP_ProductStrDescHandler,
+ USBD_VCP_SerialStrDescHandler,
+#if USBD_SUP_LPM
+ USBD_VCP_BosDescHandler,
+#endif
+ NULL,
+ USBD_OtherSpeedConfigDescHandler,
+ USBD_DevQualifierDescHandler,
+};
+
+/* Private variables ******************************************************/
+
+/**
+ * @brief Device descriptor
+ */
+static uint8_t USBD_DeviceDesc[USBD_DEVICE_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ 0x12,
+ /* bDescriptorType */
+ USBD_DESC_DEVICE,
+ /* bcdUSB */
+#if USBD_SUP_LPM
+ 0x01, /*> 8,
+ /* idProduct */
+ USBD_PID & 0xFF, USBD_PID >> 8,
+ /* bcdDevice = 2.00 */
+ 0x00, 0x02,
+ /* Index of string descriptor describing manufacturer */
+ USBD_DESC_STR_MFC,
+ /* Index of string descriptor describing product */
+ USBD_DESC_STR_PRODUCT,
+ /* Index of string descriptor describing the device serial number */
+ USBD_DESC_STR_SERIAL,
+ /* bNumConfigurations */
+ USBD_SUP_CONFIGURATION_MAX_NUM,
+};
+
+/**
+ * @brief Configuration descriptor
+ */
+static uint8_t USBD_ConfigDesc[USBD_CONFIG_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_CONFIGURATION,
+ /* wTotalLength */
+ USBD_CONFIG_DESCRIPTOR_SIZE & 0xFF,
+ USBD_CONFIG_DESCRIPTOR_SIZE >> 8,
+
+ /* bNumInterfaces */
+ 0x02,
+ /* bConfigurationValue */
+ 0x01,
+ /* iConfiguration */
+ 0x00,
+ /* bmAttributes */
+#if USBD_SUP_SELF_PWR
+ 0xC0,
+#else
+ 0x80,
+#endif
+ /* MaxPower */
+ 0x32,
+
+ /* CDC Command Interface */
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_INTERFACE,
+ /* bInterfaceNumber */
+ 0x00,
+ /* bAlternateSetting */
+ 0x00,
+ /* bNumEndpoints */
+ 0x01,
+ /* bInterfaceClass */
+ USBD_CDC_CTRL_ITF_CLASS_ID,
+ /* bInterfaceSubClass */
+ USBD_CDC_SUB_CLASS_ACM,
+ /* bInterfaceProtocol */
+ USBD_CDC_ITF_PORTOCOL_AT,
+ /* iInterface */
+ 0x00,
+
+ /* CDC Header Function Descriptor */
+ /* bLength: Endpoint Descriptor size */
+ 0x05,
+ /* bDescriptorType: CS_INTERFACE */
+ 0x24,
+ /* bDescriptorSubtype: Header Func Desc */
+ 0x00,
+ /* bcdCDC: spec release number */
+ 0x10, 0x01,
+
+ /* CDC Call Management Function Descriptor */
+ /* bFunctionLength */
+ 0x05,
+ /* bDescriptorType: CS_INTERFACE */
+ 0x24,
+ /* bDescriptorSubtype: Call Management Func Desc */
+ 0x01,
+ /* bmCapabilities: D0+D1 */
+ 0x00,
+ /* bDataInterface: 1 */
+ 0x01,
+
+ /* CDC ACM Function Descriptor */
+ /* bFunctionLength */
+ 0x04,
+ /* bDescriptorType: CS_INTERFACE */
+ 0x24,
+ /* bDescriptorSubtype: Abstract Control Management desc */
+ 0x02,
+ /* bmCapabilities */
+ 0x02,
+
+ /* CDC Union Function Descriptor */
+ /* bFunctionLength */
+ 0x05,
+ /* bDescriptorType: CS_INTERFACE */
+ 0x24,
+ /* bDescriptorSubtype: Union func desc */
+ 0x06,
+ /* bMasterInterface: Communication class interface */
+ 0x00,
+ /* bSlaveInterface0: Data Class Interface */
+ 0x01,
+
+ /* Endpoint 2 */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_CDC_CMD_EP_ADDR,
+ /* bmAttributes */
+ 0x03,
+ /* wMaxPacketSize: */
+ USBD_CDC_CMD_MP_SIZE & 0xFF,
+ USBD_CDC_CMD_MP_SIZE >> 8,
+ /* bInterval: */
+ USBD_CDC_FS_INTERVAL,
+
+ /* CDC Data Interface */
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_INTERFACE,
+ /* bInterfaceNumber */
+ 0x01,
+ /* bAlternateSetting */
+ 0x00,
+ /* bNumEndpoints */
+ 0x02,
+ /* bInterfaceClass */
+ USBD_CDC_DATA_ITF_CLASS_ID,
+ /* bInterfaceSubClass */
+ 0x00,
+ /* bInterfaceProtocol */
+ 0x00,
+ /* iInterface */
+ 0x00,
+
+ /* Endpoint OUT */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_CDC_DATA_OUT_EP_ADDR,
+ /* bmAttributes */
+ 0x02,
+ /* wMaxPacketSize: */
+ USBD_CDC_FS_MP_SIZE & 0xFF,
+ USBD_CDC_FS_MP_SIZE >> 8,
+ /* bInterval: */
+ 0x00,
+
+ /* Endpoint IN */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_CDC_DATA_IN_EP_ADDR,
+ /* bmAttributes */
+ 0x02,
+ /* wMaxPacketSize: */
+ USBD_CDC_FS_MP_SIZE & 0xFF,
+ USBD_CDC_FS_MP_SIZE >> 8,
+ /* bInterval: */
+ 0x00,
+};
+
+/**
+ * @brief Other speed configuration descriptor
+ */
+static uint8_t USBD_OtherSpeedCfgDesc[USBD_CONFIG_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_OTHER_SPEED,
+ /* wTotalLength */
+ USBD_CONFIG_DESCRIPTOR_SIZE & 0xFF,
+ USBD_CONFIG_DESCRIPTOR_SIZE >> 8,
+
+ /* bNumInterfaces */
+ 0x02,
+ /* bConfigurationValue */
+ 0x01,
+ /* iConfiguration */
+ 0x00,
+ /* bmAttributes */
+#if USBD_SUP_SELF_PWR
+ 0xC0,
+#else
+ 0x80,
+#endif
+ /* MaxPower */
+ 0x32,
+
+ /* CDC Command Interface */
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_INTERFACE,
+ /* bInterfaceNumber */
+ 0x00,
+ /* bAlternateSetting */
+ 0x00,
+ /* bNumEndpoints */
+ 0x01,
+ /* bInterfaceClass */
+ USBD_CDC_CTRL_ITF_CLASS_ID,
+ /* bInterfaceSubClass */
+ USBD_CDC_SUB_CLASS_ACM,
+ /* bInterfaceProtocol */
+ USBD_CDC_ITF_PORTOCOL_AT,
+ /* iInterface */
+ 0x00,
+
+ /* CDC Header Function Descriptor */
+ /* bLength: Endpoint Descriptor size */
+ 0x05,
+ /* bDescriptorType: CS_INTERFACE */
+ 0x24,
+ /* bDescriptorSubtype: Header Func Desc */
+ 0x00,
+ /* bcdCDC: spec release number */
+ 0x10, 0x01,
+
+ /* CDC Call Management Function Descriptor */
+ /* bFunctionLength */
+ 0x05,
+ /* bDescriptorType: CS_INTERFACE */
+ 0x24,
+ /* bDescriptorSubtype: Call Management Func Desc */
+ 0x01,
+ /* bmCapabilities: D0+D1 */
+ 0x00,
+ /* bDataInterface: 1 */
+ 0x01,
+
+ /* CDC ACM Function Descriptor */
+ /* bFunctionLength */
+ 0x04,
+ /* bDescriptorType: CS_INTERFACE */
+ 0x24,
+ /* bDescriptorSubtype: Abstract Control Management desc */
+ 0x02,
+ /* bmCapabilities */
+ 0x02,
+
+ /* CDC Union Function Descriptor */
+ /* bFunctionLength */
+ 0x05,
+ /* bDescriptorType: CS_INTERFACE */
+ 0x24,
+ /* bDescriptorSubtype: Union func desc */
+ 0x06,
+ /* bMasterInterface: Communication class interface */
+ 0x00,
+ /* bSlaveInterface0: Data Class Interface */
+ 0x01,
+
+ /* Endpoint 2 */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_CDC_CMD_EP_ADDR,
+ /* bmAttributes */
+ 0x03,
+ /* wMaxPacketSize: */
+ USBD_CDC_CMD_MP_SIZE & 0xFF,
+ USBD_CDC_CMD_MP_SIZE >> 8,
+ /* bInterval: */
+ USBD_CDC_FS_INTERVAL,
+
+ /* CDC Data Interface */
+ /* bLength */
+ 0x09,
+ /* bDescriptorType */
+ USBD_DESC_INTERFACE,
+ /* bInterfaceNumber */
+ 0x01,
+ /* bAlternateSetting */
+ 0x00,
+ /* bNumEndpoints */
+ 0x02,
+ /* bInterfaceClass */
+ USBD_CDC_DATA_ITF_CLASS_ID,
+ /* bInterfaceSubClass */
+ 0x00,
+ /* bInterfaceProtocol */
+ 0x00,
+ /* iInterface */
+ 0x00,
+
+ /* Endpoint OUT */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_CDC_DATA_OUT_EP_ADDR,
+ /* bmAttributes */
+ 0x02,
+ /* wMaxPacketSize: */
+ USBD_CDC_FS_MP_SIZE & 0xFF,
+ USBD_CDC_FS_MP_SIZE >> 8,
+ /* bInterval: */
+ 0x00,
+
+ /* Endpoint IN */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType: Endpoint */
+ USBD_DESC_ENDPOINT,
+ /* bEndpointAddress */
+ USBD_CDC_DATA_IN_EP_ADDR,
+ /* bmAttributes */
+ 0x02,
+ /* wMaxPacketSize: */
+ USBD_CDC_FS_MP_SIZE & 0xFF,
+ USBD_CDC_FS_MP_SIZE >> 8,
+ /* bInterval: */
+ 0x00,
+};
+
+#if USBD_SUP_LPM
+/**
+ * @brief BOS descriptor
+ */
+uint8_t USBD_BosDesc[USBD_BOS_DESCRIPTOR_SIZE] =
+{
+ /* bLength */
+ 0x05,
+ /* bDescriptorType */
+ USBD_DESC_BOS,
+ /* wtotalLength */
+ 0x0C, 0x00,
+ /* bNumDeviceCaps */
+ 0x01,
+
+ /* Device Capability */
+ /* bLength */
+ 0x07,
+ /* bDescriptorType */
+ USBD_DEVICE_CAPABILITY_TYPE,
+ /* bDevCapabilityType */
+ USBD_20_EXTENSION_TYPE,
+ /* bmAttributes */
+ 0x02, 0x00, 0x00, 0x00,
+};
+#endif
+
+/**
+ * @brief Serial string descriptor
+ */
+static uint8_t USBD_SerialStrDesc[USBD_SERIAL_STRING_SIZE] =
+{
+ USBD_SERIAL_STRING_SIZE,
+ USBD_DESC_STRING,
+};
+
+/**
+ * @brief Language ID string descriptor
+ */
+static uint8_t USBD_LandIDStrDesc[USBD_LANGID_STRING_SIZE] =
+{
+ /* Size */
+ USBD_LANGID_STRING_SIZE,
+ /* bDescriptorType */
+ USBD_DESC_STRING,
+ USBD_LANGID_STR & 0xFF, USBD_LANGID_STR >> 8
+};
+
+/**
+ * @brief Device qualifier descriptor
+ */
+static uint8_t USBD_DevQualifierDesc[USBD_DEVICE_QUALIFIER_DESCRIPTOR_SIZE] =
+{
+ /* Size */
+ USBD_DEVICE_QUALIFIER_DESCRIPTOR_SIZE,
+ /* bDescriptorType */
+ USBD_DESC_DEVICE_QUALIFIER,
+ 0x00,
+ 0x02,
+ 0x00,
+ 0x00,
+ 0x00,
+ USBD_CDC_FS_MP_SIZE, /* In FS device*/
+ 0x01,
+ 0x00,
+};
+
+/* Private functions ******************************************************/
+
+/**
+ * @brief USB device convert ascii string descriptor to unicode format
+ *
+ * @param desc : descriptor string
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_DESC_Ascii2Unicode(uint8_t* desc)
+{
+ USBD_DESC_INFO_T descInfo;
+ uint8_t* buffer;
+ uint8_t str[USBD_SUP_STR_DESC_MAX_NUM];
+
+ uint8_t* unicode = str;
+ uint16_t length = 0;
+ __IO uint8_t index = 0;
+
+ if (desc == NULL)
+ {
+ descInfo.desc = NULL;
+ descInfo.size = 0;
+ }
+ else
+ {
+ buffer = desc;
+ length = (strlen((char*)buffer) * 2) + 2;
+ /* Get unicode descriptor */
+ unicode[index] = length;
+
+ index++;
+ unicode[index] = USBD_DESC_STRING;
+ index++;
+
+ while (*buffer != '\0')
+ {
+ unicode[index] = *buffer;
+ buffer++;
+ index++;
+
+ unicode[index] = 0x00;
+ index++;
+ }
+ }
+
+ descInfo.desc = unicode;
+ descInfo.size = length;
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device device descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_DeviceDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_DeviceDesc;
+ descInfo.size = sizeof(USBD_DeviceDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device configuration descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_ConfigDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_ConfigDesc;
+ descInfo.size = sizeof(USBD_ConfigDesc);
+
+ return descInfo;
+}
+
+#if USBD_SUP_LPM
+/**
+ * @brief USB device BOS descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_BosDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_BosDesc;
+ descInfo.size = sizeof(USBD_BosDesc);
+
+ return descInfo;
+}
+#endif
+
+/**
+ * @brief USB device configuration string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_ConfigStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ if (usbSpeed == USBD_SPEED_HS)
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_CONFIGURATION_HS_STR);
+ }
+ else
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_CONFIGURATION_FS_STR);
+ }
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device interface string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_InterfaceStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ if (usbSpeed == USBD_SPEED_HS)
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_INTERFACE_HS_STR);
+ }
+ else
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_INTERFACE_FS_STR);
+ }
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device LANG ID string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_LangIdStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo.desc = USBD_LandIDStrDesc;
+ descInfo.size = sizeof(USBD_LandIDStrDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device manufacturer string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_ManufacturerStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_MANUFACTURER_STR);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device product string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_ProductStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ if (usbSpeed == USBD_SPEED_HS)
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_PRODUCT_HS_STR);
+ }
+ else
+ {
+ descInfo = USBD_DESC_Ascii2Unicode((uint8_t*)USBD_PRODUCT_FS_STR);
+ }
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device serial string descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_VCP_SerialStrDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ /* Update the serial number string descriptor with the data from the unique ID*/
+ Get_SerialNum();
+
+ descInfo.desc = USBD_SerialStrDesc;
+ descInfo.size = sizeof(USBD_SerialStrDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device other speed configuration descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_OtherSpeedConfigDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ /* Use FS configuration */
+ descInfo.desc = USBD_OtherSpeedCfgDesc;
+ descInfo.size = sizeof(USBD_OtherSpeedCfgDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief USB device device qualifier descriptor
+ *
+ * @param usbSpeed : usb speed
+ *
+ * @retval usb descriptor information
+ */
+static USBD_DESC_INFO_T USBD_DevQualifierDescHandler(uint8_t usbSpeed)
+{
+ USBD_DESC_INFO_T descInfo;
+
+ UNUSED(usbSpeed);
+
+ /* Use FS configuration */
+ descInfo.desc = USBD_DevQualifierDesc;
+ descInfo.size = sizeof(USBD_DevQualifierDesc);
+
+ return descInfo;
+}
+
+/**
+ * @brief Create the serial number string descriptor
+ * @param None
+ * @retval None
+ */
+static void Get_SerialNum(void)
+{
+ uint32_t deviceserial0, deviceserial1, deviceserial2;
+
+ deviceserial0 = U_ID_0;
+ deviceserial1 = U_ID_1;
+ deviceserial2 = U_ID_2;
+
+ deviceserial0 += deviceserial2;
+
+ if (deviceserial0 != 0)
+ {
+ IntToUnicode (deviceserial0, &USBD_SerialStrDesc[2] ,8);
+ IntToUnicode (deviceserial1, &USBD_SerialStrDesc[18] ,4);
+ }
+}
+
+/**
+ * @brief Convert Hex 32Bits value into char
+ * @param value: value to convert
+ * @param pbuf: pointer to the buffer
+ * @param len: buffer length
+ * @retval None
+ */
+static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len)
+{
+ uint8_t idx = 0;
+
+ for ( idx = 0; idx < len; idx ++)
+ {
+ if ( ((value >> 28)) < 0xA )
+ {
+ pbuf[ 2* idx] = (value >> 28) + '0';
+ }
+ else
+ {
+ pbuf[2* idx] = (value >> 28) + 'A' - 10;
+ }
+
+ value = value << 4;
+
+ pbuf[ 2* idx + 1] = 0;
+ }
+}
diff --git a/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_descriptor.h b/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_descriptor.h
new file mode 100644
index 0000000000..f5e55e3fd8
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_descriptor.h
@@ -0,0 +1,57 @@
+/**
+ * @file usbd_descriptor.h
+ *
+ * @brief usb device descriptor
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Define to prevent recursive inclusion */
+#ifndef _USBD_DESCRIPTOR_H_
+#define _USBD_DESCRIPTOR_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Includes ***************************************************************/
+#include "usbd_core.h"
+
+/* Exported macro *********************************************************/
+#define USBD_DEVICE_DESCRIPTOR_SIZE 18
+#define USBD_CONFIG_DESCRIPTOR_SIZE 67
+#define USBD_SERIAL_STRING_SIZE 0x1A
+#define USBD_LANGID_STRING_SIZE 4
+#define USBD_DEVICE_QUALIFIER_DESCRIPTOR_SIZE 10
+#define USBD_BOS_DESCRIPTOR_SIZE 12
+
+#define USBD_DEVICE_CAPABILITY_TYPE 0x10
+#define USBD_20_EXTENSION_TYPE 0x02
+
+#define USBD_CDC_CTRL_ITF_CLASS_ID 0x02
+#define USBD_CDC_DATA_ITF_CLASS_ID 0x0A
+#define USBD_CDC_SUB_CLASS_ACM 0x02
+#define USBD_CDC_ITF_PORTOCOL_AT 0x01
+
+/* Exported typedef *******************************************************/
+
+/* Exported function prototypes *******************************************/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_vcp.c b/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_vcp.c
new file mode 100644
index 0000000000..b3524a5173
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_vcp.c
@@ -0,0 +1,484 @@
+/**
+ * @file usbd_cdc_vcp.c
+ *
+ * @brief usb device CDC class Virtual Com Port handler
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Includes ***************************************************************/
+#include "usbd_cdc_vcp.h"
+
+/* Private includes *******************************************************/
+#include
+
+#include "platform.h"
+
+#include "build/atomic.h"
+
+#include "drivers/nvic.h"
+#include "drivers/serial_usb_vcp.h"
+#include "drivers/time.h"
+
+/* Private macro **********************************************************/
+#define APP_RX_DATA_SIZE 2048
+#define APP_TX_DATA_SIZE 2048
+
+#define APP_TX_BLOCK_SIZE 512
+
+/* Private variables ******************************************************/
+volatile uint8_t cdcTxBuffer[APP_RX_DATA_SIZE];
+volatile uint8_t cdcRxBuffer[APP_TX_DATA_SIZE];
+uint32_t BuffLength;
+volatile uint32_t cdcTxBufPtrIn = 0;/* Increment this pointer or roll it back to
+ start address when data are received over USART */
+volatile uint32_t cdcTxBufPtrOut = 0; /* Increment this pointer or roll it back to
+ start address when data are sent over USB */
+
+uint32_t rxAvailable = 0;
+uint8_t* rxBuffPtr = NULL;
+
+/* Private typedef ********************************************************/
+
+static USBD_STA_T USBD_CDC_ItfInit(void);
+static USBD_STA_T USBD_CDC_ItfDeInit(void);
+static USBD_STA_T USBD_CDC_ItfCtrl(uint8_t command, uint8_t *buffer, uint16_t length);
+static USBD_STA_T USBD_CDC_ItfSend(uint8_t *buffer, uint16_t length);
+static USBD_STA_T USBD_CDC_ItfSendEnd(uint8_t epNum, uint8_t *buffer, uint32_t *length);
+static USBD_STA_T USBD_CDC_ItfReceive(uint8_t *buffer, uint32_t *length);
+static USBD_STA_T USBD_CDC_ItfSOF(void);
+
+
+/* USB CDC interface handler */
+USBD_CDC_INTERFACE_T USBD_CDC_INTERFACE =
+{
+ "CDC Interface",
+ USBD_CDC_ItfInit,
+ USBD_CDC_ItfDeInit,
+ USBD_CDC_ItfCtrl,
+ USBD_CDC_ItfSend,
+ USBD_CDC_ItfSendEnd,
+ USBD_CDC_ItfReceive,
+ USBD_CDC_ItfSOF,
+};
+
+/* CDC Line Code Information */
+USBD_CDC_LINE_CODING_T LineCoding =
+{
+ 115200, /*!< baud rate */
+ 0x00, /*!< stop bits 1 */
+ 0x00, /*!< parity none */
+ 0x08, /*!< word length 8 bits */
+};
+
+/* Private function prototypes ********************************************/
+static void (*ctrlLineStateCb)(void *context, uint16_t ctrlLineState);
+static void *ctrlLineStateCbContext;
+static void (*baudRateCb)(void *context, uint32_t baud);
+static void *baudRateCbContext;
+
+/* External variables *****************************************************/
+extern USBD_INFO_T gUsbDevice;
+
+/* External functions *****************************************************/
+
+/**
+ * @brief USB device initializes CDC media handler
+ *
+ * @param None
+ *
+ * @retval USB device operation status
+ */
+static USBD_STA_T USBD_CDC_ItfInit(void)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ USBD_CDC_ConfigRxBuffer(&gUsbDevice, (uint8_t *)cdcRxBuffer);
+ USBD_CDC_ConfigTxBuffer(&gUsbDevice, (uint8_t *)cdcTxBuffer, 0);
+
+ ctrlLineStateCb = NULL;
+ baudRateCb = NULL;
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device deinitializes CDC media handler
+ *
+ * @param None
+ *
+ * @retval USB device operation status
+ */
+static USBD_STA_T USBD_CDC_ItfDeInit(void)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device CDC interface control request handler
+ *
+ * @param command: Command code
+ *
+ * @param buffer: Command data buffer
+ *
+ * @param length: Command data length
+ *
+ * @retval USB device operation status
+ */
+static USBD_STA_T USBD_CDC_ItfCtrl(uint8_t command, uint8_t *buffer, uint16_t length)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+ LINE_CODING* plc = (LINE_CODING*)buffer;
+
+ switch(command)
+ {
+ case USBD_CDC_SEND_ENCAPSULATED_COMMAND:
+ break;
+
+ case USBD_CDC_GET_ENCAPSULATED_RESPONSE:
+ break;
+
+ case USBD_CDC_SET_COMM_FEATURE:
+ break;
+
+ case USBD_CDC_GET_COMM_FEATURE:
+ break;
+
+ case USBD_CDC_CLEAR_COMM_FEATURE:
+ break;
+
+ /* Line Coding Data Structure
+ * | Offset(Byte) | Field | Length | Desc |
+ * | 0 | dwDTERate | 4 | Data Terminal rate |
+ * | 4 | bCharFormat | 1 | Stop bits |
+ * (0 : 1 Stop bit)
+ * (1 : 1.5 Stop bits)
+ * (2 : 2 Stop bits)
+ * | 5 | bParityType | 1 | Parity |
+ * (0 : None)
+ * (1 : Odd)
+ * (2 : Even)
+ * (3 : Mark)
+ * (4 : Space)
+ * | 6 | bDataBits | 1 | Data bits |
+ * (5 : 5 bits)
+ * (6 : 6 bits)
+ * (7 : 7 bits)
+ * (8 : 8 bits)
+ * (16 : 16 bits)
+ */
+ case USBD_CDC_SET_LINE_CODING:
+ if (buffer && (length == sizeof(*plc))) {
+ LineCoding.baudRate = plc->bitrate;
+ LineCoding.format = plc->format;
+ LineCoding.parityType = plc->paritytype;
+ LineCoding.WordLen = plc->datatype;
+
+ // If a callback is provided, tell the upper driver of changes in baud rate
+ if (baudRateCb) {
+ baudRateCb(baudRateCbContext, LineCoding.baudRate);
+ }
+ }
+ break;
+
+ case USBD_CDC_GET_LINE_CODING:
+ if (buffer && (length == sizeof(*plc))) {
+ plc->bitrate = LineCoding.baudRate;
+ plc->format = LineCoding.format;
+ plc->paritytype = LineCoding.parityType;
+ plc->datatype = LineCoding.WordLen;
+ }
+ break;
+
+ case USBD_CDC_SET_CONTROL_LINE_STATE:
+ // If a callback is provided, tell the upper driver of changes in DTR/RTS state
+ if (buffer && (length == sizeof(uint16_t))) {
+ if (ctrlLineStateCb) {
+ ctrlLineStateCb(ctrlLineStateCbContext, *((uint16_t *)buffer));
+ }
+ }
+ break;
+
+ case USBD_CDC_SEND_BREAK:
+ break;
+
+ default:
+ break;
+ }
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device CDC interface send handler
+ *
+ * @param buffer: Command data buffer
+ *
+ * @param length: Command data length
+ *
+ * @retval USB device operation status
+ */
+static USBD_STA_T USBD_CDC_ItfSend(uint8_t *buffer, uint16_t length)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ USBD_CDC_INFO_T *usbDevCDC = (USBD_CDC_INFO_T*)gUsbDevice.devClass[gUsbDevice.classID]->classData;
+
+ if(usbDevCDC->cdcTx.state != USBD_CDC_XFER_IDLE)
+ {
+ return USBD_BUSY;
+ }
+
+ USBD_CDC_ConfigTxBuffer(&gUsbDevice, buffer, length);
+
+ usbStatus = USBD_CDC_TxPacket(&gUsbDevice);
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device CDC interface send end event handler
+ *
+ * @param epNum: endpoint number
+ *
+ * @param buffer: Command data buffer
+ *
+ * @param length: Command data length
+ *
+ * @retval USB device operation status
+ */
+static USBD_STA_T USBD_CDC_ItfSendEnd(uint8_t epNum, uint8_t *buffer, uint32_t *length)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ UNUSED(epNum);
+ UNUSED(buffer);
+ UNUSED(length);
+
+ return usbStatus;
+}
+
+/**
+ * @brief USB device CDC interface receive handler
+ *
+ * @param buffer: Command data buffer
+ *
+ * @param length: Command data length
+ *
+ * @retval USB device operation status
+ */
+static USBD_STA_T USBD_CDC_ItfReceive(uint8_t *buffer, uint32_t *length)
+{
+ USBD_STA_T usbStatus = USBD_OK;
+
+ // USBD_CDC_ConfigRxBuffer(&gUsbDevice, &buffer[0]);
+ rxAvailable = *length;
+ rxBuffPtr = buffer;
+ if (!rxAvailable) {
+ // Received an empty packet, trigger receiving the next packet.
+ // This will happen after a packet that's exactly 64 bytes is received.
+ // The USB protocol requires that an empty (0 byte) packet immediately follow.
+ USBD_CDC_RxPacket(&gUsbDevice);
+ }
+
+ return usbStatus;
+}
+
+static USBD_STA_T USBD_CDC_ItfSOF(void)
+{
+ static uint32_t FrameCount = 0;
+
+ uint32_t buffsize;
+ static uint32_t lastBuffsize = 0;
+
+ USBD_CDC_INFO_T *usbDevCDC = (USBD_CDC_INFO_T*)gUsbDevice.devClass[gUsbDevice.classID]->classData;
+
+ if (FrameCount++ == USBD_CDC_FS_INTERVAL)
+ {
+ FrameCount = 0;
+
+ if (usbDevCDC->cdcTx.state == USBD_CDC_XFER_IDLE) {
+ // endpoint has finished transmitting previous block
+ if (lastBuffsize) {
+ bool needZeroLengthPacket = lastBuffsize % 64 == 0;
+
+ // move the ring buffer tail based on the previous succesful transmission
+ cdcTxBufPtrOut += lastBuffsize;
+ if (cdcTxBufPtrOut == APP_TX_DATA_SIZE) {
+ cdcTxBufPtrOut = 0;
+ }
+ lastBuffsize = 0;
+
+ if (needZeroLengthPacket) {
+ USBD_CDC_ConfigTxBuffer(&gUsbDevice, (uint8_t*)&cdcTxBuffer[cdcTxBufPtrOut], 0);
+ return USBD_OK;
+ }
+ }
+ if (cdcTxBufPtrOut != cdcTxBufPtrIn) {
+ if (cdcTxBufPtrOut > cdcTxBufPtrIn) { /* Roll-back */
+ buffsize = APP_TX_DATA_SIZE - cdcTxBufPtrOut;
+ } else {
+ buffsize = cdcTxBufPtrIn - cdcTxBufPtrOut;
+ }
+ if (buffsize > APP_TX_BLOCK_SIZE) {
+ buffsize = APP_TX_BLOCK_SIZE;
+ }
+
+ USBD_CDC_ConfigTxBuffer(&gUsbDevice, (uint8_t*)&cdcTxBuffer[cdcTxBufPtrOut], buffsize);
+
+ if (USBD_CDC_TxPacket(&gUsbDevice) == USBD_OK) {
+ lastBuffsize = buffsize;
+ }
+ }
+ }
+ }
+
+ return USBD_OK;
+}
+
+/*******************************************************************************
+ * Function Name : Send DATA .
+ * Description : send the data received from the STM32 to the PC through USB
+ * Input : buffer to send, and the length of the buffer.
+ * Output : None.
+ * Return : None.
+ *******************************************************************************/
+uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
+{
+ for (uint32_t i = 0; i < sendLength; i++) {
+ while (CDC_Send_FreeBytes() == 0) {
+ // block until there is free space in the ring buffer
+ delay(1);
+ }
+ ATOMIC_BLOCK(NVIC_BUILD_PRIORITY(6, 0)) { // Paranoia
+ cdcTxBuffer[cdcTxBufPtrIn] = ptrBuffer[i];
+ cdcTxBufPtrIn = (cdcTxBufPtrIn + 1) % APP_TX_DATA_SIZE;
+ }
+ }
+ return sendLength;
+}
+
+/*******************************************************************************
+ * Function Name : Receive DATA .
+ * Description : receive the data from the PC to STM32 and send it through USB
+ * Input : None.
+ * Output : None.
+ * Return : None.
+ *******************************************************************************/
+uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len)
+{
+ uint32_t count = 0;
+ if ( (rxBuffPtr != NULL))
+ {
+ while ((rxAvailable > 0) && count < len)
+ {
+ recvBuf[count] = rxBuffPtr[0];
+ rxBuffPtr++;
+ rxAvailable--;
+ count++;
+ if (rxAvailable < 1)
+ USBD_CDC_RxPacket(&gUsbDevice);
+ }
+ }
+ return count;
+}
+
+uint32_t CDC_Receive_BytesAvailable(void)
+{
+ return rxAvailable;
+}
+
+uint32_t CDC_Send_FreeBytes(void)
+{
+ /*
+ return the bytes free in the circular buffer
+
+ functionally equivalent to:
+ (APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE - APP_Rx_ptr_in + APP_Rx_ptr_in)
+ but without the impact of the condition check.
+ */
+ uint32_t freeBytes;
+
+ ATOMIC_BLOCK(NVIC_BUILD_PRIORITY(6, 0)) {
+ freeBytes = ((cdcTxBufPtrOut - cdcTxBufPtrIn) + (-((int)(cdcTxBufPtrOut <= cdcTxBufPtrIn)) & APP_TX_DATA_SIZE)) - 1;
+ }
+
+ return freeBytes;
+}
+
+/*******************************************************************************
+ * Function Name : usbIsConfigured.
+ * Description : Determines if USB VCP is configured or not
+ * Input : None.
+ * Output : None.
+ * Return : True if configured.
+ *******************************************************************************/
+uint8_t usbIsConfigured(void)
+{
+ return (gUsbDevice.devState == USBD_DEV_CONFIGURE);
+}
+
+/*******************************************************************************
+ * Function Name : usbIsConnected.
+ * Description : Determines if USB VCP is connected ot not
+ * Input : None.
+ * Output : None.
+ * Return : True if connected.
+ *******************************************************************************/
+uint8_t usbIsConnected(void)
+{
+ return (gUsbDevice.devState != USBD_DEV_DEFAULT);
+}
+
+/*******************************************************************************
+ * Function Name : CDC_BaudRate.
+ * Description : Get the current baud rate
+ * Input : None.
+ * Output : None.
+ * Return : Baud rate in bps
+ *******************************************************************************/
+uint32_t CDC_BaudRate(void)
+{
+ return LineCoding.baudRate;
+}
+
+/*******************************************************************************
+ * Function Name : CDC_SetBaudRateCb
+ * Description : Set a callback to call when baud rate changes
+ * Input : callback function and context.
+ * Output : None.
+ * Return : None.
+ *******************************************************************************/
+void CDC_SetBaudRateCb(void (*cb)(void *context, uint32_t baud), void *context)
+{
+ baudRateCbContext = context;
+ baudRateCb = cb;
+}
+
+/*******************************************************************************
+ * Function Name : CDC_SetCtrlLineStateCb
+ * Description : Set a callback to call when control line state changes
+ * Input : callback function and context.
+ * Output : None.
+ * Return : None.
+ *******************************************************************************/
+void CDC_SetCtrlLineStateCb(void (*cb)(void *context, uint16_t ctrlLineState), void *context)
+{
+ ctrlLineStateCbContext = context;
+ ctrlLineStateCb = cb;
+}
diff --git a/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_vcp.h b/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_vcp.h
new file mode 100644
index 0000000000..9ad4f63d28
--- /dev/null
+++ b/src/main/drivers/mcu/apm32/usb/vcp/usbd_cdc_vcp.h
@@ -0,0 +1,68 @@
+/**
+ * @file usbd_cdc_vcp.h
+ *
+ * @brief usb device CDC class VCP handler header file
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/* Define to prevent recursive inclusion */
+#ifndef _USBD_CDC_VCP_H_
+#define _USBD_CDC_VCP_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Includes ***************************************************************/
+#include "usbd_cdc.h"
+
+/* Exported macro *********************************************************/
+
+/* Periodically, the state of the buffer "cdcTxBuffer" is checked.
+ The period depends on CDC_POLLING_INTERVAL */
+#define CDC_POLLING_INTERVAL 5 /* in ms. The max is 65 and the min is 1 */
+
+/* Exported typedef *******************************************************/
+/* The following structures groups all needed parameters to be configured for the
+ ComPort. These parameters can modified on the fly by the host through CDC class
+ command class requests. */
+typedef struct __attribute__ ((packed))
+{
+ uint32_t bitrate;
+ uint8_t format;
+ uint8_t paritytype;
+ uint8_t datatype;
+} LINE_CODING;
+
+/* Exported function prototypes *******************************************/
+uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength);
+uint32_t CDC_Send_FreeBytes(void);
+uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len);
+uint32_t CDC_Receive_BytesAvailable(void);
+
+uint8_t usbIsConfigured(void);
+uint8_t usbIsConnected(void);
+uint32_t CDC_BaudRate(void);
+void CDC_SetCtrlLineStateCb(void (*cb)(void *context, uint16_t ctrlLineState), void *context);
+void CDC_SetBaudRateCb(void (*cb)(void *context, uint32_t baud), void *context);
+
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#endif
diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c
index bbb2247db7..3ee30f7253 100644
--- a/src/main/drivers/motor.c
+++ b/src/main/drivers/motor.c
@@ -380,7 +380,7 @@ timeMs_t motorGetMotorEnableTimeMs(void)
#ifdef USE_DSHOT_BITBANG
bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig)
{
-#ifdef STM32F4
+#if defined(STM32F4) || defined(APM32F4)
return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
(motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
#else
diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h
index 21b2c5913a..b7a4725fc7 100644
--- a/src/main/drivers/rcc.h
+++ b/src/main/drivers/rcc.h
@@ -52,6 +52,12 @@ enum rcc_reg {
RCC_AHB3,
RCC_APB2,
RCC_APB1,
+#elif defined(APM32F4)
+ RCC_AHB1,
+ RCC_AHB2,
+ RCC_AHB3,
+ RCC_APB2,
+ RCC_APB1,
#else
RCC_AHB,
RCC_APB2,
@@ -101,6 +107,11 @@ enum rcc_reg {
#define RCC_AHB3(periph) RCC_ENCODE(RCC_AHB3, CRM_AHB3_ ## periph ## _PER_MASK)
#define RCC_APB1(periph) RCC_ENCODE(RCC_APB1, CRM_APB1_ ## periph ## _PER_MASK)
#define RCC_APB2(periph) RCC_ENCODE(RCC_APB2, CRM_APB2_ ## periph ## _PER_MASK)
+#elif defined(APM32F4)
+#define RCC_AHB(periph) RCC_ENCODE(RCC_AHB, RCM_AHBCLKEN_ ## periph ## EN)
+#define RCC_APB2(periph) RCC_ENCODE(RCC_APB2, RCM_APB2CLKEN_ ## periph ## EN)
+#define RCC_APB1(periph) RCC_ENCODE(RCC_APB1, RCM_APB1CLKEN_ ## periph ## EN)
+#define RCC_AHB1(periph) RCC_ENCODE(RCC_AHB1, RCM_AHB1CLKEN_ ## periph ## EN)
#endif
void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState);
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index 297016ff73..8a12b6186d 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -56,7 +56,7 @@
#elif defined(STM32F7)
#define UART_TX_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT // DTCM RAM
#define UART_RX_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT // DTCM RAM
-#elif defined(STM32F4) || defined(AT32F4)
+#elif defined(STM32F4) || defined(AT32F4) || defined(APM32F4)
#define UART_TX_BUFFER_ATTRIBUTE // NONE
#define UART_RX_BUFFER_ATTRIBUTE // NONE
#else
@@ -395,7 +395,7 @@ void uartConfigureDma(uartDevice_t *uartdev)
dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_UART_TX, device, serialUartConfig(device)->txDmaopt);
if (dmaChannelSpec) {
uartPort->txDMAResource = dmaChannelSpec->ref;
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
uartPort->txDMAChannel = dmaChannelSpec->channel;
#elif defined(AT32F4)
uartPort->txDMAMuxId = dmaChannelSpec->dmaMuxId;
@@ -407,7 +407,7 @@ void uartConfigureDma(uartDevice_t *uartdev)
dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_UART_RX, device, serialUartConfig(device)->txDmaopt);
if (dmaChannelSpec) {
uartPort->rxDMAResource = dmaChannelSpec->ref;
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
uartPort->rxDMAChannel = dmaChannelSpec->channel;
#elif defined(AT32F4)
uartPort->rxDMAMuxId = dmaChannelSpec->dmaMuxId;
@@ -419,7 +419,7 @@ void uartConfigureDma(uartDevice_t *uartdev)
if (hardware->rxDMAResource) {
uartPort->rxDMAResource = hardware->rxDMAResource;
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
uartPort->rxDMAChannel = hardware->rxDMAChannel;
#elif defined(AT32F4)
uartPort->rxDMAMuxId = hardware->rxDMAMuxId;
@@ -428,7 +428,7 @@ void uartConfigureDma(uartDevice_t *uartdev)
if (hardware->txDMAResource) {
uartPort->txDMAResource = hardware->txDMAResource;
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
uartPort->txDMAChannel = hardware->txDMAChannel;
#elif defined(AT32F4)
uartPort->txDMAMuxId = hardware->txDMAMuxId;
diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h
index 51b28876a2..48e72e6216 100644
--- a/src/main/drivers/serial_uart_impl.h
+++ b/src/main/drivers/serial_uart_impl.h
@@ -87,6 +87,19 @@
#define UART_TX_BUFFER_SIZE 256
#endif
#endif
+#elif defined(APM32F4)
+#define UARTDEV_COUNT_MAX 6
+#define UARTHARDWARE_MAX_PINS 4
+#ifndef UART_RX_BUFFER_SIZE
+#define UART_RX_BUFFER_SIZE 256
+#endif
+#ifndef UART_TX_BUFFER_SIZE
+#ifdef USE_MSP_DISPLAYPORT
+#define UART_TX_BUFFER_SIZE 1280
+#else
+#define UART_TX_BUFFER_SIZE 256
+#endif
+#endif
#else
#error unknown MCU family
#endif
@@ -163,7 +176,7 @@
typedef struct uartPinDef_s {
ioTag_t pin;
-#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F43x)
+#if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F43x) || defined(APM32F4)
uint8_t af;
#endif
} uartPinDef_t;
@@ -178,7 +191,7 @@ typedef struct uartHardware_s {
// For H7 and G4 , {tx|rx}DMAChannel are DMAMUX input index for peripherals (DMA_REQUEST_xxx); H7:RM0433 Table 110, G4:RM0440 Table 80.
// For F4 and F7, these are 32-bit channel identifiers (DMA_CHANNEL_x)
// For at32f435/7 DmaChannel is the dmamux ,need to call dmamuxenable using dmamuxid
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
uint32_t txDMAChannel;
uint32_t rxDMAChannel;
#elif defined(AT32F4)
@@ -230,7 +243,7 @@ typedef struct uartDevice_s {
uartPinDef_t tx;
volatile uint8_t *rxBuffer;
volatile uint8_t *txBuffer;
-#if !defined(STM32F4) // Don't support pin swap.
+#if !defined(STM32F4) || !defined(APM32F4) // Don't support pin swap.
bool pinSwap;
#endif
txPinState_t txPinState;
@@ -264,6 +277,9 @@ void uartTxMonitor(uartPort_t *s);
#elif defined(AT32F43x)
#define UART_REG_RXD(base) ((base)->dt)
#define UART_REG_TXD(base) ((base)->dt)
+#elif defined(APM32F4)
+#define UART_REG_RXD(base) ((base)->DATA)
+#define UART_REG_TXD(base) ((base)->DATA)
#endif
#define UART_BUFFER(type, n, rxtx) type volatile uint8_t uart ## n ## rxtx ## xBuffer[UART_ ## rxtx ## X_BUFFER_SIZE]
diff --git a/src/main/drivers/serial_uart_pinconfig.c b/src/main/drivers/serial_uart_pinconfig.c
index 7a8a56b895..49ef1226ad 100644
--- a/src/main/drivers/serial_uart_pinconfig.c
+++ b/src/main/drivers/serial_uart_pinconfig.c
@@ -53,7 +53,7 @@ void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig)
const uartHardware_t *hardware = &uartHardware[hindex];
const UARTDevice_e device = hardware->device;
-#if !defined(STM32F4) // Don't support pin swap.
+#if !defined(STM32F4) || !defined(APM32F4) // Don't support pin swap.
uartdev->pinSwap = false;
#endif
for (int pindex = 0 ; pindex < UARTHARDWARE_MAX_PINS ; pindex++) {
@@ -66,7 +66,7 @@ void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig)
}
-#if !defined(STM32F4)
+#if !defined(STM32F4) || !defined(APM32F4)
// Check for swapped pins
if (pSerialPinConfig->ioTagTx[device] && (pSerialPinConfig->ioTagTx[device] == hardware->rxPins[pindex].pin)) {
uartdev->tx = hardware->rxPins[pindex];
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index 03dbd13c9b..3b97b6ed31 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -35,7 +35,7 @@
#include "system.h"
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(AT32F4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(AT32F4) || defined(APM32F4)
// See "RM CoreSight Architecture Specification"
// B2.3.10 "LSR and LAR, Software Lock Status Register and Software Lock Access Register"
// "E1.2.11 LAR, Lock Access Register"
@@ -77,7 +77,7 @@ void cycleCounterInit(void)
ITM->LAR = DWT_LAR_UNLOCK_VALUE;
#elif defined(STM32F7)
DWT->LAR = DWT_LAR_UNLOCK_VALUE;
-#elif defined(STM32F4)
+#elif defined(STM32F4) || defined(APM32F4)
// Note: DWT_Type does not contain LAR member.
#define DWT_LAR
__O uint32_t *DWTLAR = (uint32_t *)(DWT_BASE + 0x0FB0);
diff --git a/src/main/drivers/transponder_ir.h b/src/main/drivers/transponder_ir.h
index 332039c1c9..94298aa7e2 100644
--- a/src/main/drivers/transponder_ir.h
+++ b/src/main/drivers/transponder_ir.h
@@ -78,7 +78,7 @@
uint8_t erlt[TRANSPONDER_DMA_BUFFER_SIZE_ERLT]; // 91-200
} transponderIrDMABuffer_t;
-#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4)
typedef union transponderIrDMABuffer_s {
uint32_t arcitimer[TRANSPONDER_DMA_BUFFER_SIZE_ARCITIMER]; // 620
@@ -94,7 +94,7 @@ typedef struct transponder_s {
uint16_t bitToggleOne;
uint32_t dma_buffer_size;
- #if defined(STM32F4)|| defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(UNIT_TEST)
+ #if defined(STM32F4)|| defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || 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 8d85cb2cad..68f34e2317 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(UNIT_TEST)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || 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 9e3e89ab9c..54d603eb49 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(UNIT_TEST)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || 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 aa5b50de91..9ecac736da 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(UNIT_TEST)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST)
static uint16_t dmaBufferOffset;
extern const struct transponderVTable ilapTansponderVTable;
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index 770d4e108f..9cbee07e90 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -478,7 +478,7 @@ void init(void)
}
#endif
-#if defined(STM32F4) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32G4) || defined(APM32F4)
// F4 has non-8MHz boards
// G4 for Betaflight allow 24 or 27MHz oscillator
systemClockSetHSEValue(systemConfig()->hseMhz * 1000000U);
@@ -493,7 +493,7 @@ void init(void)
// Note that mcoConfigure must be augmented with an additional argument to
// indicate which device instance to configure when MCO and MCO2 are both supported
-#if defined(STM32F4) || defined(STM32F7)
+#if defined(STM32F4) || defined(STM32F7) || defined(APM32F4)
// F4 and F7 support MCO on PA8 and MCO2 on PC9, but only MCO2 is supported for now
mcoConfigure(MCODEV_2, mcoConfig(MCODEV_2));
#elif defined(STM32G4)
diff --git a/src/main/startup/apm32/apm32f4xx_dal_cfg.h b/src/main/startup/apm32/apm32f4xx_dal_cfg.h
new file mode 100644
index 0000000000..bb99f4489a
--- /dev/null
+++ b/src/main/startup/apm32/apm32f4xx_dal_cfg.h
@@ -0,0 +1,352 @@
+/**
+ * @file apm32f4xx_dal_cfg.h
+ *
+ * @brief DAL configuration file
+ *
+ * @version V1.0.0
+ *
+ * @date 2023-07-31
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+
+/* Define to prevent recursive inclusion */
+#ifndef APM32F4xx_DAL_CFG_H
+#define APM32F4xx_DAL_CFG_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Configuration settings for log component */
+#define USE_LOG_COMPONENT 0U
+/* Include log header file */
+#include "apm32f4xx_dal_log.h"
+
+/* Configuration settings for assert enable */
+/* #define USE_FULL_ASSERT 1U */
+
+/* DAL module configuration */
+#define DAL_MODULE_ENABLED
+#define DAL_ADC_MODULE_ENABLED
+// #define DAL_CAN_MODULE_ENABLED
+// #define DAL_CRC_MODULE_ENABLED
+// #define DAL_CRYP_MODULE_ENABLED
+// #define DAL_DAC_MODULE_ENABLED
+// #define DAL_DCI_MODULE_ENABLED
+#define DAL_DMA_MODULE_ENABLED
+// #define DAL_ETH_MODULE_ENABLED
+#define DAL_FLASH_MODULE_ENABLED
+// #define DAL_NAND_MODULE_ENABLED
+// #define DAL_NOR_MODULE_ENABLED
+// #define DAL_PCCARD_MODULE_ENABLED
+// #define DAL_SRAM_MODULE_ENABLED
+// #define DAL_SDRAM_MODULE_ENABLED
+// #define DAL_HASH_MODULE_ENABLED
+#define DAL_GPIO_MODULE_ENABLED
+#define DAL_EINT_MODULE_ENABLED
+#define DAL_I2C_MODULE_ENABLED
+// #define DAL_SMBUS_MODULE_ENABLED
+// #define DAL_I2S_MODULE_ENABLED
+// #define DAL_IWDT_MODULE_ENABLED
+#define DAL_PMU_MODULE_ENABLED
+#define DAL_RCM_MODULE_ENABLED
+// #define DAL_RNG_MODULE_ENABLED
+#define DAL_RTC_MODULE_ENABLED
+// #define DAL_SD_MODULE_ENABLED
+#define DAL_SPI_MODULE_ENABLED
+#define DAL_TMR_MODULE_ENABLED
+#define DAL_UART_MODULE_ENABLED
+#define DAL_USART_MODULE_ENABLED
+// #define DAL_IRDA_MODULE_ENABLED
+// #define DAL_SMARTCARD_MODULE_ENABLED
+// #define DAL_WWDT_MODULE_ENABLED
+#define DAL_CORTEX_MODULE_ENABLED
+#define DAL_PCD_MODULE_ENABLED
+// #define DAL_HCD_MODULE_ENABLED
+// #define DAL_MMC_MODULE_ENABLED
+
+/* Value of the external high speed oscillator in Hz */
+#if !defined (HSE_VALUE)
+ #define HSE_VALUE 8000000U
+#endif /* HSE_VALUE */
+
+/* Timeout for external high speed oscillator in ms */
+#if !defined (HSE_STARTUP_TIMEOUT)
+ #define HSE_STARTUP_TIMEOUT 100U
+#endif /* HSE_STARTUP_TIMEOUT */
+
+/* Value of the internal high speed oscillator in Hz */
+#if !defined (HSI_VALUE)
+ #define HSI_VALUE 16000000U
+#endif /* HSI_VALUE */
+
+/* Value of the internal low speed oscillator in Hz */
+#if !defined (LSI_VALUE)
+ #define LSI_VALUE 32000U
+#endif /* LSI_VALUE */
+
+/* Value of the external low speed oscillator in Hz */
+#if !defined (LSE_VALUE)
+ #define LSE_VALUE 32768U
+#endif /* LSE_VALUE */
+
+/* Timeout for external low speed oscillator in ms */
+#if !defined (LSE_STARTUP_TIMEOUT)
+ #define LSE_STARTUP_TIMEOUT 5000U
+#endif /* LSE_STARTUP_TIMEOUT */
+
+/* Value of the external high speed oscillator in Hz for I2S peripheral */
+#if !defined (EXTERNAL_CLOCK_VALUE)
+ #define EXTERNAL_CLOCK_VALUE 12288000U
+#endif /* EXTERNAL_CLOCK_VALUE */
+
+/* System Configuration */
+#define VDD_VALUE 3300U /*!< Value of VDD in mv */
+#define TICK_INT_PRIORITY 0x0FU /*!< tick interrupt priority */
+#define USE_RTOS 0U
+#define PREFETCH_ENABLE 1U
+#define INSTRUCTION_CACHE_ENABLE 1U
+#define DATA_CACHE_ENABLE 1U
+
+/* DAL peripheral register callbacks */
+#define USE_DAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */
+#define USE_DAL_CAN_REGISTER_CALLBACKS 0U /* CAN register callback disabled */
+#define USE_DAL_CRYP_REGISTER_CALLBACKS 0U /* CRYP register callback disabled */
+#define USE_DAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */
+#define USE_DAL_DCI_REGISTER_CALLBACKS 0U /* DCI register callback disabled */
+#define USE_DAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */
+#define USE_DAL_HASH_REGISTER_CALLBACKS 0U /* HASH register callback disabled */
+#define USE_DAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */
+#define USE_DAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */
+#define USE_DAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */
+#define USE_DAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */
+#define USE_DAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */
+#define USE_DAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */
+#define USE_DAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */
+#define USE_DAL_PCCARD_REGISTER_CALLBACKS 0U /* PCCARD register callback disabled */
+#define USE_DAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */
+#define USE_DAL_RNG_REGISTER_CALLBACKS 0U /* RNG register callback disabled */
+#define USE_DAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */
+#define USE_DAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */
+#define USE_DAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */
+#define USE_DAL_SDRAM_REGISTER_CALLBACKS 0U /* SDRAM register callback disabled */
+#define USE_DAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */
+#define USE_DAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */
+#define USE_DAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */
+#define USE_DAL_TMR_REGISTER_CALLBACKS 0U /* TMR register callback disabled */
+#define USE_DAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */
+#define USE_DAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */
+#define USE_DAL_WWDT_REGISTER_CALLBACKS 0U /* WWDT register callback disabled */
+
+/* Ethernet peripheral configuration */
+/* Addr and buffer size */
+
+/* MAC ADDRESS */
+#define ETH_MAC_ADDR_0 2U
+#define ETH_MAC_ADDR_1 0U
+#define ETH_MAC_ADDR_2 0U
+#define ETH_MAC_ADDR_3 0U
+#define ETH_MAC_ADDR_4 0U
+#define ETH_MAC_ADDR_5 0U
+
+/* Ethernet driver buffers size and number */
+#define ETH_BUFFER_SIZE_RX ETH_MAX_PACKET_SIZE /* Buffer size for receive */
+#define ETH_BUFFER_SIZE_TX ETH_MAX_PACKET_SIZE /* Buffer size for transmit */
+#define ETH_BUFFER_NUMBER_RX 4U /* 4 Rx buffers of size ETH_BUFFER_SIZE_RX */
+#define ETH_BUFFER_NUMBER_TX 4U /* 4 Tx buffers of size ETH_BUFFER_SIZE_TX */
+
+/* Delay and timeout */
+
+/* PHY Reset MAX Delay */
+#define EXT_PHY_RESET_MAX_DELAY 0x000000FFU
+/* PHY Configuration MAX Delay */
+#define EXT_PHY_CONFIG_MAX_DELAY 0x00000FFFU
+
+#define EXT_PHY_READ_TIMEOUT 0x0000FFFFU
+#define EXT_PHY_WRITE_TIMEOUT 0x0000FFFFU
+
+/* SPI peripheral configuration */
+
+/* SPI CRC FEATURE */
+#define USE_SPI_CRC 1U
+
+/* Include module's header file */
+#ifdef DAL_RCM_MODULE_ENABLED
+ #include "apm32f4xx_dal_rcm.h"
+#endif /* DAL_RCM_MODULE_ENABLED */
+
+#ifdef DAL_GPIO_MODULE_ENABLED
+ #include "apm32f4xx_dal_gpio.h"
+#endif /* DAL_GPIO_MODULE_ENABLED */
+
+#ifdef DAL_EINT_MODULE_ENABLED
+ #include "apm32f4xx_dal_eint.h"
+#endif /* DAL_EINT_MODULE_ENABLED */
+
+#ifdef DAL_DMA_MODULE_ENABLED
+ #include "apm32f4xx_dal_dma.h"
+#endif /* DAL_DMA_MODULE_ENABLED */
+
+#ifdef DAL_CORTEX_MODULE_ENABLED
+ #include "apm32f4xx_dal_cortex.h"
+#endif /* DAL_CORTEX_MODULE_ENABLED */
+
+#ifdef DAL_ADC_MODULE_ENABLED
+ #include "apm32f4xx_dal_adc.h"
+#endif /* DAL_ADC_MODULE_ENABLED */
+
+#ifdef DAL_CAN_MODULE_ENABLED
+ #include "apm32f4xx_dal_can.h"
+#endif /* DAL_CAN_MODULE_ENABLED */
+
+#ifdef DAL_CRC_MODULE_ENABLED
+ #include "apm32f4xx_dal_crc.h"
+#endif /* DAL_CRC_MODULE_ENABLED */
+
+#ifdef DAL_CRYP_MODULE_ENABLED
+ #include "apm32f4xx_dal_cryp.h"
+#endif /* DAL_CRYP_MODULE_ENABLED */
+
+#ifdef DAL_DAC_MODULE_ENABLED
+ #include "apm32f4xx_dal_dac.h"
+#endif /* DAL_DAC_MODULE_ENABLED */
+
+#ifdef DAL_DCI_MODULE_ENABLED
+ #include "apm32f4xx_dal_dci.h"
+#endif /* DAL_DCI_MODULE_ENABLED */
+
+#ifdef DAL_ETH_MODULE_ENABLED
+ #include "apm32f4xx_dal_eth.h"
+#endif /* DAL_ETH_MODULE_ENABLED */
+
+#ifdef DAL_FLASH_MODULE_ENABLED
+ #include "apm32f4xx_dal_flash.h"
+#endif /* DAL_FLASH_MODULE_ENABLED */
+
+#ifdef DAL_HASH_MODULE_ENABLED
+ #include "apm32f4xx_dal_hash.h"
+#endif /* DAL_HASH_MODULE_ENABLED */
+
+#ifdef DAL_HCD_MODULE_ENABLED
+ #include "apm32f4xx_dal_hcd.h"
+#endif /* DAL_HCD_MODULE_ENABLED */
+
+#ifdef DAL_I2C_MODULE_ENABLED
+ #include "apm32f4xx_dal_i2c.h"
+#endif /* DAL_I2C_MODULE_ENABLED */
+
+#ifdef DAL_I2S_MODULE_ENABLED
+ #include "apm32f4xx_dal_i2s.h"
+#endif /* DAL_I2S_MODULE_ENABLED */
+
+#ifdef DAL_IRDA_MODULE_ENABLED
+ #include "apm32f4xx_dal_irda.h"
+#endif /* DAL_IRDA_MODULE_ENABLED */
+
+#ifdef DAL_MMC_MODULE_ENABLED
+ #include "apm32f4xx_dal_mmc.h"
+#endif /* DAL_MMC_MODULE_ENABLED */
+
+#ifdef DAL_NAND_MODULE_ENABLED
+ #include "apm32f4xx_dal_nand.h"
+#endif /* DAL_NAND_MODULE_ENABLED */
+
+#ifdef DAL_NOR_MODULE_ENABLED
+ #include "apm32f4xx_dal_nor.h"
+#endif /* DAL_NOR_MODULE_ENABLED */
+
+#ifdef DAL_PCCARD_MODULE_ENABLED
+ #include "apm32f4xx_dal_pccard.h"
+#endif /* DAL_PCCARD_MODULE_ENABLED */
+
+#ifdef DAL_PCD_MODULE_ENABLED
+ #include "apm32f4xx_dal_pcd.h"
+#endif /* DAL_PCD_MODULE_ENABLED */
+
+#ifdef DAL_PMU_MODULE_ENABLED
+ #include "apm32f4xx_dal_pmu.h"
+#endif /* DAL_PMU_MODULE_ENABLED */
+
+#ifdef DAL_RNG_MODULE_ENABLED
+ #include "apm32f4xx_dal_rng.h"
+#endif /* DAL_RNG_MODULE_ENABLED */
+
+#ifdef DAL_RTC_MODULE_ENABLED
+ #include "apm32f4xx_dal_rtc.h"
+#endif /* DAL_RTC_MODULE_ENABLED */
+
+#ifdef DAL_SRAM_MODULE_ENABLED
+ #include "apm32f4xx_dal_sram.h"
+#endif /* DAL_SRAM_MODULE_ENABLED */
+
+#ifdef DAL_SDRAM_MODULE_ENABLED
+ #include "apm32f4xx_dal_sdram.h"
+#endif /* DAL_SDRAM_MODULE_ENABLED */
+
+#ifdef DAL_SMBUS_MODULE_ENABLED
+ #include "apm32f4xx_dal_smbus.h"
+#endif /* DAL_SMBUS_MODULE_ENABLED */
+
+#ifdef DAL_SD_MODULE_ENABLED
+ #include "apm32f4xx_dal_sd.h"
+#endif /* DAL_SD_MODULE_ENABLED */
+
+#ifdef DAL_SPI_MODULE_ENABLED
+ #include "apm32f4xx_dal_spi.h"
+#endif /* DAL_SPI_MODULE_ENABLED */
+
+#ifdef DAL_SMARTCARD_MODULE_ENABLED
+ #include "apm32f4xx_dal_smartcard.h"
+#endif /* DAL_SMARTCARD_MODULE_ENABLED */
+
+#ifdef DAL_TMR_MODULE_ENABLED
+ #include "apm32f4xx_dal_tmr.h"
+#endif /* DAL_TMR_MODULE_ENABLED */
+
+#ifdef DAL_UART_MODULE_ENABLED
+ #include "apm32f4xx_dal_uart.h"
+#endif /* DAL_UART_MODULE_ENABLED */
+
+#ifdef DAL_USART_MODULE_ENABLED
+ #include "apm32f4xx_dal_usart.h"
+#endif /* DAL_USART_MODULE_ENABLED */
+
+#ifdef DAL_IWDT_MODULE_ENABLED
+ #include "apm32f4xx_dal_iwdt.h"
+#endif /* DAL_IWDT_MODULE_ENABLED */
+
+#ifdef DAL_WWDT_MODULE_ENABLED
+ #include "apm32f4xx_dal_wwdt.h"
+#endif /* DAL_WWDT_MODULE_ENABLED */
+
+/* Assert Component */
+#if (USE_FULL_ASSERT == 1U)
+ #define ASSERT_PARAM(_PARAM_) ((_PARAM_) ? (void)(_PARAM_) : AssertFailedHandler((uint8_t *)__FILE__, __LINE__))
+ /* Declaration */
+ void AssertFailedHandler(uint8_t *file, uint32_t line);
+#else
+ #define ASSERT_PARAM(_PARAM_) ((void)(_PARAM_))
+#endif /* USE_FULL_ASSERT */
+
+void DAL_ErrorHandler(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* APM32F4xx_DAL_CFG_H */
diff --git a/src/main/startup/apm32/startup_apm32f405xx.S b/src/main/startup/apm32/startup_apm32f405xx.S
new file mode 100644
index 0000000000..6ea19e4ec4
--- /dev/null
+++ b/src/main/startup/apm32/startup_apm32f405xx.S
@@ -0,0 +1,544 @@
+/**
+ * @file startup_apm32f405xx.S
+ *
+ * @brief APM32F405xx Devices vector table for GCC based toolchains.
+ * This module performs:
+ * - Set the initial SP
+ * - Set the initial PC == Reset_Handler,
+ * - Set the vector table entries with the exceptions ISR address
+ * - Branches to main in the C library (which eventually
+ * calls main()).
+ * After Reset the Cortex-M4 processor is in Thread mode,
+ * priority is Privileged, and the Stack is set to Main.
+ *
+ * @version V1.0.0
+ *
+ * @date 2023-07-31
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+ .syntax unified
+ .cpu cortex-m4
+ .fpu softvfp
+ .thumb
+
+.global g_apm32_Vectors
+.global Default_Handler
+
+/* start address for the initialization values of the .data section.
+defined in linker script */
+.word _sidata
+/* start address for the .data section. defined in linker script */
+.word _sdata
+/* end address for the .data section. defined in linker script */
+.word _edata
+/* start address for the .bss section. defined in linker script */
+.word _sbss
+/* end address for the .bss section. defined in linker script */
+.word _ebss
+/* start address for the .fastram_bss section. defined in linker script */
+.word __fastram_bss_start__
+/* end address for the .fastram_bss section. defined in linker script */
+.word __fastram_bss_end__
+/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
+
+/**
+ * @brief This is the code that gets called when the processor first
+ * starts execution following a reset event. Only the absolutely
+ * necessary set is performed, after which the application
+ * supplied main() routine is called.
+ * @param None
+ * @retval : None
+*/
+
+ .section .text.Reset_Handler
+ .weak Reset_Handler
+ .type Reset_Handler, %function
+Reset_Handler:
+ // Enable CCM
+ // RCC->AHB1ENR |= RCC_AHB1ENR_CCMDATARAMEN;
+ ldr r0, =0x40023800 // RCC_BASE
+ ldr r1, [r0, #0x30] // AHB1ENR
+ orr r1, r1, 0x00100000 // RCC_AHB1ENR_CCMDATARAMEN
+ str r1, [r0, #0x30]
+ dsb
+
+ // Defined in C code
+ bl persistentObjectInit
+ bl checkForBootLoaderRequest
+
+/* Copy the data segment initializers from flash to SRAM */
+ movs r1, #0
+ b LoopCopyDataInit
+
+CopyDataInit:
+ ldr r3, =_sidata
+ ldr r3, [r3, r1]
+ str r3, [r0, r1]
+ adds r1, r1, #4
+
+LoopCopyDataInit:
+ ldr r0, =_sdata
+ ldr r3, =_edata
+ adds r2, r0, r1
+ cmp r2, r3
+ bcc CopyDataInit
+
+ ldr r2, =_sbss
+ b LoopFillZerobss
+/* Zero fill the bss segment. */
+FillZerobss:
+ movs r3, #0
+ str r3, [r2], #4
+
+LoopFillZerobss:
+ ldr r3, = _ebss
+ cmp r2, r3
+ bcc FillZerobss
+
+ ldr r2, =__fastram_bss_start__
+ b LoopFillZerofastram_bss
+/* Zero fill the fastram_bss segment. */
+FillZerofastram_bss:
+ movs r3, #0
+ str r3, [r2], #4
+
+LoopFillZerofastram_bss:
+ ldr r3, = __fastram_bss_end__
+ cmp r2, r3
+ bcc FillZerofastram_bss
+
+/* Mark the heap and stack */
+ ldr r2, =_heap_stack_begin
+ b LoopMarkHeapStack
+
+MarkHeapStack:
+ movs r3, 0xa5a5a5a5
+ str r3, [r2], #4
+
+LoopMarkHeapStack:
+ ldr r3, = _heap_stack_end
+ cmp r2, r3
+ bcc MarkHeapStack
+
+/*FPU settings*/
+ ldr r0, =0xE000ED88 /* Enable CP10,CP11 */
+ ldr r1,[r0]
+ orr r1,r1,#(0xF << 20)
+ str r1,[r0]
+
+/* Call the clock system intitialization function.*/
+/* Done in system_stm32f4xx.c */
+ bl SystemInit
+
+/* Call the application's entry point.*/
+ bl main
+ bx lr
+
+LoopForever:
+ b LoopForever
+
+.size Reset_Handler, .-Reset_Handler
+
+// This is the code that gets called when the processor receives an unexpected interrupt.
+ .section .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+ b Infinite_Loop
+ .size Default_Handler, .-Default_Handler
+// The minimal vector table for a Cortex M4.
+ .section .isr_vector,"a",%progbits
+ .type g_apm32_Vectors, %object
+ .size g_apm32_Vectors, .-g_apm32_Vectors
+
+// Vector Table Mapped to Address 0 at Reset
+g_apm32_Vectors:
+ .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 */
+ .word WWDT_IRQHandler // Window WatchDog
+ .word PVD_IRQHandler // PVD through EINT Line detection
+ .word TAMP_STAMP_IRQHandler // Tamper and TimeStamps through the EINT line
+ .word RTC_WKUP_IRQHandler // RTC Wakeup through the EINT line
+ .word FLASH_IRQHandler // FLASH
+ .word RCM_IRQHandler // RCM
+ .word EINT0_IRQHandler // EINT Line0
+ .word EINT1_IRQHandler // EINT Line1
+ .word EINT2_IRQHandler // EINT Line2
+ .word EINT3_IRQHandler // EINT Line3
+ .word EINT4_IRQHandler // EINT Line4
+ .word DMA1_STR0_IRQHandler // DMA1 Stream 0
+ .word DMA1_STR1_IRQHandler // DMA1 Stream 1
+ .word DMA1_STR2_IRQHandler // DMA1 Stream 2
+ .word DMA1_STR3_IRQHandler // DMA1 Stream 3
+ .word DMA1_STR4_IRQHandler // DMA1 Stream 4
+ .word DMA1_STR5_IRQHandler // DMA1 Stream 5
+ .word DMA1_STR6_IRQHandler // DMA1 Stream 6
+ .word ADC_IRQHandler // ADC1, ADC2 and ADC3s
+ .word CAN1_TX_IRQHandler // CAN1 TX
+ .word CAN1_RX0_IRQHandler // CAN1 RX0
+ .word CAN1_RX1_IRQHandler // CAN1 RX1
+ .word CAN1_SCE_IRQHandler // CAN1 SCE
+ .word EINT9_5_IRQHandler // External Line[9:5]s
+ .word TMR1_BRK_TMR9_IRQHandler // TMR1 Break and TMR9
+ .word TMR1_UP_TMR10_IRQHandler // TMR1 Update and TMR10
+ .word TMR1_TRG_COM_TMR11_IRQHandler // TMR1 Trigger and Commutation and TMR11
+ .word TMR1_CC_IRQHandler // TMR1 Capture Compare
+ .word TMR2_IRQHandler // TMR2
+ .word TMR3_IRQHandler // TMR3
+ .word TMR4_IRQHandler // TMR4
+ .word I2C1_EV_IRQHandler // I2C1 Event
+ .word I2C1_ER_IRQHandler // I2C1 Error
+ .word I2C2_EV_IRQHandler // I2C2 Event
+ .word I2C2_ER_IRQHandler // I2C2 Error
+ .word SPI1_IRQHandler // SPI1
+ .word SPI2_IRQHandler // SPI2
+ .word USART1_IRQHandler // USART1
+ .word USART2_IRQHandler // USART2
+ .word USART3_IRQHandler // USART3
+ .word EINT15_10_IRQHandler // External Line[15:10]s
+ .word RTC_Alarm_IRQHandler // RTC Alarm (A and B) through EINT Line
+ .word OTG_FS_WKUP_IRQHandler // USB OTG FS Wakeup through EINT line
+ .word TMR8_BRK_TMR12_IRQHandler // TMR8 Break and TMR12
+ .word TMR8_UP_TMR13_IRQHandler // TMR8 Update and TMR13
+ .word TMR8_TRG_COM_TMR14_IRQHandler // TMR8 Trigger and Commutation and TMR14
+ .word TMR8_CC_IRQHandler // TMR8 Capture Compare
+ .word DMA1_STR7_IRQHandler // DMA1 Stream7
+ .word EMMC_IRQHandler // EMMC
+ .word SDIO_IRQHandler // SDIO
+ .word TMR5_IRQHandler // TMR5
+ .word SPI3_IRQHandler // SPI3
+ .word UART4_IRQHandler // UART4
+ .word UART5_IRQHandler // UART5
+ .word TMR6_DAC_IRQHandler // TMR6 and DAC1&2 underrun errors
+ .word TMR7_IRQHandler // TMR7
+ .word DMA2_STR0_IRQHandler // DMA2 Stream 0
+ .word DMA2_STR1_IRQHandler // DMA2 Stream 1
+ .word DMA2_STR2_IRQHandler // DMA2 Stream 2
+ .word DMA2_STR3_IRQHandler // DMA2 Stream 3
+ .word DMA2_STR4_IRQHandler // DMA2 Stream 4
+ .word 0 // Reserved
+ .word 0 // Reserved
+ .word CAN2_TX_IRQHandler // CAN2 TX
+ .word CAN2_RX0_IRQHandler // CAN2 RX0
+ .word CAN2_RX1_IRQHandler // CAN2 RX1
+ .word CAN2_SCE_IRQHandler // CAN2 SCE
+ .word OTG_FS_IRQHandler // USB OTG FS
+ .word DMA2_STR5_IRQHandler // DMA2 Stream 5
+ .word DMA2_STR6_IRQHandler // DMA2 Stream 6
+ .word DMA2_STR7_IRQHandler // DMA2 Stream 7
+ .word USART6_IRQHandler // USART6
+ .word I2C3_EV_IRQHandler // I2C3 event
+ .word I2C3_ER_IRQHandler // I2C3 error
+ .word OTG_HS1_EP1_OUT_IRQHandler // USB OTG HS End Point 1 Out
+ .word OTG_HS1_EP1_IN_IRQHandler // USB OTG HS End Point 1 In
+ .word OTG_HS1_WKUP_IRQHandler // USB OTG HS Wakeup through EINT
+ .word OTG_HS1_IRQHandler // USB OTG HS
+ .word 0 // Reserved
+ .word 0 // Reserved
+ .word HASH_RNG_IRQHandler // Hash and Rng
+ .word FPU_IRQHandler // FPU
+ .word SM3_IRQHandler // SM3
+ .word SM4_IRQHandler // SM4
+ .word BN_IRQHandler // BN
+
+// Default exception/interrupt handler
+
+ .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 WWDT_IRQHandler
+ .thumb_set WWDT_IRQHandler,Default_Handler
+
+ .weak PVD_IRQHandler
+ .thumb_set PVD_IRQHandler,Default_Handler
+
+ .weak TAMP_STAMP_IRQHandler
+ .thumb_set TAMP_STAMP_IRQHandler,Default_Handler
+
+ .weak RTC_WKUP_IRQHandler
+ .thumb_set RTC_WKUP_IRQHandler,Default_Handler
+
+ .weak FLASH_IRQHandler
+ .thumb_set FLASH_IRQHandler,Default_Handler
+
+ .weak RCM_IRQHandler
+ .thumb_set RCM_IRQHandler,Default_Handler
+
+ .weak EINT0_IRQHandler
+ .thumb_set EINT0_IRQHandler,Default_Handler
+
+ .weak EINT1_IRQHandler
+ .thumb_set EINT1_IRQHandler,Default_Handler
+
+ .weak EINT2_IRQHandler
+ .thumb_set EINT2_IRQHandler,Default_Handler
+
+ .weak EINT3_IRQHandler
+ .thumb_set EINT3_IRQHandler,Default_Handler
+
+ .weak EINT4_IRQHandler
+ .thumb_set EINT4_IRQHandler,Default_Handler
+
+ .weak DMA1_STR0_IRQHandler
+ .thumb_set DMA1_STR0_IRQHandler,Default_Handler
+
+ .weak DMA1_STR1_IRQHandler
+ .thumb_set DMA1_STR1_IRQHandler,Default_Handler
+
+ .weak DMA1_STR2_IRQHandler
+ .thumb_set DMA1_STR2_IRQHandler,Default_Handler
+
+ .weak DMA1_STR3_IRQHandler
+ .thumb_set DMA1_STR3_IRQHandler,Default_Handler
+
+ .weak DMA1_STR4_IRQHandler
+ .thumb_set DMA1_STR4_IRQHandler,Default_Handler
+
+ .weak DMA1_STR5_IRQHandler
+ .thumb_set DMA1_STR5_IRQHandler,Default_Handler
+
+ .weak DMA1_STR6_IRQHandler
+ .thumb_set DMA1_STR6_IRQHandler,Default_Handler
+
+ .weak ADC_IRQHandler
+ .thumb_set ADC_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_SCE_IRQHandler
+ .thumb_set CAN1_SCE_IRQHandler,Default_Handler
+
+ .weak EINT9_5_IRQHandler
+ .thumb_set EINT9_5_IRQHandler,Default_Handler
+
+ .weak TMR1_BRK_TMR9_IRQHandler
+ .thumb_set TMR1_BRK_TMR9_IRQHandler,Default_Handler
+
+ .weak TMR1_UP_TMR10_IRQHandler
+ .thumb_set TMR1_UP_TMR10_IRQHandler,Default_Handler
+
+ .weak TMR1_TRG_COM_TMR11_IRQHandler
+ .thumb_set TMR1_TRG_COM_TMR11_IRQHandler,Default_Handler
+
+ .weak TMR1_CC_IRQHandler
+ .thumb_set TMR1_CC_IRQHandler,Default_Handler
+
+ .weak TMR2_IRQHandler
+ .thumb_set TMR2_IRQHandler,Default_Handler
+
+ .weak TMR3_IRQHandler
+ .thumb_set TMR3_IRQHandler,Default_Handler
+
+ .weak TMR4_IRQHandler
+ .thumb_set TMR4_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 I2C2_EV_IRQHandler
+ .thumb_set I2C2_EV_IRQHandler,Default_Handler
+
+ .weak I2C2_ER_IRQHandler
+ .thumb_set I2C2_ER_IRQHandler,Default_Handler
+
+ .weak SPI1_IRQHandler
+ .thumb_set SPI1_IRQHandler,Default_Handler
+
+ .weak SPI2_IRQHandler
+ .thumb_set SPI2_IRQHandler,Default_Handler
+
+ .weak USART1_IRQHandler
+ .thumb_set USART1_IRQHandler,Default_Handler
+
+ .weak USART2_IRQHandler
+ .thumb_set USART2_IRQHandler,Default_Handler
+
+ .weak USART3_IRQHandler
+ .thumb_set USART3_IRQHandler,Default_Handler
+
+ .weak EINT15_10_IRQHandler
+ .thumb_set EINT15_10_IRQHandler,Default_Handler
+
+ .weak RTC_Alarm_IRQHandler
+ .thumb_set RTC_Alarm_IRQHandler,Default_Handler
+
+ .weak OTG_FS_WKUP_IRQHandler
+ .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
+
+ .weak TMR8_BRK_TMR12_IRQHandler
+ .thumb_set TMR8_BRK_TMR12_IRQHandler,Default_Handler
+
+ .weak TMR8_UP_TMR13_IRQHandler
+ .thumb_set TMR8_UP_TMR13_IRQHandler,Default_Handler
+
+ .weak TMR8_TRG_COM_TMR14_IRQHandler
+ .thumb_set TMR8_TRG_COM_TMR14_IRQHandler,Default_Handler
+
+ .weak TMR8_CC_IRQHandler
+ .thumb_set TMR8_CC_IRQHandler,Default_Handler
+
+ .weak DMA1_STR7_IRQHandler
+ .thumb_set DMA1_STR7_IRQHandler,Default_Handler
+
+ .weak EMMC_IRQHandler
+ .thumb_set EMMC_IRQHandler,Default_Handler
+
+ .weak SDIO_IRQHandler
+ .thumb_set SDIO_IRQHandler,Default_Handler
+
+ .weak TMR5_IRQHandler
+ .thumb_set TMR5_IRQHandler,Default_Handler
+
+ .weak SPI3_IRQHandler
+ .thumb_set SPI3_IRQHandler,Default_Handler
+
+ .weak UART4_IRQHandler
+ .thumb_set UART4_IRQHandler,Default_Handler
+
+ .weak UART5_IRQHandler
+ .thumb_set UART5_IRQHandler,Default_Handler
+
+ .weak TMR6_DAC_IRQHandler
+ .thumb_set TMR6_DAC_IRQHandler,Default_Handler
+
+ .weak TMR7_IRQHandler
+ .thumb_set TMR7_IRQHandler,Default_Handler
+
+ .weak DMA2_STR0_IRQHandler
+ .thumb_set DMA2_STR0_IRQHandler,Default_Handler
+
+ .weak DMA2_STR1_IRQHandler
+ .thumb_set DMA2_STR1_IRQHandler,Default_Handler
+
+ .weak DMA2_STR2_IRQHandler
+ .thumb_set DMA2_STR2_IRQHandler,Default_Handler
+
+ .weak DMA2_STR3_IRQHandler
+ .thumb_set DMA2_STR3_IRQHandler,Default_Handler
+
+ .weak DMA2_STR4_IRQHandler
+ .thumb_set DMA2_STR4_IRQHandler,Default_Handler
+
+ .weak CAN2_TX_IRQHandler
+ .thumb_set CAN2_TX_IRQHandler,Default_Handler
+
+ .weak CAN2_RX0_IRQHandler
+ .thumb_set CAN2_RX0_IRQHandler,Default_Handler
+
+ .weak CAN2_RX1_IRQHandler
+ .thumb_set CAN2_RX1_IRQHandler,Default_Handler
+
+ .weak CAN2_SCE_IRQHandler
+ .thumb_set CAN2_SCE_IRQHandler,Default_Handler
+
+ .weak OTG_FS_IRQHandler
+ .thumb_set OTG_FS_IRQHandler,Default_Handler
+
+ .weak DMA2_STR5_IRQHandler
+ .thumb_set DMA2_STR5_IRQHandler,Default_Handler
+
+ .weak DMA2_STR6_IRQHandler
+ .thumb_set DMA2_STR6_IRQHandler,Default_Handler
+
+ .weak DMA2_STR7_IRQHandler
+ .thumb_set DMA2_STR7_IRQHandler,Default_Handler
+
+ .weak USART6_IRQHandler
+ .thumb_set USART6_IRQHandler,Default_Handler
+
+ .weak I2C3_EV_IRQHandler
+ .thumb_set I2C3_EV_IRQHandler,Default_Handler
+
+ .weak I2C3_ER_IRQHandler
+ .thumb_set I2C3_ER_IRQHandler,Default_Handler
+
+ .weak OTG_HS1_EP1_OUT_IRQHandler
+ .thumb_set OTG_HS1_EP1_OUT_IRQHandler,Default_Handler
+
+ .weak OTG_HS1_EP1_IN_IRQHandler
+ .thumb_set OTG_HS1_EP1_IN_IRQHandler,Default_Handler
+
+ .weak OTG_HS1_WKUP_IRQHandler
+ .thumb_set OTG_HS1_WKUP_IRQHandler,Default_Handler
+
+ .weak OTG_HS1_IRQHandler
+ .thumb_set OTG_HS1_IRQHandler,Default_Handler
+
+ .weak HASH_RNG_IRQHandler
+ .thumb_set HASH_RNG_IRQHandler,Default_Handler
+
+ .weak FPU_IRQHandler
+ .thumb_set FPU_IRQHandler,Default_Handler
+
+ .weak SM3_IRQHandler
+ .thumb_set SM3_IRQHandler,Default_Handler
+
+ .weak SM4_IRQHandler
+ .thumb_set SM4_IRQHandler,Default_Handler
+
+ .weak BN_IRQHandler
+ .thumb_set BN_IRQHandler,Default_Handler
diff --git a/src/main/startup/apm32/startup_apm32f407xx.S b/src/main/startup/apm32/startup_apm32f407xx.S
new file mode 100644
index 0000000000..b5a145e42e
--- /dev/null
+++ b/src/main/startup/apm32/startup_apm32f407xx.S
@@ -0,0 +1,553 @@
+/**
+ * @file startup_apm32f407xx.S
+ *
+ * @brief APM32F407xx Devices vector table for GCC based toolchains.
+ * This module performs:
+ * - Set the initial SP
+ * - Set the initial PC == Reset_Handler,
+ * - Set the vector table entries with the exceptions ISR address
+ * - Branches to main in the C library (which eventually
+ * calls main()).
+ * After Reset the Cortex-M4 processor is in Thread mode,
+ * priority is Privileged, and the Stack is set to Main.
+ *
+ * @version V1.0.0
+ *
+ * @date 2023-07-31
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+ .syntax unified
+ .cpu cortex-m4
+ .fpu softvfp
+ .thumb
+
+.global g_apm32_Vectors
+.global Default_Handler
+
+/* start address for the initialization values of the .data section.
+defined in linker script */
+.word _sidata
+/* start address for the .data section. defined in linker script */
+.word _sdata
+/* end address for the .data section. defined in linker script */
+.word _edata
+/* start address for the .bss section. defined in linker script */
+.word _sbss
+/* end address for the .bss section. defined in linker script */
+.word _ebss
+/* start address for the .fastram_bss section. defined in linker script */
+.word __fastram_bss_start__
+/* end address for the .fastram_bss section. defined in linker script */
+.word __fastram_bss_end__
+/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
+
+/**
+ * @brief This is the code that gets called when the processor first
+ * starts execution following a reset event. Only the absolutely
+ * necessary set is performed, after which the application
+ * supplied main() routine is called.
+ * @param None
+ * @retval : None
+*/
+
+ .section .text.Reset_Handler
+ .weak Reset_Handler
+ .type Reset_Handler, %function
+Reset_Handler:
+ // Enable CCM
+ // RCC->AHB1ENR |= RCC_AHB1ENR_CCMDATARAMEN;
+ ldr r0, =0x40023800 // RCC_BASE
+ ldr r1, [r0, #0x30] // AHB1ENR
+ orr r1, r1, 0x00100000 // RCC_AHB1ENR_CCMDATARAMEN
+ str r1, [r0, #0x30]
+ dsb
+
+ // Defined in C code
+ bl persistentObjectInit
+ bl checkForBootLoaderRequest
+
+/* Copy the data segment initializers from flash to SRAM */
+ movs r1, #0
+ b LoopCopyDataInit
+
+CopyDataInit:
+ ldr r3, =_sidata
+ ldr r3, [r3, r1]
+ str r3, [r0, r1]
+ adds r1, r1, #4
+
+LoopCopyDataInit:
+ ldr r0, =_sdata
+ ldr r3, =_edata
+ adds r2, r0, r1
+ cmp r2, r3
+ bcc CopyDataInit
+
+ ldr r2, =_sbss
+ b LoopFillZerobss
+/* Zero fill the bss segment. */
+FillZerobss:
+ movs r3, #0
+ str r3, [r2], #4
+
+LoopFillZerobss:
+ ldr r3, = _ebss
+ cmp r2, r3
+ bcc FillZerobss
+
+ ldr r2, =__fastram_bss_start__
+ b LoopFillZerofastram_bss
+/* Zero fill the fastram_bss segment. */
+FillZerofastram_bss:
+ movs r3, #0
+ str r3, [r2], #4
+
+LoopFillZerofastram_bss:
+ ldr r3, = __fastram_bss_end__
+ cmp r2, r3
+ bcc FillZerofastram_bss
+
+/* Mark the heap and stack */
+ ldr r2, =_heap_stack_begin
+ b LoopMarkHeapStack
+
+MarkHeapStack:
+ movs r3, 0xa5a5a5a5
+ str r3, [r2], #4
+
+LoopMarkHeapStack:
+ ldr r3, = _heap_stack_end
+ cmp r2, r3
+ bcc MarkHeapStack
+
+/*FPU settings*/
+ ldr r0, =0xE000ED88 /* Enable CP10,CP11 */
+ ldr r1,[r0]
+ orr r1,r1,#(0xF << 20)
+ str r1,[r0]
+
+/* Call the clock system intitialization function.*/
+/* Done in system_stm32f4xx.c */
+ bl SystemInit
+
+/* Call the application's entry point.*/
+ bl main
+ bx lr
+
+LoopForever:
+ b LoopForever
+
+.size Reset_Handler, .-Reset_Handler
+
+// This is the code that gets called when the processor receives an unexpected interrupt.
+ .section .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+ b Infinite_Loop
+ .size Default_Handler, .-Default_Handler
+// The minimal vector table for a Cortex M4.
+ .section .isr_vector,"a",%progbits
+ .type g_apm32_Vectors, %object
+ .size g_apm32_Vectors, .-g_apm32_Vectors
+
+// Vector Table Mapped to Address 0 at Reset
+g_apm32_Vectors:
+ .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 */
+ .word WWDT_IRQHandler // Window WatchDog
+ .word PVD_IRQHandler // PVD through EINT Line detection
+ .word TAMP_STAMP_IRQHandler // Tamper and TimeStamps through the EINT line
+ .word RTC_WKUP_IRQHandler // RTC Wakeup through the EINT line
+ .word FLASH_IRQHandler // FLASH
+ .word RCM_IRQHandler // RCM
+ .word EINT0_IRQHandler // EINT Line0
+ .word EINT1_IRQHandler // EINT Line1
+ .word EINT2_IRQHandler // EINT Line2
+ .word EINT3_IRQHandler // EINT Line3
+ .word EINT4_IRQHandler // EINT Line4
+ .word DMA1_STR0_IRQHandler // DMA1 Stream 0
+ .word DMA1_STR1_IRQHandler // DMA1 Stream 1
+ .word DMA1_STR2_IRQHandler // DMA1 Stream 2
+ .word DMA1_STR3_IRQHandler // DMA1 Stream 3
+ .word DMA1_STR4_IRQHandler // DMA1 Stream 4
+ .word DMA1_STR5_IRQHandler // DMA1 Stream 5
+ .word DMA1_STR6_IRQHandler // DMA1 Stream 6
+ .word ADC_IRQHandler // ADC1, ADC2 and ADC3s
+ .word CAN1_TX_IRQHandler // CAN1 TX
+ .word CAN1_RX0_IRQHandler // CAN1 RX0
+ .word CAN1_RX1_IRQHandler // CAN1 RX1
+ .word CAN1_SCE_IRQHandler // CAN1 SCE
+ .word EINT9_5_IRQHandler // External Line[9:5]s
+ .word TMR1_BRK_TMR9_IRQHandler // TMR1 Break and TMR9
+ .word TMR1_UP_TMR10_IRQHandler // TMR1 Update and TMR10
+ .word TMR1_TRG_COM_TMR11_IRQHandler // TMR1 Trigger and Commutation and TMR11
+ .word TMR1_CC_IRQHandler // TMR1 Capture Compare
+ .word TMR2_IRQHandler // TMR2
+ .word TMR3_IRQHandler // TMR3
+ .word TMR4_IRQHandler // TMR4
+ .word I2C1_EV_IRQHandler // I2C1 Event
+ .word I2C1_ER_IRQHandler // I2C1 Error
+ .word I2C2_EV_IRQHandler // I2C2 Event
+ .word I2C2_ER_IRQHandler // I2C2 Error
+ .word SPI1_IRQHandler // SPI1
+ .word SPI2_IRQHandler // SPI2
+ .word USART1_IRQHandler // USART1
+ .word USART2_IRQHandler // USART2
+ .word USART3_IRQHandler // USART3
+ .word EINT15_10_IRQHandler // External Line[15:10]s
+ .word RTC_Alarm_IRQHandler // RTC Alarm (A and B) through EINT Line
+ .word OTG_FS_WKUP_IRQHandler // USB OTG FS Wakeup through EINT line
+ .word TMR8_BRK_TMR12_IRQHandler // TMR8 Break and TMR12
+ .word TMR8_UP_TMR13_IRQHandler // TMR8 Update and TMR13
+ .word TMR8_TRG_COM_TMR14_IRQHandler // TMR8 Trigger and Commutation and TMR14
+ .word TMR8_CC_IRQHandler // TMR8 Capture Compare
+ .word DMA1_STR7_IRQHandler // DMA1 Stream7
+ .word EMMC_IRQHandler // EMMC
+ .word SDIO_IRQHandler // SDIO
+ .word TMR5_IRQHandler // TMR5
+ .word SPI3_IRQHandler // SPI3
+ .word UART4_IRQHandler // UART4
+ .word UART5_IRQHandler // UART5
+ .word TMR6_DAC_IRQHandler // TMR6 and DAC1&2 underrun errors
+ .word TMR7_IRQHandler // TMR7
+ .word DMA2_STR0_IRQHandler // DMA2 Stream 0
+ .word DMA2_STR1_IRQHandler // DMA2 Stream 1
+ .word DMA2_STR2_IRQHandler // DMA2 Stream 2
+ .word DMA2_STR3_IRQHandler // DMA2 Stream 3
+ .word DMA2_STR4_IRQHandler // DMA2 Stream 4
+ .word ETH_IRQHandler // Ethernet
+ .word ETH_WKUP_IRQHandler // Ethernet Wakeup through EINT line
+ .word CAN2_TX_IRQHandler // CAN2 TX
+ .word CAN2_RX0_IRQHandler // CAN2 RX0
+ .word CAN2_RX1_IRQHandler // CAN2 RX1
+ .word CAN2_SCE_IRQHandler // CAN2 SCE
+ .word OTG_FS_IRQHandler // USB OTG FS
+ .word DMA2_STR5_IRQHandler // DMA2 Stream 5
+ .word DMA2_STR6_IRQHandler // DMA2 Stream 6
+ .word DMA2_STR7_IRQHandler // DMA2 Stream 7
+ .word USART6_IRQHandler // USART6
+ .word I2C3_EV_IRQHandler // I2C3 event
+ .word I2C3_ER_IRQHandler // I2C3 error
+ .word OTG_HS1_EP1_OUT_IRQHandler // USB OTG HS End Point 1 Out
+ .word OTG_HS1_EP1_IN_IRQHandler // USB OTG HS End Point 1 In
+ .word OTG_HS1_WKUP_IRQHandler // USB OTG HS Wakeup through EINT
+ .word OTG_HS1_IRQHandler // USB OTG HS
+ .word DCI_IRQHandler // DCI
+ .word 0 // Reserved
+ .word HASH_RNG_IRQHandler // Hash and Rng
+ .word FPU_IRQHandler // FPU
+ .word SM3_IRQHandler // SM3
+ .word SM4_IRQHandler // SM4
+ .word BN_IRQHandler // BN
+
+// Default exception/interrupt handler
+
+ .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 WWDT_IRQHandler
+ .thumb_set WWDT_IRQHandler,Default_Handler
+
+ .weak PVD_IRQHandler
+ .thumb_set PVD_IRQHandler,Default_Handler
+
+ .weak TAMP_STAMP_IRQHandler
+ .thumb_set TAMP_STAMP_IRQHandler,Default_Handler
+
+ .weak RTC_WKUP_IRQHandler
+ .thumb_set RTC_WKUP_IRQHandler,Default_Handler
+
+ .weak FLASH_IRQHandler
+ .thumb_set FLASH_IRQHandler,Default_Handler
+
+ .weak RCM_IRQHandler
+ .thumb_set RCM_IRQHandler,Default_Handler
+
+ .weak EINT0_IRQHandler
+ .thumb_set EINT0_IRQHandler,Default_Handler
+
+ .weak EINT1_IRQHandler
+ .thumb_set EINT1_IRQHandler,Default_Handler
+
+ .weak EINT2_IRQHandler
+ .thumb_set EINT2_IRQHandler,Default_Handler
+
+ .weak EINT3_IRQHandler
+ .thumb_set EINT3_IRQHandler,Default_Handler
+
+ .weak EINT4_IRQHandler
+ .thumb_set EINT4_IRQHandler,Default_Handler
+
+ .weak DMA1_STR0_IRQHandler
+ .thumb_set DMA1_STR0_IRQHandler,Default_Handler
+
+ .weak DMA1_STR1_IRQHandler
+ .thumb_set DMA1_STR1_IRQHandler,Default_Handler
+
+ .weak DMA1_STR2_IRQHandler
+ .thumb_set DMA1_STR2_IRQHandler,Default_Handler
+
+ .weak DMA1_STR3_IRQHandler
+ .thumb_set DMA1_STR3_IRQHandler,Default_Handler
+
+ .weak DMA1_STR4_IRQHandler
+ .thumb_set DMA1_STR4_IRQHandler,Default_Handler
+
+ .weak DMA1_STR5_IRQHandler
+ .thumb_set DMA1_STR5_IRQHandler,Default_Handler
+
+ .weak DMA1_STR6_IRQHandler
+ .thumb_set DMA1_STR6_IRQHandler,Default_Handler
+
+ .weak ADC_IRQHandler
+ .thumb_set ADC_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_SCE_IRQHandler
+ .thumb_set CAN1_SCE_IRQHandler,Default_Handler
+
+ .weak EINT9_5_IRQHandler
+ .thumb_set EINT9_5_IRQHandler,Default_Handler
+
+ .weak TMR1_BRK_TMR9_IRQHandler
+ .thumb_set TMR1_BRK_TMR9_IRQHandler,Default_Handler
+
+ .weak TMR1_UP_TMR10_IRQHandler
+ .thumb_set TMR1_UP_TMR10_IRQHandler,Default_Handler
+
+ .weak TMR1_TRG_COM_TMR11_IRQHandler
+ .thumb_set TMR1_TRG_COM_TMR11_IRQHandler,Default_Handler
+
+ .weak TMR1_CC_IRQHandler
+ .thumb_set TMR1_CC_IRQHandler,Default_Handler
+
+ .weak TMR2_IRQHandler
+ .thumb_set TMR2_IRQHandler,Default_Handler
+
+ .weak TMR3_IRQHandler
+ .thumb_set TMR3_IRQHandler,Default_Handler
+
+ .weak TMR4_IRQHandler
+ .thumb_set TMR4_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 I2C2_EV_IRQHandler
+ .thumb_set I2C2_EV_IRQHandler,Default_Handler
+
+ .weak I2C2_ER_IRQHandler
+ .thumb_set I2C2_ER_IRQHandler,Default_Handler
+
+ .weak SPI1_IRQHandler
+ .thumb_set SPI1_IRQHandler,Default_Handler
+
+ .weak SPI2_IRQHandler
+ .thumb_set SPI2_IRQHandler,Default_Handler
+
+ .weak USART1_IRQHandler
+ .thumb_set USART1_IRQHandler,Default_Handler
+
+ .weak USART2_IRQHandler
+ .thumb_set USART2_IRQHandler,Default_Handler
+
+ .weak USART3_IRQHandler
+ .thumb_set USART3_IRQHandler,Default_Handler
+
+ .weak EINT15_10_IRQHandler
+ .thumb_set EINT15_10_IRQHandler,Default_Handler
+
+ .weak RTC_Alarm_IRQHandler
+ .thumb_set RTC_Alarm_IRQHandler,Default_Handler
+
+ .weak OTG_FS_WKUP_IRQHandler
+ .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
+
+ .weak TMR8_BRK_TMR12_IRQHandler
+ .thumb_set TMR8_BRK_TMR12_IRQHandler,Default_Handler
+
+ .weak TMR8_UP_TMR13_IRQHandler
+ .thumb_set TMR8_UP_TMR13_IRQHandler,Default_Handler
+
+ .weak TMR8_TRG_COM_TMR14_IRQHandler
+ .thumb_set TMR8_TRG_COM_TMR14_IRQHandler,Default_Handler
+
+ .weak TMR8_CC_IRQHandler
+ .thumb_set TMR8_CC_IRQHandler,Default_Handler
+
+ .weak DMA1_STR7_IRQHandler
+ .thumb_set DMA1_STR7_IRQHandler,Default_Handler
+
+ .weak EMMC_IRQHandler
+ .thumb_set EMMC_IRQHandler,Default_Handler
+
+ .weak SDIO_IRQHandler
+ .thumb_set SDIO_IRQHandler,Default_Handler
+
+ .weak TMR5_IRQHandler
+ .thumb_set TMR5_IRQHandler,Default_Handler
+
+ .weak SPI3_IRQHandler
+ .thumb_set SPI3_IRQHandler,Default_Handler
+
+ .weak UART4_IRQHandler
+ .thumb_set UART4_IRQHandler,Default_Handler
+
+ .weak UART5_IRQHandler
+ .thumb_set UART5_IRQHandler,Default_Handler
+
+ .weak TMR6_DAC_IRQHandler
+ .thumb_set TMR6_DAC_IRQHandler,Default_Handler
+
+ .weak TMR7_IRQHandler
+ .thumb_set TMR7_IRQHandler,Default_Handler
+
+ .weak DMA2_STR0_IRQHandler
+ .thumb_set DMA2_STR0_IRQHandler,Default_Handler
+
+ .weak DMA2_STR1_IRQHandler
+ .thumb_set DMA2_STR1_IRQHandler,Default_Handler
+
+ .weak DMA2_STR2_IRQHandler
+ .thumb_set DMA2_STR2_IRQHandler,Default_Handler
+
+ .weak DMA2_STR3_IRQHandler
+ .thumb_set DMA2_STR3_IRQHandler,Default_Handler
+
+ .weak DMA2_STR4_IRQHandler
+ .thumb_set DMA2_STR4_IRQHandler,Default_Handler
+
+ .weak ETH_IRQHandler
+ .thumb_set ETH_IRQHandler,Default_Handler
+
+ .weak ETH_WKUP_IRQHandler
+ .thumb_set ETH_WKUP_IRQHandler,Default_Handler
+
+ .weak CAN2_TX_IRQHandler
+ .thumb_set CAN2_TX_IRQHandler,Default_Handler
+
+ .weak CAN2_RX0_IRQHandler
+ .thumb_set CAN2_RX0_IRQHandler,Default_Handler
+
+ .weak CAN2_RX1_IRQHandler
+ .thumb_set CAN2_RX1_IRQHandler,Default_Handler
+
+ .weak CAN2_SCE_IRQHandler
+ .thumb_set CAN2_SCE_IRQHandler,Default_Handler
+
+ .weak OTG_FS_IRQHandler
+ .thumb_set OTG_FS_IRQHandler,Default_Handler
+
+ .weak DMA2_STR5_IRQHandler
+ .thumb_set DMA2_STR5_IRQHandler,Default_Handler
+
+ .weak DMA2_STR6_IRQHandler
+ .thumb_set DMA2_STR6_IRQHandler,Default_Handler
+
+ .weak DMA2_STR7_IRQHandler
+ .thumb_set DMA2_STR7_IRQHandler,Default_Handler
+
+ .weak USART6_IRQHandler
+ .thumb_set USART6_IRQHandler,Default_Handler
+
+ .weak I2C3_EV_IRQHandler
+ .thumb_set I2C3_EV_IRQHandler,Default_Handler
+
+ .weak I2C3_ER_IRQHandler
+ .thumb_set I2C3_ER_IRQHandler,Default_Handler
+
+ .weak OTG_HS1_EP1_OUT_IRQHandler
+ .thumb_set OTG_HS1_EP1_OUT_IRQHandler,Default_Handler
+
+ .weak OTG_HS1_EP1_IN_IRQHandler
+ .thumb_set OTG_HS1_EP1_IN_IRQHandler,Default_Handler
+
+ .weak OTG_HS1_WKUP_IRQHandler
+ .thumb_set OTG_HS1_WKUP_IRQHandler,Default_Handler
+
+ .weak OTG_HS1_IRQHandler
+ .thumb_set OTG_HS1_IRQHandler,Default_Handler
+
+ .weak DCI_IRQHandler
+ .thumb_set DCI_IRQHandler,Default_Handler
+
+ .weak HASH_RNG_IRQHandler
+ .thumb_set HASH_RNG_IRQHandler,Default_Handler
+
+ .weak FPU_IRQHandler
+ .thumb_set FPU_IRQHandler,Default_Handler
+
+ .weak SM3_IRQHandler
+ .thumb_set SM3_IRQHandler,Default_Handler
+
+ .weak SM4_IRQHandler
+ .thumb_set SM4_IRQHandler,Default_Handler
+
+ .weak BN_IRQHandler
+ .thumb_set BN_IRQHandler,Default_Handler
diff --git a/src/main/startup/apm32/system_apm32f4xx.c b/src/main/startup/apm32/system_apm32f4xx.c
new file mode 100644
index 0000000000..c4596b5e04
--- /dev/null
+++ b/src/main/startup/apm32/system_apm32f4xx.c
@@ -0,0 +1,463 @@
+/**
+ *
+ * @file system_apm32f4xx.c
+ *
+ * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
+ *
+ * @version V1.0.0
+ *
+ * @date 2023-07-31
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ */
+
+/** @addtogroup CMSIS
+ * @{
+ */
+
+/** @addtogroup apm32f4xx_system
+ * @{
+ */
+
+/** @addtogroup APM32F4xx_System_Private_Includes
+ * @{
+ */
+
+#include "apm32f4xx.h"
+#include "system_apm32f4xx.h"
+#include "platform.h"
+#include "drivers/system.h"
+#include "drivers/persistent.h"
+
+#include
+
+/* Value of the external oscillator in Hz */
+#if !defined (HSE_VALUE)
+ #define HSE_VALUE ((uint32_t)8000000U)
+#endif /* HSE_VALUE */
+
+/* Value of the internal oscillator in Hz */
+#if !defined (HSI_VALUE)
+ #define HSI_VALUE ((uint32_t)16000000U)
+#endif /* HSI_VALUE */
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Private_TypesDefinitions
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Private_Defines
+ * @{
+ */
+/* Uncomment the following line if you need to relocate your vector table in internal SRAM */
+/* #define VECT_TAB_SRAM */
+
+/* Vector table base offset field. This value must be a multiple of 0x200 */
+#define VECT_TAB_OFFSET 0x00
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Private_Macros
+ * @{
+ */
+#define PLLI2S_TARGET_FREQ_MHZ (27 * 4)
+#define PLLI2S_R 2
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Private_Variables
+ * @{
+ */
+uint32_t SystemCoreClock = 16000000;
+const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4};
+
+uint32_t pll_src, pll_input, pll_m, pll_p, pll_n, pll_q;
+
+typedef struct pllConfig_s {
+ uint16_t mhz; // target SYSCLK
+ uint16_t n;
+ uint16_t p;
+ uint16_t q;
+} pllConfig_t;
+
+// PLL parameters for PLL input = 1MHz.
+// For PLL input = 2MHz, divide n by 2; see SystemInitPLLParameters below.
+
+static const pllConfig_t overclockLevels[] = {
+ { 168, 336, 2, 7 }, // 168 MHz
+ { 192, 384, 2, 8 }, // 192 MHz
+ { 216, 432, 2, 9 }, // 216 MHz
+ { 240, 480, 2, 10 } // 240 MHz
+};
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Private_FunctionPrototypes
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Private_Functions
+ * @{
+ */
+
+static void SystemInitPLLParameters(void);
+void DAL_SysClkConfig(void);
+void DAL_ErrorHandler(void);
+
+/**
+ * @brief Setup the microcontroller system
+ *
+ * @param None
+ *
+ * @retval None
+ */
+void SystemInit(void)
+{
+ initialiseMemorySections();
+
+ /* FPU settings */
+#if (__FPU_PRESENT == 1U) && (__FPU_USED == 1U)
+ SCB->CPACR |= ((3UL << 10U * 2U)|(3UL << 11U * 2U)); /* set CP10 and CP11 Full Access */
+#endif
+
+ /* Reset the RCM clock configuration to the default reset state */
+ /* Set HSIEN bit */
+ RCM->CTRL |= (uint32_t)0x00000001;
+
+ /* Reset CFG register */
+ RCM->CFG = 0x00000000;
+
+ /* Reset HSEEN, CSSEN and PLL1EN bits */
+ RCM->CTRL &= (uint32_t)0xFEF6FFFF;
+
+ /* Reset PLL1CFG register */
+ RCM->PLL1CFG = 0x24003010;
+
+ /* Reset HSEBCFG bit */
+ RCM->CTRL &= (uint32_t)0xFFFBFFFF;
+
+ /* Disable all interrupts */
+ RCM->INT = 0x00000000;
+
+ /* Configure the Vector Table location add offset address */
+ extern uint8_t isr_vector_table_flash_base;
+ extern uint8_t isr_vector_table_base;
+ extern uint8_t isr_vector_table_end;
+
+ if (&isr_vector_table_base != &isr_vector_table_flash_base) {
+ memcpy(&isr_vector_table_base, &isr_vector_table_flash_base, (size_t) (&isr_vector_table_end - &isr_vector_table_base));
+ }
+
+ SCB->VTOR = (uint32_t)&isr_vector_table_base;
+}
+
+/**
+ * @brief Update SystemCoreClock variable according to clock register values
+ * The SystemCoreClock variable contains the core clock (HCLK)
+ *
+ * @param None
+ * @retval None
+ */
+void SystemCoreClockUpdate(void)
+{
+ uint32_t sysClock = 0, pllvco = 0, pllc, pllClock, pllb;
+
+ uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE);
+
+ /* Get SYSCLK source */
+ sysClock = RCM->CFG & RCM_CFG_SCLKSWSTS;
+
+ switch (sysClock)
+ {
+ case 0x00: /* HSI used as system clock source */
+ SystemCoreClock = HSI_VALUE;
+ break;
+
+ case 0x04: /* HSE used as system clock source */
+ SystemCoreClock = hse_value;
+ break;
+
+ case 0x08: /* PLL used as system clock source */
+ pllClock = (RCM->PLL1CFG & RCM_PLL1CFG_PLL1CLKS) >> 22;
+ pllb = RCM->PLL1CFG & RCM_PLL1CFG_PLLB;
+
+ if (pllClock != 0)
+ {
+ /* HSE used as PLL clock source */
+ pllvco = (hse_value / pllb) * ((RCM->PLL1CFG & RCM_PLL1CFG_PLL1A) >> 6);
+ }
+ else
+ {
+ /* HSI used as PLL clock source */
+ pllvco = (HSI_VALUE / pllb) * ((RCM->PLL1CFG & RCM_PLL1CFG_PLL1A) >> 6);
+ }
+
+ pllc = (((RCM->PLL1CFG & RCM_PLL1CFG_PLL1C) >> 16) + 1 ) * 2;
+ SystemCoreClock = pllvco / pllc;
+ break;
+
+ default:
+ SystemCoreClock = HSI_VALUE;
+ break;
+ }
+
+ /* Compute HCLK frequency --------------------------------------------------*/
+ /* Get HCLK prescaler */
+ sysClock = AHBPrescTable[((RCM->CFG & RCM_CFG_AHBPSC) >> 4)];
+ /* HCLK frequency */
+ SystemCoreClock >>= sysClock;
+}
+
+/**
+ * @brief System clock configuration
+ *
+ * @param None
+ *
+ * @retval None
+ */
+void DAL_SysClkConfig(void)
+{
+ RCM_ClkInitTypeDef RCM_ClkInitStruct = {0U};
+ RCM_OscInitTypeDef RCM_OscInitStruct = {0U};
+ RCM_PeriphCLKInitTypeDef PeriphClk_InitStruct = {0U};
+
+ uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE);
+ uint32_t hse_mhz = hse_value / 1000000;
+
+ if (hse_value == 0) {
+ pll_src = RCM_PLLSOURCE_HSI;
+
+ // HSI is fixed at 16MHz
+ pll_m = 8;
+ pll_input = 2;
+ }
+ else {
+ // HSE frequency is given
+ pll_src = RCM_PLLSOURCE_HSE;
+
+ pll_m = hse_mhz / 2;
+ if (pll_m * 2 != hse_mhz) {
+ pll_m = hse_mhz;
+ }
+ pll_input = hse_mhz / pll_m;
+ }
+
+ SystemInitPLLParameters();
+
+ /* Enable PMU clock */
+ __DAL_RCM_PMU_CLK_ENABLE();
+
+ /* Configure the voltage scaling value */
+ __DAL_PMU_VOLTAGESCALING_CONFIG(PMU_REGULATOR_VOLTAGE_SCALE1);
+
+ /* Enable HSE Oscillator and activate PLL with HSE as source */
+ RCM_OscInitStruct.OscillatorType = RCM_OSCILLATORTYPE_HSI | RCM_OSCILLATORTYPE_HSE | RCM_OSCILLATORTYPE_LSI;
+ RCM_OscInitStruct.HSEState = RCM_HSE_ON;
+ RCM_OscInitStruct.HSIState = RCM_HSI_ON;
+ RCM_OscInitStruct.LSIState = RCM_LSI_ON;
+ RCM_OscInitStruct.PLL.PLLState = RCM_PLL_ON;
+ RCM_OscInitStruct.PLL.PLLSource = pll_src;
+ RCM_OscInitStruct.PLL.PLLB = pll_m;
+ RCM_OscInitStruct.PLL.PLL1A = pll_n;
+ RCM_OscInitStruct.PLL.PLL1C = pll_p;
+ RCM_OscInitStruct.PLL.PLLD = pll_q;
+ RCM_OscInitStruct.HSICalibrationValue = 0x10;
+ if(DAL_RCM_OscConfig(&RCM_OscInitStruct) != DAL_OK)
+ {
+ DAL_ErrorHandler();
+ }
+
+ /* Configure clock */
+ RCM_ClkInitStruct.ClockType = (RCM_CLOCKTYPE_SYSCLK | RCM_CLOCKTYPE_HCLK | RCM_CLOCKTYPE_PCLK1 | RCM_CLOCKTYPE_PCLK2);
+ RCM_ClkInitStruct.SYSCLKSource = RCM_SYSCLKSOURCE_PLLCLK;
+ RCM_ClkInitStruct.AHBCLKDivider = RCM_SYSCLK_DIV1;
+ RCM_ClkInitStruct.APB1CLKDivider = RCM_HCLK_DIV4;
+ RCM_ClkInitStruct.APB2CLKDivider = RCM_HCLK_DIV2;
+ if(DAL_RCM_ClockConfig(&RCM_ClkInitStruct, FLASH_LATENCY_5) != DAL_OK)
+ {
+ DAL_ErrorHandler();
+ }
+
+ /* I2S clock */
+ // Configure PLLI2S for 27MHz operation
+ // Use pll_input (1 or 2) to derive multiplier N for
+ // 108MHz (27 * 4) PLLI2SCLK with R divider fixed at 2.
+ // 108MHz will further be prescaled by 4 by mcoInit.
+
+ uint32_t plli2s_n = (PLLI2S_TARGET_FREQ_MHZ * PLLI2S_R) / pll_input;
+
+ PeriphClk_InitStruct.PeriphClockSelection = RCM_PERIPHCLK_I2S;
+ PeriphClk_InitStruct.PLLI2S.PLL2A = plli2s_n;
+ PeriphClk_InitStruct.PLLI2S.PLL2C = PLLI2S_R;
+ if (DAL_RCMEx_PeriphCLKConfig(&PeriphClk_InitStruct) != DAL_OK)
+ {
+ DAL_ErrorHandler();
+ }
+}
+
+/**
+ * @brief Error handler
+ *
+ * @param None
+ *
+ * @retval None
+ */
+void DAL_ErrorHandler(void)
+{
+ /* When the function is needed, this function
+ could be implemented in the user file
+ */
+ while(1)
+ {
+ }
+}
+
+void AssertFailedHandler(uint8_t *file, uint32_t line)
+{
+ /* When the function is needed, this function
+ could be implemented in the user file
+ */
+ UNUSED(file);
+ UNUSED(line);
+ while(1)
+ {
+ }
+}
+
+/**
+ * @brief Get the SYSCLK source
+ *
+ * @param None
+ *
+ * @retval The SYSCLK source
+ * - 0x00: HSI used as system clock source
+ * - 0x01: HSE used as system clock source
+ * - 0x02: PLL used as system clock source
+ */
+int SystemSYSCLKSource(void)
+{
+ return (int)((RCM->CFG & RCM_CFG_SCLKSWSTS) >> 2);
+}
+
+/**
+ * @brief Get the PLL source
+ *
+ * @param None
+ *
+ * @retval The PLL source
+ * - 0x00: HSI used as PLL clock source
+ * - 0x01: HSE used as PLL clock source
+ */
+int SystemPLLSource(void)
+{
+ return (int)((RCM->PLL1CFG & RCM_PLL1CFG_PLL1CLKS) >> 22);
+}
+
+/**
+ * @brief Reboot the system if necessary after changing the overclock level
+ *
+ * @param overclockLevel
+ *
+ * @retval None
+ */
+void OverclockRebootIfNecessary(uint32_t overclockLevel)
+{
+ if (overclockLevel >= ARRAYLEN(overclockLevels)) {
+ return;
+ }
+
+ const pllConfig_t * const pll = overclockLevels + overclockLevel;
+
+ // Reboot to adjust overclock frequency
+ if (SystemCoreClock != pll->mhz * 1000000U) {
+ persistentObjectWrite(PERSISTENT_OBJECT_OVERCLOCK_LEVEL, overclockLevel);
+ __disable_irq();
+ NVIC_SystemReset();
+ }
+}
+
+/**
+ * @brief Set the HSE value
+ *
+ * @param frequency
+ *
+ * @retval None
+ */
+void systemClockSetHSEValue(uint32_t frequency)
+{
+ uint32_t hse_value = persistentObjectRead(PERSISTENT_OBJECT_HSE_VALUE);
+
+ if (hse_value != frequency) {
+ persistentObjectWrite(PERSISTENT_OBJECT_HSE_VALUE, frequency);
+ __disable_irq();
+ NVIC_SystemReset();
+ }
+}
+
+/**
+ * @brief Initialize the PLL parameters
+ *
+ * @param None
+ *
+ * @retval None
+ *
+ */
+static void SystemInitPLLParameters(void)
+{
+ /* PLL setting for overclocking */
+
+ uint32_t currentOverclockLevel = persistentObjectRead(PERSISTENT_OBJECT_OVERCLOCK_LEVEL);
+
+ if (currentOverclockLevel >= ARRAYLEN(overclockLevels)) {
+ return;
+ }
+
+ const pllConfig_t * const pll = overclockLevels + currentOverclockLevel;
+
+ pll_n = pll->n / pll_input;
+ pll_p = pll->p;
+ pll_q = pll->q;
+}
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
diff --git a/src/main/startup/apm32/system_apm32f4xx.h b/src/main/startup/apm32/system_apm32f4xx.h
new file mode 100644
index 0000000000..b1afe11a11
--- /dev/null
+++ b/src/main/startup/apm32/system_apm32f4xx.h
@@ -0,0 +1,112 @@
+/**
+ *
+ * @file system_apm32f4xx.h
+ *
+ * @brief CMSIS Cortex-M4 Device System Source File for APM32F4xx devices.
+ *
+ * @version V1.0.0
+ *
+ * @date 2023-07-31
+ *
+ * @attention
+ *
+ * Copyright (C) 2023 Geehy Semiconductor
+ *
+ * You may not use this file except in compliance with the
+ * GEEHY COPYRIGHT NOTICE (GEEHY SOFTWARE PACKAGE LICENSE).
+ *
+ * The program is only for reference, which is distributed in the hope
+ * that it will be useful and instructional for customers to develop
+ * their software. Unless required by applicable law or agreed to in
+ * writing, the program is distributed on an "AS IS" BASIS, WITHOUT
+ * ANY WARRANTY OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the GEEHY SOFTWARE PACKAGE LICENSE for the governing permissions
+ * and limitations under the License.
+ *
+ */
+
+/** @addtogroup CMSIS
+ * @{
+ */
+
+/** @addtogroup apm32f4xx_system
+ * @{
+ */
+
+/**
+ * @brief Define to prevent recursive inclusion
+ */
+#ifndef __SYSTEM_APM32F4XX_H
+#define __SYSTEM_APM32F4XX_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/** @addtogroup APM32F4xx_System_Includes
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+
+/** @addtogroup APM32F4xx_System_Exported_types
+ * @{
+ */
+extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
+
+extern const uint8_t AHBPrescTable[16]; /*!< AHB prescalers table values */
+extern const uint8_t APBPrescTable[8]; /*!< APB prescalers table values */
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Exported_Constants
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Exported_Macros
+ * @{
+ */
+
+/**
+ * @}
+ */
+
+/** @addtogroup APM32F4xx_System_Exported_Functions
+ * @{
+ */
+
+extern void SystemInit(void);
+extern void SystemCoreClockUpdate(void);
+extern void OverclockRebootIfNecessary(uint32_t overclockLevel);
+extern void systemClockSetHSEValue(uint32_t frequency);
+extern int SystemSYSCLKSource(void);
+extern int SystemPLLSource(void);
+extern void DAL_ErrorHandler(void);
+extern void DAL_SysClkConfig(void);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*__SYSTEM_APM32F4XX_H */
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
diff --git a/src/main/target/APM32F405/target.h b/src/main/target/APM32F405/target.h
new file mode 100644
index 0000000000..f823194180
--- /dev/null
+++ b/src/main/target/APM32F405/target.h
@@ -0,0 +1,89 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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
+
+#ifndef TARGET_BOARD_IDENTIFIER
+#define TARGET_BOARD_IDENTIFIER "A405"
+#endif
+
+#ifndef USBD_PRODUCT_STRING
+#define USBD_PRODUCT_STRING "Betaflight APM32F405"
+#endif
+
+#ifndef APM32F405
+#define APM32F405
+#endif
+
+#define USE_I2C_DEVICE_1
+#define USE_I2C_DEVICE_2
+#define USE_I2C_DEVICE_3
+
+#define USE_VCP
+
+#define USE_SOFTSERIAL
+
+#define UNIFIED_SERIAL_PORT_COUNT 3
+
+#define USE_UART1
+#define USE_UART2
+#define USE_UART3
+#define USE_UART4
+#define USE_UART5
+#define USE_UART6
+
+#define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 6)
+
+#define USE_INVERTER
+
+#define USE_SPI_DEVICE_1
+#define USE_SPI_DEVICE_2
+#define USE_SPI_DEVICE_3
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTE 0xffff
+#define TARGET_IO_PORTF 0xffff
+
+#define USE_I2C
+#define I2C_FULL_RECONFIGURABILITY
+
+#define USE_DSHOT_BITBAND
+
+#define USE_BEEPER
+
+#define USE_SPI
+#define SPI_FULL_RECONFIGURABILITY
+#define USE_SPI_DMA_ENABLE_EARLY
+
+#define USE_USB_DETECT
+
+#define USE_ESCSERIAL
+
+#define USE_ADC
+
+#define USE_EXTI
+
+#define USE_PID_DENOM_CHECK
+#define USE_PID_DENOM_OVERCLOCK_LEVEL 2
+
+#define FLASH_PAGE_SIZE ((uint32_t)0x4000) // 16K sectors
diff --git a/src/main/target/APM32F405/target.mk b/src/main/target/APM32F405/target.mk
new file mode 100644
index 0000000000..80ac8b3339
--- /dev/null
+++ b/src/main/target/APM32F405/target.mk
@@ -0,0 +1,3 @@
+TARGET_MCU := APM32F405xx
+TARGET_MCU_FAMILY := APM32F4
+
diff --git a/src/main/target/APM32F407/target.h b/src/main/target/APM32F407/target.h
new file mode 100644
index 0000000000..93e5a64920
--- /dev/null
+++ b/src/main/target/APM32F407/target.h
@@ -0,0 +1,89 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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
+
+#ifndef TARGET_BOARD_IDENTIFIER
+#define TARGET_BOARD_IDENTIFIER "A407"
+#endif
+
+#ifndef USBD_PRODUCT_STRING
+#define USBD_PRODUCT_STRING "Betaflight APM32F407"
+#endif
+
+#ifndef APM32F407
+#define APM32F407
+#endif
+
+#define USE_I2C_DEVICE_1
+#define USE_I2C_DEVICE_2
+#define USE_I2C_DEVICE_3
+
+#define USE_VCP
+
+#define USE_SOFTSERIAL
+
+#define UNIFIED_SERIAL_PORT_COUNT 3
+
+#define USE_UART1
+#define USE_UART2
+#define USE_UART3
+#define USE_UART4
+#define USE_UART5
+#define USE_UART6
+
+#define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 6)
+
+#define USE_INVERTER
+
+#define USE_SPI_DEVICE_1
+#define USE_SPI_DEVICE_2
+#define USE_SPI_DEVICE_3
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+#define TARGET_IO_PORTE 0xffff
+#define TARGET_IO_PORTF 0xffff
+
+#define USE_I2C
+#define I2C_FULL_RECONFIGURABILITY
+
+#define USE_DSHOT_BITBAND
+
+#define USE_BEEPER
+
+#define USE_SPI
+#define SPI_FULL_RECONFIGURABILITY
+#define USE_SPI_DMA_ENABLE_EARLY
+
+#define USE_USB_DETECT
+
+#define USE_ESCSERIAL
+
+#define USE_ADC
+
+#define USE_EXTI
+
+#define USE_PID_DENOM_CHECK
+#define USE_PID_DENOM_OVERCLOCK_LEVEL 2
+
+#define FLASH_PAGE_SIZE ((uint32_t)0x4000) // 16K sectors
diff --git a/src/main/target/APM32F407/target.mk b/src/main/target/APM32F407/target.mk
new file mode 100644
index 0000000000..80ac8b3339
--- /dev/null
+++ b/src/main/target/APM32F407/target.mk
@@ -0,0 +1,3 @@
+TARGET_MCU := APM32F405xx
+TARGET_MCU_FAMILY := APM32F4
+