diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h index 22a59c65f3..8658225393 100644 --- a/src/main/build/build_config.h +++ b/src/main/build/build_config.h @@ -65,6 +65,8 @@ typedef enum { MCU_TYPE_AT32F435M, MCU_TYPE_RP2350A, MCU_TYPE_RP2350B, + MCU_TYPE_GD32F425, + MCU_TYPE_GD32F460, MCU_TYPE_COUNT, MCU_TYPE_UNKNOWN = 255, } mcuTypeId_e; diff --git a/src/main/common/utils.h b/src/main/common/utils.h index c23155cfa1..2745be6e79 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -70,6 +70,9 @@ #define STATIC_ASSERT(condition, name) static_assert((condition), #name) #endif +#if defined(USE_GDBSP_DRIVER) +#undef BIT +#endif #define BIT(x) (1 << (x)) /* diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 958c9815d4..ce092d2e21 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -55,12 +55,25 @@ uint8_t adcChannelByTag(ioTag_t ioTag) return 0; } +#if defined(USE_GDBSP_DRIVER) +ADCDevice adcDeviceByInstance(const uint32_t instance) +{ + if (instance == ADC0) { + return ADCDEV_0; + } + +#if defined(ADC1) + if (instance == ADC1) { + return ADCDEV_1; + } +#endif +#else ADCDevice adcDeviceByInstance(const ADC_TypeDef *instance) { if (instance == ADC1) { return ADCDEV_1; } - +#endif #if defined(ADC2) if (instance == ADC2) { return ADCDEV_2; diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 9e272d9e19..d38b65a35a 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -25,9 +25,15 @@ #include "drivers/io_types.h" #include "drivers/time.h" +#if defined(USE_ADC_DEVICE_0) +#ifndef ADC_INSTANCE +#define ADC_INSTANCE ADC0 +#endif +#else #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 #endif +#endif #if defined(STM32F4) || defined(STM32F7) || defined(APM32F4) #ifndef ADC1_DMA_STREAM @@ -45,7 +51,14 @@ typedef enum ADCDevice { ADCINVALID = -1, +#if defined(USE_ADC_DEVICE_0) + ADCDEV_0 = 0, +#if defined(ADC1) + ADCDEV_1, +#endif +#else ADCDEV_1 = 0, +#endif #if defined(ADC2) ADCDEV_2, #endif @@ -114,5 +127,9 @@ int16_t adcInternalComputeTemperature(uint16_t tempAdcValue, uint16_t vrefValue) #endif #if !defined(SIMULATOR_BUILD) +#if defined(USE_GDBSP_DRIVER) +ADCDevice adcDeviceByInstance(const uint32_t instance); +#else ADCDevice adcDeviceByInstance(const ADC_TypeDef *instance); #endif +#endif diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index add1083bf2..3bc2c4de89 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -51,6 +51,12 @@ #endif #elif defined(APM32F4) #define ADC_TAG_MAP_COUNT 16 +#elif defined(GD32F4) +#ifdef USE_ADC_INTERNAL +#define ADC_TAG_MAP_COUNT 19 +#else +#define ADC_TAG_MAP_COUNT 16 +#endif #else #define ADC_TAG_MAP_COUNT 10 #endif @@ -66,6 +72,18 @@ typedef struct adcTagMap_s { // Encoding for adcTagMap_t.devices +#if defined(USE_ADC_DEVICE_0) + +#define ADC_DEVICES_0 (1 << ADCDEV_0) +#define ADC_DEVICES_1 (1 << ADCDEV_1) +#define ADC_DEVICES_2 (1 << ADCDEV_2) +#define ADC_DEVICES_3 (1 << ADCDEV_3) +#define ADC_DEVICES_4 (1 << ADCDEV_4) +#define ADC_DEVICES_01 ((1 << ADCDEV_0)|(1 << ADCDEV_1)) +#define ADC_DEVICES_23 ((1 << ADCDEV_2)|(1 << ADCDEV_3)) +#define ADC_DEVICES_012 ((1 << ADCDEV_0)|(1 << ADCDEV_1)|(1 << ADCDEV_2)) +#define ADC_DEVICES_234 ((1 << ADCDEV_2)|(1 << ADCDEV_3)|(1 << ADCDEV_4)) +#else #define ADC_DEVICES_1 (1 << ADCDEV_1) #define ADC_DEVICES_2 (1 << ADCDEV_2) #define ADC_DEVICES_3 (1 << ADCDEV_3) @@ -75,6 +93,7 @@ typedef struct adcTagMap_s { #define ADC_DEVICES_34 ((1 << ADCDEV_3)|(1 << ADCDEV_4)) #define ADC_DEVICES_123 ((1 << ADCDEV_1)|(1 << ADCDEV_2)|(1 << ADCDEV_3)) #define ADC_DEVICES_345 ((1 << ADCDEV_3)|(1 << ADCDEV_4)|(1 << ADCDEV_5)) +#endif typedef struct adcDevice_s { ADC_TypeDef* ADCx; @@ -83,7 +102,7 @@ typedef struct adcDevice_s { #endif #if !defined(USE_DMA_SPEC) dmaResource_t* dmaResource; -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) uint32_t channel; #endif #endif // !defined(USE_DMA_SPEC) @@ -111,8 +130,12 @@ extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; uint8_t adcChannelByTag(ioTag_t ioTag); #if !defined(SIMULATOR_BUILD) +#if defined(USE_GDBSP_DRIVER) +ADCDevice adcDeviceByInstance(const uint32_t instance); +#else ADCDevice adcDeviceByInstance(const ADC_TypeDef *instance); #endif +#endif bool adcVerifyPin(ioTag_t tag, ADCDevice device); // Marshall values in DMA instance/channel based order to adcChannel based order. @@ -164,3 +187,12 @@ void adcGetChannelValues(void); #define TEMPSENSOR_CAL1_V (1.27f) #define TEMPSENSOR_SLOPE (-4.13f) // mV/C #endif + +#ifdef GD32F4 +#define VREFINT_EXPECTED (1489U) // 1.2/3.3*4095 +#define VREFINT_CAL_VREF (3300U) +#define TEMPSENSOR_CAL_VREFANALOG (3300U) +#define TEMPSENSOR_CAL1_TEMP ((int32_t) 25) +#define TEMPSENSOR_CAL1_V (1.40f) +#define TEMPSENSOR_SLOPE (-4.4f) // mV/C +#endif diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 615470aeb4..e6ec2c25da 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -45,8 +45,14 @@ static uint8_t spiRegisteredDeviceCount = 0; spiDevice_t spiDevice[SPIDEV_COUNT]; busDevice_t spiBusDevice[SPIDEV_COUNT]; +#if defined(USE_GDBSP_DRIVER) +SPIDevice spiDeviceByInstance(const SPI_TypeDef *instance_t) +{ + uint32_t instance = (uint32_t) instance_t; +#else SPIDevice spiDeviceByInstance(const SPI_TypeDef *instance) { +#endif #ifdef USE_SPI_DEVICE_0 if (instance == SPI0) { return SPIDEV_0; diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index f592aa19ee..0f9d51e018 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -70,7 +70,11 @@ bool spiInit(SPIDevice device); // Called after all devices are initialised to enable SPI DMA where streams are available. void spiInitBusDMA(void); +#if defined(USE_GDBSP_DRIVER) +SPIDevice spiDeviceByInstance(const SPI_TypeDef *instance_t); +#else SPIDevice spiDeviceByInstance(const SPI_TypeDef *instance); +#endif SPI_TypeDef *spiInstanceByDevice(SPIDevice device); // BusDevice API diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index e3b1ef17be..1d5916efda 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -36,9 +36,20 @@ #define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? #define NVIC_PRIO_SERIALUART_TXDMA NVIC_BUILD_PRIORITY(1, 1) // Highest of all SERIALUARTx_TXDMA + +#if defined(USE_UART0) +#define NVIC_PRIO_SERIALUART0_TXDMA NVIC_BUILD_PRIORITY(1, 1) +#define NVIC_PRIO_SERIALUART0_RXDMA NVIC_BUILD_PRIORITY(1, 1) +#define NVIC_PRIO_SERIALUART0 NVIC_BUILD_PRIORITY(1, 1) +#define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 0) +#define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1) +#define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 2) +#else #define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 1) +#endif + #define NVIC_PRIO_SERIALUART2_TXDMA NVIC_BUILD_PRIORITY(1, 0) #define NVIC_PRIO_SERIALUART2_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART2 NVIC_BUILD_PRIORITY(1, 2) @@ -91,6 +102,13 @@ #define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING))))>>4) #define NVIC_PRIORITY_SUB(prio) (((prio)&((0x0f>>(7-(NVIC_PRIORITY_GROUPING)))<<4))>>4) +#elif defined(USE_GDBSP_DRIVER) + +#define NVIC_PRIORITY_GROUPING NVIC_PRIGROUP_PRE2_SUB2 +#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING>>8))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8)))))<<4)&0xf0) +#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING>>8))))>>4) +#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8))))>>4) + #else #define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_2 diff --git a/src/main/drivers/rx/rx_pwm.c b/src/main/drivers/rx/rx_pwm.c index 5042c73445..f0f92e6125 100644 --- a/src/main/drivers/rx/rx_pwm.c +++ b/src/main/drivers/rx/rx_pwm.c @@ -301,6 +301,8 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture pwmInputPort->state = 1; #if defined(USE_HAL_DRIVER) pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPOLARITY_FALLING); +#elif defined(USE_GDBSP_DRIVER) + pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIMER_IC_POLARITY_FALLING); #else pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); #endif @@ -315,6 +317,8 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture pwmInputPort->state = 0; #if defined(USE_HAL_DRIVER) pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPOLARITY_RISING); +#elif defined(USE_GDBSP_DRIVER) + pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIMER_IC_POLARITY_RISING); #else pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); #endif @@ -322,7 +326,7 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture } } -#ifdef USE_HAL_DRIVER +#if defined(USE_HAL_DRIVER) void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { @@ -344,6 +348,24 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) HAL_TIM_IC_ConfigChannel(Handle, &TIM_ICInitStructure, channel); HAL_TIM_IC_Start_IT(Handle,channel); } +#elif defined(USE_GDBSP_DRIVER) +void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) +{ + timer_ic_parameter_struct timer_icinitpara; + + timer_channel_input_struct_para_init(&timer_icinitpara); + timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI; + timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1; + timer_icinitpara.icpolarity = polarity; + + if (inputFilteringMode == INPUT_FILTERING_ENABLED) { + timer_icinitpara.icfilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX; + } else { + timer_icinitpara.icfilter = 0x00; + } + + timer_input_capture_config((uint32_t)tim, channel, &timer_icinitpara); +} #else void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) { @@ -396,6 +418,8 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) #if defined(USE_HAL_DRIVER) pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING); +#elif defined(USE_GDBSP_DRIVER) + pwmICConfig(timer->tim, timer->channel, TIMER_IC_POLARITY_RISING); #else pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising); #endif @@ -406,6 +430,20 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) #define FIRST_PWM_PORT 0 #ifdef USE_PWM_OUTPUT +#if defined(USE_GDBSP_DRIVER) +static void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer) +{ + pwmOutputPort_t *motors = pwmGetMotors(); + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) { + if (!motors[motorIndex].enabled || motors[motorIndex].channel.tim != pwmTimer) { + continue; + } + + ppmCountDivisor = timerClock(pwmTimer) / (TIMER_PSC((uint32_t)pwmTimer) + 1); + return; + } +} +#else static void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer) { pwmOutputPort_t *motors = pwmGetMotors(); @@ -419,6 +457,7 @@ static void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer) } } #endif +#endif void ppmRxInit(const ppmConfig_t *ppmConfig) { @@ -450,6 +489,8 @@ void ppmRxInit(const ppmConfig_t *ppmConfig) #if defined(USE_HAL_DRIVER) pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING); +#elif defined(USE_GDBSP_DRIVER) + pwmICConfig(timer->tim, timer->channel, TIMER_IC_POLARITY_RISING); #else pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising); #endif diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 0f69dee869..50677d3641 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -111,9 +111,13 @@ static void serialEnableCC(softSerial_t *softSerial) { #ifdef USE_HAL_DRIVER TIM_CCxChannelCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_ENABLE); +#else +#if defined(USE_GDBSP_DRIVER) + gd32_timer_input_capture_config(softSerial->timerHardware->tim, softSerial->timerHardware->channel, ENABLE); #else TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Enable); #endif +#endif } // switch to receive mode @@ -139,8 +143,12 @@ static void serialInputPortDeActivate(softSerial_t *softSerial) #ifdef USE_HAL_DRIVER TIM_CCxChannelCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_DISABLE); +#else +#if defined(USE_GDBSP_DRIVER) + gd32_timer_input_capture_config(softSerial->timerHardware->tim, softSerial->timerHardware->channel, DISABLE); #else TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Disable); +#endif #endif IOConfigGPIO(softSerial->rxIO, IOCFG_IN_FLOATING); // leave AF mode; serialOutputPortActivate will follow immediately softSerial->rxActive = false; diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 172719885e..45d4fc27bd 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -452,6 +452,56 @@ const struct serialPortVTable uartVTable[] = { } \ /**/ +#ifdef USE_GDBSP_DRIVER + +UART_IRQHandler(USART, 0, UARTDEV_0) // USART0 Rx/Tx IRQ Handler + +#ifdef USE_UART1 +UART_IRQHandler(USART, 1, UARTDEV_1) // USART1 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART2 +UART_IRQHandler(USART, 2, UARTDEV_2) // USART2 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART3 +UART_IRQHandler(UART, 3, UARTDEV_3) // UART3 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART4 +UART_IRQHandler(UART, 4, UARTDEV_4) // UART4 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART5 +UART_IRQHandler(USART, 5, UARTDEV_5) // USART5 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART6 +UART_IRQHandler(UART, 6, UARTDEV_6) // UART6 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART7 +UART_IRQHandler(UART, 7, UARTDEV_7) // UART7 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART8 +UART_IRQHandler(UART, 8, UARTDEV_8) // UART8 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART9 +UART_IRQHandler(UART, 9, UARTDEV_9) // UART9 Rx/Tx IRQ Handler +#endif + +#ifdef USE_UART10 +UART_IRQHandler(USART, 10, UARTDEV_10) // UART10 Rx/Tx IRQ Handler +#endif + +#ifdef USE_LPUART1 +UART_IRQHandler(LPUART, 1, UARTDEV_LP1) // LPUART1 Rx/Tx IRQ Handler +#endif + +#else // USE_GDBSP_DRIVER + #ifdef USE_UART1 UART_IRQHandler(USART, 1, UARTDEV_1) // USART1 Rx/Tx IRQ Handler #endif @@ -496,4 +546,6 @@ UART_IRQHandler(USART, 10, UARTDEV_10) // UART10 Rx/Tx IRQ Handler UART_IRQHandler(LPUART, 1, UARTDEV_LP1) // LPUART1 Rx/Tx IRQ Handler #endif +#endif // USE_GDBSP_DRIVER + #endif // USE_UART diff --git a/src/main/drivers/transponder_ir.h b/src/main/drivers/transponder_ir.h index a4f8a44f35..f06cf4eef3 100644 --- a/src/main/drivers/transponder_ir.h +++ b/src/main/drivers/transponder_ir.h @@ -75,7 +75,7 @@ uint8_t erlt[TRANSPONDER_DMA_BUFFER_SIZE_ERLT]; // 91-200 } transponderIrDMABuffer_t; -#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) +#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) typedef union transponderIrDMABuffer_s { uint32_t arcitimer[TRANSPONDER_DMA_BUFFER_SIZE_ARCITIMER]; // 620 @@ -91,7 +91,7 @@ typedef struct transponder_s { uint16_t bitToggleOne; uint32_t dma_buffer_size; - #if defined(STM32F4)|| defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST) + #if defined(STM32F4)|| defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) || defined(UNIT_TEST) transponderIrDMABuffer_t transponderIrDMABuffer; #endif diff --git a/src/main/drivers/transponder_ir_arcitimer.c b/src/main/drivers/transponder_ir_arcitimer.c index 4229af979e..498299ddc1 100644 --- a/src/main/drivers/transponder_ir_arcitimer.c +++ b/src/main/drivers/transponder_ir_arcitimer.c @@ -29,7 +29,7 @@ #include "drivers/transponder_ir.h" #include "drivers/transponder_ir_arcitimer.h" -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) || defined(UNIT_TEST) extern const struct transponderVTable arcitimerTansponderVTable; static uint16_t dmaBufferOffset; diff --git a/src/main/drivers/transponder_ir_erlt.c b/src/main/drivers/transponder_ir_erlt.c index 54d603eb49..856d569062 100644 --- a/src/main/drivers/transponder_ir_erlt.c +++ b/src/main/drivers/transponder_ir_erlt.c @@ -28,7 +28,7 @@ #include "drivers/transponder_ir.h" #include "drivers/transponder_ir_erlt.h" -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) || defined(UNIT_TEST) static uint16_t dmaBufferOffset; extern const struct transponderVTable erltTansponderVTable; diff --git a/src/main/drivers/transponder_ir_ilap.c b/src/main/drivers/transponder_ir_ilap.c index 9ecac736da..14a08d589a 100644 --- a/src/main/drivers/transponder_ir_ilap.c +++ b/src/main/drivers/transponder_ir_ilap.c @@ -28,7 +28,7 @@ #include "drivers/transponder_ir.h" #include "drivers/transponder_ir_ilap.h" -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(UNIT_TEST) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(APM32F4) || defined(GD32F4) || defined(UNIT_TEST) static uint16_t dmaBufferOffset; extern const struct transponderVTable ilapTansponderVTable; diff --git a/src/main/pg/adc.c b/src/main/pg/adc.c index b32de9545f..6ede9cce7c 100644 --- a/src/main/pg/adc.c +++ b/src/main/pg/adc.c @@ -39,7 +39,14 @@ PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0); void pgResetFn_adcConfig(adcConfig_t *adcConfig) { adcConfig->device = ADC_DEV_TO_CFG(adcDeviceByInstance(ADC_INSTANCE)); +#if defined(USE_ADC_DEVICE_0) + adcConfig->dmaopt[ADCDEV_0] = ADC0_DMA_OPT; +#if defined(ADC1) adcConfig->dmaopt[ADCDEV_1] = ADC1_DMA_OPT; +#endif +#else + adcConfig->dmaopt[ADCDEV_1] = ADC1_DMA_OPT; +#endif // These conditionals need to match the ones used in 'src/main/drivers/adc.h'. #if defined(ADC2) adcConfig->dmaopt[ADCDEV_2] = ADC2_DMA_OPT; diff --git a/src/platform/GD32/adc_gd32.c b/src/platform/GD32/adc_gd32.c new file mode 100755 index 0000000000..08aaa0c687 --- /dev/null +++ b/src/platform/GD32/adc_gd32.c @@ -0,0 +1,322 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_ADC + +#include "build/debug.h" + +#include "drivers/dma_reqmap.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "platform/rcc.h" +#include "drivers/dma.h" +#include "drivers/sensor.h" +#include "drivers/adc.h" +#include "drivers/adc_impl.h" + +#include "pg/adc.h" + +const adcDevice_t adcHardware[] = { + { + .ADCx = (void *)ADC0, + .rccADC = RCC_APB2(ADC0), +#if !defined(USE_DMA_SPEC) + .dmaResource = (dmaResource_t *)ADC0_DMA_STREAM, + .channel = DMA_SUBPERI0 +#endif + }, + { + .ADCx = (void *)ADC1, + .rccADC = RCC_APB2(ADC1), +#if !defined(USE_DMA_SPEC) + .dmaResource = (dmaResource_t *)ADC1_DMA_STREAM, + .channel = DMA_SUBPERI1 +#endif + }, + { + .ADCx = (void *)ADC2, + .rccADC = RCC_APB2(ADC2), +#if !defined(USE_DMA_SPEC) + .dmaResource = (dmaResource_t *)ADC2_DMA_STREAM, + .channel = DMA_SUBPERI2 +#endif + } +}; + +/* note these could be packed up for saving space */ +const adcTagMap_t adcTagMap[] = { + + { DEFIO_TAG_E__PC0, ADC_DEVICES_012, ADC_CHANNEL_10 }, + { DEFIO_TAG_E__PC1, ADC_DEVICES_012, ADC_CHANNEL_11 }, + { DEFIO_TAG_E__PC2, ADC_DEVICES_012, ADC_CHANNEL_12 }, + { DEFIO_TAG_E__PC3, ADC_DEVICES_012, ADC_CHANNEL_13 }, + { DEFIO_TAG_E__PC4, ADC_DEVICES_01, ADC_CHANNEL_14 }, + { DEFIO_TAG_E__PC5, ADC_DEVICES_01, ADC_CHANNEL_15 }, + { DEFIO_TAG_E__PB0, ADC_DEVICES_01, ADC_CHANNEL_8 }, + { DEFIO_TAG_E__PB1, ADC_DEVICES_01, ADC_CHANNEL_9 }, + { DEFIO_TAG_E__PA0, ADC_DEVICES_012, ADC_CHANNEL_0 }, + { DEFIO_TAG_E__PA1, ADC_DEVICES_012, ADC_CHANNEL_1 }, + { DEFIO_TAG_E__PA2, ADC_DEVICES_012, ADC_CHANNEL_2 }, + { DEFIO_TAG_E__PA3, ADC_DEVICES_012, ADC_CHANNEL_3 }, + { DEFIO_TAG_E__PA4, ADC_DEVICES_01, ADC_CHANNEL_4 }, + { DEFIO_TAG_E__PA5, ADC_DEVICES_01, ADC_CHANNEL_5 }, + { DEFIO_TAG_E__PA6, ADC_DEVICES_01, ADC_CHANNEL_6 }, + { DEFIO_TAG_E__PA7, ADC_DEVICES_01, ADC_CHANNEL_7 }, +}; + + +static void adcInitDevice(uint32_t adc_periph, int channelCount) +{ + // Multiple injected channel seems to require scan conversion mode to be + // enabled even if main (non-injected) channel count is 1. +#ifdef USE_ADC_INTERNAL + adc_special_function_config(adc_periph, ADC_SCAN_MODE, ENABLE); +#else + if(channelCount > 1) + adc_special_function_config(adc_periph, ADC_SCAN_MODE, ENABLE); + else + adc_special_function_config(adc_periph, ADC_SCAN_MODE, DISABLE); +#endif + + adc_special_function_config(adc_periph, ADC_CONTINUOUS_MODE, ENABLE); + adc_resolution_config(adc_periph, ADC_RESOLUTION_12B); + adc_data_alignment_config(adc_periph, ADC_DATAALIGN_RIGHT); + + /* routine channel config */ + adc_external_trigger_source_config(adc_periph, ADC_ROUTINE_CHANNEL, ADC_EXTTRIG_ROUTINE_T0_CH0); + adc_external_trigger_config(adc_periph, ADC_ROUTINE_CHANNEL, EXTERNAL_TRIGGER_DISABLE); + adc_channel_length_config(adc_periph, ADC_ROUTINE_CHANNEL, channelCount); +} + +#ifdef USE_ADC_INTERNAL + +static void adcInitInternalInjected(const adcConfig_t *config) +{ + adc_channel_16_to_18(ADC_TEMP_VREF_CHANNEL_SWITCH, ENABLE); + adc_discontinuous_mode_config(ADC0, ADC_CHANNEL_DISCON_DISABLE, 0); + adc_channel_length_config(ADC0, ADC_INSERTED_CHANNEL, 2); + + adc_inserted_channel_config(ADC0, 1, ADC_CHANNEL_17, ADC_SAMPLETIME_480); // ADC_Channel_Vrefint + adc_inserted_channel_config(ADC0, 0, ADC_CHANNEL_16, ADC_SAMPLETIME_480); // ADC_Channel_TempSensor + + adcVREFINTCAL = config->vrefIntCalibration ? config->vrefIntCalibration : VREFINT_EXPECTED; + adcTSCAL1 = config->tempSensorCalibration1 ? config->tempSensorCalibration1 : ((TEMPSENSOR_CAL1_V * 4095.0f) / 3.3f); + + adcTSSlopeK = lrintf(3300.0f*1000.0f/4095.0f/TEMPSENSOR_SLOPE); +} + +// Note on sampling time for temperature sensor and vrefint: +// Both sources have minimum sample time of 10us. +// With prescaler = 8: +// 168MHz : fAPB2 = 84MHz, fADC = 10.5MHz, tcycle = 0.090us, 10us = 105cycle < 144cycle +// 240MHz : fAPB2 = 120MHz, fADC = 15.0MHz, tcycle = 0.067usk 10us = 150cycle < 480cycle +// +// 480cycles@15.0MHz = 32us + +static bool adcInternalConversionInProgress = false; + +bool adcInternalIsBusy(void) +{ + if (adcInternalConversionInProgress) { + if (adc_flag_get(ADC0, ADC_FLAG_EOIC) != RESET) { + adcInternalConversionInProgress = false; + } + } + + return adcInternalConversionInProgress; +} + +void adcInternalStartConversion(void) +{ + adc_flag_clear(ADC0, ADC_FLAG_EOIC); + adc_software_trigger_enable(ADC0, ADC_INSERTED_CHANNEL); + + adcInternalConversionInProgress = true; +} + +uint16_t adcInternalReadVrefint(void) +{ + return adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_1); +} + +uint16_t adcInternalReadTempsensor(void) +{ + return adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_0); +} +#endif + +void adcInit(const adcConfig_t *config) +{ + uint8_t i; + uint8_t configuredAdcChannels = 0; + + memset(&adcOperatingConfig, 0, sizeof(adcOperatingConfig)); + + if (config->vbat.enabled) { + adcOperatingConfig[ADC_BATTERY].tag = config->vbat.ioTag; + } + + if (config->rssi.enabled) { + adcOperatingConfig[ADC_RSSI].tag = config->rssi.ioTag; //RSSI_ADC_CHANNEL; + } + + if (config->external1.enabled) { + adcOperatingConfig[ADC_EXTERNAL1].tag = config->external1.ioTag; //EXTERNAL1_ADC_CHANNEL; + } + + if (config->current.enabled) { + adcOperatingConfig[ADC_CURRENT].tag = config->current.ioTag; //CURRENT_METER_ADC_CHANNEL; + } + + ADCDevice device = ADC_CFG_TO_DEV(config->device); + + if (device == ADCINVALID) { + return; + } + + adcDevice_t adc = adcHardware[device]; + + bool adcActive = false; + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcVerifyPin(adcOperatingConfig[i].tag, device)) { + continue; + } + + adcActive = true; + IOInit(IOGetByTag(adcOperatingConfig[i].tag), OWNER_ADC_BATT + i, 0); + IOConfigGPIO(IOGetByTag(adcOperatingConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_OTYPE_OD, GPIO_PUPD_NONE)); + adcOperatingConfig[i].adcChannel = adcChannelByTag(adcOperatingConfig[i].tag); + adcOperatingConfig[i].dmaIndex = configuredAdcChannels++; + adcOperatingConfig[i].sampleTime = ADC_SAMPLETIME_480; + adcOperatingConfig[i].enabled = true; + } + +#ifndef USE_ADC_INTERNAL + if (!adcActive) { + return; + } +#endif + + RCC_ClockCmd(adc.rccADC, ENABLE); + + adc_sync_mode_config(ADC_SYNC_MODE_INDEPENDENT); + adc_clock_config(ADC_ADCCK_PCLK2_DIV8); + adc_sync_dma_config(ADC_SYNC_DMA_DISABLE); + adc_sync_delay_config(ADC_SYNC_DELAY_5CYCLE); + +#ifdef USE_ADC_INTERNAL + // If device is not ADC1 or there's no active channel, then initialize ADC1 separately + if (device != ADCDEV_1 || !adcActive) { + RCC_ClockCmd(adcHardware[ADCDEV_1].rccADC, ENABLE); + adcInitDevice(ADC0, 2); + adc_enable(ADC0); + } + + // Initialize for injected conversion + adcInitInternalInjected(config); + + if (!adcActive) { + return; + } +#endif + + adcInitDevice((uint32_t)(adc.ADCx), configuredAdcChannels); // Note type conversion. + + uint8_t rank = 0; + for (i = 0; i < ADC_CHANNEL_COUNT; i++) { + if (!adcOperatingConfig[i].enabled) { + continue; + } + adc_routine_channel_config((uint32_t)(adc.ADCx), rank++, adcOperatingConfig[i].adcChannel, adcOperatingConfig[i].sampleTime); + } + + adc_dma_request_after_last_enable((uint32_t)(adc.ADCx)); + + adc_dma_mode_enable((uint32_t)(adc.ADCx)); + adc_enable((uint32_t)(adc.ADCx)); + +#ifdef USE_DMA_SPEC + const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, device, config->dmaopt[device]); + + if (!dmaSpec || !dmaAllocate(dmaGetIdentifier(dmaSpec->ref), OWNER_ADC, RESOURCE_INDEX(device))) { + return; + } + + dmaEnable(dmaGetIdentifier(dmaSpec->ref)); + + xDMA_DeInit(dmaSpec->ref); +#else + if (!dmaAllocate(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0)) { + return; + } + + dmaEnable(dmaGetIdentifier(adc.dmaResource)); + + xDMA_DeInit(adc.dmaResource); +#endif + + dma_single_data_parameter_struct dma_init_struct; + dma_single_data_para_struct_init(&dma_init_struct); + dma_init_struct.periph_addr = (uint32_t)(&ADC_RDATA((uint32_t)(adc.ADCx))); // (uint32_t)(&ADC_RDATA(ADC0)), ADC0即(uint32_t)(adc.ADCx) + + uint32_t temp_dma_periph; + int temp_dma_channel; + gd32_dma_chbase_parse((uint32_t)dmaSpec->ref, &temp_dma_periph, &temp_dma_channel); + +#ifdef USE_DMA_SPEC + dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, dmaSpec->channel); +#else + dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, adc.channel); +#endif + + dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dma_init_struct.memory0_addr = (uint32_t)(adcValues); + dma_init_struct.memory_inc = configuredAdcChannels > 1 ? DMA_MEMORY_INCREASE_ENABLE : DMA_MEMORY_INCREASE_DISABLE; + dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_16BIT; + dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_ENABLE; + dma_init_struct.direction = DMA_PERIPH_TO_MEMORY; + dma_init_struct.number = configuredAdcChannels; + dma_init_struct.priority = DMA_PRIORITY_HIGH; + + +#ifdef USE_DMA_SPEC + gd32_dma_init((uint32_t)dmaSpec->ref, &dma_init_struct); + xDMA_Cmd(dmaSpec->ref, ENABLE); +#else + gd32_dma_init((uint32_t)adc.dmaResource, &dma_init_struct); + xDMA_Cmd(adc.dmaResource, ENABLE); +#endif + + adc_software_trigger_enable((uint32_t)(adc.ADCx), ADC_ROUTINE_CHANNEL); +} + +void adcGetChannelValues(void) +{ + // Nothing to do +} +#endif diff --git a/src/platform/GD32/audio_gd32.c b/src/platform/GD32/audio_gd32.c new file mode 100755 index 0000000000..a023c7c078 --- /dev/null +++ b/src/platform/GD32/audio_gd32.c @@ -0,0 +1,84 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/maths.h" + +#include "drivers/audio.h" + +void audioSetupIO(void) +{ + rcu_periph_clock_enable(RCU_DAC); + rcu_periph_clock_enable(RCU_TIMER5); + + rcu_periph_clock_enable(RCU_GPIOA); + gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO_PIN_4); +} + +void audioGenerateWhiteNoise(void) +{ + rcu_timer_clock_prescaler_config(RCU_TIMER_PSC_MUL4); + + timer_deinit(TIMER5); + timer_parameter_struct timer_initpara; + timer_initpara.prescaler = 0; // 如果需要计数频率为50Mhz,这里可以4-1=3,进行分频。 + timer_initpara.alignedmode = TIMER_COUNTER_EDGE; + timer_initpara.counterdirection = TIMER_COUNTER_UP; + timer_initpara.period = 0xFF; + timer_initpara.clockdivision = TIMER_CKDIV_DIV1; + timer_initpara.repetitioncounter = 0; + timer_init(TIMER5, &timer_initpara); + + timer_master_slave_mode_config(TIMER5, TIMER_MASTER_SLAVE_MODE_ENABLE); + timer_master_output_trigger_source_select(TIMER5, TIMER_TRI_OUT_SRC_UPDATE); + + timer_enable(TIMER5); + + dac_deinit(DAC0); + dac_trigger_source_config(DAC0, DAC_OUT0, DAC_TRIGGER_T5_TRGO); + dac_trigger_enable(DAC0, DAC_OUT0); + dac_output_buffer_enable(DAC0, DAC_OUT0); + + dac_wave_mode_config(DAC0, DAC_OUT0, DAC_WAVE_MODE_LFSR); + dac_lfsr_noise_config(DAC0, DAC_OUT0, DAC_LFSR_BITS10_0); + dac_data_set(DAC0, DAC_OUT0, DAC_ALIGN_12B_R, 0xD00); + + dac_enable(DAC0, DAC_OUT0); +} + +#define TONE_SPREAD 8 + +void audioPlayTone(uint8_t tone) +{ + uint32_t autoreload_value = 64 + (MAX(TONE_MIN,MIN(tone, TONE_MAX)) * TONE_SPREAD); + timer_autoreload_value_config(TIMER5, autoreload_value); +} + +void audioSilence(void) +{ + dac_disable(DAC0, DAC_OUT0); + timer_disable(TIMER5); +} diff --git a/src/platform/GD32/bus_i2c_gd32.c b/src/platform/GD32/bus_i2c_gd32.c new file mode 100755 index 0000000000..6bdaff3ce2 --- /dev/null +++ b/src/platform/GD32/bus_i2c_gd32.c @@ -0,0 +1,493 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_I2C) && !defined(SOFT_I2C) + +#include "drivers/io.h" +#include "drivers/time.h" +#include "drivers/nvic.h" +#include "platform/rcc.h" + +#include "drivers/bus_i2c.h" +#include "drivers/bus_i2c_impl.h" +#include "drivers/bus_i2c_utils.h" + +static void i2c_er_handler(I2CDevice device); +static void i2c_ev_handler(I2CDevice device); + +#if defined(GD32F4) +#define IOCFG_I2C_PU IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_OD, GPIO_PUPD_PULLUP) +#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_OD, GPIO_PUPD_NONE) +#else // GD32F4 +#define IOCFG_I2C IO_CONFIG(GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ) +#endif + +const i2cHardware_t i2cHardware[I2CDEV_COUNT] = { +#ifdef USE_I2C_DEVICE_0 + { + .device = I2CDEV_0, + .reg = (void *)I2C0, + .sclPins = { + I2CPINDEF(PB6, GPIO_AF_4), + I2CPINDEF(PB8, GPIO_AF_4), + }, + .sdaPins = { + I2CPINDEF(PB7, GPIO_AF_4), + I2CPINDEF(PB9, GPIO_AF_4), + }, + .rcc = RCC_APB1(I2C0), + .ev_irq = I2C0_EV_IRQn, + .er_irq = I2C0_ER_IRQn, + }, +#endif + #ifdef USE_I2C_DEVICE_1 + { + .device = I2CDEV_1, + .reg = (void *)I2C1, + .sclPins = { + I2CPINDEF(PB10, GPIO_AF_4), + I2CPINDEF(PF1, GPIO_AF_4), + }, + .sdaPins = { + I2CPINDEF(PB11, GPIO_AF_4), + I2CPINDEF(PF0, GPIO_AF_4), + }, + .rcc = RCC_APB1(I2C1), + .ev_irq = I2C1_EV_IRQn, + .er_irq = I2C1_ER_IRQn, + }, + #endif + #ifdef USE_I2C_DEVICE_2 + { + .device = I2CDEV_2, + .reg = (void *)I2C2, + .sclPins = { + I2CPINDEF(PA8, GPIO_AF_4), + }, + .sdaPins = { + I2CPINDEF(PC9, GPIO_AF_4), + }, + .rcc = RCC_APB1(I2C2), + .ev_irq = I2C2_EV_IRQn, + .er_irq = I2C2_ER_IRQn, + }, + #endif +}; + +i2cDevice_t i2cDevice[I2CDEV_COUNT]; + +// State used by event handler ISR +typedef struct { + uint8_t subaddress_sent; // flag to indicate if subaddess sent + uint8_t final_stop; // flag to indicate final bus condition + int8_t index; // index is signed -1 == send the subaddress +} i2cEvState_t; + static i2cEvState_t i2c_ev_state[I2CDEV_COUNT]; + +static volatile uint16_t i2cErrorCount = 0; + +void I2C0_ER_IRQHandler(void) +{ + i2c_er_handler(I2CDEV_0); +} + +void I2C0_EV_IRQHandler(void) +{ + i2c_ev_handler(I2CDEV_0); +} + +void I2C1_ER_IRQHandler(void) +{ + i2c_er_handler(I2CDEV_1); +} + +void I2C1_EV_IRQHandler(void) +{ + i2c_ev_handler(I2CDEV_1); +} + +void I2C2_ER_IRQHandler(void) +{ + i2c_er_handler(I2CDEV_2); +} + +void I2C2_EV_IRQHandler(void) +{ + i2c_ev_handler(I2CDEV_2); +} + + static bool i2cHandleHardwareFailure(I2CDevice device) + { + i2cErrorCount++; + // reinit peripheral + clock out garbage + i2cInit(device); + return false; + } + +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) +{ + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + uint32_t I2Cx = (uint32_t )(i2cDevice[device].reg); + + if (!I2Cx) { + return false; + } + + i2cState_t *state = &(i2cDevice[device].state); + if (state->busy) { + return false; + } + + timeUs_t timeoutStartUs = microsISR(); + + state->addr = addr_ << 1; + state->reg = reg_; + state->writing = 1; + state->reading = 0; + state->write_p = data; + state->read_p = data; + state->bytes = len_; + state->busy = 1; + state->error = false; + + if (!(I2C_CTL1(I2Cx) & I2C_CTL1_EVIE)) { // if we are restarting the driver + if (!(I2C_CTL0(I2Cx) & I2C_CTL0_START)) { // ensure sending a start + while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for any stop to finish sending + if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) { + return i2cHandleHardwareFailure(device); + } + } + i2c_start_on_bus(I2Cx); // send the start for the new job + } + i2c_interrupt_enable(I2Cx, I2C_INT_ERR); // allow the interrupts to fire off again + i2c_interrupt_enable(I2Cx, I2C_INT_EV); + } + + return true; +} + +bool i2cBusy(I2CDevice device, bool *error) +{ + i2cState_t *state = &i2cDevice[device].state; + + if (error) { + *error = state->error; + } + return state->busy; +} + +static bool i2cWait(I2CDevice device) +{ + i2cState_t *state = &(i2cDevice[device].state); + timeUs_t timeoutStartUs = microsISR(); + + while (state->busy) { + if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) { + return i2cHandleHardwareFailure(device) && i2cWait(device); + } + } + + return !(state->error); +} + +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) +{ + return i2cWriteBuffer(device, addr_, reg_, 1, &data) && i2cWait(device); +} + +bool i2cReadBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +{ + + if (device == I2CINVALID || device >= I2CDEV_COUNT) { + return false; + } + + uint32_t I2Cx = (uint32_t )(i2cDevice[device].reg); + if (!I2Cx) { + return false; + } + + i2cState_t *state = &i2cDevice[device].state; + if (state->busy) { + return false; + } + + timeUs_t timeoutStartUs = microsISR(); + + state->addr = addr_ << 1; + state->reg = reg_; + state->writing = 0; + state->reading = 1; + state->read_p = buf; + state->write_p = buf; + state->bytes = len; + state->busy = 1; + state->error = false; + + if (!(I2C_CTL1(I2Cx) & I2C_CTL1_EVIE)) { // if we are restarting the driver + if (!(I2C_CTL0(I2Cx) & I2C_CTL0_START)) { // ensure sending a start + while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) { // wait for any stop to finish sending + if (cmpTimeUs(microsISR(), timeoutStartUs) >= I2C_TIMEOUT_US) { + return i2cHandleHardwareFailure(device); + } + } + + /* enable acknowledge */ + i2c_ack_config(I2Cx, I2C_ACK_ENABLE); + uint8_t wait_count = 0; + /* wait until I2C bus is idle */ + while(i2c_flag_get(I2Cx, I2C_FLAG_I2CBSY)){ + if(wait_count>10) { + break; + } + wait_count++; + delay(5); + } + if(2 == len) { + i2c_ackpos_config(I2Cx, I2C_ACKPOS_NEXT); + } + i2c_start_on_bus(I2Cx); // send the start for the new job + } + i2c_interrupt_enable(I2Cx, I2C_INT_ERR); // allow the interrupts to fire off again + i2c_interrupt_enable(I2Cx, I2C_INT_EV); + } + + return true; +} + +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +{ + return i2cReadBuffer(device, addr_, reg_, len, buf) && i2cWait(device); +} + +static void i2c_er_handler(I2CDevice device) +{ + uint32_t I2Cx = (uint32_t )(i2cDevice[device].hardware->reg); + + i2cState_t *state = &i2cDevice[device].state; + + // Read the I2C status register + volatile uint32_t SR1Register = I2C_STAT0(I2Cx); + + if (SR1Register & (I2C_STAT0_BERR | I2C_STAT0_LOSTARB | I2C_STAT0_AERR | I2C_STAT0_OUERR)) // an error + state->error = true; + + if (SR1Register & (I2C_STAT0_BERR | I2C_STAT0_LOSTARB | I2C_STAT0_AERR)) { + // I2C_STAT0(I2Cx); + I2C_STAT1(I2Cx); // read second status register to clear ADDR if it is set (note that BTC will not be set after a NACK) + i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable the RBNE/TBE interrupt - prevent the ISR tailchaining onto the ERR + if (!(SR1Register & I2C_STAT0_LOSTARB) && !(I2C_CTL0(I2Cx) & I2C_CTL0_STOP)) { + if (I2C_CTL0(I2Cx) & I2C_CTL0_START) { // We are currently trying to send a start, this is very bad as start, stop will hang the peripheral + while (I2C_CTL0(I2Cx) & I2C_CTL0_START) {; } // wait for any start to finish sending + i2c_stop_on_bus(I2Cx); // send stop to finalise bus transaction + while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) {; } // wait for stop to finish sending + i2cInit(device); // reset and configure the hardware + } else { + i2c_stop_on_bus(I2Cx); // stop to free up the bus + while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) {; } + i2c_interrupt_disable(I2Cx, I2C_INT_ERR); // Disable EV and ERR interrupts while bus inactive + i2c_interrupt_disable(I2Cx, I2C_INT_EV); + } + } + } + I2C_STAT0(I2Cx) &= ~(I2C_STAT0_BERR | I2C_STAT0_LOSTARB | I2C_STAT0_AERR | I2C_STAT0_OUERR); // reset all the error bits to clear the interrupt + state->busy = 0; +} + +void i2c_ev_handler(I2CDevice device) +{ + uint32_t I2Cx = (uint32_t)(i2cDevice[device].hardware->reg); + + i2cEvState_t *ev_state = &i2c_ev_state[device]; + i2cState_t *state = &i2cDevice[device].state; + + uint8_t SReg_1 = I2C_STAT0(I2Cx); // read the status register here + + if (SReg_1 & I2C_STAT0_SBSEND) { // we just sent a start + ev_state->index = 0; // reset the index + if (state->reading && (ev_state->subaddress_sent || 0xFF == state->reg)) { // we have sent the subaddr + ev_state->subaddress_sent = 1; // make sure this is set in case of no subaddress, so following code runs correctly + i2c_master_addressing(I2Cx, state->addr, I2C_RECEIVER); // send the address and set hardware mode + } else { // direction is TB, or we havent sent the sub and rep start + i2c_master_addressing(I2Cx, state->addr, I2C_TRANSMITTER); // send the address and set hardware mode + if (state->reg != 0xFF) // 0xFF as subaddress means it will be ignored, in TB or RB mode + ev_state->index = -1; // send a subaddress + } + } else if (SReg_1 & I2C_STAT0_ADDSEND) { // we just sent the address + // Read SR1,2 to clear ADDR + __DMB(); // memory fence to control hardware + if (state->bytes == 1 && state->reading && ev_state->subaddress_sent) { // we are receiving 1 byte + i2c_ack_config(I2C0, I2C_ACK_DISABLE); // turn off ACK + __DMB(); + I2C_STAT1(I2Cx); // clear ADDR after ACK is turned off + i2c_stop_on_bus(I2Cx); // program the stop + while(I2C_CTL0(I2Cx) & I2C_CTL0_STOP){;} + + ev_state->final_stop = 1; + i2c_interrupt_enable(I2Cx, I2C_INT_BUF); + } else { + I2C_STAT1(I2Cx); // clear the ADDR here + __DMB(); + if (state->bytes == 2 && state->reading && ev_state->subaddress_sent) { // rx 2 bytes + i2c_ack_config(I2Cx, I2C_ACK_DISABLE); // turn off ACK + i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable TBE to allow the buffer to fill + } else if (state->bytes == 3 && state->reading && ev_state->subaddress_sent) { // rx 3 bytes + i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // make sure RBNE disabled so we get a BTC in two bytes time + } else { // receiving greater than three bytes, sending subaddress, or transmitting + i2c_interrupt_enable(I2Cx, I2C_INT_BUF); + } + } + } else if (SReg_1 & I2C_STAT0_BTC) { // Byte transfer finished + ev_state->final_stop = 1; + if (state->reading && ev_state->subaddress_sent) { + if (state->bytes > 2) { + i2c_ack_config(I2Cx, I2C_ACK_DISABLE); // turn off ACK + state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); // read data N-2 + i2c_stop_on_bus(I2Cx); // program the Stop + while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) {; } + + ev_state->final_stop = 1; // required to fix hardware + state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); // read data N - 1 + i2c_interrupt_enable(I2Cx, I2C_INT_BUF); + } else { + if (ev_state->final_stop) { + i2c_stop_on_bus(I2Cx); // program the Stop + while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) {; } + } else { + i2c_start_on_bus(I2Cx); // program a rep start + } + + state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); // read data N - 1 + state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); // read data N + ev_state->index++; + } + } else { + if (ev_state->subaddress_sent || (state->writing)) { + if (ev_state->final_stop) { + i2c_stop_on_bus(I2Cx); // program the Stop + while (I2C_CTL0(I2Cx) & I2C_CTL0_STOP) {; } + } else { + i2c_start_on_bus(I2Cx); // program a rep start + } + + ev_state->index++; + } else { // We need to send a subaddress + i2c_start_on_bus(I2Cx); // program the repeated Start + ev_state->subaddress_sent = 1; // this is set back to zero upon completion of the current task + } + } + // we must wait for the start to clear, otherwise we get constant BTC + while (I2C_CTL0(I2Cx) & I2C_CTL0_START) {; } + } else if (SReg_1 & I2C_STAT0_RBNE) { // Byte received + state->read_p[ev_state->index++] = (uint8_t)I2C_DATA(I2Cx); + if (state->bytes == (ev_state->index + 3)) + i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable TBE to allow the buffer to flush + if (state->bytes == ev_state->index) + ev_state->index++; + } else if (SReg_1 & I2C_STAT0_TBE) { // Byte transmitted + if (ev_state->index != -1) { + if (state->bytes == (ev_state->index + 1) ) // we have sent all the data + i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable TXE to allow the buffer to flush + I2C_DATA(I2Cx) = state->write_p[ev_state->index++]; + } else { + ev_state->index++; + if (state->reading || !(state->bytes)) // if receiving or sending 0 bytes, flush now + i2c_interrupt_disable(I2Cx, I2C_INT_BUF); // disable TBE to allow the buffer to flush + I2C_DATA(I2Cx) = state->reg; // send the subaddress + + } + } + if (ev_state->index == state->bytes + 1) { + ev_state->subaddress_sent = 0; // reset this here + if (ev_state->final_stop){ + i2c_interrupt_disable(I2Cx, I2C_INT_ERR); + i2c_interrupt_disable(I2Cx, I2C_INT_EV); + } // If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTC + + state->busy = 0; + } +} + +void i2cInit(I2CDevice device) +{ + if (device == I2CINVALID) + return; + + i2cDevice_t *pDev = &i2cDevice[device]; + const i2cHardware_t *hw = pDev->hardware; + const IO_t scl = pDev->scl; + const IO_t sda = pDev->sda; + + if (!hw || IOGetOwner(scl) || IOGetOwner(sda)) { + return; + } + + uint32_t I2Cx = (uint32_t )i2cDevice[device].reg; + memset(&pDev->state, 0, sizeof(pDev->state)); + + IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device)); + IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device)); + + // Enable RCC + RCC_ClockCmd(hw->rcc, ENABLE); + + i2c_interrupt_disable(I2Cx, I2C_INT_EV); + i2cUnstick(scl, sda); + + // Init pins + #ifdef GD32F4 + IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sclAF); + IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, pDev->sdaAF); + #else + IOConfigGPIO(scl, IOCFG_I2C); + IOConfigGPIO(sda, IOCFG_I2C); + #endif + + i2c_deinit(I2Cx); + i2c_interrupt_disable(I2Cx, I2C_INT_ERR); + pDev->clockSpeed = ((pDev->clockSpeed > 400) ? 400 : pDev->clockSpeed); + i2c_clock_config(I2Cx, pDev->clockSpeed * 1000, I2C_DTCY_2); + i2c_mode_addr_config(I2Cx, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, 0); + + i2c_stretch_scl_low_config(I2Cx, I2C_SCLSTRETCH_ENABLE); + i2c_enable(I2Cx); + i2c_ack_config(I2Cx, I2C_ACK_ENABLE); + // I2C Interrupt + nvic_irq_enable(hw->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER)); + nvic_irq_enable(hw->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV)); +} + +uint16_t i2cGetErrorCounter(void) +{ + return i2cErrorCount; +} + +#endif diff --git a/src/platform/GD32/bus_spi_gd32.c b/src/platform/GD32/bus_spi_gd32.c new file mode 100755 index 0000000000..692bead6a7 --- /dev/null +++ b/src/platform/GD32/bus_spi_gd32.c @@ -0,0 +1,382 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_SPI + +// GD32F405 can't DMA to/from TCMSRAM +#define IS_TCM(p) (((uint32_t)p & 0xffff0000) == 0x10000000) + +#include "common/maths.h" +#include "drivers/bus.h" +#include "drivers/bus_spi.h" +#include "drivers/bus_spi_impl.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "platform/rcc.h" + +// Use DMA if possible if this many bytes are to be transferred +#define SPI_DMA_THRESHOLD 8 + +static spi_parameter_struct defaultInit = { + .device_mode = SPI_MASTER, + .trans_mode = SPI_TRANSMODE_FULLDUPLEX, + .frame_size = SPI_FRAMESIZE_8BIT, + .nss = SPI_NSS_SOFT, + .endian = SPI_ENDIAN_MSB, + .clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE, + .prescale = SPI_PSC_8 +}; + +static uint32_t spiDivisorToBRbits(const SPI_TypeDef *instance, uint16_t divisor) +{ + // SPI1 and SPI2 are on APB1/AHB1 which PCLK is half that of APB2/AHB2. + if ((uint32_t)instance == SPI1 || (uint32_t)instance == SPI2) { + divisor /= 2; // Safe for divisor == 0 or 1 + } + + divisor = constrain(divisor, 2, 256); + + return ((uint32_t)(ffs(divisor) - 2) << 3); +} + +static void spiSetDivisorBRreg(SPI_TypeDef *instance, uint16_t divisor) +{ +#define BR_BITS ((BIT(5) | BIT(4) | BIT(3))) + const uint32_t tempRegister = (SPI_CTL0((uint32_t)instance) & ~BR_BITS); + SPI_CTL0((uint32_t)instance) = (tempRegister | (spiDivisorToBRbits(instance, divisor) & BR_BITS)); +#undef BR_BITS +} + +void spiInitDevice(SPIDevice device) +{ + spiDevice_t *spi = &(spiDevice[device]); + + if (!spi->dev) { + return; + } + + // Enable SPI clock + RCC_ClockCmd(spi->rcc, ENABLE); + 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->sck), SPI_IO_AF_SCK_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_SDI_CFG, spi->af); + IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); + + // Init SPI hardware + spi_i2s_deinit((uint32_t)spi->dev); + + spi_dma_disable((uint32_t)spi->dev, SPI_DMA_TRANSMIT); + spi_dma_disable((uint32_t)spi->dev, SPI_DMA_RECEIVE); + spi_init((uint32_t)spi->dev, &defaultInit); + spi_crc_off((uint32_t)spi->dev); + spi_enable((uint32_t)spi->dev); +} + +void spiInternalResetDescriptors(busDevice_t *bus) +{ + DMA_InitTypeDef *dmaGenerInitTx = bus->dmaInitTx; + dmaGenerInitTx->data_mode = DMA_DATA_MODE_SINGLE; + dmaGenerInitTx->sub_periph = bus->dmaTx->channel; + dma_single_data_parameter_struct *dmaInitTx = &dmaGenerInitTx->config.init_struct_s; + + dma_single_data_para_struct_init(dmaInitTx); + dmaInitTx->direction = DMA_MEMORY_TO_PERIPH; + dmaInitTx->circular_mode = DMA_CIRCULAR_MODE_DISABLE; + dmaInitTx->periph_addr = (uint32_t)&SPI_DATA((uint32_t)(bus->busType_u.spi.instance)); + dmaInitTx->priority = DMA_PRIORITY_LOW; + dmaInitTx->periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dmaInitTx->periph_memory_width = DMA_PERIPH_WIDTH_8BIT; + + if (bus->dmaRx) { + DMA_InitTypeDef *dmaGenerInitRx = bus->dmaInitRx; + dmaGenerInitRx->data_mode = DMA_DATA_MODE_SINGLE; + dmaGenerInitRx->sub_periph = bus->dmaRx->channel; + dma_single_data_parameter_struct *dmaInitRx = &dmaGenerInitRx->config.init_struct_s; + + dma_single_data_para_struct_init(dmaInitRx); + dmaInitRx->direction = DMA_PERIPH_TO_MEMORY; + dmaInitRx->circular_mode = DMA_CIRCULAR_MODE_DISABLE; + dmaInitRx->periph_addr = (uint32_t)&SPI_DATA((uint32_t)(bus->busType_u.spi.instance)); + dmaInitRx->priority = DMA_PRIORITY_LOW; + dmaInitRx->periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dmaInitRx->periph_memory_width = DMA_PERIPH_WIDTH_8BIT; + } +} + +void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) +{ + DMA_Stream_TypeDef *streamRegs = (DMA_Stream_TypeDef *)descriptor->ref; + + // Disable the stream + REG32(streamRegs) = 0U; + + // Clear any pending interrupt flags + dma_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF); +} + +bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) +{ + uint8_t b; + + while (len--) { + b = txData ? *(txData++) : 0xFF; + while (spi_i2s_flag_get((uint32_t)instance, I2S_FLAG_TBE) == RESET); + spi_i2s_data_transmit((uint32_t)instance, b); + + while (spi_i2s_flag_get((uint32_t)instance, I2S_FLAG_RBNE) == RESET); + b = spi_i2s_data_receive((uint32_t)instance); + if (rxData) { + *(rxData++) = b; + } + } + + return true; +} + +void spiInternalInitStream(const extDevice_t *dev, bool preInit) +{ + STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff; + STATIC_DMA_DATA_AUTO uint8_t dummyRxByte; + busDevice_t *bus = dev->bus; + + volatile busSegment_t *segment = bus->curSegment; + + if (preInit) { + // Prepare the init structure for the next segment to reduce inter-segment interval + segment++; + if(segment->len == 0) { + // There's no following segment + return; + } + } + + int len = segment->len; + + uint8_t *txData = segment->u.buffers.txData; + + DMA_InitTypeDef *dmaGenerInitTx = bus->dmaInitTx; + dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx; + dmaGenerInitTx->sub_periph = dmaTx->channel; + dmaGenerInitTx->data_mode = DMA_DATA_MODE_SINGLE; + dma_single_data_parameter_struct *dmaInitTx = &dmaGenerInitTx->config.init_struct_s; + + if (txData) { + dmaInitTx->memory0_addr = (uint32_t)txData; + dmaInitTx->memory_inc = DMA_MEMORY_INCREASE_ENABLE; + } else { + dummyTxByte = 0xff; + dmaInitTx->memory0_addr = (uint32_t)&dummyTxByte; + dmaInitTx->memory_inc = DMA_MEMORY_INCREASE_DISABLE; + } + dmaInitTx->number = len; + + if (dev->bus->dmaRx) { + uint8_t *rxData = segment->u.buffers.rxData; + + DMA_InitTypeDef *dmaGenerInitRx = bus->dmaInitRx; + dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx; + dmaGenerInitRx->sub_periph = dmaRx->channel; + dmaGenerInitRx->data_mode = DMA_DATA_MODE_SINGLE; + dma_single_data_parameter_struct *dmaInitRx = &dmaGenerInitRx->config.init_struct_s; + + if (rxData) { + dmaInitRx->memory0_addr = (uint32_t)rxData; + dmaInitRx->memory_inc = DMA_MEMORY_INCREASE_ENABLE; + } else { + dmaInitRx->memory0_addr = (uint32_t)&dummyRxByte; + dmaInitRx->memory_inc = DMA_MEMORY_INCREASE_DISABLE; + } + // If possible use 16 bit memory writes to prevent atomic access issues on gyro data + if ((dmaInitRx->memory0_addr & 0x1) || (len & 0x1)) { + dmaInitRx->periph_memory_width = DMA_PERIPH_WIDTH_8BIT; + } else { + dmaInitRx->periph_memory_width = DMA_PERIPH_WIDTH_16BIT; + } + dmaInitRx->number = len; + } +} + +void spiInternalStartDMA(const extDevice_t *dev) +{ + dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx; + dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx; + DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref; + if (dmaRx) { + DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref; + + // Use the correct callback argument + dmaRx->userParam = (uint32_t)dev; + + // Clear transfer flags + dma_flag_clear((uint32_t)dmaTx->dma, dmaTx->stream, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF); + dma_flag_clear((uint32_t)dmaRx->dma, dmaRx->stream, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF); + + // Disable streams to enable update + REG32(streamRegsTx) = 0U; + REG32(streamRegsRx) = 0U; + + /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt + * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress + */ + xDMA_ITConfig(streamRegsRx, DMA_INT_FTF, ENABLE); + + // Update streams + + xDMA_Init(streamRegsTx, (dma_general_config_struct *)dev->bus->dmaInitTx); + + dma_channel_subperipheral_select((uint32_t)(dmaTx->dma), dmaTx->stream, dmaTx->channel); + + xDMA_Init(streamRegsRx, (dma_general_config_struct *)dev->bus->dmaInitRx); + + dma_channel_subperipheral_select((uint32_t)(dmaRx->dma), dmaRx->stream, dmaRx->channel); + + // Enable streams + dma_channel_enable((uint32_t)(dmaTx->dma), dmaTx->stream); + dma_channel_enable((uint32_t)(dmaRx->dma), dmaRx->stream); + + /* Enable the SPI DMA Tx & Rx requests */ + spi_dma_enable((uint32_t)(dev->bus->busType_u.spi.instance), SPI_DMA_TRANSMIT); + spi_dma_enable((uint32_t)(dev->bus->busType_u.spi.instance), SPI_DMA_RECEIVE); + } else { + // Use the correct callback argument + dmaTx->userParam = (uint32_t)dev; + + // Clear transfer flags + dma_flag_clear((uint32_t)dmaTx->dma, dmaTx->stream, DMA_FLAG_FEE | DMA_FLAG_SDE | DMA_FLAG_TAE | DMA_FLAG_HTF | DMA_FLAG_FTF); + + // Disable stream to enable update + REG32(streamRegsTx) = 0U; + + xDMA_ITConfig(streamRegsTx, DMA_INT_FTF, ENABLE); + + // Update stream + xDMA_Init(streamRegsTx, dev->bus->dmaInitTx); + + // Enable stream + dma_channel_enable((uint32_t)(dmaTx->dma), dmaTx->stream); + + /* Enable the SPI DMA Tx request */ + spi_dma_enable((uint32_t)(dev->bus->busType_u.spi.instance), SPI_DMA_TRANSMIT); + } +} + +void spiInternalStopDMA (const extDevice_t *dev) +{ + dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx; + dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx; + SPI_TypeDef *instance = dev->bus->busType_u.spi.instance; + DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref; + + if (dmaRx) { + DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref; + + // Disable streams + REG32(streamRegsTx) = 0U; + REG32(streamRegsRx) = 0U; + + spi_dma_disable((uint32_t)instance, SPI_DMA_TRANSMIT); + spi_dma_disable((uint32_t)instance, SPI_DMA_RECEIVE); + } else { + // Ensure the current transmission is complete + while (spi_i2s_flag_get((uint32_t)instance, I2S_FLAG_TRANS)); + + // Drain the RX buffer + while (spi_i2s_flag_get((uint32_t)instance, I2S_FLAG_RBNE)) { + SPI_DATA((uint32_t)instance); + } + + // Disable stream + REG32(streamRegsTx) = 0U; + + spi_dma_disable((uint32_t)instance, SPI_DMA_TRANSMIT); + } +} + +// DMA transfer setup and start +void spiSequenceStart(const extDevice_t *dev) +{ + busDevice_t *bus = dev->bus; + SPI_TypeDef *instance = bus->busType_u.spi.instance; + bool dmaSafe = dev->useDMA; + uint32_t xferLen = 0; + uint32_t segmentCount = 0; + + dev->bus->initSegment = true; + + spi_disable((uint32_t)instance); + + // Switch bus speed + if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) { + spiSetDivisorBRreg(bus->busType_u.spi.instance, dev->busType_u.spi.speed); + bus->busType_u.spi.speed = dev->busType_u.spi.speed; + } + + if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) { + // Switch SPI clock polarity/phase + SPI_CTL0((uint32_t)instance) &= ~(SPI_CTL0_CKPL | SPI_CTL0_CKPH); + + // Apply setting + if (dev->busType_u.spi.leadingEdge) { + SPI_CTL0((uint32_t)instance) |= SPI_CK_PL_LOW_PH_1EDGE; + } else + { + SPI_CTL0((uint32_t)instance) |= SPI_CK_PL_HIGH_PH_2EDGE; + } + bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge; + } + + spi_enable((uint32_t)instance); + + // Check that any there are no attempts to DMA to/from CCD SRAM + for (busSegment_t *checkSegment = (busSegment_t *)bus->curSegment; checkSegment->len; checkSegment++) { + // Check there is no receive data as only transmit DMA is available + if (((checkSegment->u.buffers.rxData) && (IS_TCM(checkSegment->u.buffers.rxData) || (bus->dmaRx == (dmaChannelDescriptor_t *)NULL))) || + ((checkSegment->u.buffers.txData) && IS_TCM(checkSegment->u.buffers.txData))) { + dmaSafe = false; + break; + } + // Note that these counts are only valid if dmaSafe is true + segmentCount++; + xferLen += checkSegment->len; + } + + // Use DMA if possible + // If there are more than one segments, or a single segment with negateCS negated in the list terminator then force DMA irrespective of length + if (bus->useDMA && dmaSafe && ((segmentCount > 1) || + (xferLen >= SPI_DMA_THRESHOLD) || + !bus->curSegment[segmentCount].negateCS)) { + spiProcessSegmentsDMA(dev); + } else { + spiProcessSegmentsPolled(dev); + } +} +#endif diff --git a/src/platform/GD32/camera_control_gd32.c b/src/platform/GD32/camera_control_gd32.c new file mode 100755 index 0000000000..25a556b1e8 --- /dev/null +++ b/src/platform/GD32/camera_control_gd32.c @@ -0,0 +1,95 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_CAMERA_CONTROL + +#include + +#include "drivers/camera_control_impl.h" +#include "drivers/nvic.h" +#include "drivers/pwm_output.h" +#include "platform/rcc.h" + +#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE +#include "build/atomic.h" +#endif + +#ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE +void cameraControlHardwarePwmInit(timerChannel_t *channel, const timerHardware_t *timerHardware, uint8_t inverted) +{ + pwmOutConfig(channel, timerHardware, timerClock((void *)TIMER5), CAMERA_CONTROL_PWM_RESOLUTION, 0, inverted); +} +#endif + +#ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE +void TIMER5_DAC_IRQHandler(void) +{ + cameraControlHi(); + TIMER_INTF(TIMER5) = 0; +} + +void TIMER6_IRQHandler(void) +{ + cameraControlLo(); + TIMER_INTF(TIMER6) = 0; +} + +void cameraControlSoftwarePwmInit(void) +{ + nvic_irq_enable(TIMER5_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER)); + rcu_periph_clock_enable(RCU_TIMER5); + TIMER_PSC(TIMER5) = 0; + + nvic_irq_enable(TIMER6_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER)); + rcu_periph_clock_enable(RCU_TIMER6); + TIMER_PSC(TIMER6) = 0; +} + +void cameraControlSoftwarePwmEnable(uint32_t hiTime, uint32_t period) +{ + timer_counter_value_config(TIMER5, hiTime); + timer_autoreload_value_config(TIMER5, period); + + timer_counter_value_config(TIMER6, 0); + timer_autoreload_value_config(TIMER6, period); + + ATOMIC_BLOCK(NVIC_PRIO_TIMER) { + timer_enable(TIMER5); + timer_enable(TIMER6); + } + + timer_interrupt_enable(TIMER5, TIMER_INT_UP); + timer_interrupt_enable(TIMER6, TIMER_INT_UP); +} + +void cameraControlSoftwarePwmDisable(void) +{ + timer_disable(TIMER5); + timer_disable(TIMER6); + + TIMER_DMAINTEN(TIMER5) = 0; + TIMER_DMAINTEN(TIMER6) = 0; +} +#endif + +#endif // USE_CAMERA_CONTROL diff --git a/src/platform/GD32/debug.c b/src/platform/GD32/debug.c new file mode 100755 index 0000000000..038ed5a196 --- /dev/null +++ b/src/platform/GD32/debug.c @@ -0,0 +1,36 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" +#include "build/debug.h" +#include "drivers/io.h" + +void debugInit(void) +{ + IO_t io = IOGetByTag(DEFIO_TAG_E(PA13)); // SWDIO + if (IOGetOwner(io) == OWNER_FREE) { + IOInit(io, OWNER_SWD, 0); + } + io = IOGetByTag(DEFIO_TAG_E(PA14)); // SWCLK + if (IOGetOwner(io) == OWNER_FREE) { + IOInit(io, OWNER_SWD, 0); + } +} diff --git a/src/platform/GD32/dma_gd32.c b/src/platform/GD32/dma_gd32.c new file mode 100755 index 0000000000..d3f3f8f59c --- /dev/null +++ b/src/platform/GD32/dma_gd32.c @@ -0,0 +1,305 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_DMA + +#include "drivers/nvic.h" +#include "drivers/dma.h" +#include "platform/rcc.h" +#include "drivers/resource.h" + +/* + * DMA descriptors. + */ +dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = { + DEFINE_DMA_CHANNEL(DMA0, 0, 0), + DEFINE_DMA_CHANNEL(DMA0, 1, 6), + DEFINE_DMA_CHANNEL(DMA0, 2, 16), + DEFINE_DMA_CHANNEL(DMA0, 3, 22), + DEFINE_DMA_CHANNEL(DMA0, 4, 32), + DEFINE_DMA_CHANNEL(DMA0, 5, 38), + DEFINE_DMA_CHANNEL(DMA0, 6, 48), + DEFINE_DMA_CHANNEL(DMA0, 7, 54), + + DEFINE_DMA_CHANNEL(DMA1, 0, 0), + DEFINE_DMA_CHANNEL(DMA1, 1, 6), + DEFINE_DMA_CHANNEL(DMA1, 2, 16), + DEFINE_DMA_CHANNEL(DMA1, 3, 22), + DEFINE_DMA_CHANNEL(DMA1, 4, 32), + DEFINE_DMA_CHANNEL(DMA1, 5, 38), + DEFINE_DMA_CHANNEL(DMA1, 6, 48), + DEFINE_DMA_CHANNEL(DMA1, 7, 54), +}; + +/* + * DMA IRQ Handlers + */ +DEFINE_DMA_IRQ_HANDLER(0, 0, DMA0_CH0_HANDLER) +DEFINE_DMA_IRQ_HANDLER(0, 1, DMA0_CH1_HANDLER) +DEFINE_DMA_IRQ_HANDLER(0, 2, DMA0_CH2_HANDLER) +DEFINE_DMA_IRQ_HANDLER(0, 3, DMA0_CH3_HANDLER) +DEFINE_DMA_IRQ_HANDLER(0, 4, DMA0_CH4_HANDLER) +DEFINE_DMA_IRQ_HANDLER(0, 5, DMA0_CH5_HANDLER) +DEFINE_DMA_IRQ_HANDLER(0, 6, DMA0_CH6_HANDLER) +DEFINE_DMA_IRQ_HANDLER(0, 7, DMA0_CH7_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_CH0_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_CH1_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_CH2_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_CH3_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_CH4_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_CH5_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_CH6_HANDLER) +DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_CH7_HANDLER) + +#define DMA_RCU(x) ((x) == (void *)DMA0 ? RCC_AHB1(DMA0) : RCC_AHB1(DMA1)) +void dmaEnable(dmaIdentifier_e identifier) +{ + (void) identifier; + const int index = DMA_IDENTIFIER_TO_INDEX(identifier); + RCC_ClockCmd(DMA_RCU(dmaDescriptors[index].dma), ENABLE); +} + +#define RETURN_TCIF_FLAG(s, n) if (s == DMA0_CH ## n ## _BASE || s == DMA1_CH ## n ## _BASE) return DMA_IT_TCIF ## n + +static int gd32_dma_channel_get(uint32_t dma_chan_base, uint32_t dma_periph) +{ + uint32_t chan_base = (dma_chan_base - dma_periph); + int channel = DMA_CH0; + + switch(chan_base) { + case 0x10: + channel = DMA_CH0; + break; + case 0x28: + channel = DMA_CH1; + break; + case 0x40: + channel = DMA_CH2; + break; + case 0x58: + channel = DMA_CH3; + break; + case 0x70: + channel = DMA_CH4; + break; + case 0x88: + channel = DMA_CH5; + break; + case 0xA0: + channel = DMA_CH6; + break; + case 0xB8: + channel = DMA_CH7; + break; + default: + break; + } + return channel; +} + +static uint32_t dma_int_ftf_flag_get(uint32_t dma_chan_base) +{ + uint32_t status = 0; + uint32_t dma_periph; + int channel; + + if(DMA1 > dma_chan_base) { + dma_periph = DMA0; + } else { + dma_periph = DMA1; + } + + channel = gd32_dma_channel_get(dma_chan_base, dma_periph); + + status = dma_interrupt_flag_get(dma_periph, channel ,DMA_INT_FLAG_FTF); + + return status; +} + +uint32_t dma_enable_status_get(uint32_t dma_chan_base) +{ + uint32_t dma_periph; + uint32_t status; + int channel; + + if(DMA1 > dma_chan_base) { + dma_periph = DMA0; + } else { + dma_periph = DMA1; + } + + channel = gd32_dma_channel_get(dma_chan_base, dma_periph); + + status = (DMA_CHCTL(dma_periph, channel) & DMA_CHXCTL_CHEN); + + return status; +} + +void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +{ + + const int index = DMA_IDENTIFIER_TO_INDEX(identifier); + + RCC_ClockCmd(DMA_RCU(dmaDescriptors[index].dma), ENABLE); + dmaDescriptors[index].irqHandlerCallback = callback; + dmaDescriptors[index].userParam = userParam; + dmaDescriptors[index].completeFlag = dma_int_ftf_flag_get((uint32_t)dmaDescriptors[index].ref); + + nvic_irq_enable(dmaDescriptors[index].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority)); +} + + +void gd32_dma_chbase_parse(uint32_t dma_chan_base, uint32_t *dma_periph, int *dma_channel) +{ + if(DMA1 > dma_chan_base) { + *dma_periph = DMA0; + } else { + *dma_periph = DMA1; + } + + *dma_channel = (int)(dma_chan_base - (*dma_periph) - 0x10)/0x18; +} + +void gd32_dma_init(uint32_t dma_chan_base, dma_single_data_parameter_struct *init_struct) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + dma_single_data_mode_init(dma_periph, channel, init_struct); +} + +void gd32_dma_general_init(uint32_t dma_chan_base, dma_general_config_struct *init_struct) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + if(DMA_DATA_MODE_MULTI == init_struct->data_mode) { + dma_multi_data_mode_init(dma_periph, channel, (dma_multi_data_parameter_struct *)&init_struct->config.init_struct_m); + } else { + dma_single_data_mode_init(dma_periph, channel, (dma_single_data_parameter_struct *)&init_struct->config.init_struct_s); + } + + dma_channel_subperipheral_select(dma_periph, channel, init_struct->sub_periph); +} + +void gd32_dma_deinit(uint32_t dma_chan_base) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + dma_deinit(dma_periph, channel); +} + +void gd32_dma_channel_state_config(uint32_t dma_chan_base, ControlStatus new_state) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + if(ENABLE == new_state) { + dma_flag_clear(dma_periph, channel, DMA_FLAG_FTF); + dma_flag_clear(dma_periph, channel, DMA_FLAG_HTF); + dma_channel_enable(dma_periph, channel); + } else { + dma_channel_disable(dma_periph, channel); + } +} + +void gd32_dma_int_config(uint32_t dma_chan_base, uint32_t source, ControlStatus new_state) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + if(ENABLE == new_state) { + dma_interrupt_enable(dma_periph, channel, source); + } else { + dma_interrupt_disable(dma_periph, channel, source); + } +} + +void gd32_dma_transnum_config(uint32_t dma_chan_base, uint32_t number) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + dma_transfer_number_config(dma_periph, channel, number); +} + +uint16_t gd32_dma_transnum_get(uint32_t dma_chan_base) +{ + uint32_t dma_periph ; + uint16_t number; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + number = (uint16_t)dma_transfer_number_get(dma_periph, channel); + return number; +} + +FlagStatus gd32_dma_flags_get(uint32_t dma_chan_base, uint32_t flag) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + return dma_flag_get(dma_periph, channel, flag); +} + +void gd32_dma_flag_clear(uint32_t dma_chan_base, uint32_t flag) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + dma_flag_clear(dma_periph, channel, flag); +} + +void gd32_dma_memory_addr_config(uint32_t dma_chan_base, uint32_t address, uint8_t memory_flag) +{ + uint32_t dma_periph ; + int channel; + + gd32_dma_chbase_parse(dma_chan_base, &dma_periph, &channel); + + dma_memory_address_config(dma_periph, channel, memory_flag, address); +} + +#endif diff --git a/src/platform/GD32/dma_reqmap_mcu.c b/src/platform/GD32/dma_reqmap_mcu.c new file mode 100755 index 0000000000..2cb47cc881 --- /dev/null +++ b/src/platform/GD32/dma_reqmap_mcu.c @@ -0,0 +1,216 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" + +#ifdef USE_DMA_SPEC + +#include "timer_def.h" +#include "drivers/adc.h" +#include "drivers/bus_spi.h" +#include "drivers/dma_reqmap.h" +#include "drivers/serial.h" +#include "drivers/serial_uart.h" +#include "drivers/serial_uart_impl.h" + +#include "pg/timerio.h" + +typedef struct dmaPeripheralMapping_s { + dmaPeripheral_e device; + uint8_t index; + dmaChannelSpec_t channelSpec[MAX_PERIPHERAL_DMA_OPTIONS]; +} dmaPeripheralMapping_t; + +typedef struct dmaTimerMapping_s { + TIM_TypeDef *tim; + uint8_t channel; + dmaChannelSpec_t channelSpec[MAX_TIMER_DMA_OPTIONS]; +} dmaTimerMapping_t; + +#define DMA(d, s, c) { DMA_CODE(d, s, c), (dmaResource_t *)DMA ## d ## _CH ## s ## _BASE, DMA_SUBPERI ## c } + +static const dmaPeripheralMapping_t dmaPeripheralMapping[] = { +#ifdef USE_SPI + { DMA_PERIPH_SPI_SDO, SPIDEV_0, { DMA(1, 3, 3), DMA(1, 5, 3) } }, + { DMA_PERIPH_SPI_SDI, SPIDEV_0, { DMA(1, 0, 3), DMA(1, 2, 3) } }, + { DMA_PERIPH_SPI_SDO, SPIDEV_1, { DMA(0, 4, 0) } }, + { DMA_PERIPH_SPI_SDI, SPIDEV_1, { DMA(0, 3, 0) } }, + { DMA_PERIPH_SPI_SDO, SPIDEV_2, { DMA(0, 5, 0), DMA(0, 7, 0) } }, + { DMA_PERIPH_SPI_SDI, SPIDEV_2, { DMA(0, 0, 0), DMA(0, 2, 0) } }, + +#endif + +#ifdef USE_ADC + { DMA_PERIPH_ADC, ADCDEV_0, { DMA(1, 0, 0), DMA(1, 4, 0) } }, + { DMA_PERIPH_ADC, ADCDEV_1, { DMA(1, 2, 1), DMA(1, 3, 1) } }, + { DMA_PERIPH_ADC, ADCDEV_2, { DMA(1, 0, 2), DMA(1, 1, 2) } }, +#endif + +#ifdef USE_SDCARD_SDIO + { DMA_PERIPH_SDIO, 0, { DMA(1, 3, 4), DMA(1, 6, 4) } }, +#endif + +#ifdef USE_UART0 + { DMA_PERIPH_UART_TX, UARTDEV_0, { DMA(1, 7, 4) } }, + { DMA_PERIPH_UART_RX, UARTDEV_0, { DMA(1, 5, 4), DMA(1, 2, 4) } }, +#endif + +#ifdef USE_UART1 + { DMA_PERIPH_UART_TX, UARTDEV_1, { DMA(0, 6, 4) } }, + { DMA_PERIPH_UART_RX, UARTDEV_1, { DMA(0, 5, 4) } }, +#endif + +#ifdef USE_UART2 + { DMA_PERIPH_UART_TX, UARTDEV_2, { DMA(0, 3, 4) } }, + { DMA_PERIPH_UART_RX, UARTDEV_2, { DMA(0, 1, 4) } }, +#endif + +#ifdef USE_UART3 + { DMA_PERIPH_UART_TX, UARTDEV_3, { DMA(0, 4, 4) } }, + { DMA_PERIPH_UART_RX, UARTDEV_3, { DMA(0, 2, 4) } }, +#endif + +#ifdef USE_UART4 + { DMA_PERIPH_UART_TX, UARTDEV_4, { DMA(0, 7, 4) } }, + { DMA_PERIPH_UART_RX, UARTDEV_4, { DMA(0, 0, 4) } }, +#endif + +#ifdef USE_UART5 + { DMA_PERIPH_UART_TX, UARTDEV_5, { DMA(1, 6, 5), DMA(1, 7, 5) } }, + { DMA_PERIPH_UART_RX, UARTDEV_5, { DMA(1, 1, 5), DMA(1, 2, 5) } }, +#endif +}; + +#define TC(chan) DEF_TIM_CHANNEL(CH_ ## chan) + +static const dmaTimerMapping_t dmaTimerMapping[] = { + // Generated from 'timer_def.h' + { (void *)TIMER0, TC(CH0), { DMA(1, 6, 0), DMA(1, 1, 6), DMA(1, 3, 6) } }, + { (void *)TIMER0, TC(CH1), { DMA(1, 6, 0), DMA(1, 2, 6) } }, + { (void *)TIMER0, TC(CH2), { DMA(1, 6, 0), DMA(1, 6, 6) } }, + { (void *)TIMER0, TC(CH3), { DMA(1, 4, 6) } }, + + { (void *)TIMER1, TC(CH0), { DMA(0, 5, 3) } }, + { (void *)TIMER1, TC(CH1), { DMA(0, 6, 3) } }, + { (void *)TIMER1, TC(CH2), { DMA(0, 1, 3) } }, + { (void *)TIMER1, TC(CH3), { DMA(0, 7, 3), DMA(0, 6, 3) } }, + + { (void *)TIMER2, TC(CH0), { DMA(0, 4, 5) } }, + { (void *)TIMER2, TC(CH1), { DMA(0, 5, 5) } }, + { (void *)TIMER2, TC(CH2), { DMA(0, 7, 5) } }, + { (void *)TIMER2, TC(CH3), { DMA(0, 2, 5) } }, + + { (void *)TIMER3, TC(CH0), { DMA(0, 0, 2) } }, + { (void *)TIMER3, TC(CH1), { DMA(0, 3, 2) } }, + { (void *)TIMER3, TC(CH2), { DMA(0, 7, 2) } }, + + { (void *)TIMER4, TC(CH0), { DMA(0, 2, 6) } }, + { (void *)TIMER4, TC(CH1), { DMA(0, 4, 6) } }, + { (void *)TIMER4, TC(CH2), { DMA(0, 0, 6) } }, + { (void *)TIMER4, TC(CH3), { DMA(0, 1, 6), DMA(0, 3, 6) } }, + + { (void *)TIMER7, TC(CH0), { DMA(1, 2, 0), DMA(1, 2, 7) } }, + { (void *)TIMER7, TC(CH1), { DMA(1, 2, 0), DMA(1, 3, 7) } }, + { (void *)TIMER7, TC(CH2), { DMA(1, 2, 0), DMA(1, 4, 7) } }, + { (void *)TIMER7, TC(CH3), { DMA(1, 7, 7) } }, +}; + +#undef TC +#undef DMA + +const dmaChannelSpec_t *dmaGetChannelSpecByPeripheral(dmaPeripheral_e device, uint8_t index, int8_t opt) +{ + if (opt < 0 || opt >= MAX_PERIPHERAL_DMA_OPTIONS) { + return NULL; + } + + for (const dmaPeripheralMapping_t *periph = dmaPeripheralMapping; periph < ARRAYEND(dmaPeripheralMapping); periph++) { + if (periph->device == device && periph->index == index && periph->channelSpec[opt].ref) { + return &periph->channelSpec[opt]; + } + } + + return NULL; +} + +dmaoptValue_t dmaoptByTag(ioTag_t ioTag) +{ +#ifdef USE_TIMER_MGMT + for (unsigned i = 0; i < MAX_TIMER_PINMAP_COUNT; i++) { + if (timerIOConfig(i)->ioTag == ioTag) { + return timerIOConfig(i)->dmaopt; + } + } +#else + UNUSED(ioTag); +#endif + + return DMA_OPT_UNUSED; +} + +const dmaChannelSpec_t *dmaGetChannelSpecByTimerValue(TIM_TypeDef *tim, uint8_t channel, dmaoptValue_t dmaopt) +{ + if (dmaopt < 0 || dmaopt >= MAX_TIMER_DMA_OPTIONS) { + return NULL; + } + + for (unsigned i = 0 ; i < ARRAYLEN(dmaTimerMapping) ; i++) { + const dmaTimerMapping_t *timerMapping = &dmaTimerMapping[i]; + + if (timerMapping->tim == tim && timerMapping->channel == channel && timerMapping->channelSpec[dmaopt].ref) { + return &timerMapping->channelSpec[dmaopt]; + } + } + + return NULL; +} + +const dmaChannelSpec_t *dmaGetChannelSpecByTimer(const timerHardware_t *timer) +{ + if (!timer) { + return NULL; + } + + dmaoptValue_t dmaopt = dmaoptByTag(timer->tag); + return dmaGetChannelSpecByTimerValue(timer->tim, timer->channel, dmaopt); +} + +// dmaGetOptionByTimer is called by pgResetFn_timerIOConfig to find out dmaopt for pre-configured timer. +dmaoptValue_t dmaGetOptionByTimer(const timerHardware_t *timer) +{ + for (unsigned i = 0 ; i < ARRAYLEN(dmaTimerMapping); i++) { + const dmaTimerMapping_t *timerMapping = &dmaTimerMapping[i]; + if (timerMapping->tim == timer->tim && timerMapping->channel == timer->channel) { + for (unsigned j = 0; j < MAX_TIMER_DMA_OPTIONS; j++) { + const dmaChannelSpec_t *dma = &timerMapping->channelSpec[j]; + if (dma->ref == timer->dmaRefConfigured && dma->channel == timer->dmaChannelConfigured) { + return j; + } + } + } + } + + return DMA_OPT_UNUSED; +} + +#endif // USE_DMA_SPEC diff --git a/src/platform/GD32/dma_reqmap_mcu.h b/src/platform/GD32/dma_reqmap_mcu.h new file mode 100755 index 0000000000..1e11e27c00 --- /dev/null +++ b/src/platform/GD32/dma_reqmap_mcu.h @@ -0,0 +1,25 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#define MAX_PERIPHERAL_DMA_OPTIONS 2 +#define MAX_TIMER_DMA_OPTIONS 3 diff --git a/src/platform/GD32/dshot_bitbang.c b/src/platform/GD32/dshot_bitbang.c new file mode 100755 index 0000000000..725b79544d --- /dev/null +++ b/src/platform/GD32/dshot_bitbang.c @@ -0,0 +1,762 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_DSHOT_BITBANG + +#include "build/debug.h" +#include "build/debug_pin.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/dma.h" +#include "drivers/dma_reqmap.h" +#include "drivers/dshot.h" +#include "drivers/dshot_bitbang.h" +#include "dshot_bitbang_impl.h" +#include "drivers/dshot_command.h" +#include "drivers/motor.h" +#include "drivers/nvic.h" +#include "pwm_output_dshot_shared.h" +#include "drivers/dshot_bitbang_decode.h" +#include "drivers/time.h" +#include "drivers/timer.h" + +#include "pg/motor.h" +#include "pg/pinio.h" + +// DEBUG_DSHOT_TELEMETRY_COUNTS +// 0 - Count of telemetry packets read +// 1 - Count of missing edge +// 2 - Count of reception not complete in time +// 3 - Number of high bits before telemetry start + +// Maximum time to wait for telemetry reception to complete +#define DSHOT_TELEMETRY_TIMEOUT 2000 + +// For MCUs that use MPU to control DMA coherency, there might be a performance hit +// on manipulating input buffer content especially if it is read multiple times, +// as the buffer region is attributed as not cachable. +// If this is not desirable, we should use manual cache invalidation. +#ifdef USE_DSHOT_CACHE_MGMT +#define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32))) +#define BB_INPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32))) +#else +#if defined(GD32F4) +#define BB_OUTPUT_BUFFER_ATTRIBUTE +#define BB_INPUT_BUFFER_ATTRIBUTE +#endif +#endif // USE_DSHOT_CACHE_MGMT + +BB_OUTPUT_BUFFER_ATTRIBUTE uint32_t bbOutputBuffer[MOTOR_DSHOT_BUF_CACHE_ALIGN_LENGTH * MAX_SUPPORTED_MOTOR_PORTS]; +BB_INPUT_BUFFER_ATTRIBUTE uint16_t bbInputBuffer[DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_LENGTH * MAX_SUPPORTED_MOTOR_PORTS]; + +uint8_t bbPuPdMode; +FAST_DATA_ZERO_INIT timeUs_t dshotFrameUs; + + const timerHardware_t bbTimerHardware[] = { + #if defined(GD32F4) + DEF_TIMER(TIMER7, CH0, NONE, 0, 1), + DEF_TIMER(TIMER7, CH1, NONE, 0, 1), + DEF_TIMER(TIMER7, CH2, NONE, 0, 1), + DEF_TIMER(TIMER7, CH3, NONE, 0, 0), + + DEF_TIMER(TIMER0, CH0, NONE, 0, 1), + DEF_TIMER(TIMER0, CH0, NONE, 0, 2), + DEF_TIMER(TIMER0, CH1, NONE, 0, 1), + DEF_TIMER(TIMER0, CH2, NONE, 0, 1), + DEF_TIMER(TIMER0, CH3, NONE, 0, 0), + + #else + #error MCU dependent code required + #endif + }; + +static FAST_DATA_ZERO_INIT timeUs_t lastSendUs; + +static motorProtocolTypes_e motorProtocol; + +// DMA GPIO output buffer formatting + +static void bbOutputDataInit(uint32_t *buffer, uint16_t portMask, bool inverted) +{ + uint32_t resetMask; + uint32_t setMask; + + if (inverted) { + resetMask = portMask; + setMask = (portMask << 16); + } else { + resetMask = (portMask << 16); + setMask = portMask; + } + + int symbol_index; + + for (symbol_index = 0; symbol_index < MOTOR_DSHOT_FRAME_BITS; symbol_index++) { + buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 0] |= setMask ; // Always set all ports + buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 1] = 0; // Reset bits are port dependent + buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 2] |= resetMask; // Always reset all ports + } + + // + // output one more 'bit' that keeps the line level at idle to allow the ESC to sample the last bit + // + // Avoid CRC errors in the case of bi-directional d-shot. CRC errors can occur if the output is + // transitioned to an input before the signal has been sampled by the ESC as the sampled voltage + // may be somewhere between logic-high and logic-low depending on how the motor output line is + // driven or floating. On some MCUs it's observed that the voltage momentarily drops low on transition + // to input. + + int hold_bit_index = MOTOR_DSHOT_FRAME_BITS * MOTOR_DSHOT_STATE_PER_SYMBOL; + buffer[hold_bit_index + 0] |= resetMask; // Always reset all ports + buffer[hold_bit_index + 1] = 0; // Never any change + buffer[hold_bit_index + 2] = 0; // Never any change +} + +static void bbOutputDataSet(uint32_t *buffer, int pinNumber, uint16_t value, bool inverted) +{ + uint32_t middleBit; + + if (inverted) { + middleBit = (1 << (pinNumber + 0)); + } else { + middleBit = (1 << (pinNumber + 16)); + } + + for (int pos = 0; pos < 16; pos++) { + if (!(value & 0x8000)) { + buffer[pos * 3 + 1] |= middleBit; + } + value <<= 1; + } +} + +static void bbOutputDataClear(uint32_t *buffer) +{ + // Middle position to no change + for (int bitpos = 0; bitpos < 16; bitpos++) { + buffer[bitpos * 3 + 1] = 0; + } +} + +// bbPacer management + +static bbPacer_t *bbFindMotorPacer(TIM_TypeDef *tim) +{ + for (int i = 0; i < MAX_MOTOR_PACERS; i++) { + + bbPacer_t *bbPacer = &bbPacers[i]; + + if (bbPacer->tim == NULL) { + bbPacer->tim = tim; + ++usedMotorPacers; + return bbPacer; + } + + if (bbPacer->tim == tim) { + return bbPacer; + } + } + + return NULL; +} + +// bbPort management + +static bbPort_t *bbFindMotorPort(int portIndex) +{ + for (int i = 0; i < usedMotorPorts; i++) { + if (bbPorts[i].portIndex == portIndex) { + return &bbPorts[i]; + } + } + return NULL; +} + +static bbPort_t *bbAllocateMotorPort(int portIndex) +{ + if (usedMotorPorts >= MAX_SUPPORTED_MOTOR_PORTS) { + bbStatus = DSHOT_BITBANG_STATUS_TOO_MANY_PORTS; + return NULL; + } + + bbPort_t *bbPort = &bbPorts[usedMotorPorts]; + + if (!bbPort->timhw) { + // No more pacer channel available + bbStatus = DSHOT_BITBANG_STATUS_NO_PACER; + return NULL; + } + + bbPort->portIndex = portIndex; + bbPort->owner.owner = OWNER_DSHOT_BITBANG; + bbPort->owner.resourceIndex = RESOURCE_INDEX(portIndex); + + ++usedMotorPorts; + + return bbPort; +} + +const timerHardware_t *dshotBitbangTimerGetAllocatedByNumberAndChannel(int8_t timerNumber, uint16_t timerChannel) +{ + for (int index = 0; index < usedMotorPorts; index++) { + const timerHardware_t *bitbangTimer = bbPorts[index].timhw; + if (bitbangTimer && timerGetTIMNumber(bitbangTimer->tim) == timerNumber && bitbangTimer->channel == timerChannel && bbPorts[index].owner.owner) { + return bitbangTimer; + } + } + + return NULL; +} + +const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer) +{ + for (int index = 0; index < usedMotorPorts; index++) { + const timerHardware_t *bitbangTimer = bbPorts[index].timhw; + if (bitbangTimer && bitbangTimer == timer) { + return &bbPorts[index].owner; + } + } + + return &freeOwner; +} + +// Return frequency of smallest change [state/sec] + +static uint32_t getDshotBaseFrequency(motorProtocolTypes_e pwmProtocolType) +{ + switch (pwmProtocolType) { + case(MOTOR_PROTOCOL_DSHOT600): + return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; + case(MOTOR_PROTOCOL_DSHOT300): + return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; + default: + case(MOTOR_PROTOCOL_DSHOT150): + return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; + } + + return 0; +} + +static void bbSetupDma(bbPort_t *bbPort) +{ + const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(bbPort->dmaResource); + dmaEnable(dmaIdentifier); + bbPort->dmaSource = timerDmaSource(bbPort->timhw->channel); + + bbPacer_t *bbPacer = bbFindMotorPacer(bbPort->timhw->tim); + bbPacer->dmaSources |= bbPort->dmaSource; + + dmaSetHandler(dmaIdentifier, bbDMAIrqHandler, NVIC_BUILD_PRIORITY(2, 1), (uint32_t)bbPort); + + bbDMA_ITConfig(bbPort); +} + +FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor) +{ + dbgPinHi(0); + + bbPort_t *bbPort = (bbPort_t *)descriptor->userParam; + + bbDMA_Cmd(bbPort, DISABLE); + + bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, DISABLE); + + if (DMA_GET_FLAG_STATUS(descriptor, DMA_INT_FLAG_TAE)) { + while (1) {}; + } + + DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_FTF); + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) { + bbPort->telemetryPending = false; +#ifdef DEBUG_COUNT_INTERRUPT + bbPort->inputIrq++; +#endif + // Disable DMA as telemetry reception is complete + bbDMA_Cmd(bbPort, DISABLE); + } else { +#ifdef DEBUG_COUNT_INTERRUPT + bbPort->outputIrq++; +#endif + + // Switch to input + + bbSwitchToInput(bbPort); + bbPort->telemetryPending = true; + + bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE); + } + } +#endif + dbgPinLo(0); +} + +// Setup bbPorts array elements so that they each have a TIM1 or TIM8 channel +// in timerHardware array for BB-DShot. + +static void bbFindPacerTimer(void) +{ + for (int bbPortIndex = 0; bbPortIndex < MAX_SUPPORTED_MOTOR_PORTS; bbPortIndex++) { + for (unsigned timerIndex = 0; timerIndex < ARRAYLEN(bbTimerHardware); timerIndex++) { + const timerHardware_t *timer = &bbTimerHardware[timerIndex]; + int timNumber = timerGetTIMNumber(timer->tim); + if ((motorConfig()->dev.useDshotBitbangedTimer == DSHOT_BITBANGED_TIMER_TIM1 && timNumber != 1) + || (motorConfig()->dev.useDshotBitbangedTimer == DSHOT_BITBANGED_TIMER_TIM8 && timNumber != 8)) { + continue; + } + bool timerConflict = false; + for (int channel = 0; channel < CC_CHANNELS_PER_TIMER; channel++) { + const timerHardware_t *timer = timerGetAllocatedByNumberAndChannel(timNumber, CC_CHANNEL_FROM_INDEX(channel)); + const resourceOwner_e timerOwner = timerGetOwner(timer)->owner; + if (timerOwner != OWNER_FREE && timerOwner != OWNER_DSHOT_BITBANG) { + timerConflict = true; + break; + } + } + + for (int index = 0; index < bbPortIndex; index++) { + const timerHardware_t* t = bbPorts[index].timhw; + if (timerGetTIMNumber(t->tim) == timNumber && timer->channel == t->channel) { + timerConflict = true; + break; + } + } + + if (timerConflict) { + continue; + } + +#ifdef USE_DMA_SPEC + dmaoptValue_t dmaopt = dmaGetOptionByTimer(timer); + const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timer->tim, timer->channel, dmaopt); + dmaResource_t *dma = dmaChannelSpec->ref; +#else + dmaResource_t *dma = timer->dmaRef; +#endif + dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dma); + if (dmaGetOwner(dmaIdentifier)->owner == OWNER_FREE) { + bbPorts[bbPortIndex].timhw = timer; + + break; + } + } + } +} + +static void bbTimebaseSetup(bbPort_t *bbPort, motorProtocolTypes_e dshotProtocolType) +{ + uint32_t timerclock = timerClock(bbPort->timhw->tim); + + uint32_t outputFreq = getDshotBaseFrequency(dshotProtocolType); + dshotFrameUs = 1000000 * 17 * 3 / outputFreq; + bbPort->outputARR = timerclock / outputFreq - 1; + + // XXX Explain this formula + uint32_t inputFreq = outputFreq * 5 * 2 * DSHOT_BITBANG_TELEMETRY_OVER_SAMPLE / 24; + bbPort->inputARR = timerclock / inputFreq - 1; +} + +// +// bb only use pin info associated with timerHardware entry designated as TIM_USE_MOTOR; +// it does not use the timer channel associated with the pin. +// + +static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output) +{ + // Return if no GPIO is specified + if (!io) { + return false; + } + + int pinIndex = IO_GPIOPinIdx(io); + int portIndex = IO_GPIOPortIdx(io); + + bbPort_t *bbPort = bbFindMotorPort(portIndex); + + if (!bbPort) { + + // New port group + + bbPort = bbAllocateMotorPort(portIndex); + + if (bbPort) { + const timerHardware_t *timhw = bbPort->timhw; + +#ifdef USE_DMA_SPEC + const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timhw->tim, timhw->channel, dmaGetOptionByTimer(timhw)); + bbPort->dmaResource = dmaChannelSpec->ref; + bbPort->dmaChannel = dmaChannelSpec->channel; +#else + bbPort->dmaResource = timhw->dmaRef; + bbPort->dmaChannel = timhw->dmaChannel; +#endif + } + + if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) { + return false; + } + + bbPort->gpio = IO_GPIO(io); + + bbPort->portOutputCount = MOTOR_DSHOT_BUF_LENGTH; + bbPort->portOutputBuffer = &bbOutputBuffer[(bbPort - bbPorts) * MOTOR_DSHOT_BUF_CACHE_ALIGN_LENGTH]; + + bbPort->portInputCount = DSHOT_BB_PORT_IP_BUF_LENGTH; + bbPort->portInputBuffer = &bbInputBuffer[(bbPort - bbPorts) * DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_LENGTH]; + + bbTimebaseSetup(bbPort, pwmProtocolType); + bbTIM_TimeBaseInit(bbPort, bbPort->outputARR); + bbTimerChannelInit(bbPort); + + bbSetupDma(bbPort); + bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_OUTPUT); + bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_INPUT); + + bbDMA_ITConfig(bbPort); + } + + bbMotors[motorIndex].pinIndex = pinIndex; + bbMotors[motorIndex].io = io; + bbMotors[motorIndex].output = output; + bbMotors[motorIndex].bbPort = bbPort; + + IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); + + // Setup GPIO_MODER and GPIO_ODR register manipulation values + + bbGpioSetup(&bbMotors[motorIndex]); + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_INVERTED); + } else +#endif + { + bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_NONINVERTED); + } + + bbSwitchToOutput(bbPort); + + bbMotors[motorIndex].configured = true; + + return true; +} + +static bool bbTelemetryWait(void) +{ + // Wait for telemetry reception to complete + bool telemetryPending; + bool telemetryWait = false; + const timeUs_t startTimeUs = micros(); + + do { + telemetryPending = false; + for (int i = 0; i < usedMotorPorts; i++) { + telemetryPending |= bbPorts[i].telemetryPending; + } + + telemetryWait |= telemetryPending; + + if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) { + break; + } + } while (telemetryPending); + + if (telemetryWait) { + DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1); + } + + return telemetryWait; +} + +static void bbUpdateInit(void) +{ + for (int i = 0; i < usedMotorPorts; i++) { + bbOutputDataClear(bbPorts[i].portOutputBuffer); + } +} + +static bool bbDecodeTelemetry(void) +{ +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { +#ifdef USE_DSHOT_TELEMETRY_STATS + const timeMs_t currentTimeMs = millis(); +#endif + +#ifdef USE_DSHOT_CACHE_MGMT + for (int i = 0; i < usedMotorPorts; i++) { + bbPort_t *bbPort = &bbPorts[i]; + SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES); + } +#endif + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { +#ifdef GD32F4 + uint32_t rawValue = decode_bb_bitband( + bbMotors[motorIndex].bbPort->portInputBuffer, + bbMotors[motorIndex].bbPort->portInputCount, + bbMotors[motorIndex].pinIndex); +#else + uint32_t rawValue = decode_bb( + bbMotors[motorIndex].bbPort->portInputBuffer, + bbMotors[motorIndex].bbPort->portInputCount, + bbMotors[motorIndex].pinIndex); +#endif + if (rawValue == DSHOT_TELEMETRY_NOEDGE) { + DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1); + continue; + } + DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1); + dshotTelemetryState.readCount++; + + if (rawValue != DSHOT_TELEMETRY_INVALID) { + // Check EDT enable or store raw value + if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) { + dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS; + } else { + dshotTelemetryState.motorState[motorIndex].rawValue = rawValue; + } + } else { + dshotTelemetryState.invalidPacketCount++; + } +#ifdef USE_DSHOT_TELEMETRY_STATS + updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs); +#endif + } + + dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED; + } +#endif + + return true; +} + +static void bbWriteInt(uint8_t motorIndex, uint16_t value) +{ + bbMotor_t *const bbmotor = &bbMotors[motorIndex]; + + if (!bbmotor->configured) { + return; + } + + // If there is a command ready to go overwrite the value and send that instead + if (dshotCommandIsProcessing()) { + value = dshotCommandGetCurrent(motorIndex); + if (value) { + bbmotor->protocolControl.requestTelemetry = true; + } + } + + bbmotor->protocolControl.value = value; + + uint16_t packet = prepareDshotPacket(&bbmotor->protocolControl); + + bbPort_t *bbPort = bbmotor->bbPort; + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_INVERTED); + } else +#endif + { + bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_NONINVERTED); + } +} + +static void bbWrite(uint8_t motorIndex, float value) +{ + bbWriteInt(motorIndex, lrintf(value)); +} + +static void bbUpdateComplete(void) +{ + // If there is a dshot command loaded up, time it correctly with motor update + + if (!dshotCommandQueueEmpty()) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { + return; + } + } + + for (int i = 0; i < usedMotorPorts; i++) { + bbPort_t *bbPort = &bbPorts[i]; +#ifdef USE_DSHOT_CACHE_MGMT + SCB_CleanDCache_by_Addr(bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES); +#endif + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) { + bbPort->inputActive = false; + bbSwitchToOutput(bbPort); + } + } else +#endif + { + // Nothing to do + } + + bbDMA_Cmd(bbPort, ENABLE); + } + + lastSendUs = micros(); + for (int i = 0; i < usedMotorPacers; i++) { + bbPacer_t *bbPacer = &bbPacers[i]; + bbTIM_DMACmd(bbPacer->tim, bbPacer->dmaSources, ENABLE); + } +} + +static bool bbEnableMotors(void) +{ + for (int i = 0; i < dshotMotorCount; i++) { + if (bbMotors[i].configured) { + IOConfigGPIO(bbMotors[i].io, bbMotors[i].iocfg); + } + } + return true; +} + +static void bbDisableMotors(void) +{ + return; +} + +static void bbShutdown(void) +{ + return; +} + +static bool bbIsMotorEnabled(unsigned index) +{ + return bbMotors[index].enabled; +} + +static void bbPostInit(void) +{ + bbFindPacerTimer(); + + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { + + if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { + return; + } + + bbMotors[motorIndex].enabled = true; + } +} + +static const motorVTable_t bbVTable = { + .postInit = bbPostInit, + .enable = bbEnableMotors, + .disable = bbDisableMotors, + .isMotorEnabled = bbIsMotorEnabled, + .telemetryWait = bbTelemetryWait, + .decodeTelemetry = bbDecodeTelemetry, + .updateInit = bbUpdateInit, + .write = bbWrite, + .writeInt = bbWriteInt, + .updateComplete = bbUpdateComplete, + .convertExternalToMotor = dshotConvertFromExternal, + .convertMotorToExternal = dshotConvertToExternal, + .shutdown = bbShutdown, + .isMotorIdle = bbDshotIsMotorIdle, + .requestTelemetry = bbDshotRequestTelemetry, + .getMotorIO = bbGetMotorIO, +}; + +dshotBitbangStatus_e dshotBitbangGetStatus(void) +{ + return bbStatus; +} + +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) +{ + dbgPinLo(0); + dbgPinLo(1); + + if (!device || !motorConfig) { + return false; + } + + motorProtocol = motorConfig->motorProtocol; + device->vTable = &bbVTable; + + dshotMotorCount = device->count; + bbStatus = DSHOT_BITBANG_STATUS_OK; + +#ifdef USE_DSHOT_TELEMETRY + useDshotTelemetry = motorConfig->useDshotTelemetry; +#endif + + memset(bbOutputBuffer, 0, sizeof(bbOutputBuffer)); + + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { + const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; + const timerHardware_t *timerHardware = timerGetConfiguredByTag(motorConfig->ioTags[reorderedMotorIndex]); + const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]); + + uint8_t output = motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output; + bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP; + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + output ^= TIMER_OUTPUT_INVERTED; + } +#endif + + if (!IOIsFreeOrPreinit(io)) { + /* not enough motors initialised for the mixer or a break in the motors */ + device->vTable = NULL; + dshotMotorCount = 0; + bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT; + return false; + } + + int pinIndex = IO_GPIOPinIdx(io); + + bbMotors[motorIndex].pinIndex = pinIndex; + bbMotors[motorIndex].io = io; + bbMotors[motorIndex].output = output; + + bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_MODE_OUTPUT, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, bbPuPdMode); + + IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); + IOConfigGPIO(io, bbMotors[motorIndex].iocfg); + if (output & TIMER_OUTPUT_INVERTED) { + IOLo(io); + } else { + IOHi(io); + } + } + + return true; +} + +#endif // USE_DSHOT_BB diff --git a/src/platform/GD32/dshot_bitbang_stdperiph.c b/src/platform/GD32/dshot_bitbang_stdperiph.c new file mode 100755 index 0000000000..e7e72bed37 --- /dev/null +++ b/src/platform/GD32/dshot_bitbang_stdperiph.c @@ -0,0 +1,296 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_DSHOT_BITBANG + +#include "build/atomic.h" +#include "build/debug.h" +#include "build/debug_pin.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/dma.h" +#include "drivers/dma_reqmap.h" +#include "drivers/dshot.h" +#include "dshot_bitbang_impl.h" +#include "drivers/dshot_command.h" +#include "drivers/motor.h" +#include "drivers/nvic.h" +#include "drivers/time.h" +#include "drivers/timer.h" +#include "pg/motor.h" + +void bbGpioSetup(bbMotor_t *bbMotor) +{ + bbPort_t *bbPort = bbMotor->bbPort; + int pinIndex = bbMotor->pinIndex; + + bbPort->gpioModeMask |= (CTL_CLTR(3) << (pinIndex * 2)); + bbPort->gpioModeInput |= (GPIO_MODE_INPUT << (pinIndex * 2)); + bbPort->gpioModeOutput |= (GPIO_MODE_OUTPUT << (pinIndex * 2)); + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + bbPort->gpioIdleBSRR |= (1 << pinIndex); // BS (lower half) + } else +#endif + { + bbPort->gpioIdleBSRR |= (1 << (pinIndex + 16)); // BR (higher half) + } + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + IOWrite(bbMotor->io, 1); + } else +#endif + { + IOWrite(bbMotor->io, 0); + } +} + +void bbTimerChannelInit(bbPort_t *bbPort) +{ + const timerHardware_t *timhw = bbPort->timhw; + + timer_oc_parameter_struct timer_ocintpara; + + timer_channel_output_struct_para_init(&timer_ocintpara); + + timer_channel_output_mode_config((uint32_t)(timhw->tim), timhw->channel, TIMER_OC_MODE_PWM0); + + timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_HIGH; + timer_ocintpara.outputstate = TIMER_CCX_ENABLE; + timer_ocintpara.ocpolarity = TIMER_OC_POLARITY_LOW; + + timer_channel_output_pulse_value_config((uint32_t)(timhw->tim), timhw->channel, 10); + + timer_disable((uint32_t)(bbPort->timhw->tim)); + + timerOCInit(timhw->tim, timhw->channel, &timer_ocintpara); + timerOCModeConfig(timhw->tim, timhw->channel, TIMER_OC_MODE_PWM0); + +#ifdef DEBUG_MONITOR_PACER + if (timhw->tag) { + IO_t io = IOGetByTag(timhw->tag); + IOConfigGPIOAF(io, IOCFG_AF_PP, timhw->alternateFunction); + IOInit(io, OWNER_DSHOT_BITBANG, 0); + timer_primary_output_config((uint32_t)(timhw->tim), ENABLE); + } +#endif + + // Enable and keep it running + + timer_enable((uint32_t)(bbPort->timhw->tim)); +} + +#ifdef USE_DMA_REGISTER_CACHE + +static void bbLoadDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache) +{ + uint32_t dma_periph; + int dma_channel; + gd32_dma_chbase_parse((uint32_t)dmaResource, &dma_periph, &dma_channel); + + DMA_CHCTL(dma_periph, dma_channel) = dmaRegCache->CHCTL; + DMA_CHCNT(dma_periph, dma_channel) = dmaRegCache->CHCNT; + DMA_CHPADDR(dma_periph, dma_channel) = dmaRegCache->CHPADDR; + DMA_CHM0ADDR(dma_periph, dma_channel) = dmaRegCache->CHM0ADDR; + DMA_CHFCTL(dma_periph, dma_channel) = dmaRegCache->CHFCTL; +} + +static void bbSaveDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache) +{ + uint32_t dma_periph; + int dma_channel; + gd32_dma_chbase_parse((uint32_t)dmaResource, &dma_periph, &dma_channel); + + dmaRegCache->CHCTL = DMA_CHCTL(dma_periph, dma_channel); + dmaRegCache->CHCNT = DMA_CHCNT(dma_periph, dma_channel); + dmaRegCache->CHPADDR = DMA_CHPADDR(dma_periph, dma_channel); + dmaRegCache->CHM0ADDR = DMA_CHM0ADDR(dma_periph, dma_channel); + dmaRegCache->CHFCTL = DMA_CHFCTL(dma_periph, dma_channel); +} +#endif + +void bbSwitchToOutput(bbPort_t * bbPort) +{ + dbgPinHi(1); + // Output idle level before switching to output + // Use BOP register for this + // Normal: Use CR (higher half) + // Inverted: Use BOP (lower half) + + WRITE_REG(GPIO_BOP((uint32_t)bbPort->gpio), bbPort->gpioIdleBSRR); + + // Set GPIO to output + ATOMIC_BLOCK(NVIC_PRIO_TIMER) { + MODIFY_REG(GPIO_CTL((uint32_t)bbPort->gpio), bbPort->gpioModeMask, bbPort->gpioModeOutput); + } + + // Reinitialize port group DMA for output + + dmaResource_t *dmaResource = bbPort->dmaResource; +#ifdef USE_DMA_REGISTER_CACHE + bbLoadDMARegs(dmaResource, &bbPort->dmaRegOutput); +#else + xDMA_DeInit(dmaResource); + xDMA_Init(dmaResource, &bbPort->outputDmaInit); + // Needs this, as it is DeInit'ed above... + xDMA_ITConfig(dmaResource, DMA_INT_FTF, ENABLE); +#endif + + // Reinitialize pacer timer for output + + TIMER_CAR((uint32_t)(bbPort->timhw->tim)) = bbPort->outputARR; + + bbPort->direction = DSHOT_BITBANG_DIRECTION_OUTPUT; + + dbgPinLo(1); +} + +#ifdef USE_DSHOT_TELEMETRY +void bbSwitchToInput(bbPort_t *bbPort) +{ + dbgPinHi(1); + + // Set GPIO to input + + ATOMIC_BLOCK(NVIC_PRIO_TIMER) { + MODIFY_REG(GPIO_CTL((uint32_t)bbPort->gpio), bbPort->gpioModeMask, bbPort->gpioModeInput); + } + + // Reinitialize port group DMA for input + + dmaResource_t *dmaResource = bbPort->dmaResource; +#ifdef USE_DMA_REGISTER_CACHE + bbLoadDMARegs(dmaResource, &bbPort->dmaRegInput); +#else + xDMA_DeInit(dmaResource); + xDMA_Init(dmaResource, &bbPort->inputDmaInit); + // Needs this, as it is DeInit'ed above... + xDMA_ITConfig(dmaResource, DMA_INT_FTF, ENABLE); +#endif + + // Reinitialize pacer timer for input + + TIMER_CNT((uint32_t)(bbPort->timhw->tim)) = 0; + TIMER_CAR((uint32_t)(bbPort->timhw->tim)) = bbPort->inputARR; + + bbDMA_Cmd(bbPort, ENABLE); + + bbPort->direction = DSHOT_BITBANG_DIRECTION_INPUT; + + dbgPinLo(1); +} +#endif + +void bbDMAPreconfigure(bbPort_t *bbPort, uint8_t direction) +{ + DMA_InitTypeDef *gener_dmainit = (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) ? &bbPort->outputDmaInit : &bbPort->inputDmaInit; + gener_dmainit->data_mode = DMA_DATA_MODE_MULTI; + dma_multi_data_parameter_struct *dmainit = &gener_dmainit->config.init_struct_m; + + dma_multi_data_para_struct_init(dmainit); + + gener_dmainit->sub_periph = bbPort->dmaChannel; + + dmainit->circular_mode = DMA_CIRCULAR_MODE_DISABLE; + dmainit->periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dmainit->memory_inc = DMA_MEMORY_INCREASE_ENABLE; + dmainit->critical_value = DMA_FIFO_STATUS_1_WORD; + dmainit->memory_burst_width = DMA_MEMORY_BURST_SINGLE; + dmainit->periph_burst_width = DMA_PERIPH_BURST_SINGLE; + + if (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) { + dmainit->priority = DMA_PRIORITY_HIGH; + dmainit->direction = DMA_MEMORY_TO_PERIPH; + dmainit->number = bbPort->portOutputCount; + dmainit->periph_addr = (uint32_t)&GPIO_BOP((uint32_t)bbPort->gpio); + dmainit->periph_width = DMA_PERIPH_WIDTH_32BIT; + dmainit->memory0_addr = (uint32_t)bbPort->portOutputBuffer; + dmainit->memory_width = DMA_MEMORY_WIDTH_32BIT; + +#ifdef USE_DMA_REGISTER_CACHE + xDMA_Init(bbPort->dmaResource, gener_dmainit); + bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegOutput); +#endif + } else { + dmainit->priority = DMA_PRIORITY_ULTRA_HIGH; + dmainit->direction = DMA_PERIPH_TO_MEMORY; + dmainit->number = bbPort->portInputCount; + dmainit->periph_addr = (uint32_t)&GPIO_ISTAT((uint32_t)bbPort->gpio); + dmainit->periph_width = DMA_PERIPH_WIDTH_16BIT; + dmainit->memory0_addr = (uint32_t)bbPort->portInputBuffer; + dmainit->memory_width = DMA_MEMORY_WIDTH_32BIT; + +#ifdef USE_DMA_REGISTER_CACHE + xDMA_Init(bbPort->dmaResource, gener_dmainit); + bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegInput); +#endif + } +} + +void bbTIM_TimeBaseInit(bbPort_t *bbPort, uint16_t period) +{ + timer_parameter_struct *init = &bbPort->timeBaseInit; + + init->prescaler = 0; // Feed raw timerClock + init->clockdivision = TIMER_CKDIV_DIV1; + init->alignedmode = TIMER_COUNTER_EDGE; + init->counterdirection = TIMER_COUNTER_UP; + init->period = period; + init->repetitioncounter = 0; + + timer_init((uint32_t)(bbPort->timhw->tim), init); + timer_auto_reload_shadow_enable((uint32_t)(bbPort->timhw->tim)); +} + +void bbTIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState) +{ + if(ENABLE == NewState){ + timer_dma_enable((uint32_t)TIMx, TIM_DMASource); + } else { + timer_dma_disable((uint32_t)TIMx, TIM_DMASource); + } +} + +void bbDMA_ITConfig(bbPort_t *bbPort) +{ + xDMA_ITConfig(bbPort->dmaResource, DMA_INT_FTF, ENABLE); +} + +void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState) +{ + xDMA_Cmd(bbPort->dmaResource, NewState); +} + +int bbDMA_Count(bbPort_t *bbPort) +{ + return xDMA_GetCurrDataCounter(bbPort->dmaResource); +} + +#endif // USE_DSHOT_BB diff --git a/src/platform/GD32/exti_gd32.c b/src/platform/GD32/exti_gd32.c new file mode 100755 index 0000000000..69189a0254 --- /dev/null +++ b/src/platform/GD32/exti_gd32.c @@ -0,0 +1,188 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_EXTI + +#include "drivers/nvic.h" +#include "drivers/io_impl.h" +#include "drivers/exti.h" +#include "platform/rcc.h" + +typedef struct { + extiCallbackRec_t* handler; +} extiChannelRec_t; + +extiChannelRec_t extiChannelRecs[16]; + +// IRQ grouping +#define EXTI_IRQ_GROUPS 7 +// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 +static const uint8_t extiGroups[16] = { 0, 1, 2, 3, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6 }; +static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS]; + +#if defined(GD32F4) +static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { + EXTI0_IRQn, + EXTI1_IRQn, + EXTI2_IRQn, + EXTI3_IRQn, + EXTI4_IRQn, + EXTI5_9_IRQn, + EXTI10_15_IRQn +}; +#else +# warning "Unknown MCU" +#endif + +static uint32_t triggerLookupTable[] = { + [BETAFLIGHT_EXTI_TRIGGER_RISING] = EXTI_TRIG_RISING, + [BETAFLIGHT_EXTI_TRIGGER_FALLING] = EXTI_TRIG_FALLING, + [BETAFLIGHT_EXTI_TRIGGER_BOTH] = EXTI_TRIG_BOTH +}; + +void EXTIInit(void) +{ + /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ + RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); + + memset(extiChannelRecs, 0, sizeof(extiChannelRecs)); + memset(extiGroupPriority, 0xff, sizeof(extiGroupPriority)); +} + +void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn) +{ + self->fn = fn; +} + +void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config, extiTrigger_t trigger) +{ + int chIdx = IO_GPIOPinIdx(io); + + if (chIdx < 0) { + return; + } + + int group = extiGroups[chIdx]; + + extiChannelRec_t *rec = &extiChannelRecs[chIdx]; + rec->handler = cb; + + EXTIDisable(io); + + IOConfigGPIO(io, config); + + syscfg_exti_line_config(IO_GPIO_PortSource(io), IO_GPIO_PinSource(io)); + + uint32_t extiLine = IO_EXTI_Line(io); + + exti_init(extiLine, EXTI_INTERRUPT, triggerLookupTable[trigger]); + + if (extiGroupPriority[group] > irqPriority) { + extiGroupPriority[group] = irqPriority; + nvic_irq_enable(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); + } +} + +void EXTIRelease(IO_t io) +{ + // don't forget to match cleanup with config + EXTIDisable(io); + + const int chIdx = IO_GPIOPinIdx(io); + + if (chIdx < 0) { + return; + } + + extiChannelRec_t *rec = &extiChannelRecs[chIdx]; + rec->handler = NULL; +} + +void EXTIEnable(IO_t io) +{ +#if defined(GD32F4) + uint32_t extiLine = IO_EXTI_Line(io); + + if (!extiLine) { + return; + } + + EXTI_INTEN |= extiLine; +#else +# error "Unknown MCU" +#endif +} + +void EXTIDisable(IO_t io) +{ + (void) io; +#if defined(GD32F4) + uint32_t extiLine = IO_EXTI_Line(io); + + if (!extiLine) { + return; + } + + EXTI_INTEN &= ~extiLine; + EXTI_PD = extiLine; +#else +# error "Unknown MCU" +#endif +} + +#define EXTI_EVENT_MASK 0xFFFF // first 16 bits only, see also definition of extiChannelRecs. + +static void EXTI_IRQHandler(uint32_t mask) +{ + uint32_t exti_active = (EXTI_INTEN & EXTI_PD) & mask; + + EXTI_PD = exti_active; // clear pending mask (by writing 1) + + while (exti_active) { + unsigned idx = 31 - __builtin_clz(exti_active); + uint32_t mask = 1 << idx; + extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler); + exti_active &= ~mask; + } +} + +#define _EXTI_IRQ_HANDLER(name, mask) \ + void name(void) { \ + EXTI_IRQHandler(mask & EXTI_EVENT_MASK); \ + } \ + struct dummy \ + /**/ + +_EXTI_IRQ_HANDLER(EXTI0_IRQHandler, 0x0001); +_EXTI_IRQ_HANDLER(EXTI1_IRQHandler, 0x0002); +_EXTI_IRQ_HANDLER(EXTI2_IRQHandler, 0x0004); +_EXTI_IRQ_HANDLER(EXTI3_IRQHandler, 0x0008); +_EXTI_IRQ_HANDLER(EXTI4_IRQHandler, 0x0010); +_EXTI_IRQ_HANDLER(EXTI5_9_IRQHandler, 0x03e0); +_EXTI_IRQ_HANDLER(EXTI10_15_IRQHandler, 0xfc00); + +#endif diff --git a/src/platform/GD32/include/platform/dma.h b/src/platform/GD32/include/platform/dma.h new file mode 100755 index 0000000000..8f8be008cb --- /dev/null +++ b/src/platform/GD32/include/platform/dma.h @@ -0,0 +1,114 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#include "platform.h" +#include "drivers/resource.h" + +#if defined(GD32F4) +#define PLATFORM_TRAIT_DMA_STREAM_REQUIRED 1 +#endif + +typedef enum { + DMA_NONE = 0, + DMA_FIRST_HANDLER = 1, + DMA0_CH0_HANDLER = DMA_FIRST_HANDLER, + DMA0_CH1_HANDLER, + DMA0_CH2_HANDLER, + DMA0_CH3_HANDLER, + DMA0_CH4_HANDLER, + DMA0_CH5_HANDLER, + DMA0_CH6_HANDLER, + DMA0_CH7_HANDLER, + DMA1_CH0_HANDLER, + DMA1_CH1_HANDLER, + DMA1_CH2_HANDLER, + DMA1_CH3_HANDLER, + DMA1_CH4_HANDLER, + DMA1_CH5_HANDLER, + DMA1_CH6_HANDLER, + DMA1_CH7_HANDLER, + DMA_LAST_HANDLER = DMA1_CH7_HANDLER +} dmaIdentifier_e; + +/* DMA general initialize struct */ +typedef struct +{ + uint32_t single_mode; /* single or multiple mode */ + int sub_periph; /* specify DMA channel peripheral */ + void * general_init_s; /* single or multiple struct */ +} dma_general_init_struct; + + +uint32_t dmaGetChannel(const uint8_t channel); + +#define DMA_DEVICE_NO(x) ((((x)-1) / 8) + 1) +#define DMA_DEVICE_INDEX(x) ((((x)-1) % 8)) +#define DMA_OUTPUT_INDEX 0 +#define DMA_OUTPUT_STRING "DMA%d Channel %d:" +#define DMA_INPUT_STRING "DMA%d_CH%d" + +#define DEFINE_DMA_CHANNEL(d, s, f) { \ + .dma = (void *)d, \ + .ref = (dmaResource_t *)d ## _CH ## s ## _BASE, \ + .stream = s, \ + .irqHandlerCallback = NULL, \ + .flagsShift = f, \ + .irqN = d ## _Channel ## s ## _IRQn, \ + .userParam = 0, \ + .owner.owner = 0, \ + .owner.resourceIndex = 0 \ + } + +#define DEFINE_DMA_IRQ_HANDLER(d, s, i) FAST_IRQ_HANDLER void DMA ## d ## _Channel ## s ## _IRQHandler(void) {\ + const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \ + dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \ + if (handler) \ + handler(&dmaDescriptors[index]); \ + } + +#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) DMA_INTC1((uint32_t)d->dma) = (flag << (d->flagsShift - 32)); else DMA_INTC0((uint32_t)d->dma) = (flag << d->flagsShift) +#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? DMA_INTF1((uint32_t)d->dma) & (flag << (d->flagsShift - 32)): DMA_INTF0((uint32_t)d->dma) & (flag << d->flagsShift)) + +extern void gd32_dma_init(uint32_t dma_chan_base, dma_single_data_parameter_struct *init_struct); +extern void gd32_dma_general_init(uint32_t dma_chan_base, dma_general_config_struct *init_struct); +extern void gd32_dma_deinit(uint32_t dma_chan_base); +extern void gd32_dma_channel_state_config(uint32_t dma_chan_base, ControlStatus new_state); +extern void gd32_dma_int_config(uint32_t dma_chan_base, uint32_t source, ControlStatus new_state); +extern void gd32_dma_transnum_config(uint32_t dma_chan_base, uint32_t number); +extern uint16_t gd32_dma_transnum_get(uint32_t dma_chan_base); +extern FlagStatus gd32_dma_flags_get(uint32_t dma_chan_base, uint32_t flag); +extern void gd32_dma_chbase_parse(uint32_t dma_chan_base, uint32_t *dma_periph, int *dma_channel); +extern void gd32_dma_memory_addr_config(uint32_t dma_chan_base, uint32_t address, uint8_t memory_flag); + +#define xDMA_Init(dmaResource, initStruct) gd32_dma_general_init((uint32_t)dmaResource, initStruct) +#define xDMA_DeInit(dmaResource) gd32_dma_deinit((uint32_t)dmaResource) +#define xDMA_Cmd(dmaResource, newState) gd32_dma_channel_state_config((uint32_t)dmaResource, newState) +#define xDMA_ITConfig(dmaResource, flags, newState) gd32_dma_int_config((uint32_t)(dmaResource), flags, newState) +#define xDMA_GetCurrDataCounter(dmaResource) gd32_dma_transnum_get((uint32_t)(dmaResource)) +#define xDMA_SetCurrDataCounter(dmaResource, count) gd32_dma_transnum_config((uint32_t)(dmaResource), count) +#define xDMA_GetFlagStatus(dmaResource, flags) gd32_dma_flags_get((uint32_t)(dmaResource), flags) +#define xDMA_ClearFlag(dmaResource, flags) gd32_dma_flags_clear((uint32_t)(dmaResource), flags) +#define xDMA_MemoryTargetConfig(dmaResource, address, target) gd32_dma_memory_addr_config((uint32_t)(dmaResource), address, target) + +extern uint32_t dma_enable_status_get(uint32_t dma_chan_base); +#define IS_DMA_ENABLED(reg) (dma_enable_status_get((uint32_t)reg)) diff --git a/src/platform/GD32/include/platform/platform.h b/src/platform/GD32/include/platform/platform.h new file mode 100755 index 0000000000..654a637bf4 --- /dev/null +++ b/src/platform/GD32/include/platform/platform.h @@ -0,0 +1,260 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#if defined(GD32F405) || defined(GD32F407) || defined(GD32F425) || defined(GD32F427) || defined(GD32F450) || defined(GD32F460) || defined(GD32F470) + +#include "gd32f4xx.h" + +// Chip Unique ID on F4xx +#define U_ID_0 (*(uint32_t*)0x1fff7a10) +#define U_ID_1 (*(uint32_t*)0x1fff7a14) +#define U_ID_2 (*(uint32_t*)0x1fff7a18) + +#ifndef GD32F4 +#define GD32F4 +#endif + +#endif + +#ifdef GD32F4 + +/* DMA data mode type */ +typedef enum { + DMA_DATA_MODE_SINGLE = 0, + DMA_DATA_MODE_MULTI = 1 +} dma_data_mode_enum; + +/* DMA general configuration struct */ +typedef struct +{ + dma_data_mode_enum data_mode; + dma_subperipheral_enum sub_periph; + union { + dma_single_data_parameter_struct init_struct_s; + dma_multi_data_parameter_struct init_struct_m; + } config; +} dma_general_config_struct; //todo: move to dma_bsp.h + +#endif // GD32F4 + +#ifdef GD32F4 + +#define SPI_TRAIT_AF_PORT 1 +#define SPI_TRAIT_AF_PIN 1 +#define I2C_TRAIT_STATE 1 +#define I2C_TRAIT_AF_PIN 1 +#define I2CDEV_COUNT 3 +#define PLATFORM_TRAIT_RCC 1 +#define UART_TRAIT_AF_PORT 1 +#define UART_TRAIT_AF_PIN 1 +#define UART_TRAIT_PINSWAP 1 +#define SERIAL_TRAIT_PIN_CONFIG 1 +#define DMA_TRAIT_CHANNEL 1 + +#define USE_FAST_DATA +#define USE_RPM_FILTER +#define USE_DYN_IDLE +#define USE_DYN_NOTCH_FILTER +#define USE_ADC_INTERNAL +#define USE_USB_CDC_HID +#define USE_USB_MSC +#define USE_PERSISTENT_MSC_RTC +#define USE_MCO +#define USE_DMA_SPEC +#define USE_PERSISTENT_OBJECTS +#define USE_LATE_TASK_STATISTICS + +#define SET_BIT(REG, BIT) ((REG) |= (BIT)) +#define CLEAR_BIT(REG, BIT) ((REG) &= ~(BIT)) +#define READ_BIT(REG, BIT) ((REG) & (BIT)) +#define CLEAR_REG(REG) ((REG) = (0x0)) +#define WRITE_REG(REG, VAL) ((REG) = (VAL)) +#define READ_REG(REG) ((REG)) +#define MODIFY_REG(REG, CLEARMASK, SETMASK) WRITE_REG((REG), (((READ_REG(REG)) & (~(CLEARMASK))) | (SETMASK))) + +#define I2C_TypeDef void +#define I2C_HandleTypeDef void +#define GPIO_TypeDef void +#define GPIO_InitTypeDef void +#define TIM_TypeDef void +#define DMA_TypeDef void +#define DMA_Stream_TypeDef void + +#define DMA_InitTypeDef dma_general_config_struct + +#define DMA_Channel_TypeDef void +#define SPI_TypeDef void +#define ADC_TypeDef void +#define USART_TypeDef void +#define TIM_OCInitTypeDef timer_oc_parameter_struct +#define TIM_ICInitTypeDef timer_ic_parameter_struct +#define TIM_OCStructInit timer_channel_output_struct_para_init +#define TIM_Cmd void +#define TIM_CtrlPWMOutputs void +#define TIM_TimeBaseInit void +#define TIM_TimeBaseInitTypeDef timer_parameter_struct +#define TIM_ARRPreloadConfig void +#define EXTI_TypeDef void +#define EXTI_InitTypeDef void +#define USART_TypeDef void + +#define Bit_RESET 0 + +#define DMA0_CH0_BASE (DMA0 + 0x10) +#define DMA0_CH1_BASE (DMA0 + 0x28) +#define DMA0_CH2_BASE (DMA0 + 0x40) +#define DMA0_CH3_BASE (DMA0 + 0x58) +#define DMA0_CH4_BASE (DMA0 + 0x70) +#define DMA0_CH5_BASE (DMA0 + 0x88) +#define DMA0_CH6_BASE (DMA0 + 0xA0) +#define DMA0_CH7_BASE (DMA0 + 0xB8) +#define DMA1_CH0_BASE (DMA1 + 0x10) +#define DMA1_CH1_BASE (DMA1 + 0x28) +#define DMA1_CH2_BASE (DMA1 + 0x40) +#define DMA1_CH3_BASE (DMA1 + 0x58) +#define DMA1_CH4_BASE (DMA1 + 0x70) +#define DMA1_CH5_BASE (DMA1 + 0x88) +#define DMA1_CH6_BASE (DMA1 + 0xA0) +#define DMA1_CH7_BASE (DMA1 + 0xB8) + +extern void timerOCModeConfig(void *tim, uint8_t channel, uint16_t ocmode); +extern void gd32_timer_input_capture_config(void* timer, uint16_t channel, uint8_t state); + +#define UART_TX_BUFFER_ATTRIBUTE /* EMPTY */ +#define UART_RX_BUFFER_ATTRIBUTE /* EMPTY */ + +#endif // GD32F4 + +#define USE_ADC_DEVICE_0 + +#define GPIOA_BASE GPIOA + +#if defined(GD32F4) +#define TASK_GYROPID_DESIRED_PERIOD 125 // 125us = 8kHz +#define SCHEDULER_DELAY_LIMIT 10 +#else +#define TASK_GYROPID_DESIRED_PERIOD 1000 // 1000us = 1kHz +#define SCHEDULER_DELAY_LIMIT 100 +#endif + +#define DEFAULT_CPU_OVERCLOCK 0 + +#if defined(GD32F4) +#define FAST_IRQ_HANDLER +#endif + +#if defined(GD32F4) +// F4 can't DMA to/from TCM (tightly-coupled memory) SRAM (where the stack lives) +#define DMA_DATA_ZERO_INIT +#define DMA_DATA +#define STATIC_DMA_DATA_AUTO static +#endif + +#if defined(GD32F4) +// Data in RAM which is guaranteed to not be reset on hot reboot +#define PERSISTENT __attribute__ ((section(".persistent_data"), aligned(4))) +#endif + +#ifdef USE_DMA_RAM +// Reserved for other GD series +#else +#define DMA_RAM +#define DMA_RW_AXI +#define DMA_RAM_R +#define DMA_RAM_W +#define DMA_RAM_RW + +#endif // USE_DMA_RAM + +#define USE_TIMER_MGMT +#define USE_TIMER_AF + +#if defined(GD32F4) + +#define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5)) + +#define IOCFG_OUT_PP IO_CONFIG(GPIO_MODE_OUTPUT, 0, GPIO_OTYPE_PP, GPIO_PUPD_NONE) // TODO +#define IOCFG_OUT_PP_UP IO_CONFIG(GPIO_MODE_OUTPUT, 0, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP) +#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_MODE_OUTPUT, GPIO_OSPEED_25MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE) +#define IOCFG_OUT_OD IO_CONFIG(GPIO_MODE_OUTPUT, 0, GPIO_OTYPE_OD, GPIO_PUPD_NONE) +#define IOCFG_AF_PP IO_CONFIG(GPIO_MODE_AF, 0, GPIO_OTYPE_PP, GPIO_PUPD_NONE) +#define IOCFG_AF_PP_PD IO_CONFIG(GPIO_MODE_AF, 0, GPIO_OTYPE_PP, GPIO_PUPD_PULLDOWN) +#define IOCFG_AF_PP_UP IO_CONFIG(GPIO_MODE_AF, 0, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP) +#define IOCFG_AF_OD IO_CONFIG(GPIO_MODE_AF, 0, GPIO_OTYPE_PP, GPIO_PUPD_NONE) +#define IOCFG_IPD IO_CONFIG(GPIO_MODE_INPUT, 0, 0, GPIO_PUPD_PULLDOWN) +#define IOCFG_IPU IO_CONFIG(GPIO_MODE_INPUT, 0, 0, GPIO_PUPD_PULLUP) +#define IOCFG_IN_FLOATING IO_CONFIG(GPIO_MODE_INPUT, 0, 0, GPIO_PUPD_NONE) +#define IOCFG_IPU_25 IO_CONFIG(GPIO_MODE_INPUT, GPIO_OSPEED_25MHZ, 0, GPIO_PUPD_PULLUP) + +#endif + +#if defined(GD32F4) +#define FLASH_CONFIG_BUFFER_TYPE uint32_t +#endif + +#if defined(GD32F4) +#define SPI_IO_AF_CFG IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE) +#define SPI_IO_AF_SCK_CFG IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLDOWN) +#define SPI_IO_AF_SDI_CFG IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP) +#define SPI_IO_CS_CFG IO_CONFIG(GPIO_MODE_OUTPUT, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE) +#define SPI_IO_CS_HIGH_CFG IO_CONFIG(GPIO_MODE_INPUT, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP) +#else +#error "Invalid GD32 MCU defined - requires SPI implementation" +#endif + +#if defined(GD32F4) +#if defined(GD32F425) +#define SPIDEV_COUNT 3 +#elif defined(GD32F460) +#define SPIDEV_COUNT 5 +#endif +#else +#define SPIDEV_COUNT 4 +#endif + +#define SPI_RX_DATA_REGISTER(base) ((base)->DR) + +#if defined(GD32F4) +#define MAX_SPI_PIN_SEL 3 +#else +#error Unknown MCU family +#endif + +#if defined(GD32F4) +#define USE_TX_IRQ_HANDLER +#endif + +// all pins on given uart use same AF + + +#if defined(GD32F4) +#define UARTHARDWARE_MAX_PINS 4 +#endif + +#if defined(GD32F4) +#define UART_REG_RXD(base) (USART_DATA((uint32_t)base)) +#define UART_REG_TXD(base) (USART_DATA((uint32_t)base)) +#endif + +#define SERIAL_TRAIT_PIN_CONFIG 1 +#define USB_DP_PIN PA12 diff --git a/src/platform/GD32/io_gd32.c b/src/platform/GD32/io_gd32.c new file mode 100755 index 0000000000..1c0b76857c --- /dev/null +++ b/src/platform/GD32/io_gd32.c @@ -0,0 +1,148 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "platform/rcc.h" + +#include "common/utils.h" + +// io ports defs are stored in array by index now +struct ioPortDef_s { + rccPeriphTag_t rcc; +}; + +#if defined(GD32F4) +const struct ioPortDef_s ioPortDefs[] = { + { RCC_AHB1(PA) }, + { RCC_AHB1(PB) }, + { RCC_AHB1(PC) }, + { RCC_AHB1(PD) }, + { RCC_AHB1(PE) }, + { RCC_AHB1(PF) }, +}; +#else +# error "IO PortDefs not defined for MCU" +#endif + +uint32_t IO_EXTI_Line(IO_t io) +{ + if (!io) { + return 0; + } +#if defined(GD32F4) + return 1 << IO_GPIOPinIdx(io); +#elif defined(SIMULATOR_BUILD) + return 0; +#else +# error "Unknown target type" +#endif + return 0; +} + +bool IORead(IO_t io) +{ + if (!io) { + return false; + } + + return (GPIO_ISTAT((uint32_t)IO_GPIO(io)) & IO_Pin(io)); +} + +void IOWrite(IO_t io, bool hi) +{ + if (!io) { + return; + } + + if (hi) { + GPIO_BOP((uint32_t)IO_GPIO(io)) = IO_Pin(io); + } else { + GPIO_BC((uint32_t)IO_GPIO(io)) = IO_Pin(io); + } +} + +void IOHi(IO_t io) +{ + if (!io) { + return; + } + + GPIO_BOP((uint32_t)IO_GPIO(io)) = IO_Pin(io); +} + +void IOLo(IO_t io) +{ + if (!io) { + return; + } +#if defined(GD32F4) + GPIO_BC((uint32_t)IO_GPIO(io)) = IO_Pin(io); +#endif +} + +void IOToggle(IO_t io) +{ + if (!io) { + return; + } + + uint32_t mask = IO_Pin(io); + // For GD32F4,use toggle register to toggle GPIO pin status +#if defined(GD32F4) + GPIO_TG((uint32_t)IO_GPIO(io)) = mask; +#endif +} + +#if defined(GD32F4) + +void IOConfigGPIO(IO_t io, ioConfig_t cfg) +{ + if (!io) { + return; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + RCC_ClockCmd(rcc, ENABLE); + + gpio_mode_set((uint32_t)IO_GPIO(io), ((cfg >> 0) & 0x03), ((cfg >> 5) & 0x03), IO_Pin(io)); + gpio_output_options_set((uint32_t)IO_GPIO(io), ((cfg >> 4) & 0x01), ((cfg >> 2) & 0x03), IO_Pin(io)); +} + +void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) +{ + if (!io) { + return; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + RCC_ClockCmd(rcc, ENABLE); + + gpio_af_set((uint32_t)IO_GPIO(io), af, IO_Pin(io)); + gpio_mode_set((uint32_t)IO_GPIO(io), ((cfg >> 0) & 0x03), ((cfg >> 5) & 0x03), IO_Pin(io)); + gpio_output_options_set((uint32_t)IO_GPIO(io), ((cfg >> 4) & 0x01), ((cfg >> 2) & 0x03), IO_Pin(io)); +} + +#else +# warning MCU not set +#endif diff --git a/src/platform/GD32/light_ws2811strip_stdperiph.c b/src/platform/GD32/light_ws2811strip_stdperiph.c new file mode 100755 index 0000000000..e99c9a20d8 --- /dev/null +++ b/src/platform/GD32/light_ws2811strip_stdperiph.c @@ -0,0 +1,223 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_LED_STRIP + +#include "build/debug.h" + +#include "common/color.h" + +#include "drivers/dma.h" +#include "drivers/dma_reqmap.h" +#include "drivers/io.h" +#include "drivers/nvic.h" +#include "platform/rcc.h" +#include "drivers/timer.h" + +#include "drivers/light_ws2811strip.h" + +static IO_t ws2811IO = IO_NONE; +#if defined(GD32F4) +static dmaResource_t *dmaRef = NULL; +#else +#error "No MCU definition in light_ws2811strip_stdperiph.c" +#endif +static TIM_TypeDef *timer = NULL; + +static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) +{ +#if defined(USE_WS2811_SINGLE_COLOUR) + static uint32_t counter = 0; +#endif + + if (DMA_GET_FLAG_STATUS(descriptor, DMA_INT_FLAG_FTF)) { +#if defined(USE_WS2811_SINGLE_COLOUR) + counter++; + if (counter == WS2811_LED_STRIP_LENGTH) { + // Output low for 50us delay + memset(ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); + } else if (counter == (WS2811_LED_STRIP_LENGTH + WS2811_DELAY_ITERATIONS)) { + counter = 0; + ws2811LedDataTransferInProgress = false; + xDMA_Cmd(descriptor->ref, DISABLE); + } +#else + ws2811LedDataTransferInProgress = false; + xDMA_Cmd(descriptor->ref, DISABLE); +#endif + + DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_FTF); + } +} + +bool ws2811LedStripHardwareInit(ioTag_t ioTag) +{ + if (!ioTag) { + return false; + } + + timer_parameter_struct timer_initpara; + timer_oc_parameter_struct timer_ocintpara; + dma_single_data_parameter_struct dma_init_struct; + + const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_LED_STRIP, 0); + + if (timerHardware == NULL) { + return false; + } + + timer = timerHardware->tim; + +#if defined(USE_DMA_SPEC) + const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware); + + if (dmaSpec == NULL) { + return false; + } + + dmaRef = dmaSpec->ref; +#if defined(GD32F4) + uint32_t dmaChannel = dmaSpec->channel; +#endif +#else + dmaRef = timerHardware->dmaRef; +#if defined(GD32F4) + uint32_t dmaChannel = timerHardware->dmaChannel; +#endif +#endif + + if (dmaRef == NULL || !dmaAllocate(dmaGetIdentifier(dmaRef), OWNER_LED_STRIP, 0)) { + return false; + } + + ws2811IO = IOGetByTag(ioTag); + IOInit(ws2811IO, OWNER_LED_STRIP, 0); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP), timerHardware->alternateFunction); + + RCC_ClockCmd(timerRCC(timer), ENABLE); + + // Stop timer + timer_disable((uint32_t)timer); + + /* Compute the prescaler value */ + uint16_t prescaler = timerGetPrescalerByDesiredMhz(timer, WS2811_TIMER_MHZ); + uint16_t period = timerGetPeriodByPrescaler(timer, prescaler, WS2811_CARRIER_HZ); + + BIT_COMPARE_1 = period / 3 * 2; + BIT_COMPARE_0 = period / 3; + + /* Time base configuration */ + timer_struct_para_init(&timer_initpara); + timer_initpara.period = period; // 800kHz + timer_initpara.prescaler = prescaler; + timer_initpara.clockdivision = TIMER_CKDIV_DIV1; + timer_initpara.alignedmode = TIMER_COUNTER_EDGE; + timer_initpara.counterdirection = TIMER_COUNTER_UP; + timer_initpara.repetitioncounter = 0; + timer_init((uint32_t)timer, &timer_initpara); + + /* PWM1 Mode configuration */ + timer_channel_output_struct_para_init(&timer_ocintpara); + timer_channel_output_mode_config((uint32_t)timer, timerHardware->channel, TIMER_OC_MODE_PWM0); + + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + timer_ocintpara.outputnstate = TIMER_CCXN_ENABLE; + timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; + timer_ocintpara.ocnpolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIMER_OCN_IDLE_STATE_LOW : TIMER_OCN_POLARITY_HIGH; + } else { + timer_ocintpara.outputstate = TIMER_CCX_ENABLE; + timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_HIGH; + timer_ocintpara.ocpolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIMER_OC_IDLE_STATE_LOW : TIMER_OC_POLARITY_HIGH; + } + + timer_channel_output_pulse_value_config((uint32_t)timer, timerHardware->channel, 0); + + timerOCInit(timer, timerHardware->channel, &timer_ocintpara); + timerOCModeConfig(timer, timerHardware->channel, TIMER_OC_MODE_PWM0); + timerOCPreloadConfig(timer, timerHardware->channel, TIMER_OC_SHADOW_ENABLE); + + timer_primary_output_config((uint32_t)timer, ENABLE); + timer_auto_reload_shadow_enable((uint32_t)timer); + + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + timer_channel_complementary_output_state_config((uint32_t)timer, timerHardware->channel, TIMER_CCXN_ENABLE); + } else { + timer_channel_output_state_config((uint32_t)timer, timerHardware->channel, TIMER_CCX_ENABLE); + } + + timer_enable((uint32_t)timer); + + dmaEnable(dmaGetIdentifier(dmaRef)); + dmaSetHandler(dmaGetIdentifier(dmaRef), WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + + xDMA_DeInit(dmaRef); + + /* configure DMA */ + xDMA_Cmd(dmaRef, DISABLE); + xDMA_DeInit(dmaRef); + dma_single_data_para_struct_init(&dma_init_struct); + dma_init_struct.periph_addr = (uint32_t)timerCCR(timer, timerHardware->channel); + dma_init_struct.number = WS2811_DMA_BUFFER_SIZE; + dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + +#if defined(GD32F4) + + uint32_t temp_dma_periph; + int temp_dma_channel; + + gd32_dma_chbase_parse((uint32_t)dmaRef, &temp_dma_periph, &temp_dma_channel); + + dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, dmaChannel); + + dma_init_struct.memory0_addr = (uint32_t)ledStripDMABuffer; + dma_init_struct.direction = DMA_MEMORY_TO_PERIPH; + dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_32BIT; + dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; +#endif + +#if defined(USE_WS2811_SINGLE_COLOUR) + dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_ENABLE; +#else + dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_DISABLE; +#endif + + gd32_dma_init((uint32_t)dmaRef, &dma_init_struct); + timer_dma_enable((uint32_t)timer, timerDmaSource(timerHardware->channel)); + xDMA_ITConfig(dmaRef, DMA_INT_FTF, ENABLE); + + return true; +} + +void ws2811LedStripDMAEnable(void) +{ + xDMA_SetCurrDataCounter(dmaRef, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + timer_counter_value_config((uint32_t)timer, 0); + timer_enable((uint32_t)timer); + xDMA_Cmd(dmaRef, ENABLE); +} +#endif diff --git a/src/platform/GD32/link/gd32_flash_f4_split.ld b/src/platform/GD32/link/gd32_flash_f4_split.ld new file mode 100755 index 0000000000..b4d7ce5201 --- /dev/null +++ b/src/platform/GD32/link/gd32_flash_f4_split.ld @@ -0,0 +1,217 @@ +/* +***************************************************************************** +** +** File : gd32_flash_f4_split.ld +** +** Abstract : Common linker script for GD32 devices. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_Hot_Reboot_Flags_Size = 16; +_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM) - _Hot_Reboot_Flags_Size; /* end of RAM */ + +/* Base address where the config is stored. */ +__config_start = ORIGIN(FLASH_CONFIG); +__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG); + +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0x200; /* required amount of heap */ +_Min_Stack_Size = 0x800; /* required amount of stack */ + +/* Define output sections */ +SECTIONS +{ + /* + * The ISR vector table is loaded at the beginning of the FLASH, + * But it is linked (space reserved) at the beginning of the VECTAB region, + * which is aliased either to FLASH or RAM. + * When linked to RAM, the table can optionally be copied from FLASH to RAM + * for table relocation. + */ + + _isr_vector_table_flash_base = LOADADDR(.vectors); + PROVIDE (isr_vector_table_flash_base = _isr_vector_table_flash_base); + + .vectors : + { + . = ALIGN(4); + PROVIDE (isr_vector_table_base = .); + KEEP(*(.vectors)) /* Startup code */ + . = ALIGN(4); + PROVIDE (isr_vector_table_end = .); + } >VECTAB AT> FLASH + + /* System memory (read-only bootloader) interrupt vector */ + .system_isr_vector (NOLOAD) : + { + . = ALIGN(4); + PROVIDE (system_isr_vector_table_base = .); + KEEP(*(.system_isr_vector)) /* Bootloader code */ + . = ALIGN(4); + } >SYSTEM_MEMORY + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH1 + + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >MOVABLE_FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >MOVABLE_FLASH + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >MOVABLE_FLASH + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(.fini_array*)) + KEEP (*(SORT(.fini_array.*))) + PROVIDE_HIDDEN (__fini_array_end = .); + } >MOVABLE_FLASH + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >MOVABLE_FLASH + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH1 + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT> MOVABLE_FLASH + + /* Uninitialized data section */ + . = ALIGN(4); + .bss (NOLOAD) : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(SORT_BY_ALIGNMENT(.bss*)) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* used during startup to initialized fastram_data */ + _sfastram_idata = LOADADDR(.fastram_data); + + /* Initialized FAST_DATA section for unsuspecting developers */ + .fastram_data : + { + . = ALIGN(4); + _sfastram_data = .; /* create a global symbol at data start */ + *(.fastram_data) /* .data sections */ + *(.fastram_data*) /* .data* sections */ + + . = ALIGN(4); + _efastram_data = .; /* define a global symbol at data end */ + } >FASTRAM AT> FLASH1 + + . = ALIGN(4); + .fastram_bss (NOLOAD) : + { + __fastram_bss_start__ = .; + *(.fastram_bss) + *(SORT_BY_ALIGNMENT(.fastram_bss*)) + . = ALIGN(4); + __fastram_bss_end__ = .; + } >FASTRAM + + .persistent_data (NOLOAD) : + { + __persistent_data_start__ = .; + *(.persistent_data) + . = ALIGN(4); + __persistent_data_end__ = .; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + _heap_stack_end = ORIGIN(STACKRAM) + LENGTH(STACKRAM) - _Hot_Reboot_Flags_Size; + _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; + . = _heap_stack_begin; + ._user_heap_stack : + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(4); + } >STACKRAM = 0xa5 + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + libnosys.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/src/platform/GD32/link/gd32f405_425xg_flash.ld b/src/platform/GD32/link/gd32f405_425xg_flash.ld new file mode 100755 index 0000000000..b82de71162 --- /dev/null +++ b/src/platform/GD32/link/gd32f405_425xg_flash.ld @@ -0,0 +1,44 @@ +/* +***************************************************************************** +** +** File : gd32f405_425xg_flash.ld +** +** Abstract : Linker script for GD32F405_425xG Device with +** 1024KByte FLASH, 256KByte SRAM (that include 64K TCM SRAM) +** +** Target : GD32F405_425xG +** +** Environment : Arm gcc toolchain +** +***************************************************************************** +*/ + +/* +0x08000000 to 0x080FFFFF 1024K full flash, +0x08000000 to 0x08003FFF 16K isr vector, startup code, +0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 +0x08008000 to 0x080FFFFF 992K firmware, +*/ + +/* memory map */ +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K + FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 992K + + SYSTEM_MEMORY (rx) : ORIGIN = 0x1FFF0000, LENGTH = 29K + + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 192K + TCMRAM (rw) : ORIGIN = 0x10000000, LENGTH = 64K + BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +REGION_ALIAS("STACKRAM", TCMRAM) +REGION_ALIAS("FASTRAM", TCMRAM) +REGION_ALIAS("VECTAB", RAM) + +REGION_ALIAS("MOVABLE_FLASH", FLASH1) + +INCLUDE "gd32_flash_f4_split.ld" diff --git a/src/platform/GD32/link/gd32f460xg_flash.ld b/src/platform/GD32/link/gd32f460xg_flash.ld new file mode 100755 index 0000000000..7b00ae0a95 --- /dev/null +++ b/src/platform/GD32/link/gd32f460xg_flash.ld @@ -0,0 +1,44 @@ +/* +***************************************************************************** +** +** File : gd32_flash_f460xg.ld +** +** Abstract : Linker script for GD32F460xG Device with +** 1024KByte FLASH, 512KByte SRAM (that include 64K TCM SRAM) +** +** Target : GD32F460xg +** +** Environment : Arm gcc toolchain +** +***************************************************************************** +*/ + +/* +0x08000000 to 0x080FFFFF 1024K full flash, +0x08000000 to 0x08003FFF 16K isr vector, startup code, +0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 +0x08008000 to 0x080FFFFF 992K firmware, +*/ + +/* memory map */ +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K + FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 992K + + SYSTEM_MEMORY (rx) : ORIGIN = 0x1FFF0000, LENGTH = 29K + + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 448K + TCMRAM (rw) : ORIGIN = 0x10000000, LENGTH = 64K + BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +REGION_ALIAS("STACKRAM", TCMRAM) +REGION_ALIAS("FASTRAM", TCMRAM) +REGION_ALIAS("VECTAB", RAM) + +REGION_ALIAS("MOVABLE_FLASH", FLASH1) + +INCLUDE "gd32_flash_f4_split.ld" diff --git a/src/platform/GD32/mk/GD32F4.mk b/src/platform/GD32/mk/GD32F4.mk new file mode 100755 index 0000000000..4002ab691d --- /dev/null +++ b/src/platform/GD32/mk/GD32F4.mk @@ -0,0 +1,236 @@ +# +# GD32F4 Make file include +# + +CMSIS_DIR := $(LIB_MAIN_DIR)/CMSIS +STDPERIPH_DIR = $(LIB_MAIN_DIR)/GD32F4/Drivers/GD32F4xx_standard_peripheral +USB_LIB_DIR := $(LIB_MAIN_DIR)/GD32F4/Middlewares/GD32F4xx_usb_library + +STDPERIPH_SRC = \ + gd32f4xx_adc.c \ + gd32f4xx_dac.c \ + gd32f4xx_dci.c \ + gd32f4xx_dma.c \ + gd32f4xx_exti.c \ + gd32f4xx_fmc.c \ + gd32f4xx_fwdgt.c \ + gd32f4xx_gpio.c \ + gd32f4xx_i2c.c \ + gd32f4xx_ipa.c \ + gd32f4xx_misc.c\ + gd32f4xx_pmu.c \ + gd32f4xx_rcu.c \ + gd32f4xx_rtc.c \ + gd32f4xx_sdio.c \ + gd32f4xx_spi.c \ + gd32f4xx_syscfg.c \ + gd32f4xx_timer.c \ + gd32f4xx_tli.c \ + gd32f4xx_trng.c \ + gd32f4xx_usart.c \ + gd32f4xx_wwdgt.c + +VPATH := $(VPATH):$(STDPERIPH_DIR)/Source + +DEVICE_FLAGS = -DGD32F4XX + +ifneq ($(TARGET_MCU),$(filter $(TARGET_MCU),GDF450 GD32F470)) +STDPERIPH_SRC += gd32f4xx_exmc.c +endif + + +USBDEVICE_DIR = $(USB_LIB_DIR)/device + +USBDCORE_DIR = $(USBDEVICE_DIR)/core +USBCORE_SRC = \ + $(USBDCORE_DIR)/Source/usbd_core.c \ + $(USBDCORE_DIR)/Source/usbd_enum.c \ + $(USBDCORE_DIR)/Source/usbd_transc.c + +USBCDC_DIR = $(USBDEVICE_DIR)/class/cdc +USBCDC_SRC = \ + $(USBCDC_DIR)/Source/cdc_acm_core.c + +USBMSC_DIR = $(USBDEVICE_DIR)/class/msc +USBMSC_SRC = \ + $(USBMSC_DIR)/Source/usbd_msc_bbb.c \ + $(USBMSC_DIR)/Source/usbd_msc_core.c \ + $(USBMSC_DIR)/Source/usbd_msc_scsi.c + +USBHID_DIR = $(USBDEVICE_DIR)/class/hid +USBHID_SRC = \ + $(USBHID_DIR)/Source/standard_hid_core.c + + +USBDRV_DIR = $(USB_LIB_DIR)/driver +USBDRV_SRC = \ + $(USB_LIB_DIR)/driver/Source/drv_usb_core.c \ + $(USB_LIB_DIR)/driver/Source/drv_usb_dev.c \ + $(USB_LIB_DIR)/driver/Source/drv_usbd_int.c + + + +USBSTD_DIR = $(USB_LIB_DIR)/ustd + + +DEVICE_STDPERIPH_SRC := \ + $(STDPERIPH_SRC) \ + $(USBOTG_SRC) \ + $(USBCORE_SRC) \ + $(USBCDC_SRC) \ + $(USBHID_SRC) \ + $(USBMSC_SRC) \ + $(USBDRV_SRC) + + +VPATH := $(VPATH):$(CMSIS_DIR)/Core/Include:$(LIB_MAIN_DIR)/GD32F4/Drivers/CMSIS/GD/GD32F4xx/Source + + +CMSIS_SRC := \ + gd32f4xx_gpio.c \ + gd32f4xx_rcu.c + +INCLUDE_DIRS := \ + $(INCLUDE_DIRS) \ + $(TARGET_PLATFORM_DIR) \ + $(TARGET_PLATFORM_DIR)/include \ + $(TARGET_PLATFORM_DIR)/startup \ + $(STDPERIPH_DIR)/Include \ + $(USBDEVICE_DIR)/class/msc/Include \ + $(USBHID_DIR)/Include \ + $(USBCDC_DIR)/Include \ + $(USBMSC_DIR)/Include \ + $(USBDCORE_DIR)/Include \ + $(USBDRV_DIR)/Include \ + $(USBSTD_DIR)/common \ + $(USBSTD_DIR)/class/cdc \ + $(USBSTD_DIR)/class/msc \ + $(USBSTD_DIR)/class/hid \ + $(CMSIS_DIR)/Core/Include \ + $(LIB_MAIN_DIR)/GD32F4/Drivers/CMSIS/GD/GD32F4xx/Include \ + $(TARGET_PLATFORM_DIR)/usb_f4 + +DEVICE_FLAGS += -DUSE_STDPERIPH_DRIVER + + +#Flags +ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 + +ifeq ($(TARGET_MCU),GD32F407xx) +DEVICE_FLAGS += -DGD32F407 +LD_SCRIPT = $(LINKER_DIR)/gd32_flash_f407_425.ld +STARTUP_SRC = GD32/startup/startup_gd32f407_427.s +MCU_FLASH_SIZE := 1024 + +else ifeq ($(TARGET_MCU),GD32F425xx) +DEVICE_FLAGS += -DGD32F425 +LD_SCRIPT = $(LINKER_DIR)/gd32f405_425xg_flash.ld +STARTUP_SRC = GD32/startup/startup_gd32f405_425.S +MCU_FLASH_SIZE := 1024 + +else ifeq ($(TARGET_MCU),GD32F460xg) +DEVICE_FLAGS += -DGD32F460 +LD_SCRIPT = $(LINKER_DIR)/gd32f460xg_flash.ld +STARTUP_SRC = GD32/startup/startup_gd32f460.S +MCU_FLASH_SIZE := 1024 + +else +$(error Unknown MCU for F4 target) +endif +DEVICE_FLAGS += -DHXTAL_VALUE=$(HSE_VALUE) + +DSP_LIB := $(LIB_MAIN_DIR)/CMSIS/DSP +DEVICE_FLAGS += -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE -DARM_MATH_CM4 +DEVICE_FLAGS += -DUSE_GDBSP_DRIVER -DUSE_USB_FS +DEVICE_FLAGS += -DVECT_TAB_SRAM + +MCU_COMMON_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/dshot_bitbang_decode.c \ + drivers/inverter.c \ + GD32/adc_gd32.c \ + GD32/audio_gd32.c \ + GD32/bus_i2c_gd32.c \ + GD32/bus_spi_gd32.c \ + GD32/camera_control_gd32.c \ + GD32/debug.c \ + GD32/dma_gd32.c \ + GD32/dma_reqmap_mcu.c \ + GD32/dshot_bitbang.c \ + GD32/dshot_bitbang_stdperiph.c \ + GD32/exti_gd32.c \ + GD32/io_gd32.c \ + GD32/light_ws2811strip_stdperiph.c \ + GD32/persistent_gd32.c \ + GD32/rcu_gd32.c \ + GD32/sdio_gdf4xx.c \ + GD32/serial_uart_stdperiph.c \ + GD32/serial_uart_gd32f4xx.c \ + GD32/system_gd32f4xx.c \ + GD32/pwm_output_gd32.c \ + GD32/pwm_output_dshot.c \ + GD32/timer_stdperiph.c \ + GD32/timer_gd32f4xx.c \ + GD32/transponder_ir_io_stdperiph.c \ + drivers/adc.c \ + drivers/bus_spi_config.c \ + drivers/serial_escserial.c \ + drivers/serial_pinconfig.c \ + GD32/usb_f4/usbd_msc_desc.c \ + GD32/startup/system_gd32f4xx.c + +VCP_SRC = \ + GD32/usb_f4/gd32f4xx_it.c \ + GD32/usb_f4/usb_bsp.c \ + GD32/usb_f4/usbd_desc.c \ + GD32/usb_f4/usb_cdc_hid.c \ + GD32/usbd_cdc_vcp.c \ + GD32/serial_usb_vcp.c \ + drivers/usb_io.c + +MSC_SRC = \ + drivers/usb_msc_common.c \ + GD32/usb_msc_f4xx.c \ + msc/usbd_storage.c \ + msc/usbd_storage_emfat.c \ + msc/emfat.c \ + msc/emfat_file.c \ + msc/usbd_storage_sd_spi.c \ + msc/usbd_storage_sdio.c + + +INCLUDE_DIRS += $(PLATFORM_DIR)/common/stm32 + +MCU_COMMON_SRC += \ + common/stm32/bus_i2c_pinconfig.c \ + common/stm32/bus_spi_hw.c \ + common/stm32/bus_spi_pinconfig.c \ + common/stm32/config_flash.c \ + common/stm32/dshot_bitbang_shared.c \ + common/stm32/dshot_dpwm.c \ + common/stm32/mco.c \ + common/stm32/io_impl.c \ + common/stm32/pwm_output_beeper.c \ + common/stm32/pwm_output_dshot_shared.c \ + common/stm32/serial_uart_hw.c \ + common/stm32/serial_uart_pinconfig.c \ + common/stm32/system.c + +SPEED_OPTIMISED_SRC += \ + common/stm32/system.c \ + common/stm32/bus_spi_hw.c \ + common/stm32/pwm_output_dshot_shared.c \ + common/stm32/dshot_bitbang_shared.c \ + common/stm32/io_impl.c + +SIZE_OPTIMISED_SRC += \ + GD32/serial_usb_vcp.c \ + drivers/inverter.c \ + drivers/serial_escserial.c \ + drivers/serial_pinconfig.c \ + drivers/bus_spi_config.c \ + common/stm32/bus_i2c_pinconfig.c \ + common/stm32/bus_spi_pinconfig.c \ + common/stm32/config_flash.c \ + common/stm32/pwm_output_beeper.c \ + common/stm32/bus_spi_pinconfig.c diff --git a/src/platform/GD32/persistent_gd32.c b/src/platform/GD32/persistent_gd32.c new file mode 100755 index 0000000000..c4386f11f5 --- /dev/null +++ b/src/platform/GD32/persistent_gd32.c @@ -0,0 +1,85 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +/* + * An implementation of persistent data storage utilizing RTC backup data register. + * Retains values written across software resets and boot loader activities. + */ + +#include +#include "platform.h" + +#include "drivers/persistent.h" +#include "drivers/system.h" + +#define PERSISTENT_OBJECT_MAGIC_VALUE (('B' << 24)|('e' << 16)|('f' << 8)|('1' << 0)) + +uint32_t persistentObjectRead(persistentObjectId_e id) +{ + uint32_t value = REG32((RTC) + 0x50U + (id * 4U)); + + return value; +} + +void persistentObjectWrite(persistentObjectId_e id, uint32_t value) +{ + RTC_WPK = RTC_UNLOCK_KEY1; + RTC_WPK = RTC_UNLOCK_KEY2; + + REG32(RTC + 0x50U + (id * 4U)) = value; + + RTC_WPK = RTC_LOCK_KEY; +} + +void persistentObjectRTCEnable(void) +{ + /* Enable access to the backup domain */ + rcu_periph_clock_enable(RCU_PMU); + pmu_backup_write_enable(); + rcu_periph_clock_enable(RCU_RTC); + + rtc_register_sync_wait(); + + /* enable the write protection */ + RTC_WPK = RTC_LOCK_KEY; + /* disable the write protection */ + RTC_WPK = RTC_UNLOCK_KEY1; + RTC_WPK = RTC_UNLOCK_KEY2; +} + +void persistentObjectInit(void) +{ + // Enable RTC and backup register access + persistentObjectRTCEnable(); + + // Check if the system was a software reset + uint32_t wasSoftReset = RCU_RSTSCK & RCU_RSTSCK_SWRSTF; + + // Check if the magic value is present in the backup register + if (!wasSoftReset || (persistentObjectRead(PERSISTENT_OBJECT_MAGIC) != PERSISTENT_OBJECT_MAGIC_VALUE)) { + // Clear all persistent objects + for (int i = 1; i < PERSISTENT_OBJECT_COUNT; i++) { + persistentObjectWrite(i, 0); + } + // Write the magic value to indicate valid data + persistentObjectWrite(PERSISTENT_OBJECT_MAGIC, PERSISTENT_OBJECT_MAGIC_VALUE); + } +} diff --git a/src/platform/GD32/pwm_output_dshot.c b/src/platform/GD32/pwm_output_dshot.c new file mode 100755 index 0000000000..a7c7f4ba80 --- /dev/null +++ b/src/platform/GD32/pwm_output_dshot.c @@ -0,0 +1,483 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_DSHOT + +#include "build/debug.h" + +#include "drivers/dma.h" +#include "drivers/dma_reqmap.h" +#include "drivers/io.h" +#include "drivers/nvic.h" +#include "platform/rcc.h" +#include "drivers/time.h" +#include "drivers/timer.h" +#include "drivers/system.h" + +#if defined(GD32F4) +#include "gd32f4xx.h" +#endif + +#include "drivers/pwm_output.h" +#include "drivers/dshot.h" +#include "dshot_dpwm.h" +#include "drivers/dshot_command.h" +#include "pwm_output_dshot_shared.h" +#include "drivers/motor.h" + +static uint16_t timer_oc_modes[MAX_SUPPORTED_MOTORS]; +static uint16_t timer_oc_pulses[MAX_SUPPORTED_MOTORS]; + +#ifdef USE_DSHOT_TELEMETRY + +void dshotEnableChannels(unsigned motorCount) +{ + for (unsigned i = 0; i < motorCount; i++) { + if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) { + timer_channel_complementary_output_state_config((uint32_t)(dmaMotors[i].timerHardware->tim), dmaMotors[i].timerHardware->channel, TIMER_CCXN_ENABLE); + } else { + timer_channel_output_state_config((uint32_t)(dmaMotors[i].timerHardware->tim), dmaMotors[i].timerHardware->channel, TIMER_CCX_ENABLE); + } + } +} + +#endif + +FAST_CODE void pwmDshotSetDirectionOutput( + motorDmaOutput_t * const motor +#ifndef USE_DSHOT_TELEMETRY + ,timer_oc_parameter_struct *pOcInit, DMA_InitTypeDef* pGenerDmaInit +#endif +) +{ +#ifdef USE_DSHOT_TELEMETRY + timer_oc_parameter_struct* pOcInit = &motor->ocInitStruct; + DMA_InitTypeDef* pGenerDmaInit = &motor->dmaInitStruct; +#endif + + const timerHardware_t * const timerHardware = motor->timerHardware; + TIM_TypeDef *timer = timerHardware->tim; + + dmaResource_t *dmaRef = motor->dmaRef; + +#if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT_TELEMETRY) + if (useBurstDshot) { + dmaRef = timerHardware->dmaTimUPRef; + } +#endif + + xDMA_DeInit(dmaRef); + +#ifdef USE_DSHOT_TELEMETRY + motor->isInput = false; +#endif + + timerOCPreloadConfig(timer, timerHardware->channel, TIMER_OC_SHADOW_DISABLE); + timerOCInit(timer, timerHardware->channel, pOcInit); + + timerOCModeConfig(timer, timerHardware->channel, timer_oc_modes[motor->index]); + timer_channel_output_pulse_value_config((uint32_t)timer, timerHardware->channel, timer_oc_pulses[motor->index]); + timerOCPreloadConfig(timer, timerHardware->channel, TIMER_OC_SHADOW_ENABLE); + +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + pGenerDmaInit->config.init_struct_m.direction = DMA_MEMORY_TO_PERIPH; + } else +#endif + { + pGenerDmaInit->config.init_struct_s.direction = DMA_MEMORY_TO_PERIPH; + + } + + xDMA_Init(dmaRef, pGenerDmaInit); + + xDMA_ITConfig(dmaRef, DMA_INT_FTF, ENABLE); +} + +#ifdef USE_DSHOT_TELEMETRY +FAST_CODE +static void pwmDshotSetDirectionInput( + motorDmaOutput_t * const motor +) +{ + DMA_InitTypeDef* pGenerDmaInit = &motor->dmaInitStruct; + + const timerHardware_t * const timerHardware = motor->timerHardware; + TIM_TypeDef *timer = timerHardware->tim; + + dmaResource_t *dmaRef = motor->dmaRef; + + xDMA_DeInit(dmaRef); + + motor->isInput = true; + if (!inputStampUs) { + inputStampUs = micros(); + } + + timer_auto_reload_shadow_enable((uint32_t)timer); + TIMER_CAR((uint32_t)timer) = 0xffffffff; + + timer_input_capture_config((uint32_t)timer, timerHardware->channel, &motor->icInitStruct); + + if (useBurstDshot) { + pGenerDmaInit->config.init_struct_m.direction = DMA_PERIPH_TO_MEMORY; + } else { + pGenerDmaInit->config.init_struct_s.direction = DMA_PERIPH_TO_MEMORY; + } + + xDMA_Init(dmaRef, pGenerDmaInit); +} +#endif + +void pwmCompleteDshotMotorUpdate(void) +{ + /* If there is a dshot command loaded up, time it correctly with motor update*/ + if (!dshotCommandQueueEmpty()) { + if (!dshotCommandOutputIsEnabled(pwmMotorCount)) { + return; + } + } + +#ifdef GD32F4 + if (!useBurstDshot) { + for(uint8_t i = 0; i < motorDeviceCount(); i++) { + memmove((uint8_t *)&dshotDmaBuffer[i][2], (uint8_t *)&dshotDmaBuffer[i][1], 60); + } + } +#endif + + for (int i = 0; i < dmaMotorTimerCount; i++) { +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + xDMA_SetCurrDataCounter(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength); + xDMA_Cmd(dmaMotorTimers[i].dmaBurstRef, ENABLE); + timer_dma_transfer_config((uint32_t)(dmaMotorTimers[i].timer), TIMER_DMACFG_DMATA_CH0CV, TIMER_DMACFG_DMATC_4TRANSFER); + timer_dma_enable((uint32_t)(dmaMotorTimers[i].timer), TIMER_DMA_UPD); + } else +#endif + { + timer_counter_value_config((uint32_t)(dmaMotorTimers[i].timer), 0); + timer_dma_enable((uint32_t)(dmaMotorTimers[i].timer), dmaMotorTimers[i].timerDmaSources); + dmaMotorTimers[i].timerDmaSources = 0; + } + } +} + +FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) +{ + if (DMA_GET_FLAG_STATUS(descriptor, DMA_INT_FLAG_FTF)) { + motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam]; +#ifdef USE_DSHOT_TELEMETRY + dshotDMAHandlerCycleCounters.irqAt = getCycleCounter(); +#endif +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE); + timer_dma_disable((uint32_t)(motor->timerHardware->tim), TIMER_DMA_UPD); + } else +#endif + { + xDMA_Cmd(motor->dmaRef, DISABLE); + timer_dma_disable((uint32_t)(motor->timerHardware->tim), motor->timerDmaSource); + } + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + pwmDshotSetDirectionInput(motor); + xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN); + xDMA_Cmd(motor->dmaRef, ENABLE); + timer_dma_enable((uint32_t)(motor->timerHardware->tim), motor->timerDmaSource); + dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter(); + } +#endif + DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_FTF); + DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_HTF); + } +} + +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output) +{ +#ifdef USE_DSHOT_TELEMETRY +#define OCINIT motor->ocInitStruct +#define DMAINIT motor->dmaInitStruct +#else + timer_oc_parameter_struct ocInitStruct; + DMA_InitTypeDef dmaInitStruct; +#define OCINIT ocInitStruct +#define DMAINIT dmaInitStruct +#endif + + dmaResource_t *dmaRef = NULL; +#if defined(GD32F4) + uint32_t dmaChannel = 0; +#endif + +#if defined(USE_DMA_SPEC) + const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware); + + if (dmaSpec != NULL) { + dmaRef = dmaSpec->ref; +#if defined(GD32F4) + dmaChannel = dmaSpec->channel; +#endif + } +#else + dmaRef = timerHardware->dmaRef; +#if defined(GD32F4) + dmaChannel = timerHardware->dmaChannel; +#endif +#endif + +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + dmaRef = timerHardware->dmaTimUPRef; +#if defined(GD32F4) + dmaChannel = timerHardware->dmaTimUPChannel; +#endif + } +#endif + + if (dmaRef == NULL) { + return false; + } + + dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef); + + bool dmaIsConfigured = false; +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier); + if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) { + dmaIsConfigured = true; + } else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) { + return false; + } + } else +#endif + { + if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) { + return false; + } + } + + motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; + + motor->dmaRef = dmaRef; + + TIM_TypeDef *timer = timerHardware->tim; + + // Boolean configureTimer is always true when different channels of the same timer are processed in sequence, + // causing the timer and the associated DMA initialized more than once. + // To fix this, getTimerIndex must be expanded to return if a new timer has been requested. + // However, since the initialization is idempotent, it is left as is in a favor of flash space (for now). + const uint8_t timerIndex = getTimerIndex(timer); + const bool configureTimer = (timerIndex == dmaMotorTimerCount-1); + + motor->timer = &dmaMotorTimers[timerIndex]; + motor->index = motorIndex; + motor->timerHardware = timerHardware; + + const IO_t motorIO = IOGetByTag(timerHardware->tag); + + uint8_t pupMode = 0; + pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PUPD_PULLDOWN : GPIO_PUPD_PULLUP; + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + output ^= TIMER_OUTPUT_INVERTED; + } +#endif + + motor->iocfg = IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, pupMode); + IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction); + + if (configureTimer) { + timer_parameter_struct timer_initpara; + timer_struct_para_init(&timer_initpara); + + RCC_ClockCmd(timerRCC(timer), ENABLE); + timer_disable((uint32_t)timer); + + timer_initpara.period = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; + timer_initpara.prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1); + timer_initpara.clockdivision = TIMER_CKDIV_DIV1; + timer_initpara.alignedmode = TIMER_COUNTER_EDGE; + timer_initpara.counterdirection = TIMER_COUNTER_UP; + timer_initpara.repetitioncounter = 0; + timer_init((uint32_t)timer, &timer_initpara); + } + + timer_channel_output_struct_para_init(&OCINIT); + timer_channel_output_mode_config((uint32_t)timer, timerHardware->channel, TIMER_OC_MODE_PWM0); + timer_oc_modes[motor->index] = TIMER_OC_MODE_PWM0; + + if (output & TIMER_OUTPUT_N_CHANNEL) { + OCINIT.outputnstate = TIMER_CCXN_ENABLE; + OCINIT.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; + OCINIT.ocnpolarity = (output & TIMER_OUTPUT_INVERTED) ? TIMER_OCN_POLARITY_LOW : TIMER_OCN_POLARITY_HIGH; + } else { + OCINIT.outputstate = TIMER_CCX_ENABLE; + OCINIT.ocidlestate = TIMER_OC_IDLE_STATE_HIGH; + OCINIT.ocpolarity = (output & TIMER_OUTPUT_INVERTED) ? TIMER_OC_POLARITY_LOW : TIMER_OC_POLARITY_HIGH; + } + + timer_oc_pulses[motor->index] = 0; + timer_channel_output_pulse_value_config((uint32_t)timer, timerHardware->channel, 0); + +#ifdef USE_DSHOT_TELEMETRY + + timer_channel_input_struct_para_init(&motor->icInitStruct); + + motor->icInitStruct.icselection = TIMER_IC_SELECTION_DIRECTTI; + motor->icInitStruct.icpolarity = TIMER_IC_POLARITY_BOTH_EDGE; + motor->icInitStruct.icprescaler = TIMER_IC_PSC_DIV1; + motor->icInitStruct.icfilter = 2; + timer_input_capture_config((uint32_t)timer, timerHardware->channel, &motor->icInitStruct); // ic结构体中没有channel +#endif + +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + motor->timer->dmaBurstRef = dmaRef; + motor->dmaInitStruct.data_mode = DMA_DATA_MODE_MULTI; + } else +#endif + { + motor->timerDmaSource = timerDmaSource(timerHardware->channel); + motor->timer->timerDmaSources &= ~motor->timerDmaSource; + motor->dmaInitStruct.data_mode = DMA_DATA_MODE_SINGLE; + } + + xDMA_Cmd(dmaRef, DISABLE); + xDMA_DeInit(dmaRef); + + if (!dmaIsConfigured) { + dmaEnable(dmaIdentifier); + } + + if (useBurstDshot) { + dma_multi_data_para_struct_init(&DMAINIT.config.init_struct_m); + DMAINIT.data_mode = DMA_DATA_MODE_MULTI; + } else { + dma_single_data_para_struct_init(&DMAINIT.config.init_struct_s); + DMAINIT.data_mode = DMA_DATA_MODE_SINGLE; + } + + uint32_t temp_dma_periph; + int temp_dma_channel; + + motor->dmaInitStruct.sub_periph = dmaChannel; + +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0]; + + gd32_dma_chbase_parse((uint32_t)dmaRef, &temp_dma_periph, &temp_dma_channel); + + DMAINIT.config.init_struct_m.memory0_addr = (uint32_t)motor->timer->dmaBurstBuffer; + DMAINIT.config.init_struct_m.direction = DMA_MEMORY_TO_PERIPH; + DMAINIT.config.init_struct_m.memory_burst_width = DMA_MEMORY_BURST_SINGLE; + DMAINIT.config.init_struct_m.periph_burst_width = DMA_PERIPH_BURST_SINGLE; + DMAINIT.config.init_struct_m.periph_addr = (uint32_t)&TIMER_DMATB((uint32_t)timerHardware->tim); + DMAINIT.config.init_struct_m.number = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; + DMAINIT.config.init_struct_m.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + DMAINIT.config.init_struct_m.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + DMAINIT.config.init_struct_m.periph_width = DMA_PERIPH_WIDTH_32BIT; + DMAINIT.config.init_struct_m.memory_width = DMA_MEMORY_WIDTH_32BIT; + DMAINIT.config.init_struct_m.circular_mode = DMA_CIRCULAR_MODE_DISABLE; + DMAINIT.config.init_struct_m.priority = DMA_PRIORITY_HIGH; + + } else +#endif + { + motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0]; + + gd32_dma_chbase_parse((uint32_t)dmaRef, &temp_dma_periph, &temp_dma_channel); + + DMAINIT.config.init_struct_s.memory0_addr = (uint32_t)motor->dmaBuffer; + DMAINIT.config.init_struct_s.direction = DMA_MEMORY_TO_PERIPH; + DMAINIT.config.init_struct_s.periph_addr = (uint32_t)timerChCCR(timerHardware); + DMAINIT.config.init_struct_s.number = DSHOT_DMA_BUFFER_SIZE; + DMAINIT.config.init_struct_s.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + DMAINIT.config.init_struct_s.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + DMAINIT.config.init_struct_s.periph_memory_width = DMA_PERIPH_WIDTH_32BIT; + DMAINIT.config.init_struct_s.circular_mode = DMA_CIRCULAR_MODE_DISABLE; + DMAINIT.config.init_struct_s.priority = DMA_PRIORITY_HIGH; + + } + + // XXX Consolidate common settings in the next refactor + + motor->dmaRef = dmaRef; + +#ifdef USE_DSHOT_TELEMETRY + motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * + (16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType); + motor->timer->outputPeriod = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; + pwmDshotSetDirectionOutput(motor); +#else + pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT); +#endif + + dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, dmaChannel); + +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + if (!dmaIsConfigured) { + dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index); + } + } else +#endif + { + dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index); + } + + timer_enable((uint32_t)timer); + + if (output & TIMER_OUTPUT_N_CHANNEL) { + timer_channel_complementary_output_state_config((uint32_t)timer, timerHardware->channel, TIMER_CCXN_ENABLE); + } else { + timer_channel_output_state_config((uint32_t)timer, timerHardware->channel, TIMER_CCX_ENABLE); + } + + if (configureTimer) { + timer_auto_reload_shadow_enable((uint32_t)timer); + timer_primary_output_config((uint32_t)timer, ENABLE); + timer_enable((uint32_t)timer); + } + +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + // avoid high line during startup to prevent bootloader activation + *timerChCCR(timerHardware) = 0xffff; + } +#endif + motor->configured = true; + + return true; +} + +#endif diff --git a/src/platform/GD32/pwm_output_gd32.c b/src/platform/GD32/pwm_output_gd32.c new file mode 100755 index 0000000000..ab7ae130a0 --- /dev/null +++ b/src/platform/GD32/pwm_output_gd32.c @@ -0,0 +1,275 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_PWM_OUTPUT + +#include "drivers/io.h" +#include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" +#include "drivers/time.h" +#include "drivers/timer.h" + +#include "drivers/motor_impl.h" + +static bool useContinuousUpdate = true; + +static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) +{ + timer_oc_parameter_struct timer_ocintpara; + + timer_channel_output_struct_para_init(&timer_ocintpara); + timer_channel_output_mode_config((uint32_t)tim, channel, TIMER_OC_MODE_PWM0); + + if (output & TIMER_OUTPUT_N_CHANNEL) { + timer_ocintpara.outputnstate = TIMER_CCXN_ENABLE; + timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; + timer_ocintpara.ocnpolarity = (output & TIMER_OUTPUT_INVERTED) ? TIMER_OCN_POLARITY_LOW : TIMER_OCN_POLARITY_HIGH; + } else { + timer_ocintpara.outputstate = TIMER_CCX_ENABLE; + timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_HIGH; + timer_ocintpara.ocpolarity = (output & TIMER_OUTPUT_INVERTED) ? TIMER_OC_POLARITY_LOW : TIMER_OC_POLARITY_HIGH; + } + timer_channel_output_pulse_value_config((uint32_t)tim, channel, value); + + timerOCInit(tim, channel, &timer_ocintpara); + timerOCModeConfig(tim, channel, TIMER_OC_MODE_PWM0); + timerOCPreloadConfig(tim, channel, TIMER_OC_SHADOW_ENABLE); +} + +void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion) +{ + configTimeBase(timerHardware->tim, period, hz); + pwmOCConfig(timerHardware->tim, + timerHardware->channel, + value, + inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output + ); + + timer_primary_output_config((uint32_t)(timerHardware->tim), ENABLE); + timer_enable((uint32_t)(timerHardware->tim)); + + channel->ccr = timerChCCR(timerHardware); + channel->tim = timerHardware->tim; + *channel->ccr = 0; +} + +static void pwmWriteStandard(uint8_t index, float value) +{ + /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ + *pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset); +} + +static void pwmShutdownPulsesForAllMotors(void) +{ + for (int index = 0; index < pwmMotorCount; index++) { + // Set the compare register to 0, which stops the output pulsing if the timer overflows + if (pwmMotors[index].channel.ccr) { + *pwmMotors[index].channel.ccr = 0; + } + } +} + +static void pwmDisableMotors(void) +{ + pwmShutdownPulsesForAllMotors(); +} + +static void pwmCompleteMotorUpdate(void) +{ + if (useContinuousUpdate) { + return; + } + + for (int index = 0; index < pwmMotorCount; index++) { + if (pwmMotors[index].forceOverflow) { + timerForceOverflow(pwmMotors[index].channel.tim); + } + // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. + // This compare register will be set to the output value on the next main loop. + *pwmMotors[index].channel.ccr = 0; + } +} + +static float pwmConvertFromExternal(uint16_t externalValue) +{ + return (float)externalValue; +} + +static uint16_t pwmConvertToExternal(float motorValue) +{ + return (uint16_t)motorValue; +} + +static const motorVTable_t motorPwmVTable = { + .postInit = NULL, + .enable = pwmEnableMotors, + .disable = pwmDisableMotors, + .isMotorEnabled = pwmIsMotorEnabled, + .shutdown = pwmShutdownPulsesForAllMotors, + .convertExternalToMotor = pwmConvertFromExternal, + .convertMotorToExternal = pwmConvertToExternal, + .write = pwmWriteStandard, + .decodeTelemetry = NULL, + .updateComplete = pwmCompleteMotorUpdate, + .requestTelemetry = NULL, + .isMotorIdle = NULL, + .getMotorIO = pwmGetMotorIO, +}; + +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) +{ + memset(pwmMotors, 0, sizeof(pwmMotors)); + + pwmMotorCount = device->count; + device->vTable = &motorPwmVTable; + useContinuousUpdate = motorConfig->useContinuousUpdate; + + float sMin = 0; + float sLen = 0; + switch (motorConfig->motorProtocol) { + default: + case MOTOR_PROTOCOL_ONESHOT125: + sMin = 125e-6f; + sLen = 125e-6f; + break; + case MOTOR_PROTOCOL_ONESHOT42: + sMin = 42e-6f; + sLen = 42e-6f; + break; + case MOTOR_PROTOCOL_MULTISHOT: + sMin = 5e-6f; + sLen = 20e-6f; + break; + case MOTOR_PROTOCOL_BRUSHED: + sMin = 0; + useContinuousUpdate = true; + idlePulse = 0; + break; + case MOTOR_PROTOCOL_PWM : + sMin = 1e-3f; + sLen = 1e-3f; + useContinuousUpdate = true; + idlePulse = 0; + break; + } + + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) { + const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; + const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex]; + const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + + if (timerHardware == NULL) { + /* not enough motors initialised for the mixer or a break in the motors */ + device->vTable = NULL; + pwmMotorCount = 0; + /* TODO: block arming and add reason system cannot arm */ + return false; + } + + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + + IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); + + /* standard PWM outputs */ + // margin of safety is 4 periods when unsynced + const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4)); + + const uint32_t clock = timerClock(timerHardware->tim); + /* used to find the desired timer frequency for max resolution */ + const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */ + const uint32_t hz = clock / prescaler; + const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff; + + /* + if brushed then it is the entire length of the period. + TODO: this can be moved back to periodMin and periodLen + once mixer outputs a 0..1 float value. + */ + pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000); + + pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); + + bool timerAlreadyUsed = false; + for (int i = 0; i < motorIndex; i++) { + if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) { + timerAlreadyUsed = true; + break; + } + } + pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed; + pwmMotors[motorIndex].enabled = true; + } + + return true; +} + +pwmOutputPort_t *pwmGetMotors(void) +{ + return pwmMotors; +} + +#ifdef USE_SERVOS +static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; + +void pwmWriteServo(uint8_t index, float value) +{ + if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) { + *servos[index].channel.ccr = lrintf(value); + } +} + +void servoDevInit(const servoDevConfig_t *servoConfig) +{ + for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { + const ioTag_t tag = servoConfig->ioTags[servoIndex]; + + if (!tag) { + continue; + } + + servos[servoIndex].io = IOGetByTag(tag); + + IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex)); + + const timerHardware_t *timer = timerAllocate(tag, OWNER_SERVO, RESOURCE_INDEX(servoIndex)); + + if (timer == NULL) { + /* flag failure and disable ability to arm */ + break; + } + + IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction); + + pwmOutConfig(&servos[servoIndex].channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); + servos[servoIndex].enabled = true; + } +} +#endif // USE_SERVOS +#endif // USE_PWM_OUTPUT diff --git a/src/platform/GD32/rcu_gd32.c b/src/platform/GD32/rcu_gd32.c new file mode 100755 index 0000000000..a4c6fb0eed --- /dev/null +++ b/src/platform/GD32/rcu_gd32.c @@ -0,0 +1,162 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" +#include "platform/rcc.h" + +static void rcu_ahb1_periph_clk_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_AHB1EN |= mask; + } else { + RCU_AHB1EN &= ~mask; + } +} + +static void rcu_ahb2_periph_clk_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_AHB2EN |= mask; + } else { + RCU_AHB2EN &= ~mask; + } +} + +static void rcu_ahb3_periph_clk_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_AHB3EN |= mask; + } else { + RCU_AHB3EN &= ~mask; + } +} + +static void rcu_apb1_periph_clk_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_APB1EN |= mask; + } else { + RCU_APB1EN &= ~mask; + } +} + +static void rcu_apb2_periph_clk_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_APB2EN |= mask; + } else { + RCU_APB2EN &= ~mask; + } +} + +static void rcu_ahb1_periph_rst_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_AHB1RST |= mask; + } else { + RCU_AHB1RST &= ~mask; + } +} + +static void rcu_ahb2_periph_rst_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_AHB2RST |= mask; + } else { + RCU_AHB2RST &= ~mask; + } +} + +static void rcu_ahb3_periph_rst_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_AHB3RST |= mask; + } else { + RCU_AHB3RST &= ~mask; + } +} + +static void rcu_apb1_periph_rst_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_APB1RST |= mask; + } else { + RCU_APB1RST &= ~mask; + } +} + +static void rcu_apb2_periph_rst_config(uint32_t mask, FunctionalState NewState) +{ + if (NewState == ENABLE) { + RCU_APB2RST |= mask; + } else { + RCU_APB2RST &= ~mask; + } +} + + +void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState) +{ + int tag = periphTag >> 5; + uint32_t mask = 1 << (periphTag & 0x1f); + + switch (tag) { + case RCC_AHB1: + rcu_ahb1_periph_clk_config(mask, NewState); + break; + case RCC_AHB2: + rcu_ahb2_periph_clk_config(mask, NewState); + break; + case RCC_AHB3: + rcu_ahb3_periph_clk_config(mask, NewState); + break; + case RCC_APB1: + rcu_apb1_periph_clk_config(mask, NewState); + break; + case RCC_APB2: + rcu_apb2_periph_clk_config(mask, NewState); + break; + } +} + +void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState) +{ + int tag = periphTag >> 5; + uint32_t mask = 1 << (periphTag & 0x1f); + + switch (tag) { + case RCC_AHB1: + rcu_ahb1_periph_rst_config(mask, NewState); + break; + case RCC_AHB2: + rcu_ahb2_periph_rst_config(mask, NewState); + break; + case RCC_AHB3: + rcu_ahb3_periph_rst_config(mask, NewState); + break; + case RCC_APB2: + rcu_apb1_periph_rst_config(mask, NewState); + break; + case RCC_APB1: + rcu_apb2_periph_rst_config(mask, NewState); + break; + } +} diff --git a/src/platform/GD32/sdio_gdf4xx.c b/src/platform/GD32/sdio_gdf4xx.c new file mode 100755 index 0000000000..8d39a16af4 --- /dev/null +++ b/src/platform/GD32/sdio_gdf4xx.c @@ -0,0 +1,1607 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_SDCARD_SDIO + +#include "drivers/sdmmc_sdio.h" +#include "gd32f4xx_gpio.h" +#include "gd32f4xx_sdio.h" + +#include "pg/sdio.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/time.h" +#include "platform/rcc.h" +#include "drivers/dma.h" +#include "drivers/light_led.h" + +#include "build/debug.h" + +#define DMA_INTF0_TAEIF3 ((uint32_t)0x02000000) +#define DMA_INTC0_TAEIFC3 ((uint32_t)0x02000000) +#define DMA_INTF0_FEEIF3 ((uint32_t)0x00400000) +#define DMA_INTC0_FEEIFC3 ((uint32_t)0x00400000) +#define DMA_INTF0_DMEIF3 ((uint32_t)0x01000000) +#define DMA_INTC0_CDMEIF3 ((uint32_t)0x01000000) +#define DMA_INTF0_HTIF3 ((uint32_t)0x04000000) +#define DMA_INTC0_CHTIF3 ((uint32_t)0x04000000) +#define DMA_INTF0_TCIF3 ((uint32_t)0x08000000) +#define DMA_INTC0_CTCIF3 ((uint32_t)0x08000000) +#define DMA_HISR_TEIF6 ((uint32_t)0x00080000) +#define DMA_HIFCR_CTEIF6 ((uint32_t)0x00080000) +#define DMA_HISR_FEIF6 ((uint32_t)0x00010000) +#define DMA_HIFCR_CFEIF6 ((uint32_t)0x00010000) +#define DMA_HISR_DMEIF6 ((uint32_t)0x00040000) +#define DMA_HIFCR_CDMEIF6 ((uint32_t)0x00040000) +#define DMA_HIFCR_CHTIF6 ((uint32_t)0x00100000) +#define DMA_HISR_HTIF6 ((uint32_t)0x00100000) +#define DMA_HISR_TCIF6 ((uint32_t)0x00200000) +#define DMA_HIFCR_CTCIF6 ((uint32_t)0x00200000) + +#define DMA_CHANNEL_4 ((uint32_t)0x08000000) +#define DMA_MINC_ENABLE ((uint32_t)DMA_CHXCTL_MNAGA) +#define DMA_MDATAALIGN_WORD ((uint32_t)DMA_MEMORY_WIDTH_32BIT) +#define DMA_PDATAALIGN_WORD ((uint32_t)DMA_PERIPH_WIDTH_32BIT) +#define DMA_MBURST_INC4 ((uint32_t)DMA_MEMORY_BURST_4_BEAT) +#define DMA_PBURST_INC4 ((uint32_t)DMA_PERIPH_BURST_4_BEAT) + +#define BLOCK_SIZE ((uint32_t)(512)) + +#define INTC_CLEAR_MASK_CH3 ((uint32_t)0x0F40000) +#define INTC_CLEAR_MASK_CH6 ((uint32_t)0x003D000) + +#define SDIO_INTC_STATIC_FLAGS ((uint32_t)(SDIO_INTC_CCRCERRC | SDIO_INTC_DTCRCERRC | SDIO_INTC_CMDTMOUTC |\ + SDIO_INTC_DTTMOUTC | SDIO_INTC_TXUREC | SDIO_INTC_RXOREC |\ + SDIO_INTC_CMDRECVC | SDIO_INTC_CMDSENDC | SDIO_INTC_DTENDC |\ + SDIO_INTC_DTBLKENDC)) + +#define SD_SOFTWARE_COMMAND_TIMEOUT ((uint32_t)0x00220000) + +#define SD_OCR_ADDR_OUT_OF_RANGE ((uint32_t)0x80000000) +#define SD_OCR_ADDR_MISALIGNED ((uint32_t)0x40000000) +#define SD_OCR_BLOCK_LEN_ERR ((uint32_t)0x20000000) +#define SD_OCR_ERASE_SEQ_ERR ((uint32_t)0x10000000) +#define SD_OCR_BAD_ERASE_PARAM ((uint32_t)0x08000000) +#define SD_OCR_WRITE_PROT_VIOLATION ((uint32_t)0x04000000) +#define SD_OCR_LOCK_UNLOCK_FAILED ((uint32_t)0x01000000) +#define SD_OCR_COM_CRC_FAILED ((uint32_t)0x00800000) +#define SD_OCR_ILLEGAL_CMD ((uint32_t)0x00400000) +#define SD_OCR_CARD_ECC_FAILED ((uint32_t)0x00200000) +#define SD_OCR_CC_ERROR ((uint32_t)0x00100000) +#define SD_OCR_GENERAL_UNKNOWN_ERROR ((uint32_t)0x00080000) +#define SD_OCR_STREAM_READ_UNDERRUN ((uint32_t)0x00040000) +#define SD_OCR_STREAM_WRITE_OVERRUN ((uint32_t)0x00020000) +#define SD_OCR_CID_CSD_OVERWRITE ((uint32_t)0x00010000) +#define SD_OCR_WP_ERASE_SKIP ((uint32_t)0x00008000) +#define SD_OCR_CARD_ECC_DISABLED ((uint32_t)0x00004000) +#define SD_OCR_ERASE_RESET ((uint32_t)0x00002000) +#define SD_OCR_AKE_SEQ_ERROR ((uint32_t)0x00000008) +#define SD_OCR_ERRORBITS ((uint32_t)0xFDFFE008) + +#define SD_R6_GENERAL_UNKNOWN_ERROR ((uint32_t)0x00002000) +#define SD_R6_ILLEGAL_CMD ((uint32_t)0x00004000) +#define SD_R6_COM_CRC_FAILED ((uint32_t)0x00008000) + +#define SD_VOLTAGE_WINDOW_SD ((uint32_t)0x80100000) +#define SD_RESP_HIGH_CAPACITY ((uint32_t)0x40000000) +#define SD_RESP_STD_CAPACITY ((uint32_t)0x00000000) +#define SD_CHECK_PATTERN ((uint32_t)0x000001AA) + +#define SD_MAX_VOLT_TRIAL ((uint32_t)0x0000FFFF) +#define SD_ALLZERO ((uint32_t)0x00000000) + +#define SD_WIDE_BUS_SUPPORT ((uint32_t)0x00040000) +#define SD_SINGLE_BUS_SUPPORT ((uint32_t)0x00010000) +#define SD_CARD_LOCKED ((uint32_t)0x02000000) + +#define SD_0TO7BITS ((uint32_t)0x000000FF) +#define SD_8TO15BITS ((uint32_t)0x0000FF00) +#define SD_16TO23BITS ((uint32_t)0x00FF0000) +#define SD_24TO31BITS ((uint32_t)0xFF000000) +#define SD_MAX_DATA_LENGTH ((uint32_t)0x01FFFFFF) + +#define SD_CCCC_ERASE ((uint32_t)0x00000020) + +#define SD_SDIO_SEND_IF_COND ((uint32_t)SD_CMD_HS_SEND_EXT_CSD) + +#define SD_BUS_WIDE_1B ((uint32_t)0x00000000) +#define SD_BUS_WIDE_4B SDIO_BUSMODE_4BIT +#define SD_BUS_WIDE_8B SDIO_BUSMODE_8BIT + +#define SD_CMD_RESPONSE_SHORT SDIO_RESPONSETYPE_SHORT +#define SD_CMD_RESPONSE_LONG SDIO_RESPONSETYPE_LONG + +#define SD_DATABLOCK_SIZE_8B SDIO_DATABLOCKSIZE_8BYTES +#define SD_DATABLOCK_SIZE_64B SDIO_DATABLOCKSIZE_64BYTES +#define SD_DATABLOCK_SIZE_512B SDIO_DATABLOCKSIZE_512BYTES + +#define CLKCTL_CLEAR_MASK ((uint32_t)(SDIO_CLKCTL_DIV | SDIO_CLKCTL_CLKPWRSAV |\ + SDIO_CLKCTL_CLKBYP | SDIO_CLKCTL_BUSMODE |\ + SDIO_CLKCTL_CLKEDGE | SDIO_CLKCTL_HWCLKEN | SDIO_CLKCTL_DIV8)) + +#define DATACTRL_CLEAR_MASK ((uint32_t)(SDIO_DATACTL_DATAEN | SDIO_DATACTL_DATADIR |\ + SDIO_DATACTL_TRANSMOD | SDIO_DATACTL_BLKSZ)) + +#define CMDCTL_CLEAR_MASK ((uint32_t)(SDIO_CMDCTL_CMDIDX | SDIO_CMDCTL_CMDRESP |\ + SDIO_CMDCTL_INTWAIT | SDIO_CMDCTL_WAITDEND |\ + SDIO_CMDCTL_CSMEN | SDIO_CMDCTL_SUSPEND)) + +#define SDIO_INIT_CLK_DIV ((uint8_t)0x76) +#define SDIO_CLK_DIV ((uint8_t)0x02) + +#define SD_CMD_GO_IDLE_STATE ((uint8_t)0) // Resets the SD memory card. +#define SD_CMD_SEND_OP_COND ((uint8_t)1) // Sends host capacity support information and activates the card's initialization process. +#define SD_CMD_ALL_SEND_CID ((uint8_t)2) // Asks any card connected to the host to send the CID numbers on the CMD line. +#define SD_CMD_SET_REL_ADDR ((uint8_t)3) // Asks the card to publish a new relative address (RCA). +#define SD_CMD_HS_SWITCH ((uint8_t)6) // Checks switchable function (mode 0) and switch card function (mode 1). +#define SD_CMD_SEL_DESEL_CARD ((uint8_t)7) // Selects the card by its own relative address and gets deselected by any other address +#define SD_CMD_HS_SEND_EXT_CSD ((uint8_t)8) // Sends SD Memory Card interface condition, which includes host supply voltage information + // and asks the card whether card supports voltage. +#define SD_CMD_SEND_CSD ((uint8_t)9) // Addressed card sends its card specific data (CSD) on the CMD line. +#define SD_CMD_SEND_CID ((uint8_t)10) // Addressed card sends its card identification (CID) on the CMD line. +#define SD_CMD_STOP_TRANSMISSION ((uint8_t)12) // Forces the card to stop transmission. +#define SD_CMD_SEND_STATUS ((uint8_t)13) // Addressed card sends its status register. +#define SD_CMD_SET_BLOCKLEN ((uint8_t)16) // Sets the block length (in bytes for SDSC) for all following block commands + // (read, write, lock). Default block length is fixed to 512 Bytes. Not effective + // for SDHS and SDXC. +#define SD_CMD_READ_SINGLE_BLOCK ((uint8_t)17) // Reads single block of size selected by SET_BLOCKLEN in case of SDSC, and a block of + // fixed 512 bytes in case of SDHC and SDXC. +#define SD_CMD_READ_MULT_BLOCK ((uint8_t)18) // Continuously transfers data blocks from card to host until interrupted by + // STOP_TRANSMISSION command. +#define SD_CMD_WRITE_SINGLE_BLOCK ((uint8_t)24) // Writes single block of size selected by SET_BLOCKLEN in case of SDSC, and a block of + // fixed 512 bytes in case of SDHC and SDXC. +#define SD_CMD_WRITE_MULT_BLOCK ((uint8_t)25) // Continuously writes blocks of data until a STOP_TRANSMISSION follows. +#define SD_CMD_SD_ERASE_GRP_START ((uint8_t)32) // Sets the address of the first write block to be erased. (For SD card only). +#define SD_CMD_SD_ERASE_GRP_END ((uint8_t)33) // Sets the address of the last write block of the continuous range to be erased. + // system set by switch function command (CMD6). +#define SD_CMD_ERASE ((uint8_t)38) // Reserved for SD security applications. +#define SD_CMD_FAST_IO ((uint8_t)39) // SD card doesn't support it (Reserved). +#define SD_CMD_APP_CMD ((uint8_t)55) // Indicates to the card that the next command is an application specific command rather + // than a standard command. + +/* Following commands are SD Card Specific commands. + SDIO_APP_CMD should be sent before sending these commands. */ +#define SD_CMD_APP_SD_SET_BUSWIDTH ((uint8_t)6) // (ACMD6) Defines the data bus width to be used for data transfer. The allowed data bus + // widths are given in SCR register. +#define SD_CMD_SD_APP_STATUS ((uint8_t)13) // (ACMD13) Sends the SD status. +#define SD_CMD_SD_APP_OP_COND ((uint8_t)41) // (ACMD41) Sends host capacity support information (HCS) and asks the accessed card to + // send its operating condition register (OCR) content in the response on the CMD line. +#define SD_CMD_SD_APP_SEND_SCR ((uint8_t)51) // Reads the SD Configuration Register (SCR). + +#define SDIO_DIR_TX 1 +#define SDIO_DIR_RX 0 + +#define SDIO_DMA_ST3 1 + + +typedef enum { + SD_SINGLE_BLOCK = 0, // Single block operation + SD_MULTIPLE_BLOCK = 1, // Multiple blocks operation +} SD_Operation_t; + +typedef struct { + uint32_t CSD[4]; // SD card specific data table + uint32_t CID[4]; // SD card identification number table + volatile uint32_t TransferComplete; // SD transfer complete flag in non blocking mode + volatile uint32_t TransferError; // SD transfer error flag in non blocking mode + volatile uint32_t RXCplt; // SD RX Complete is equal 0 when no transfer + volatile uint32_t TXCplt; // SD TX Complete is equal 0 when no transfer + volatile uint32_t Operation; // SD transfer operation (read/write) +} SD_Handle_t; + +typedef enum { + SD_CARD_READY = ((uint32_t)0x00000001), // Card state is ready + SD_CARD_IDENTIFICATION = ((uint32_t)0x00000002), // Card is in identification state + SD_CARD_STANDBY = ((uint32_t)0x00000003), // Card is in standby state + SD_CARD_TRANSFER = ((uint32_t)0x00000004), // Card is in transfer state + SD_CARD_SENDING = ((uint32_t)0x00000005), // Card is sending an operation + SD_CARD_RECEIVING = ((uint32_t)0x00000006), // Card is receiving operation information + SD_CARD_PROGRAMMING = ((uint32_t)0x00000007), // Card is in programming state + SD_CARD_DISCONNECTED = ((uint32_t)0x00000008), // Card is disconnected + SD_CARD_ERROR = ((uint32_t)0x000000FF) // Card is in error state +} SD_CardState_t; + + +static SD_Handle_t SD_Handle; +SD_CardInfo_t SD_CardInfo; +static uint32_t SD_Status; +static uint32_t SD_CardRCA; +SD_CardType_t SD_CardType; +static volatile uint32_t TimeOut; +DMA_Stream_TypeDef *dmaStream; +uint32_t dma_periph_sdio; +int dma_channel_sdio; + + +static void SD_DataTransferInit (uint32_t Size, uint32_t DataBlockSize, bool IsItReadFromCard); +static SD_Error_t SD_TransmitCommand (uint32_t Command, uint32_t Argument, int8_t ResponseType); +static SD_Error_t SD_CmdResponse (uint8_t SD_CMD, int8_t ResponseType); +static void SD_GetResponse (uint32_t* pResponse); +static SD_Error_t CheckOCR_Response (uint32_t Response_R1); +static void SD_DMA_Complete (uint32_t dma_periph, dma_channel_enum channelx); +static SD_Error_t SD_InitializeCard (void); + +static SD_Error_t SD_PowerON (void); +static SD_Error_t SD_WideBusOperationConfig (uint32_t WideMode); +static SD_Error_t SD_FindSCR (uint32_t *pSCR); + +void SDIO_DMA_ST3_IRQHandler(dmaChannelDescriptor_t *dma); +void SDIO_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma); + + +/*! + \brief Prepare the state machine for transfer + \param[in] Size: data transfer size + \param[in] DataBlockSize: data block size + \param[in] IsItReadFromCard: transfer direction flag + \param[out] none + \retval none +*/ +static void SD_DataTransferInit(uint32_t Size, uint32_t DataBlockSize, bool IsItReadFromCard) + { + uint32_t Direction; + + SDIO_DATATO = SD_DATATIMEOUT; + SDIO_DATALEN = Size; + Direction = (IsItReadFromCard == true) ? SDIO_DATACTL_DATADIR : 0; + SDIO_DATACTL |= (uint32_t)(DataBlockSize | Direction | SDIO_DATACTL_DATAEN | 0x01); + return; +} + +/*! + \brief Send the command to SDIO + \param[in] Command: SDIO command + \param[in] Argument: command argument + \param[in] ResponseType: response type (must provide the response size) + \param[out] none + \retval SD Card error state +*/ +static SD_Error_t SD_TransmitCommand(uint32_t Command, uint32_t Argument, int8_t ResponseType) +{ + SD_Error_t ErrorState; + + SDIO_INTC = SDIO_INTC_STATIC_FLAGS; // Clear the Command Flags + /* disable the CSM */ + SDIO_CMDCTL &= ~SDIO_CMDCTL_CSMEN; + SDIO_CMDAGMT = (uint32_t)Argument; // Set the SDIO Argument value + SDIO_CMDCTL = (uint32_t)(Command | SDIO_CMDCTL_CSMEN); // Set SDIO command parameters + if((Argument == 0) && (ResponseType == 0)) ResponseType = -1; // Go idle command + ErrorState = SD_CmdResponse(Command & SDIO_CMDCTL_CMDIDX, ResponseType); + SDIO_INTC = SDIO_INTC_STATIC_FLAGS; // Clear the Command Flags + + return ErrorState; +} + +/*! + \brief Checks for error conditions for any response (R2, R3, etc.) + \param[in] SD_CMD: the sent command Index + \param[in] ResponseType: response type + \param[out] none + \retval SD Card error state +*/ +static SD_Error_t SD_CmdResponse(uint8_t SD_CMD, int8_t ResponseType) +{ + uint32_t Response_R1; + uint32_t TimeOut; + uint32_t Flag; + + if(ResponseType == -1) { + Flag = SDIO_STAT_CMDSEND; + } else { + Flag = SDIO_STAT_CCRCERR | SDIO_STAT_CMDRECV | SDIO_STAT_CMDTMOUT; + } + + TimeOut = SD_SOFTWARE_COMMAND_TIMEOUT; + do { + SD_Status = SDIO_STAT; + TimeOut--; + } while(((SD_Status & Flag) == 0) && (TimeOut > 0)); + + if(ResponseType <= 0) { + if(TimeOut == 0) { + return SD_CMD_RSP_TIMEOUT; + } else { + return SD_OK; + } + } + + if((SDIO_STAT & SDIO_STAT_CMDTMOUT) != 0) { + return SD_CMD_RSP_TIMEOUT; + } + + if(ResponseType == 3) { + if(TimeOut == 0) { + return SD_CMD_RSP_TIMEOUT; // Card is not V2.0 compliant or card does not support the set voltage range + } else { + return SD_OK; // Card is SD V2.0 compliant + } + } + + if((SDIO_STAT & SDIO_STAT_CCRCERR) != 0) { + return SD_CMD_CRC_FAIL; + } + if(ResponseType == 2) { + return SD_OK; + } + if((uint8_t)SDIO_RSPCMDIDX != SD_CMD) { + return SD_ILLEGAL_CMD; + } + + Response_R1 = SDIO_RESP0; // We have received response, retrieve it for analysis + + if(ResponseType == 1) { + return CheckOCR_Response(Response_R1); + } else if(ResponseType == 6) { + if((Response_R1 & (SD_R6_GENERAL_UNKNOWN_ERROR | SD_R6_ILLEGAL_CMD | SD_R6_COM_CRC_FAILED)) == SD_ALLZERO) { + SD_CardRCA = Response_R1; + } + if((Response_R1 & SD_R6_GENERAL_UNKNOWN_ERROR) == SD_R6_GENERAL_UNKNOWN_ERROR) { + return SD_GENERAL_UNKNOWN_ERROR; + } + if((Response_R1 & SD_R6_ILLEGAL_CMD) == SD_R6_ILLEGAL_CMD) { + return SD_ILLEGAL_CMD; + } + if((Response_R1 & SD_R6_COM_CRC_FAILED) == SD_R6_COM_CRC_FAILED) { + return SD_COM_CRC_FAILED; + } + } + + return SD_OK; +} + +/*! + \brief Analyze the OCR response and return the appropriate error code + \param[in] Response_R1: OCR Response code + \param[out] none + \retval SD Card error state +*/ +static SD_Error_t CheckOCR_Response(uint32_t Response_R1) +{ + if((Response_R1 & SD_OCR_ERRORBITS) == SD_ALLZERO) return SD_OK; + if((Response_R1 & SD_OCR_ADDR_OUT_OF_RANGE) == SD_OCR_ADDR_OUT_OF_RANGE) return SD_ADDR_OUT_OF_RANGE; + if((Response_R1 & SD_OCR_ADDR_MISALIGNED) == SD_OCR_ADDR_MISALIGNED) return SD_ADDR_MISALIGNED; + if((Response_R1 & SD_OCR_BLOCK_LEN_ERR) == SD_OCR_BLOCK_LEN_ERR) return SD_BLOCK_LEN_ERR; + if((Response_R1 & SD_OCR_ERASE_SEQ_ERR) == SD_OCR_ERASE_SEQ_ERR) return SD_ERASE_SEQ_ERR; + if((Response_R1 & SD_OCR_BAD_ERASE_PARAM) == SD_OCR_BAD_ERASE_PARAM) return SD_BAD_ERASE_PARAM; + if((Response_R1 & SD_OCR_WRITE_PROT_VIOLATION) == SD_OCR_WRITE_PROT_VIOLATION) return SD_WRITE_PROT_VIOLATION; + if((Response_R1 & SD_OCR_LOCK_UNLOCK_FAILED) == SD_OCR_LOCK_UNLOCK_FAILED) return SD_LOCK_UNLOCK_FAILED; + if((Response_R1 & SD_OCR_COM_CRC_FAILED) == SD_OCR_COM_CRC_FAILED) return SD_COM_CRC_FAILED; + if((Response_R1 & SD_OCR_ILLEGAL_CMD) == SD_OCR_ILLEGAL_CMD) return SD_ILLEGAL_CMD; + if((Response_R1 & SD_OCR_CARD_ECC_FAILED) == SD_OCR_CARD_ECC_FAILED) return SD_CARD_ECC_FAILED; + if((Response_R1 & SD_OCR_CC_ERROR) == SD_OCR_CC_ERROR) return SD_CC_ERROR; + if((Response_R1 & SD_OCR_GENERAL_UNKNOWN_ERROR) == SD_OCR_GENERAL_UNKNOWN_ERROR)return SD_GENERAL_UNKNOWN_ERROR; + if((Response_R1 & SD_OCR_STREAM_READ_UNDERRUN) == SD_OCR_STREAM_READ_UNDERRUN) return SD_STREAM_READ_UNDERRUN; + if((Response_R1 & SD_OCR_STREAM_WRITE_OVERRUN) == SD_OCR_STREAM_WRITE_OVERRUN) return SD_STREAM_WRITE_OVERRUN; + if((Response_R1 & SD_OCR_CID_CSD_OVERWRITE) == SD_OCR_CID_CSD_OVERWRITE) return SD_CID_CSD_OVERWRITE; + if((Response_R1 & SD_OCR_WP_ERASE_SKIP) == SD_OCR_WP_ERASE_SKIP) return SD_WP_ERASE_SKIP; + if((Response_R1 & SD_OCR_CARD_ECC_DISABLED) == SD_OCR_CARD_ECC_DISABLED) return SD_CARD_ECC_DISABLED; + if((Response_R1 & SD_OCR_ERASE_RESET) == SD_OCR_ERASE_RESET) return SD_ERASE_RESET; + if((Response_R1 & SD_OCR_AKE_SEQ_ERROR) == SD_OCR_AKE_SEQ_ERROR) return SD_AKE_SEQ_ERROR; + + return SD_OK; +} + +/*! + \brief Get response from SD device + \param[in] none + \param[out] pResponse: pointer to response buffer + \retval none +*/ +static void SD_GetResponse(uint32_t* pResponse) +{ + pResponse[0] = SDIO_RESP0; + pResponse[1] = SDIO_RESP1; + pResponse[2] = SDIO_RESP2; + pResponse[3] = SDIO_RESP3; +} + +/*! + \brief SD DMA transfer complete RX and TX callback + \param[in] dma_periph: DMA peripheral + \param[in] channelx: DMA channel + \param[out] none + \retval none +*/ +static void SD_DMA_Complete(uint32_t dma_periph, dma_channel_enum channelx) +{ + if (SD_Handle.RXCplt) { + if (SD_Handle.Operation == ((SDIO_DIR_RX << 1) | SD_MULTIPLE_BLOCK)) { + /* Send stop command in multiblock write */ + SD_TransmitCommand((SD_CMD_STOP_TRANSMISSION | SD_CMD_RESPONSE_SHORT), 0, 1); + } + + SDIO_DATACTL &= (uint32_t)~((uint32_t)SDIO_DATACTL_DMAEN); + + /* Clear all the static flags */ + SDIO_INTC = SDIO_INTC_STATIC_FLAGS; + + /* Clear flag */ + SD_Handle.RXCplt = 0; + + /* Disable the channel */ + DMA_CHCTL(dma_periph, channelx) &= ~DMA_CHXCTL_CHEN; + } else { + /* Enable Dataend IE */ + SDIO_INTEN |= SDIO_INTEN_DTENDIE; + } +} + +/*! + \brief Prepare the DMA transfer + \param[in] pBuffer: pointer to the buffer that will contain the data to transmit + \param[in] BlockSize: the SD card Data block size (must be 512 bytes) + \param[in] NumberOfBlocks: number of blocks to write + \param[in] dir: transfer direction + \param[out] none + \retval none + \note BlockSize must be 512 bytes +*/ +static SD_Error_t SD_InitializeCard(void) +{ + SD_Error_t ErrorState = SD_OK; + + if((SDIO_PWRCTL & SDIO_PWRCTL_PWRCTL) != 0) + { + if(SD_CardType != SD_SECURE_DIGITAL_IO) { + // Send CMD2 ALL_SEND_CID + if((ErrorState = SD_TransmitCommand((SD_CMD_ALL_SEND_CID | SD_CMD_RESPONSE_LONG), 0, 2)) != SD_OK) { + return ErrorState; + } + + // Get Card identification number data + SD_GetResponse(SD_Handle.CID); + } + + if((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) || + (SD_CardType == SD_SECURE_DIGITAL_IO_COMBO) || (SD_CardType == SD_HIGH_CAPACITY)) { + // Send CMD3 SET_REL_ADDR with argument 0 + // SD Card publishes its RCA. + if((ErrorState = SD_TransmitCommand((SD_CMD_SET_REL_ADDR | SD_CMD_RESPONSE_SHORT), 0, 6)) != SD_OK) { + return ErrorState; + } + } + + if(SD_CardType != SD_SECURE_DIGITAL_IO) { + // Send CMD9 SEND_CSD with argument as card's RCA + if((ErrorState = SD_TransmitCommand((SD_CMD_SEND_CSD | SD_CMD_RESPONSE_LONG), SD_CardRCA, 2)) == SD_OK) { + // Get Card Specific Data + SD_GetResponse(SD_Handle.CSD); + } + } + } else { + ErrorState = SD_REQUEST_NOT_APPLICABLE; + } + + return ErrorState; +} + +/*! + \brief Reads block(s) from a specified address in a card. The Data transfer is managed by DMA mode + \param[in] ReadAddress: address from where data is to be read + \param[in] buffer: pointer to the buffer that will contain the received data + \param[in] BlockSize: SD card Data block size (must be 512 bytes) + \param[in] NumberOfBlocks: number of blocks to read + \param[out] none + \retval SD Card error state + \note This API should be followed by the function SD_CheckOperation() to check the completion of the read process. BlockSize must be 512 bytes +*/ +static void SD_StartBlockTransfert(uint32_t* pBuffer, uint32_t BlockSize, uint32_t NumberOfBlocks, uint8_t dir) +{ + SDIO_DATACTL = 0; // Initialize data control register + SD_Handle.TransferComplete = 0; // Initialize handle flags + SD_Handle.TransferError = SD_OK; + SD_Handle.Operation = (NumberOfBlocks > 1) ? SD_MULTIPLE_BLOCK : SD_SINGLE_BLOCK; // Initialize SD Read operation + SD_Handle.Operation |= dir << 1; + SDIO_INTEN = 0; + if (dir == SDIO_DIR_RX) { + SDIO_INTEN |= (SDIO_INTEN_DTCRCERRIE | SDIO_INTEN_DTTMOUTIE | // Enable transfer interrupts + SDIO_INTEN_DTENDIE | SDIO_INTEN_RXOREIE); + } else { + SDIO_INTEN |= (SDIO_INTEN_DTCRCERRIE | SDIO_INTEN_DTTMOUTIE | // Enable transfer interrupts + SDIO_INTEN_TXUREIE); + } + if (dir == SDIO_DIR_TX) { + SDIO_DATACTL |= SDIO_DATACTL_DMAEN; // Enable SDIO DMA transfer + } + DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) &= ~DMA_CHXCTL_CHEN; // Disable the Peripheral + while (DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) & DMA_CHXCTL_CHEN); + DMA_CHCNT(dma_periph_sdio,dma_channel_sdio) = (uint32_t) (BlockSize * NumberOfBlocks) / 4; // Configure DMA Stream data length + DMA_CHM0ADDR(dma_periph_sdio,dma_channel_sdio) = (uint32_t) pBuffer; // Configure DMA Stream memory address + if (dir == SDIO_DIR_RX) { + DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) &= ~(0x01U << 6U); // Sets peripheral to memory + } else { + DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) |= DMA_MEMORY_TO_PERIPH; // Sets memory to peripheral + } + if ((uint32_t)dmaStream == DMA1_CH3_BASE) { + DMA_INTC0(dma_periph_sdio) = DMA_INTC0_TAEIFC3 | DMA_INTC0_CDMEIF3 | + DMA_INTC0_FEEIFC3 | DMA_INTC0_CHTIF3 | DMA_INTC0_CTCIF3; // Clear the transfer error flag + } else { + DMA_INTC1(dma_periph_sdio) = DMA_HIFCR_CTEIF6 | DMA_HIFCR_CDMEIF6 | + DMA_HIFCR_CFEIF6 | DMA_HIFCR_CHTIF6 | DMA_HIFCR_CTCIF6; // Clear the transfer error flag + } + DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) |= DMA_CHXCTL_FTFIE | DMA_CHXCTL_HTFIE | DMA_CHXCTL_TAEIE | DMA_CHXCTL_SDEIE; // Enable all interrupts + DMA_CHFCTL(dma_periph_sdio,dma_channel_sdio) |= DMA_CHXFCTL_FEEIE; + DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) |= DMA_CHXCTL_CHEN; // Enable the Peripheral + if (dir == SDIO_DIR_RX) { + SDIO_DATACTL |= SDIO_DATACTL_DMAEN; // Enable SDIO DMA transfer + } +} + +/*! + \brief Writes block(s) to a specified address in a card. The Data transfer is managed by DMA mode + \param[in] WriteAddress: address from where data is to be read + \param[in] buffer: pointer to the buffer that will contain the data to transmit + \param[in] BlockSize: the SD card Data block size (must be 512 bytes) + \param[in] NumberOfBlocks: number of blocks to write + \param[out] none + \retval SD Card error state + \note This API should be followed by the function SD_CheckOperation() to check the completion of the write process (by SD current status polling). BlockSize must be 512 bytes +*/ +SD_Error_t SD_ReadBlocks_DMA(uint64_t ReadAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks) +{ + SD_Error_t ErrorState = SD_OK; + uint32_t CmdIndex; + SD_Handle.RXCplt = 1; + + if(SD_CardType != SD_HIGH_CAPACITY) + { + ReadAddress *= 512; + } + + SD_StartBlockTransfert(buffer, BlockSize, NumberOfBlocks, SDIO_DIR_RX); + + // Configure the SD DPSM (Data Path State Machine) + SD_DataTransferInit(BlockSize * NumberOfBlocks, SD_DATABLOCK_SIZE_512B, true); + + // Set Block Size for Card + ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), BlockSize, 1); + + // Send CMD18 READ_MULT_BLOCK with argument data address + // or send CMD17 READ_SINGLE_BLOCK depending on number of block + uint8_t retries = 10; + CmdIndex = (NumberOfBlocks > 1) ? SD_CMD_READ_MULT_BLOCK : SD_CMD_READ_SINGLE_BLOCK; + do { + ErrorState = SD_TransmitCommand((CmdIndex | SD_CMD_RESPONSE_SHORT), (uint32_t)ReadAddress, 1); + if (ErrorState != SD_OK && retries--) { + ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1); + } + } while (ErrorState != SD_OK && retries); + + if (ErrorState != SD_OK) { + SD_Handle.RXCplt = 0; + } + + // Update the SD transfer error in SD handle + SD_Handle.TransferError = ErrorState; + + return ErrorState; +} + +/*! + \brief Writes block(s) to a specified address in a card. The Data transfer is managed by DMA mode + \param[in] WriteAddress: address from where data is to be read + \param[in] buffer: pointer to the buffer that will contain the data to transmit + \param[in] BlockSize: the SD card Data block size (must be 512 bytes) + \param[in] NumberOfBlocks: number of blocks to write + \param[out] none + \retval SD Card error state + \note This API should be followed by the function SD_CheckOperation() to check the completion of the write process (by SD current status polling). BlockSize must be 512 bytes +*/ +SD_Error_t SD_WriteBlocks_DMA(uint64_t WriteAddress, uint32_t *buffer, uint32_t BlockSize, uint32_t NumberOfBlocks) +{ + SD_Error_t ErrorState = SD_OK; + uint32_t CmdIndex; + SD_Handle.TXCplt = 1; + + //printf("Reading at %ld into %p %ld blocks\n", (uint32_t)WriteAddress, (void*)buffer, NumberOfBlocks); + + if(SD_CardType != SD_HIGH_CAPACITY) + { + WriteAddress *= 512; + } + + // Check number of blocks command + // Send CMD24 WRITE_SINGLE_BLOCK + // Send CMD25 WRITE_MULT_BLOCK with argument data address + CmdIndex = (NumberOfBlocks > 1) ? SD_CMD_WRITE_MULT_BLOCK : SD_CMD_WRITE_SINGLE_BLOCK; + + // Set Block Size for Card + uint8_t retries = 10; + do { + ErrorState = SD_TransmitCommand((CmdIndex | SD_CMD_RESPONSE_SHORT), (uint32_t)WriteAddress, 1); + if (ErrorState != SD_OK && retries--) { + ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1); + } + } while(ErrorState != SD_OK && retries); + + if (ErrorState != SD_OK) { + SD_Handle.TXCplt = 0; + return ErrorState; + } + + SD_StartBlockTransfert(buffer, BlockSize, NumberOfBlocks, SDIO_DIR_TX); + + // Configure the SD DPSM (Data Path State Machine) + SD_DataTransferInit(BlockSize * NumberOfBlocks, SD_DATABLOCK_SIZE_512B, false); + + SD_Handle.TransferError = ErrorState; + + return ErrorState; +} + +SD_Error_t SD_CheckWrite(void) +{ + if (SD_Handle.TXCplt != 0) return SD_BUSY; + return SD_OK; +} + +SD_Error_t SD_CheckRead(void) +{ + if (SD_Handle.RXCplt != 0) return SD_BUSY; + return SD_OK; +} + +/*! + \brief Returns information about specific card, contains all SD card information + \param[in] none + \param[out] none + \retval SD Card error state +*/ +SD_Error_t SD_GetCardInfo(void) +{ + SD_Error_t ErrorState = SD_OK; + uint32_t Temp = 0; + + // Byte 0 + Temp = (SD_Handle.CSD[0] & 0xFF000000) >> 24; + SD_CardInfo.SD_csd.CSDStruct = (uint8_t)((Temp & 0xC0) >> 6); + SD_CardInfo.SD_csd.SysSpecVersion = (uint8_t)((Temp & 0x3C) >> 2); + SD_CardInfo.SD_csd.Reserved1 = Temp & 0x03; + + // Byte 1 + Temp = (SD_Handle.CSD[0] & 0x00FF0000) >> 16; + SD_CardInfo.SD_csd.TAAC = (uint8_t)Temp; + + // Byte 2 + Temp = (SD_Handle.CSD[0] & 0x0000FF00) >> 8; + SD_CardInfo.SD_csd.NSAC = (uint8_t)Temp; + + // Byte 3 + Temp = SD_Handle.CSD[0] & 0x000000FF; + SD_CardInfo.SD_csd.MaxBusClkFrec = (uint8_t)Temp; + + // Byte 4 + Temp = (SD_Handle.CSD[1] & 0xFF000000) >> 24; + SD_CardInfo.SD_csd.CardComdClasses = (uint16_t)(Temp << 4); + + // Byte 5 + Temp = (SD_Handle.CSD[1] & 0x00FF0000) >> 16; + SD_CardInfo.SD_csd.CardComdClasses |= (uint16_t)((Temp & 0xF0) >> 4); + SD_CardInfo.SD_csd.RdBlockLen = (uint8_t)(Temp & 0x0F); + + // Byte 6 + Temp = (SD_Handle.CSD[1] & 0x0000FF00) >> 8; + SD_CardInfo.SD_csd.PartBlockRead = (uint8_t)((Temp & 0x80) >> 7); + SD_CardInfo.SD_csd.WrBlockMisalign = (uint8_t)((Temp & 0x40) >> 6); + SD_CardInfo.SD_csd.RdBlockMisalign = (uint8_t)((Temp & 0x20) >> 5); + SD_CardInfo.SD_csd.DSRImpl = (uint8_t)((Temp & 0x10) >> 4); + SD_CardInfo.SD_csd.Reserved2 = 0; /*!< Reserved */ + + if((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0)) { + SD_CardInfo.SD_csd.DeviceSize = (Temp & 0x03) << 10; + + // Byte 7 + Temp = (uint8_t)(SD_Handle.CSD[1] & 0x000000FF); + SD_CardInfo.SD_csd.DeviceSize |= (Temp) << 2; + + // Byte 8 + Temp = (uint8_t)((SD_Handle.CSD[2] & 0xFF000000) >> 24); + SD_CardInfo.SD_csd.DeviceSize |= (Temp & 0xC0) >> 6; + + SD_CardInfo.SD_csd.MaxRdCurrentVDDMin = (Temp & 0x38) >> 3; + SD_CardInfo.SD_csd.MaxRdCurrentVDDMax = (Temp & 0x07); + + // Byte 9 + Temp = (uint8_t)((SD_Handle.CSD[2] & 0x00FF0000) >> 16); + SD_CardInfo.SD_csd.MaxWrCurrentVDDMin = (Temp & 0xE0) >> 5; + SD_CardInfo.SD_csd.MaxWrCurrentVDDMax = (Temp & 0x1C) >> 2; + SD_CardInfo.SD_csd.DeviceSizeMul = (Temp & 0x03) << 1; + + // Byte 10 + Temp = (uint8_t)((SD_Handle.CSD[2] & 0x0000FF00) >> 8); + SD_CardInfo.SD_csd.DeviceSizeMul |= (Temp & 0x80) >> 7; + + SD_CardInfo.CardCapacity = (SD_CardInfo.SD_csd.DeviceSize + 1) ; + SD_CardInfo.CardCapacity *= (1 << (SD_CardInfo.SD_csd.DeviceSizeMul + 2)); + SD_CardInfo.CardBlockSize = 1 << (SD_CardInfo.SD_csd.RdBlockLen); + SD_CardInfo.CardCapacity = SD_CardInfo.CardCapacity * SD_CardInfo.CardBlockSize / 512; // In 512 byte blocks + } else if(SD_CardType == SD_HIGH_CAPACITY) { + // Byte 7 + Temp = (uint8_t)(SD_Handle.CSD[1] & 0x000000FF); + SD_CardInfo.SD_csd.DeviceSize = (Temp & 0x3F) << 16; + + // Byte 8 + Temp = (uint8_t)((SD_Handle.CSD[2] & 0xFF000000) >> 24); + + SD_CardInfo.SD_csd.DeviceSize |= (Temp << 8); + + // Byte 9 + Temp = (uint8_t)((SD_Handle.CSD[2] & 0x00FF0000) >> 16); + + SD_CardInfo.SD_csd.DeviceSize |= (Temp); + + // Byte 10 + Temp = (uint8_t)((SD_Handle.CSD[2] & 0x0000FF00) >> 8); + + SD_CardInfo.CardCapacity = ((uint64_t)SD_CardInfo.SD_csd.DeviceSize + 1) * 1024; + SD_CardInfo.CardBlockSize = 512; + } else { + // Not supported card type + ErrorState = SD_ERROR; + } + + SD_CardInfo.SD_csd.EraseGrSize = (Temp & 0x40) >> 6; + SD_CardInfo.SD_csd.EraseGrMul = (Temp & 0x3F) << 1; + + // Byte 11 + Temp = (uint8_t)(SD_Handle.CSD[2] & 0x000000FF); + SD_CardInfo.SD_csd.EraseGrMul |= (Temp & 0x80) >> 7; + SD_CardInfo.SD_csd.WrProtectGrSize = (Temp & 0x7F); + + // Byte 12 + Temp = (uint8_t)((SD_Handle.CSD[3] & 0xFF000000) >> 24); + SD_CardInfo.SD_csd.WrProtectGrEnable = (Temp & 0x80) >> 7; + SD_CardInfo.SD_csd.ManDeflECC = (Temp & 0x60) >> 5; + SD_CardInfo.SD_csd.WrSpeedFact = (Temp & 0x1C) >> 2; + SD_CardInfo.SD_csd.MaxWrBlockLen = (Temp & 0x03) << 2; + + // Byte 13 + Temp = (uint8_t)((SD_Handle.CSD[3] & 0x00FF0000) >> 16); + SD_CardInfo.SD_csd.MaxWrBlockLen |= (Temp & 0xC0) >> 6; + SD_CardInfo.SD_csd.WriteBlockPaPartial = (Temp & 0x20) >> 5; + SD_CardInfo.SD_csd.Reserved3 = 0; + SD_CardInfo.SD_csd.ContentProtectAppli = (Temp & 0x01); + + // Byte 14 + Temp = (uint8_t)((SD_Handle.CSD[3] & 0x0000FF00) >> 8); + SD_CardInfo.SD_csd.FileFormatGrouop = (Temp & 0x80) >> 7; + SD_CardInfo.SD_csd.CopyFlag = (Temp & 0x40) >> 6; + SD_CardInfo.SD_csd.PermWrProtect = (Temp & 0x20) >> 5; + SD_CardInfo.SD_csd.TempWrProtect = (Temp & 0x10) >> 4; + SD_CardInfo.SD_csd.FileFormat = (Temp & 0x0C) >> 2; + SD_CardInfo.SD_csd.ECC = (Temp & 0x03); + + // Byte 15 + Temp = (uint8_t)(SD_Handle.CSD[3] & 0x000000FF); + SD_CardInfo.SD_csd.CSD_CRC = (Temp & 0xFE) >> 1; + SD_CardInfo.SD_csd.Reserved4 = 1; + + // Byte 0 + Temp = (uint8_t)((SD_Handle.CID[0] & 0xFF000000) >> 24); + SD_CardInfo.SD_cid.ManufacturerID = Temp; + + // Byte 1 + Temp = (uint8_t)((SD_Handle.CID[0] & 0x00FF0000) >> 16); + SD_CardInfo.SD_cid.OEM_AppliID = Temp << 8; + + // Byte 2 + Temp = (uint8_t)((SD_Handle.CID[0] & 0x000000FF00) >> 8); + SD_CardInfo.SD_cid.OEM_AppliID |= Temp; + + // Byte 3 + Temp = (uint8_t)(SD_Handle.CID[0] & 0x000000FF); + SD_CardInfo.SD_cid.ProdName1 = Temp << 24; + + // Byte 4 + Temp = (uint8_t)((SD_Handle.CID[1] & 0xFF000000) >> 24); + SD_CardInfo.SD_cid.ProdName1 |= Temp << 16; + + // Byte 5 + Temp = (uint8_t)((SD_Handle.CID[1] & 0x00FF0000) >> 16); + SD_CardInfo.SD_cid.ProdName1 |= Temp << 8; + + // Byte 6 + Temp = (uint8_t)((SD_Handle.CID[1] & 0x0000FF00) >> 8); + SD_CardInfo.SD_cid.ProdName1 |= Temp; + + // Byte 7 + Temp = (uint8_t)(SD_Handle.CID[1] & 0x000000FF); + SD_CardInfo.SD_cid.ProdName2 = Temp; + + // Byte 8 + Temp = (uint8_t)((SD_Handle.CID[2] & 0xFF000000) >> 24); + SD_CardInfo.SD_cid.ProdRev = Temp; + + // Byte 9 + Temp = (uint8_t)((SD_Handle.CID[2] & 0x00FF0000) >> 16); + SD_CardInfo.SD_cid.ProdSN = Temp << 24; + + // Byte 10 + Temp = (uint8_t)((SD_Handle.CID[2] & 0x0000FF00) >> 8); + SD_CardInfo.SD_cid.ProdSN |= Temp << 16; + + // Byte 11 + Temp = (uint8_t)(SD_Handle.CID[2] & 0x000000FF); + SD_CardInfo.SD_cid.ProdSN |= Temp << 8; + + // Byte 12 + Temp = (uint8_t)((SD_Handle.CID[3] & 0xFF000000) >> 24); + SD_CardInfo.SD_cid.ProdSN |= Temp; + + // Byte 13 + Temp = (uint8_t)((SD_Handle.CID[3] & 0x00FF0000) >> 16); + SD_CardInfo.SD_cid.Reserved1 |= (Temp & 0xF0) >> 4; + SD_CardInfo.SD_cid.ManufactDate = (Temp & 0x0F) << 8; + + // Byte 14 + Temp = (uint8_t)((SD_Handle.CID[3] & 0x0000FF00) >> 8); + SD_CardInfo.SD_cid.ManufactDate |= Temp; + + // Byte 15 + Temp = (uint8_t)(SD_Handle.CID[3] & 0x000000FF); + SD_CardInfo.SD_cid.CID_CRC = (Temp & 0xFE) >> 1; + SD_CardInfo.SD_cid.Reserved2 = 1; + + return ErrorState; +} + +/*! + \brief Enables wide bus operation for the requested card if supported by card + \param[in] WideMode: specifies the SD card wide bus mode (SD_BUS_WIDE_8B/4B/1B) + \param[out] none + \retval SD Card error state + \note WideMode can be: SD_BUS_WIDE_8B (8-bit data transfer, only for MMC), SD_BUS_WIDE_4B (4-bit data transfer), SD_BUS_WIDE_1B (1-bit data transfer) +*/ +static SD_Error_t SD_WideBusOperationConfig(uint32_t WideMode) +{ + SD_Error_t ErrorState = SD_OK; + uint32_t Temp; + uint32_t reg; + uint32_t SCR[2] = {0, 0}; + + if((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) || + (SD_CardType == SD_HIGH_CAPACITY)) { + + if(WideMode == SD_BUS_WIDE_8B) { + ErrorState = SD_UNSUPPORTED_FEATURE; + } else if((WideMode == SD_BUS_WIDE_4B) || (WideMode == SD_BUS_WIDE_1B)) { + if((SDIO_RESP0 & SD_CARD_LOCKED) != SD_CARD_LOCKED) { + // Get SCR Register + ErrorState = SD_FindSCR(SCR); + if(ErrorState == SD_OK) { + Temp = (WideMode == SD_BUS_WIDE_4B) ? SD_WIDE_BUS_SUPPORT : SD_SINGLE_BUS_SUPPORT; + + // If requested card supports wide bus operation + if((SCR[1] & Temp) != SD_ALLZERO) { + // Send CMD55 APP_CMD with argument as card's RCA. + ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1); + if(ErrorState == SD_OK) { + Temp = (WideMode == SD_BUS_WIDE_4B) ? 2 : 0; + + // Send ACMD6 APP_CMD with argument as 2 for wide bus mode + ErrorState = SD_TransmitCommand((SD_CMD_APP_SD_SET_BUSWIDTH | SD_CMD_RESPONSE_SHORT), Temp, 1); + } + } else { + ErrorState = SD_REQUEST_NOT_APPLICABLE; + } + } + } else { + ErrorState = SD_LOCK_UNLOCK_FAILED; + } + } else { + ErrorState = SD_INVALID_PARAMETER; // WideMode is not a valid argument + } + + if(ErrorState == SD_OK) { + // Configure the SDIO peripheral, we need this delay for some reason... + while ((SDIO_CLKCTL & 0x800) != WideMode) { + reg = SDIO_CLKCTL; + reg &= ~CLKCTL_CLEAR_MASK; + reg |=(uint32_t) WideMode; + SDIO_CLKCTL = reg; + } + } + } else { + ErrorState = SD_UNSUPPORTED_FEATURE; + } + + return ErrorState; +} + +static SD_Error_t SD_HighSpeed(void) +{ + SD_Error_t ErrorState; + uint8_t SD_hs[64] = {0}; + uint32_t SD_scr[2] = {0, 0}; + uint32_t SD_SPEC = 0; + uint32_t Count = 0; + uint32_t* Buffer = (uint32_t *)SD_hs; + + // Initialize the Data control register + SDIO_DATACTL = 0; + + // Get SCR Register + if((ErrorState = SD_FindSCR(SD_scr)) != SD_OK) { + return ErrorState; + } + + // Test the Version supported by the card + SD_SPEC = (SD_scr[1] & 0x01000000) | (SD_scr[1] & 0x02000000); + + if(SD_SPEC != SD_ALLZERO) { + // Set Block Size for Card + if((ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), 64, 1)) != SD_OK) { + return ErrorState; + } + + // Configure the SD DPSM (Data Path State Machine) + SD_DataTransferInit(64, SD_DATABLOCK_SIZE_64B, true); + + // Send CMD6 switch mode + if((ErrorState =SD_TransmitCommand((SD_CMD_HS_SWITCH | SD_CMD_RESPONSE_SHORT), 0x80FFFF01, 1)) != SD_OK) { + return ErrorState; + } + + while((SDIO_STAT & (SDIO_STAT_RXORE | SDIO_STAT_DTCRCERR | SDIO_STAT_DTTMOUT | SDIO_STAT_DTBLKEND)) == 0) { + if((SDIO_STAT & SDIO_STAT_RFH) != 0) { + for(Count = 0; Count < 8; Count++) { + *(Buffer + Count) = SDIO_FIFO; + } + + Buffer += 8; + } + } + + if((SDIO_STAT & SDIO_STAT_DTTMOUT) != 0) return SD_DATA_TIMEOUT; + else if((SDIO_STAT & SDIO_STAT_DTCRCERR) != 0) return SD_DATA_CRC_FAIL; + else if((SDIO_STAT & SDIO_STAT_RXORE) != 0) return SD_RX_OVERRUN; + + Count = SD_DATATIMEOUT; + + while(((SDIO_STAT & SDIO_STAT_RXDTVAL) != 0) && (Count > 0)) { + *Buffer = SDIO_FIFO; + Buffer++; + Count--; + } + + // Test if the switch mode HS is ok + if((SD_hs[13] & 2) != 2) { + ErrorState = SD_UNSUPPORTED_FEATURE; + } + } + + return ErrorState; +} + +static SD_Error_t SD_GetStatus(void) +{ + SD_Error_t ErrorState; + uint32_t Response1; + SD_CardState_t CardState; + + // Send Status command + if((ErrorState = SD_TransmitCommand((SD_CMD_SEND_STATUS | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) == SD_OK) { + Response1 = SDIO_RESP0; + CardState = (SD_CardState_t)((Response1 >> 9) & 0x0F); + + // Find SD status according to card state + if (CardState == SD_CARD_TRANSFER) ErrorState = SD_OK; + else if(CardState == SD_CARD_ERROR) ErrorState = SD_ERROR; + else ErrorState = SD_BUSY; + } else { + ErrorState = SD_ERROR; + } + + return ErrorState; +} + +/*! + \brief Gets the SD card status + \param[in] none + \param[out] pCardStatus: pointer to SD card status structure + \retval SD Card error state +*/ +SD_Error_t SD_GetCardStatus(SD_CardStatus_t* pCardStatus) +{ + SD_Error_t ErrorState; + uint32_t Temp = 0; + uint32_t Status[16]; + uint32_t Count; + + // Check SD response + if((SDIO_RESP0 & SD_CARD_LOCKED) == SD_CARD_LOCKED) { + return SD_LOCK_UNLOCK_FAILED; + } + + // Set block size for card if it is not equal to current block size for card + if((ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), 64, 1)) != SD_OK) { + return ErrorState; + } + + // Send CMD55 + if((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) != SD_OK) { + return ErrorState; + } + + // Configure the SD DPSM (Data Path State Machine) + SD_DataTransferInit(64, SD_DATABLOCK_SIZE_64B, true); + + // Send ACMD13 (SD_APP_STAUS) with argument as card's RCA + if((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_STATUS | SD_CMD_RESPONSE_SHORT), 0, 1)) != SD_OK) { + return ErrorState; + } + + // Get status data + while((SDIO_STAT & (SDIO_STAT_RXORE | SDIO_STAT_DTCRCERR | SDIO_STAT_DTTMOUT | SDIO_STAT_DTBLKEND)) == 0) { + if((SDIO_STAT & SDIO_STAT_RFH) != 0) { + for(Count = 0; Count < 8; Count++) { + Status[Count] = SDIO_FIFO; + } + } + } + + if((SDIO_STAT & SDIO_STAT_DTTMOUT) != 0) return SD_DATA_TIMEOUT; + else if((SDIO_STAT & SDIO_STAT_DTCRCERR) != 0) return SD_DATA_CRC_FAIL; + else if((SDIO_STAT & SDIO_STAT_RXORE) != 0) return SD_RX_OVERRUN; + else + { + } + + // Byte 0 + Temp = (Status[0] & 0xC0) >> 6; + pCardStatus->DAT_BUS_WIDTH = (uint8_t)Temp; + + // Byte 0 + Temp = (Status[0] & 0x20) >> 5; + pCardStatus->SECURED_MODE = (uint8_t)Temp; + + // Byte 2 + Temp = (Status[2] & 0xFF); + pCardStatus->SD_CARD_TYPE = (uint8_t)(Temp << 8); + + // Byte 3 + Temp = (Status[3] & 0xFF); + pCardStatus->SD_CARD_TYPE |= (uint8_t)Temp; + + // Byte 4 + Temp = (Status[4] & 0xFF); + pCardStatus->SIZE_OF_PROTECTED_AREA = (uint8_t)(Temp << 24); + + // Byte 5 + Temp = (Status[5] & 0xFF); + pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)(Temp << 16); + + // Byte 6 + Temp = (Status[6] & 0xFF); + pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)(Temp << 8); + + // Byte 7 + Temp = (Status[7] & 0xFF); + pCardStatus->SIZE_OF_PROTECTED_AREA |= (uint8_t)Temp; + + // Byte 8 + Temp = (Status[8] & 0xFF); + pCardStatus->SPEED_CLASS = (uint8_t)Temp; + + // Byte 9 + Temp = (Status[9] & 0xFF); + pCardStatus->PERFORMANCE_MOVE = (uint8_t)Temp; + + // Byte 10 + Temp = (Status[10] & 0xF0) >> 4; + pCardStatus->AU_SIZE = (uint8_t)Temp; + + // Byte 11 + Temp = (Status[11] & 0xFF); + pCardStatus->ERASE_SIZE = (uint8_t)(Temp << 8); + + // Byte 12 + Temp = (Status[12] & 0xFF); + pCardStatus->ERASE_SIZE |= (uint8_t)Temp; + + // Byte 13 + Temp = (Status[13] & 0xFC) >> 2; + pCardStatus->ERASE_TIMEOUT = (uint8_t)Temp; + + // Byte 13 + Temp = (Status[13] & 0x3); + pCardStatus->ERASE_OFFSET = (uint8_t)Temp; + + return SD_OK; +} + +static SD_Error_t SD_PowerON(void) +{ + SD_Error_t ErrorState; + uint32_t Response = 0; + uint32_t Count; + uint32_t ValidVoltage; + uint32_t SD_Type; + + Count = 0; + ValidVoltage = 0; + SD_Type = SD_RESP_STD_CAPACITY; + + SDIO_CLKCTL &= ~SDIO_CLKCTL_CLKEN; // Disable SDIO Clock + SDIO_PWRCTL = SDIO_PWRCTL_PWRCTL; // Set Power State to ON + + delay(2); + + SDIO_CLKCTL |= SDIO_CLKCTL_CLKEN; // Enable SDIO Clock + + // CMD0: GO_IDLE_STATE ----------------------------------------------------- + // No CMD response required + if((ErrorState = SD_TransmitCommand(SD_CMD_GO_IDLE_STATE, 0, 0)) != SD_OK) { + // CMD Response Timeout (wait for CMDSENT flag) + return ErrorState; + } + + // CMD8: SEND_IF_COND ------------------------------------------------------ + // Send CMD8 to verify SD card interface operating condition + // Argument: - [31:12]: Reserved (shall be set to '0') + //- [11:8]: Supply Voltage (VHS) 0x1 (Range: 2.7-3.6 V) + //- [7:0]: Check Pattern (recommended 0xAA) + // CMD Response: R7 */ + if((ErrorState = SD_TransmitCommand((SD_SDIO_SEND_IF_COND | SD_CMD_RESPONSE_SHORT), SD_CHECK_PATTERN, 7)) == SD_OK) { + // SD Card 2.0 + SD_CardType = SD_STD_CAPACITY_V2_0; + SD_Type = SD_RESP_HIGH_CAPACITY; + } + + // Send CMD55 + // If ErrorState is Command Timeout, it is a MMC card + // If ErrorState is SD_OK it is a SD card: SD card 2.0 (voltage range mismatch) or SD card 1.x + if((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1)) == SD_OK) { + // SD CARD + // Send ACMD41 SD_APP_OP_COND with Argument 0x80100000 + while((ValidVoltage == 0) && (Count < SD_MAX_VOLT_TRIAL)) { + // SEND CMD55 APP_CMD with RCA as 0 + if((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), 0, 1)) != SD_OK) { + return ErrorState; + } + + // Send CMD41 + if((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_OP_COND | SD_CMD_RESPONSE_SHORT), SD_VOLTAGE_WINDOW_SD | SD_Type, 3)) != SD_OK) { + return ErrorState; + } + + Response = SDIO_RESP0; // Get command response + ValidVoltage = (((Response >> 31) == 1) ? 1 : 0); // Get operating voltage + Count++; + } + + if(Count >= SD_MAX_VOLT_TRIAL) { + return SD_INVALID_VOLTRANGE; + } + + if((Response & SD_RESP_HIGH_CAPACITY) == SD_RESP_HIGH_CAPACITY) { + SD_CardType = SD_HIGH_CAPACITY; + } + } // else MMC Card + + return ErrorState; +} + +static SD_Error_t SD_FindSCR(uint32_t *pSCR) +{ + SD_Error_t ErrorState; + uint32_t Index = 0; + uint32_t tempscr[2] = {0, 0}; + + // Set Block Size To 8 Bytes + // Send CMD55 APP_CMD with argument as card's RCA + if((ErrorState = SD_TransmitCommand((SD_CMD_SET_BLOCKLEN | SD_CMD_RESPONSE_SHORT), 8, 1)) == SD_OK) { + // Send CMD55 APP_CMD with argument as card's RCA + if((ErrorState = SD_TransmitCommand((SD_CMD_APP_CMD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1)) == SD_OK) { + SD_DataTransferInit(8, SD_DATABLOCK_SIZE_8B, true); + + // Send ACMD51 SD_APP_SEND_SCR with argument as 0 + if((ErrorState = SD_TransmitCommand((SD_CMD_SD_APP_SEND_SCR | SD_CMD_RESPONSE_SHORT), 0, 1)) == SD_OK) { + while((SDIO_STAT & (SDIO_STAT_RXORE | SDIO_STAT_DTCRCERR | SDIO_STAT_DTTMOUT | SDIO_STAT_DTBLKEND)) == 0) { + if((SDIO_STAT & SDIO_STAT_RXDTVAL) != 0) { + *(tempscr + Index) = SDIO_FIFO; + Index++; + } + } + + if ((SDIO_STAT & SDIO_STAT_DTTMOUT) != 0) ErrorState = SD_DATA_TIMEOUT; + else if((SDIO_STAT & SDIO_STAT_DTCRCERR) != 0) ErrorState = SD_DATA_CRC_FAIL; + else if((SDIO_STAT & SDIO_STAT_RXORE) != 0) ErrorState = SD_RX_OVERRUN; + else if((SDIO_STAT & SDIO_STAT_RXDTVAL) != 0) ErrorState = SD_OUT_OF_BOUND; + else + { + *(pSCR + 1) = ((tempscr[0] & SD_0TO7BITS) << 24) | ((tempscr[0] & SD_8TO15BITS) << 8) | + ((tempscr[0] & SD_16TO23BITS) >> 8) | ((tempscr[0] & SD_24TO31BITS) >> 24); + + *(pSCR) = ((tempscr[1] & SD_0TO7BITS) << 24) | ((tempscr[1] & SD_8TO15BITS) << 8) | + ((tempscr[1] & SD_16TO23BITS) >> 8) | ((tempscr[1] & SD_24TO31BITS) >> 24); + } + + } + } + } + + return ErrorState; +} + +/*! + \brief Initialize the SDIO module, DMA, and IO + \param[in] dma: DMA stream pointer + \param[out] none + \retval initialization success status +*/ +bool SD_Initialize_LL(DMA_Stream_TypeDef *dma) +{ + const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier((dmaResource_t *)dma); + + dmaStream = dma; + + if (!((uint32_t)dma == DMA1_CH3_BASE || (uint32_t)dma == DMA1_CH6_BASE) || !dmaAllocate(dmaIdentifier, OWNER_SDCARD, 0)) { + return false; + } + + // Reset SDIO Module + RCU_APB2RST |= RCU_APB2RST_SDIORST; + delay(1); + RCU_APB2RST &= ~RCU_APB2RST_SDIORST; + delay(1); + + // Enable SDIO clock + RCU_APB2EN |= RCU_APB2EN_SDIOEN; + + // Enable DMA2 clocks + RCU_AHB1EN |= RCU_AHB1EN_DMA1EN; + + //Configure Pins + RCU_AHB1EN |= RCU_AHB1EN_PCEN | RCU_AHB1EN_PDEN; + + uint8_t is4BitWidth = sdioConfig()->use4BitWidth; + + const IO_t d0 = IOGetByTag(IO_TAG(PC8)); + const IO_t d1 = IOGetByTag(IO_TAG(PC9)); + const IO_t d2 = IOGetByTag(IO_TAG(PC10)); + const IO_t d3 = IOGetByTag(IO_TAG(PC11)); + const IO_t clk = IOGetByTag(IO_TAG(PC12)); + const IO_t cmd = IOGetByTag(IO_TAG(PD2)); + + IOInit(d0, OWNER_SDCARD, 0); + if (is4BitWidth) { + IOInit(d1, OWNER_SDCARD, 0); + IOInit(d2, OWNER_SDCARD, 0); + IOInit(d3, OWNER_SDCARD, 0); + } + IOInit(clk, OWNER_SDCARD, 0); + IOInit(cmd, OWNER_SDCARD, 0); + +#define SDIO_DATA IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_25MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP) +#define SDIO_CMD IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_25MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLUP) +#define SDIO_CLK IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_25MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE) + + IOConfigGPIOAF(d0, SDIO_DATA, GPIO_AF_12); + + if (is4BitWidth) { + IOConfigGPIOAF(d1, SDIO_DATA, GPIO_AF_12); + IOConfigGPIOAF(d2, SDIO_DATA, GPIO_AF_12); + IOConfigGPIOAF(d3, SDIO_DATA, GPIO_AF_12); + } + + IOConfigGPIOAF(clk, SDIO_CLK, GPIO_AF_12); + IOConfigGPIOAF(cmd, SDIO_CMD, GPIO_AF_12); + + // NVIC configuration for SDIO interrupts + nvic_irq_enable(SDIO_IRQn, 1, 0); + + gd32_dma_chbase_parse((uint32_t)dma, &dma_periph_sdio, &dma_channel_sdio); + + RCU_AHB1EN |= RCU_AHB1EN_DMA1EN; + // Initialize DMA + DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) = 0; // Reset DMA Stream control register + DMA_CHPADDR(dma_periph_sdio,dma_channel_sdio) = (uint32_t)&SDIO_FIFO; + + if ((uint32_t)dmaStream == DMA1_CH3_BASE) { + DMA_INTC0(dma_periph_sdio) = INTC_CLEAR_MASK_CH3; // Clear all interrupt flags + } else { + DMA_INTC1(dma_periph_sdio) = INTC_CLEAR_MASK_CH6; // Clear all interrupt flags + } + + DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) = (DMA_CHANNEL_4 | DMA_CHXCTL_TFCS | // Prepare the DMA Stream configuration + DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register + DMA_MDATAALIGN_WORD | DMA_PRIORITY_ULTRA_HIGH | + DMA_MBURST_INC4 | DMA_PBURST_INC4 | + DMA_MEMORY_TO_PERIPH); + DMA_CHFCTL(dma_periph_sdio,dma_channel_sdio) = (DMA_CHXFCTL_MDMEN | DMA_FIFO_4_WORD); // Configuration FIFO control register + dmaEnable(dmaIdentifier); + + if ((uint32_t)dmaStream == DMA1_CH3_BASE) { + dmaSetHandler(dmaIdentifier, SDIO_DMA_ST3_IRQHandler, 1, 0); + } else { + dmaSetHandler(dmaIdentifier, SDIO_DMA_ST6_IRQHandler, 1, 0); + } + + return true; +} + +bool SD_GetState(void) +{ + // Check SDCARD status + if(SD_GetStatus() == SD_OK) return true; + return false; +} + +static SD_Error_t SD_DoInit(void) +{ + SD_Error_t errorState; + uint32_t reg; + + // Initialize SDIO peripheral interface with default configuration for SD card initialization. + reg = SDIO_CLKCTL; + reg &= ~CLKCTL_CLEAR_MASK; + reg |= (uint32_t)SDIO_INIT_CLK_DIV; + SDIO_CLKCTL = reg; + + // Identify card operating voltage. + errorState = SD_PowerON(); + if (errorState != SD_OK) { + return errorState; + } + + // Initialize the present card and put them in idle state. + errorState = SD_InitializeCard(); + if (errorState != SD_OK) { + return errorState; + } + + // Read CSD/CID MSD registers. + errorState = SD_GetCardInfo(); + if (errorState != SD_OK) { + return errorState; + } + + // Select the Card - Send CMD7 SDIO_SEL_DESEL_CARD. + errorState = SD_TransmitCommand((SD_CMD_SEL_DESEL_CARD | SD_CMD_RESPONSE_SHORT), SD_CardRCA, 1); + // Configure SDIO peripheral interface. + reg = SDIO_CLKCTL; + reg &= ~CLKCTL_CLEAR_MASK; + reg |= (uint32_t) SDIO_CLK_DIV; + SDIO_CLKCTL = reg; + + // Configure SD Bus width. + if (errorState == SD_OK) { + // Enable wide operation. + if (sdioConfig()->use4BitWidth) { + errorState = SD_WideBusOperationConfig(SD_BUS_WIDE_4B); + } else { + errorState = SD_WideBusOperationConfig(SD_BUS_WIDE_1B); + } + + if (errorState == SD_OK && sdioConfig()->clockBypass) { + if (SD_HighSpeed()) { + SDIO_CLKCTL |= SDIO_CLKCTL_CLKBYP; + SDIO_CLKCTL |= SDIO_CLKCTL_CLKEDGE; + } + } + } + + return errorState; +} + +SD_Error_t SD_Init(void) +{ + static bool sdInitAttempted = false; + static SD_Error_t result = SD_ERROR; + + if (sdInitAttempted) { + return result; + } + + sdInitAttempted = true; + + result = SD_DoInit(); + + return result; +} + +/*! + \brief This function handles SD card interrupt request + \param[in] none + \param[out] none + \retval none +*/ +void SDIO_IRQHandler(void) +{ + // Check for SDIO interrupt flags + if ((SDIO_STAT & SDIO_STAT_DTEND) != 0) { + SDIO_INTC = SDIO_INTC_DTENDC; + SDIO_INTC = SDIO_INTC_STATIC_FLAGS; + SDIO_INTEN &= ~(SDIO_INTEN_DTENDIE | SDIO_INTEN_DTCRCERRIE | SDIO_INTEN_DTTMOUTIE | + SDIO_INTEN_TXUREIE | SDIO_INTEN_RXOREIE | SDIO_INTEN_TFHIE | SDIO_INTEN_RFHIE); + + /* Currently doesn't implement multiple block write handling */ + if ((SD_Handle.Operation & 0x02) == (SDIO_DIR_TX << 1)) { + /* Disable the stream */ + DMA_CHCTL(dma_periph_sdio,dma_channel_sdio) &= ~DMA_CHXCTL_CHEN; + SDIO_DATACTL &= ~(SDIO_DATACTL_DMAEN); + /* Transfer is complete */ + SD_Handle.TXCplt = 0; + if ((SD_Handle.Operation & 0x01) == SD_MULTIPLE_BLOCK) { + /* Send stop command in multiblock write */ + SD_TransmitCommand((SD_CMD_STOP_TRANSMISSION | SD_CMD_RESPONSE_SHORT), 0, 1); + } + } + + SD_Handle.TransferComplete = 1; + SD_Handle.TransferError = SD_OK; + } else if ((SDIO_STAT & SDIO_STAT_DTCRCERR) != 0) + SD_Handle.TransferError = SD_DATA_CRC_FAIL; + else if ((SDIO_STAT & SDIO_STAT_DTTMOUT) != 0) + SD_Handle.TransferError = SD_DATA_TIMEOUT; + else if ((SDIO_STAT & SDIO_STAT_RXORE) != 0) + SD_Handle.TransferError = SD_RX_OVERRUN; + else if ((SDIO_STAT & SDIO_STAT_TXURE) != 0) + SD_Handle.TransferError = SD_TX_UNDERRUN; + + SDIO_INTC = SDIO_INTC_STATIC_FLAGS; + + // Disable all SDIO peripheral interrupt sources + SDIO_INTEN &= ~(SDIO_INTEN_DTCRCERRIE | SDIO_INTEN_DTTMOUTIE | + SDIO_INTEN_DTENDIE | + SDIO_INTEN_TFHIE | SDIO_INTEN_RFHIE | SDIO_INTEN_TXUREIE | + SDIO_INTEN_RXOREIE); +} + +/*! + \brief This function handles DMA2 Stream 3 interrupt request + \param[in] dma: DMA channel descriptor + \param[out] none + \retval none +*/ +void SDIO_DMA_ST3_IRQHandler(dmaChannelDescriptor_t *dma) +{ + UNUSED(dma); + // Transfer Error Interrupt management + if((DMA_INTF0(DMA1) & DMA_INTF0_TAEIF3) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_TAEIE) != 0) { + DMA_CHCTL(DMA1, DMA_CH3) &= ~DMA_CHXCTL_TAEIE; // Disable the transfer error interrupt + DMA_INTC0(DMA1) = DMA_INTC0_TAEIFC3; // Clear the transfer error flag + } + } + + // FIFO Error Interrupt management + if((DMA_INTF0(DMA1) & DMA_INTF0_FEEIF3) != 0) { + if((DMA_CHFCTL(DMA1, DMA_CH3) & DMA_CHXFCTL_FEEIE) != 0) { + DMA_CHFCTL(DMA1, DMA_CH3) &= ~DMA_CHXFCTL_FEEIE; // Disable the FIFO Error interrupt + DMA_INTC0(DMA1) = DMA_INTC0_FEEIFC3; // Clear the FIFO error flag + } + } + + // Direct Mode Error Interrupt management + if((DMA_INTF0(DMA1) & DMA_INTF0_DMEIF3) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_SDEIE) != 0) { + DMA_CHCTL(DMA1, DMA_CH3) &= ~DMA_CHXCTL_SDEIE; // Disable the direct mode Error interrupt + DMA_INTC0(DMA1) = DMA_INTC0_CDMEIF3; // Clear the FIFO error flag + } + } + + // Half Transfer Complete Interrupt management + if((DMA_INTF0(DMA1) & DMA_INTF0_HTIF3) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_HTFIE) != 0) { + if(((DMA_CHCTL(DMA1, DMA_CH3)) & (uint32_t)(DMA_CHXCTL_SBMEN)) != 0) { + DMA_INTC0(DMA1) = DMA_INTC0_CHTIF3; + } else { + if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_CMEN) == 0) { + DMA_CHCTL(DMA1, DMA_CH3) &= ~DMA_CHXCTL_HTFIE; + } + + DMA_INTC0(DMA1) = DMA_INTC0_CHTIF3; + } + } + } + + // Transfer Complete Interrupt management + if((DMA_INTF0(DMA1) & DMA_INTF0_TCIF3) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_FTFIE) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH3) & (uint32_t)(DMA_CHXCTL_SBMEN)) != 0) { + DMA_INTC0(DMA1) = DMA_INTC0_CTCIF3; + } else { + if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_CMEN) == 0) { + DMA_CHCTL(DMA1, DMA_CH3) &= ~DMA_CHXCTL_FTFIE; + } + + DMA_INTC0(DMA1) = DMA_INTC0_CTCIF3; + SD_DMA_Complete(DMA1, DMA_CH3); + } + } + } +} + +/*! + \brief This function handles DMA2 Stream 6 interrupt request + \param[in] dma: DMA channel descriptor + \param[out] none + \retval none +*/ +void SDIO_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma) +{ + UNUSED(dma); + // Transfer Error Interrupt management + if((DMA_INTF1(DMA1) & DMA_HISR_TEIF6) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH6)& DMA_CHXCTL_TAEIE) != 0) { + DMA_CHCTL(DMA1, DMA_CH6) &= ~DMA_CHXCTL_TAEIE; + DMA_INTC1(dma_periph_sdio) = DMA_HIFCR_CTEIF6; + } + } + + // FIFO Error Interrupt management + if((DMA_INTF1(DMA1) & DMA_HISR_FEIF6) != 0) { + if((DMA_CHFCTL(DMA1, DMA_CH6) & DMA_CHXFCTL_FEEIE) != 0) { + DMA_CHFCTL(DMA1, DMA_CH6) &= ~DMA_CHXFCTL_FEEIE; + DMA_INTC1(dma_periph_sdio) = DMA_HIFCR_CFEIF6; + } + } + + // Direct Mode Error Interrupt management + if((DMA_INTF1(DMA1) & DMA_HISR_DMEIF6) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH6)& DMA_CHXCTL_SDEIE) != 0) { + DMA_CHCTL(DMA1, DMA_CH6) &= ~DMA_CHXCTL_SDEIE; + DMA_INTC1(dma_periph_sdio) = DMA_HIFCR_CDMEIF6; + } + } + + // Half Transfer Complete Interrupt management + if((DMA_INTF1(DMA1) & DMA_HISR_HTIF6) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH6)& DMA_CHXCTL_HTFIE) != 0) { + if(((DMA_CHCTL(DMA1, DMA_CH6)) & (uint32_t)(DMA_CHXCTL_SBMEN)) != 0) { + DMA_INTC1(dma_periph_sdio) = DMA_HIFCR_CHTIF6; + } else { + if((DMA_CHCTL(DMA1, DMA_CH6)& DMA_CHXCTL_CMEN) == 0) { + DMA_CHCTL(DMA1, DMA_CH6)&= ~DMA_CHXCTL_HTFIE; + } + + DMA_INTC1(dma_periph_sdio) = DMA_HIFCR_CHTIF6; + } + } + } + + // Transfer Complete Interrupt management + if((DMA_INTF1(DMA1) & DMA_HISR_TCIF6) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH6)& DMA_CHXCTL_FTFIE) != 0) { + if((DMA_CHCTL(DMA1, DMA_CH6)& (uint32_t)(DMA_CHXCTL_SBMEN)) != 0) { + DMA_INTC1(dma_periph_sdio) = DMA_HIFCR_CTCIF6; + } else { + if((DMA_CHCTL(DMA1, DMA_CH6)& DMA_CHXCTL_CMEN) == 0) { + DMA_CHCTL(DMA1, DMA_CH6) &= ~DMA_CHXCTL_FTFIE; + } + + DMA_INTC1(dma_periph_sdio) = DMA_HIFCR_CTCIF6; + SD_DMA_Complete(DMA1, DMA_CH6); + } + } + } +} + +#endif diff --git a/src/platform/GD32/serial_uart_gd32f4xx.c b/src/platform/GD32/serial_uart_gd32f4xx.c new file mode 100755 index 0000000000..4382192b96 --- /dev/null +++ b/src/platform/GD32/serial_uart_gd32f4xx.c @@ -0,0 +1,311 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +/* + * jflyper - Refactoring, cleanup and made pin-configurable + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_UART + +#include "build/debug.h" + +#include "drivers/system.h" +#include "drivers/io.h" +#include "drivers/dma.h" +#include "drivers/nvic.h" +#include "platform/rcc.h" + +#include "drivers/serial.h" +#include "drivers/serial_uart.h" +#include "drivers/serial_uart_impl.h" + +const uartHardware_t uartHardware[UARTDEV_COUNT] = { +#ifdef USE_UART0 + { + .identifier = SERIAL_PORT_UART0, + .reg = (void *)USART0, + .rxDMAChannel = DMA_SUBPERI4, + .txDMAChannel = DMA_SUBPERI4, +#ifdef USE_UART0_RX_DMA + .rxDMAResource = &(dmaResource_t){DMA1, DMA_CH5}, +#endif +#ifdef USE_UART0_TX_DMA + .txDMAResource = &(dmaResource_t){DMA1, DMA_CH7}, +#endif + .rxPins = { { DEFIO_TAG_E(PA10) }, { DEFIO_TAG_E(PB3) }, { DEFIO_TAG_E(PB7) }, }, + .txPins = { { DEFIO_TAG_E(PA9) }, { DEFIO_TAG_E(PA15) }, { DEFIO_TAG_E(PB6) }, }, + .af = GPIO_AF_7, + .rcc = RCC_APB2(USART0), + .irqn = USART0_IRQn, + .txPriority = NVIC_PRIO_SERIALUART0_TXDMA, + .rxPriority = NVIC_PRIO_SERIALUART0, + .txBuffer = uart0TxBuffer, + .rxBuffer = uart0RxBuffer, + .txBufferSize = sizeof(uart0TxBuffer), + .rxBufferSize = sizeof(uart0RxBuffer), + }, +#endif + +#ifdef USE_UART1 + { + .identifier = SERIAL_PORT_USART1, + .reg = (void *)USART1, + .rxDMAChannel = DMA_SUBPERI4, + .txDMAChannel = DMA_SUBPERI4, +#ifdef USE_UART1_RX_DMA + .rxDMAResource = &(dmaResource_t){DMA0, DMA_CH5}, +#endif +#ifdef USE_UART1_TX_DMA + .txDMAResource = &(dmaResource_t){DMA0, DMA_CH6}, +#endif + .rxPins = { { DEFIO_TAG_E(PA3) }, { DEFIO_TAG_E(PD6) } }, + .txPins = { { DEFIO_TAG_E(PA2) }, { DEFIO_TAG_E(PD5) } }, + .af = GPIO_AF_7, + .rcc = RCC_APB1(USART1), + .irqn = USART1_IRQn, + .txPriority = NVIC_PRIO_SERIALUART1_TXDMA, + .rxPriority = NVIC_PRIO_SERIALUART1, + .txBuffer = uart1TxBuffer, + .rxBuffer = uart1RxBuffer, + .txBufferSize = sizeof(uart1TxBuffer), + .rxBufferSize = sizeof(uart1RxBuffer), + }, +#endif + +#ifdef USE_UART2 + { + .identifier = SERIAL_PORT_USART2, + .reg = (void *)USART2, + .rxDMAChannel = DMA_SUBPERI4, + .txDMAChannel = DMA_SUBPERI4, +#ifdef USE_UART2_RX_DMA + .rxDMAResource = &(dmaResource_t){DMA0, DMA_CH1}, +#endif +#ifdef USE_UART2_TX_DMA + .txDMAResource = &(dmaResource_t){DMA0, DMA_CH3}, +#endif + .rxPins = { { DEFIO_TAG_E(PB11) }, { DEFIO_TAG_E(PC11) }, { DEFIO_TAG_E(PD9) } }, + .txPins = { { DEFIO_TAG_E(PB10) }, { DEFIO_TAG_E(PC10) }, { DEFIO_TAG_E(PD8) } }, + .af = GPIO_AF_7, + .rcc = RCC_APB1(USART2), + .irqn = USART2_IRQn, + .txPriority = NVIC_PRIO_SERIALUART2_TXDMA, + .rxPriority = NVIC_PRIO_SERIALUART2, + .txBuffer = uart2TxBuffer, + .rxBuffer = uart2RxBuffer, + .txBufferSize = sizeof(uart2TxBuffer), + .rxBufferSize = sizeof(uart2RxBuffer), + }, +#endif + +#ifdef USE_UART3 + { + .identifier = SERIAL_PORT_UART3, + .reg = (void *)UART3, + .rxDMAChannel = DMA_SUBPERI4, + .txDMAChannel = DMA_SUBPERI4, +#ifdef USE_UART3_RX_DMA + .rxDMAResource = &(dmaResource_t){DMA0, DMA_CH2}, +#endif +#ifdef USE_UART3_TX_DMA + .txDMAResource = &(dmaResource_t){DMA0, DMA_CH4}, +#endif + .rxPins = { { DEFIO_TAG_E(PA1) }, { DEFIO_TAG_E(PC11) } }, + .txPins = { { DEFIO_TAG_E(PA0) }, { DEFIO_TAG_E(PC10) } }, + .af = GPIO_AF_8, + .rcc = RCC_APB1(UART3), + .irqn = UART3_IRQn, + .txPriority = NVIC_PRIO_SERIALUART3_TXDMA, + .rxPriority = NVIC_PRIO_SERIALUART3, + .txBuffer = uart3TxBuffer, + .rxBuffer = uart3RxBuffer, + .txBufferSize = sizeof(uart3TxBuffer), + .rxBufferSize = sizeof(uart3RxBuffer), + }, +#endif + +#ifdef USE_UART4 + { + .identifier = SERIAL_PORT_UART4, + .reg = (void *)UART4, + .rxDMAChannel = DMA_SUBPERI4, + .txDMAChannel = DMA_SUBPERI4, +#ifdef USE_UART4_RX_DMA + .rxDMAResource = &(dmaResource_t){DMA0, DMA_CH0}, +#endif +#ifdef USE_UART4_TX_DMA + .txDMAResource = &(dmaResource_t){DMA0, DMA_CH7}, +#endif + .rxPins = { { DEFIO_TAG_E(PD2) } }, + .txPins = { { DEFIO_TAG_E(PC12) } }, + .af = GPIO_AF_8, + .rcc = RCC_APB1(UART4), + .irqn = UART4_IRQn, + .txPriority = NVIC_PRIO_SERIALUART4_TXDMA, + .rxPriority = NVIC_PRIO_SERIALUART4, + .txBuffer = uart4TxBuffer, + .rxBuffer = uart4RxBuffer, + .txBufferSize = sizeof(uart4TxBuffer), + .rxBufferSize = sizeof(uart4RxBuffer), + }, +#endif + +#ifdef USE_UART5 + { + .identifier = SERIAL_PORT_UART5, + .reg = (void *)USART5, + .rxDMAChannel = DMA_SUBPERI5, + .txDMAChannel = DMA_SUBPERI5, +#ifdef USE_UART5_RX_DMA + .rxDMAResource = &(dmaResource_t){DMA1, DMA_CH1}, +#endif +#ifdef USE_UART5_TX_DMA + .txDMAResource = &(dmaResource_t){DMA1, DMA_CH6}, +#endif + .rxPins = { { DEFIO_TAG_E(PC7) }, { DEFIO_TAG_E(PG9) } }, + .txPins = { { DEFIO_TAG_E(PC6) }, { DEFIO_TAG_E(PG14) } }, + .af = GPIO_AF_8, + .rcc = RCC_APB2(USART5), + .irqn = USART5_IRQn, + .txPriority = NVIC_PRIO_SERIALUART5_TXDMA, + .rxPriority = NVIC_PRIO_SERIALUART5, + .txBuffer = uart5TxBuffer, + .rxBuffer = uart5RxBuffer, + .txBufferSize = sizeof(uart5TxBuffer), + .rxBufferSize = sizeof(uart5RxBuffer), + }, +#endif + +}; + +bool checkUsartTxOutput(uartPort_t *s) +{ + uartDevice_t *uart = container_of(s, uartDevice_t, port); + IO_t txIO = IOGetByTag(uart->tx.pin); + + if ((uart->txPinState == TX_PIN_MONITOR) && txIO) { + if (IORead(txIO)) { + uart->txPinState = TX_PIN_ACTIVE; + IOConfigGPIOAF(txIO, IOCFG_AF_PP, uart->hardware->af); + usart_transmit_config((uint32_t)s->USARTx, USART_TRANSMIT_ENABLE); + + return true; + } else { + return false; + } + } + + return true; +} + +void uartTxMonitor(uartPort_t *s) +{ + uartDevice_t *uart = container_of(s, uartDevice_t, port); + + if (uart->txPinState == TX_PIN_ACTIVE) { + IO_t txIO = IOGetByTag(uart->tx.pin); + + usart_transmit_config((uint32_t)s->USARTx, USART_TRANSMIT_DISABLE); + uart->txPinState = TX_PIN_MONITOR; + IOConfigGPIO(txIO, IOCFG_IPU); + } +} + +static void handleUsartTxDma(uartPort_t *s) +{ + uartDevice_t *uart = container_of(s, uartDevice_t, port); + + uartTryStartTxDMA(s); + + if (s->txDMAEmpty && (uart->txPinState != TX_PIN_IGNORE)) { + uartTxMonitor(s); + } +} + +void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor) +{ + uartPort_t *s = &(((uartDevice_t*)(descriptor->userParam))->port); + + if (dma_interrupt_flag_get((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_FTF)) { + dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_FTF); + dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_HTF); + + if (dma_interrupt_flag_get((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_FEE)) { + dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_FEE); + } + + handleUsartTxDma(s); + } + + if (dma_interrupt_flag_get((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_TAE)) { + dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_TAE); + } + + if (dma_interrupt_flag_get((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_SDE)) { + dma_interrupt_flag_clear((uint32_t)descriptor->dma, descriptor->stream, DMA_INT_FLAG_SDE); + } +} + +void uartIrqHandler(uartPort_t *s) +{ + if (!s->rxDMAResource && (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_RBNE) == SET)) { + if (s->port.rxCallback) { + s->port.rxCallback(usart_data_receive((uint32_t)s->USARTx), s->port.rxCallbackData); + } else { + s->port.rxBuffer[s->port.rxBufferHead] = (uint8_t)usart_data_receive((uint32_t)s->USARTx); + s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize; + } + } + + if (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_TC) == SET) { + uartTxMonitor(s); + usart_interrupt_flag_clear((uint32_t)s->USARTx, USART_INT_FLAG_TC); + } + + if (!s->txDMAResource && (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_TBE) == SET)) { + if (s->port.txBufferTail != s->port.txBufferHead) { + usart_data_transmit((uint32_t)s->USARTx, s->port.txBuffer[s->port.txBufferTail]); + s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize; + } else { + usart_interrupt_disable((uint32_t)s->USARTx, USART_INT_TBE); + } + } + + if (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_ERR_ORERR) == SET) { + usart_interrupt_flag_clear((uint32_t)s->USARTx, USART_INT_FLAG_ERR_ORERR); + } + + if (usart_interrupt_flag_get((uint32_t)s->USARTx, USART_INT_FLAG_IDLE) == SET) { + if (s->port.idleCallback) { + s->port.idleCallback(); + } + + USART_STAT0((uint32_t)s->USARTx); + USART_DATA((uint32_t)s->USARTx); + } + +} +#endif // USE_UART diff --git a/src/platform/GD32/serial_uart_stdperiph.c b/src/platform/GD32/serial_uart_stdperiph.c new file mode 100755 index 0000000000..2efe88ac51 --- /dev/null +++ b/src/platform/GD32/serial_uart_stdperiph.c @@ -0,0 +1,186 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_UART + +#include "build/build_config.h" +#include "build/atomic.h" + +#include "common/utils.h" +#include "drivers/inverter.h" +#include "drivers/nvic.h" +#include "platform/rcc.h" + +#include "drivers/serial.h" +#include "drivers/serial_uart.h" +#include "drivers/serial_uart_impl.h" + +void uartReconfigure(uartPort_t *uartPort) +{ + usart_deinit((uint32_t)uartPort->USARTx); + usart_baudrate_set((uint32_t)uartPort->USARTx, uartPort->port.baudRate); + + if(uartPort->port.options & SERIAL_PARITY_EVEN) { + usart_word_length_set((uint32_t)uartPort->USARTx, USART_WL_9BIT); + } else { + usart_word_length_set((uint32_t)uartPort->USARTx, USART_WL_8BIT); + } + + if(uartPort->port.options & SERIAL_STOPBITS_2) { + usart_stop_bit_set((uint32_t)uartPort->USARTx, USART_STB_2BIT); + } else { + usart_stop_bit_set((uint32_t)uartPort->USARTx, USART_STB_1BIT); + } + + if(uartPort->port.options & SERIAL_PARITY_EVEN) { + usart_parity_config((uint32_t)uartPort->USARTx, USART_PM_EVEN); + } else { + usart_parity_config((uint32_t)uartPort->USARTx, USART_PM_NONE); + } + + usart_hardware_flow_rts_config((uint32_t)uartPort->USARTx, USART_RTS_DISABLE); + usart_hardware_flow_cts_config((uint32_t)uartPort->USARTx, USART_CTS_DISABLE); + + if (uartPort->port.mode & MODE_RX) + usart_receive_config((uint32_t)uartPort->USARTx, USART_RECEIVE_ENABLE); + if (uartPort->port.mode & MODE_TX) + usart_transmit_config((uint32_t)uartPort->USARTx, USART_TRANSMIT_ENABLE); + + uartConfigureExternalPinInversion(uartPort); + + if (uartPort->port.options & SERIAL_BIDIR) { + usart_halfduplex_enable((uint32_t)uartPort->USARTx); + } else { + usart_halfduplex_disable((uint32_t)uartPort->USARTx); + } + + usart_enable((uint32_t)uartPort->USARTx); + + if (uartPort->port.mode & MODE_RX) { +#ifdef USE_DMA + if (uartPort->rxDMAResource) { + dma_single_data_parameter_struct dma_init_struct; + dma_single_data_para_struct_init(&dma_init_struct); + + dma_init_struct.periph_addr = uartPort->rxDMAPeripheralBaseAddr; + dma_init_struct.memory0_addr = (uint32_t)uartPort->port.rxBuffer; + dma_init_struct.number = uartPort->port.rxBufferSize; + dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_8BIT; + dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_ENABLE; + dma_init_struct.direction = DMA_PERIPH_TO_MEMORY; + dma_init_struct.priority = DMA_PRIORITY_MEDIUM; + + xDMA_DeInit(uartPort->rxDMAResource); + gd32_dma_init((uint32_t)uartPort->rxDMAResource, &dma_init_struct); + xDMA_Cmd(uartPort->rxDMAResource, ENABLE); + + usart_dma_receive_config((uint32_t)uartPort->USARTx, USART_RECEIVE_DMA_ENABLE); + + uartPort->rxDMAPos = xDMA_GetCurrDataCounter(uartPort->rxDMAResource); + } else +#endif + { + usart_interrupt_flag_clear((uint32_t)uartPort->USARTx, USART_INT_FLAG_RBNE); + usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_RBNE); + usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_IDLE); + } + } + + if (uartPort->port.mode & MODE_TX) { +#ifdef USE_DMA + if (uartPort->txDMAResource) { + dma_single_data_parameter_struct dma_init_struct; + dma_single_data_para_struct_init(&dma_init_struct); + + dma_init_struct.periph_addr = uartPort->txDMAPeripheralBaseAddr; + dma_init_struct.memory0_addr = (uint32_t)uartPort->port.txBuffer; + dma_init_struct.number = uartPort->port.txBufferSize; + dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_8BIT; + dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_DISABLE; + dma_init_struct.direction = DMA_MEMORY_TO_PERIPH; + dma_init_struct.priority = DMA_PRIORITY_MEDIUM; + + + xDMA_DeInit(uartPort->txDMAResource); + gd32_dma_init((uint32_t)uartPort->txDMAResource, &dma_init_struct); + + xDMA_ITConfig(uartPort->txDMAResource, DMA_INT_FTF | DMA_INT_FEE | DMA_INT_TAE | DMA_INT_SDE, ENABLE); + xDMA_SetCurrDataCounter(uartPort->txDMAResource, 0); + + usart_dma_transmit_config((uint32_t)uartPort->USARTx, USART_TRANSMIT_DMA_ENABLE); + } else +#endif + { + usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_TBE); + } + + usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_TC); + } + + usart_enable((uint32_t)uartPort->USARTx); +} + +#ifdef USE_DMA +void uartTryStartTxDMA(uartPort_t *s) +{ + + ATOMIC_BLOCK(NVIC_PRIO_SERIALUART_TXDMA) { + if (IS_DMA_ENABLED(s->txDMAResource)) { + return; + } + + if (xDMA_GetCurrDataCounter(s->txDMAResource)) { + goto reenable; + } + + if (s->port.txBufferHead == s->port.txBufferTail) { + s->txDMAEmpty = true; + return; + } + + xDMA_MemoryTargetConfig(s->txDMAResource, (uint32_t)&s->port.txBuffer[s->port.txBufferTail], DMA_MEMORY_0); + + unsigned chunk; + if (s->port.txBufferHead > s->port.txBufferTail) { + chunk = s->port.txBufferHead - s->port.txBufferTail; + s->port.txBufferTail = s->port.txBufferHead; + } else { + chunk = s->port.txBufferSize - s->port.txBufferTail; + s->port.txBufferTail = 0; + } + s->txDMAEmpty = false; + xDMA_SetCurrDataCounter(s->txDMAResource, chunk); + reenable: + xDMA_Cmd(s->txDMAResource, ENABLE); + } +} +#endif + +#endif // USE_UART diff --git a/src/platform/GD32/serial_usb_vcp.c b/src/platform/GD32/serial_usb_vcp.c new file mode 100755 index 0000000000..9e3b13d276 --- /dev/null +++ b/src/platform/GD32/serial_usb_vcp.c @@ -0,0 +1,247 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include "platform.h" + +#ifdef USE_VCP + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "drivers/io.h" + +#include "pg/usb.h" + +#include "usbd_core.h" +#include "usbd_cdc_vcp.h" +#ifdef USE_USB_CDC_HID +#include "usb_cdc_hid.h" +#endif +#include "drivers/usb_io.h" + +#include "drivers/time.h" + +#include "drivers/serial.h" +#include "drivers/serial_usb_vcp.h" +#include "drv_usb_hw.h" + +#define USB_TIMEOUT 50 + +static vcpPort_t vcpPort = {0}; + +static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate) +{ + UNUSED(instance); + UNUSED(baudRate); + + // TODO implement +} + +static void usbVcpSetMode(serialPort_t *instance, portMode_e mode) +{ + UNUSED(instance); + UNUSED(mode); + + // TODO implement +} + +static void usbVcpSetCtrlLineStateCb(serialPort_t *instance, void (*cb)(void *context, uint16_t ctrlLineState), void *context) +{ + UNUSED(instance); UNUSED((void (*)(void *context, uint16_t ctrlLineState))cb);UNUSED(context); + + // 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); UNUSED((void (*)(void *context, uint32_t baud))cb); UNUSED(context); + + // Register upper driver baud rate callback routine with USB driver + CDC_SetBaudRateCb((void (*)(void *context, uint32_t baud))cb, (void *)context); +} + +static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance) +{ + UNUSED(instance); + return true; +} + +static uint32_t usbVcpAvailable(const serialPort_t *instance) +{ + UNUSED(instance); + + return CDC_Receive_BytesAvailable(); +} + +static uint8_t usbVcpRead(serialPort_t *instance) +{ + UNUSED(instance); + + uint8_t buf[1]={0}; + + while (true) { + if (CDC_Receive_DATA(buf, 1)) + return buf[0]; + } +} + +static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count) +{ + UNUSED(instance); + + if (!(usbIsConnected() && usbIsConfigured())) { + return; + } + + uint32_t start = millis(); + const uint8_t *p = data; + while (count > 0) { + uint32_t txed = CDC_Send_DATA(p, count); + count -= txed; + p += txed; + + if (millis() - start > USB_TIMEOUT) { + break; + } + } +} + +static bool usbVcpFlush(vcpPort_t *port) +{ + uint32_t count = port->txAt; + port->txAt = 0; + + if (count == 0) { + return true; + } + + if (!usbIsConnected() || !usbIsConfigured()) { + return false; + } + + uint32_t start = millis(); + uint8_t *p = port->txBuf; + while (count > 0) { + uint32_t txed = CDC_Send_DATA(p, count); + count -= txed; + p += txed; + + if (millis() - start > USB_TIMEOUT) { + break; + } + } + return count == 0; +} + +static void usbVcpWrite(serialPort_t *instance, uint8_t c) +{ + vcpPort_t *port = container_of(instance, vcpPort_t, port); + + port->txBuf[port->txAt++] = c; + if (!port->buffering || port->txAt >= ARRAYLEN(port->txBuf)) { + usbVcpFlush(port); + } +} + +static void usbVcpBeginWrite(serialPort_t *instance) +{ + vcpPort_t *port = container_of(instance, vcpPort_t, port); + port->buffering = true; +} + +static uint32_t usbTxBytesFree(const serialPort_t *instance) +{ + UNUSED(instance); + return CDC_Send_FreeBytes(); +} + +static void usbVcpEndWrite(serialPort_t *instance) +{ + vcpPort_t *port = container_of(instance, vcpPort_t, port); + port->buffering = false; + usbVcpFlush(port); +} + +static const struct serialPortVTable usbVTable[] = { + { + .serialWrite = usbVcpWrite, + .serialTotalRxWaiting = usbVcpAvailable, + .serialTotalTxFree = usbTxBytesFree, + .serialRead = usbVcpRead, + .serialSetBaudRate = usbVcpSetBaudRate, + .isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty, + .setMode = usbVcpSetMode, + .setCtrlLineStateCb = usbVcpSetCtrlLineStateCb, + .setBaudRateCb = usbVcpSetBaudRateCb, + .writeBuf = usbVcpWriteBuf, + .beginWrite = usbVcpBeginWrite, + .endWrite = usbVcpEndWrite + } +}; + +void usbd_desc_string_update(void); +serialPort_t *usbVcpOpen(void) +{ + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); + IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); + + usbGenerateDisconnectPulse(); + + switch (usbDevConfig()->type) { +#ifdef USE_USB_CDC_HID + case COMPOSITE: + usb_gpio_config(); + usb_rcu_config(); + usbd_desc_string_update(); + usbd_init(&USB_OTG_dev, USB_CORE_ENUM_FS, &bf_cdc_hid_desc, &bf_usbd_cdc_hid_cb); + usb_intr_config(); + break; +#endif + default: + usb_gpio_config(); + usb_rcu_config(); + usbd_desc_string_update(); + usbd_init(&USB_OTG_dev, USB_CORE_ENUM_FS, &bf_cdc_desc, &bf_cdc_class); + usb_intr_config(); + break; + } + + vcpPort_t *s = &vcpPort; + s->port.vTable = usbVTable; + return &s->port; +} + +uint32_t usbVcpGetBaudRate(serialPort_t *instance) +{ + UNUSED(instance); + + return CDC_BaudRate(); +} + +uint8_t usbVcpIsConnected(void) +{ + return usbIsConnected(); +} +#endif diff --git a/src/platform/GD32/startup/gd32f4xx_libopt.h b/src/platform/GD32/startup/gd32f4xx_libopt.h new file mode 100755 index 0000000000..f1a43d8980 --- /dev/null +++ b/src/platform/GD32/startup/gd32f4xx_libopt.h @@ -0,0 +1,78 @@ +/*! + \file gd32f4xx_libopt.h + \brief library optional for gd32f4xx + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#ifndef GD32F4XX_LIBOPT_H +#define GD32F4XX_LIBOPT_H + +#if defined (GD32F450) || defined (GD32F405) || defined (GD32F407) || defined (GD32F460) || defined (GD32F470) || defined (GD32F425) || defined (GD32F427) +#include "gd32f4xx_rcu.h" +#include "gd32f4xx_adc.h" +#include "gd32f4xx_can.h" +#include "gd32f4xx_crc.h" +#include "gd32f4xx_ctc.h" +#include "gd32f4xx_dac.h" +#include "gd32f4xx_dbg.h" +#include "gd32f4xx_dci.h" +#include "gd32f4xx_dma.h" +#include "gd32f4xx_exti.h" +#include "gd32f4xx_fmc.h" +#include "gd32f4xx_fwdgt.h" +#include "gd32f4xx_gpio.h" +#include "gd32f4xx_syscfg.h" +#include "gd32f4xx_i2c.h" +#include "gd32f4xx_iref.h" +#include "gd32f4xx_pmu.h" +#include "gd32f4xx_rtc.h" +#include "gd32f4xx_sdio.h" +#include "gd32f4xx_spi.h" +#include "gd32f4xx_timer.h" +#include "gd32f4xx_trng.h" +#include "gd32f4xx_usart.h" +#include "gd32f4xx_wwdgt.h" +#include "gd32f4xx_misc.h" +#endif + +#if defined (GD32F450) || defined (GD32F460) || defined (GD32F470) +#include "gd32f4xx_enet.h" +#include "gd32f4xx_exmc.h" +#include "gd32f4xx_ipa.h" +#include "gd32f4xx_tli.h" +#endif + +#if defined (GD32F407) || defined (GD32F427) +#include "gd32f4xx_enet.h" +#include "gd32f4xx_exmc.h" +#endif + +#endif /* GD32F4XX_LIBOPT_H */ diff --git a/src/platform/GD32/startup/startup_gd32f405_425.S b/src/platform/GD32/startup/startup_gd32f405_425.S new file mode 100755 index 0000000000..418c23b783 --- /dev/null +++ b/src/platform/GD32/startup/startup_gd32f405_425.S @@ -0,0 +1,447 @@ + + .syntax unified + .cpu cortex-m4 + .fpu softvfp + .thumb + +.global Default_Handler + +/* necessary symbols defined in linker script to initialize data */ +.word _sidata +.word _sdata +.word _edata +.word _sbss +.word _ebss +.word __fastram_bss_start__ +.word __fastram_bss_end__ + + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function + +/* reset Handler */ +Reset_Handler: + movs r1, #0 + b DataInit + +CopyData: + ldr r3, =_sidata + ldr r3, [r3, r1] + str r3, [r0, r1] + adds r1, r1, #4 + +DataInit: + ldr r0, =_sdata + ldr r3, =_edata + adds r2, r0, r1 + cmp r2, r3 + bcc CopyData + ldr r2, =_sbss + b Zerobss + +FillZerobss: + movs r3, #0 + str r3, [r2], #4 + +Zerobss: + ldr r3, = _ebss + cmp r2, r3 + bcc FillZerobss + + + ldr r2, =__fastram_bss_start__ + b Zerofastram_bss + +FillZerofastram_bss: + movs r3, #0 + str r3, [r2], #4 + +Zerofastram_bss: + ldr r3, = __fastram_bss_end__ + cmp r2, r3 + bcc FillZerofastram_bss + + +/* Call SystemInit function */ + bl SystemInit +/* Call static constructors */ + bl __libc_init_array +/*Call the main function */ + bl main + bx lr +.size Reset_Handler, .-Reset_Handler + + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop + .size Default_Handler, .-Default_Handler + + .section .vectors,"a",%progbits + .global __gVectors + +__gVectors: + .word _estack /* Top of Stack */ + .word Reset_Handler /* Reset Handler */ + .word NMI_Handler /* NMI Handler */ + .word HardFault_Handler /* Hard Fault Handler */ + .word MemManage_Handler /* MPU Fault Handler */ + .word BusFault_Handler /* Bus Fault Handler */ + .word UsageFault_Handler /* Usage Fault Handler */ + .word 0 /* Reserved */ + .word 0 /* Reserved */ + .word 0 /* Reserved */ + .word 0 /* Reserved */ + .word SVC_Handler /* SVCall Handler */ + .word DebugMon_Handler /* Debug Monitor Handler */ + .word 0 /* Reserved */ + .word PendSV_Handler /* PendSV Handler */ + .word SysTick_Handler /* SysTick Handler */ + + /* External interrupts handler */ + .word WWDGT_IRQHandler /* Vector Number 16,Window Watchdog Timer */ + .word LVD_IRQHandler /* Vector Number 17,LVD through EXTI Line detect */ + .word TAMPER_STAMP_IRQHandler /* Vector Number 18,Tamper and TimeStamp through EXTI Line detect */ + .word RTC_WKUP_IRQHandler /* Vector Number 19,RTC Wakeup through EXTI Line */ + .word FMC_IRQHandler /* Vector Number 20,FMC */ + .word RCU_CTC_IRQHandler /* Vector Number 21,RCU and CTC */ + .word EXTI0_IRQHandler /* Vector Number 22,EXTI Line 0 */ + .word EXTI1_IRQHandler /* Vector Number 23,EXTI Line 1 */ + .word EXTI2_IRQHandler /* Vector Number 24,EXTI Line 2 */ + .word EXTI3_IRQHandler /* Vector Number 25,EXTI Line 3 */ + .word EXTI4_IRQHandler /* Vector Number 26,EXTI Line 4 */ + .word DMA0_Channel0_IRQHandler /* Vector Number 27,DMA0 Channel0 */ + .word DMA0_Channel1_IRQHandler /* Vector Number 28,DMA0 Channel1 */ + .word DMA0_Channel2_IRQHandler /* Vector Number 29,DMA0 Channel2 */ + .word DMA0_Channel3_IRQHandler /* Vector Number 30,DMA0 Channel3 */ + .word DMA0_Channel4_IRQHandler /* Vector Number 31,DMA0 Channel4 */ + .word DMA0_Channel5_IRQHandler /* Vector Number 32,DMA0 Channel5 */ + .word DMA0_Channel6_IRQHandler /* Vector Number 33,DMA0 Channel6 */ + .word ADC_IRQHandler /* Vector Number 34,ADC */ + .word CAN0_TX_IRQHandler /* Vector Number 35,CAN0 TX */ + .word CAN0_RX0_IRQHandler /* Vector Number 36,CAN0 RX0 */ + .word CAN0_RX1_IRQHandler /* Vector Number 37,CAN0 RX1 */ + .word CAN0_EWMC_IRQHandler /* Vector Number 38,CAN0 EWMC */ + .word EXTI5_9_IRQHandler /* Vector Number 39,EXTI5 to EXTI9 */ + .word TIMER0_BRK_TIMER8_IRQHandler /* Vector Number 40,TIMER0 Break and TIMER8 */ + .word TIMER0_UP_TIMER9_IRQHandler /* Vector Number 41,TIMER0 Update and TIMER9 */ + .word TIMER0_TRG_CMT_TIMER10_IRQHandler /* Vector Number 42,TIMER0 Trigger and Commutation and TIMER10 */ + .word TIMER0_Channel_IRQHandler /* Vector Number 43,TIMER0 Channel Capture Compare */ + .word TIMER1_IRQHandler /* Vector Number 44,TIMER1 */ + .word TIMER2_IRQHandler /* Vector Number 45,TIMER2 */ + .word TIMER3_IRQHandler /* Vector Number 46,TIMER3 */ + .word I2C0_EV_IRQHandler /* Vector Number 47,I2C0 Event */ + .word I2C0_ER_IRQHandler /* Vector Number 48,I2C0 Error */ + .word I2C1_EV_IRQHandler /* Vector Number 49,I2C1 Event */ + .word I2C1_ER_IRQHandler /* Vector Number 50,I2C1 Error */ + .word SPI0_IRQHandler /* Vector Number 51,SPI0 */ + .word SPI1_IRQHandler /* Vector Number 52,SPI1 */ + .word USART0_IRQHandler /* Vector Number 53,USART0 */ + .word USART1_IRQHandler /* Vector Number 54,USART1 */ + .word USART2_IRQHandler /* Vector Number 55,USART2 */ + .word EXTI10_15_IRQHandler /* Vector Number 56,EXTI10 to EXTI15 */ + .word RTC_Alarm_IRQHandler /* Vector Number 57,RTC Alarm */ + .word USBFS_WKUP_IRQHandler /* Vector Number 58,USBFS Wakeup */ + .word TIMER7_BRK_TIMER11_IRQHandler /* Vector Number 59,TIMER7 Break and TIMER11 */ + .word TIMER7_UP_TIMER12_IRQHandler /* Vector Number 60,TIMER7 Update and TIMER12 */ + .word TIMER7_TRG_CMT_TIMER13_IRQHandler /* Vector Number 61,TIMER7 Trigger and Commutation and TIMER13 */ + .word TIMER7_Channel_IRQHandler /* Vector Number 62,TIMER7 Channel Capture Compare */ + .word DMA0_Channel7_IRQHandler /* Vector Number 63,DMA0 Channel7 */ + .word 0 /* Vector Number 64,Reserved */ + .word SDIO_IRQHandler /* Vector Number 65,SDIO */ + .word TIMER4_IRQHandler /* Vector Number 66,TIMER4 */ + .word SPI2_IRQHandler /* Vector Number 67,SPI2 */ + .word UART3_IRQHandler /* Vector Number 68,UART3 */ + .word UART4_IRQHandler /* Vector Number 69,UART4 */ + .word TIMER5_DAC_IRQHandler /* Vector Number 70,TIMER5 and DAC0 DAC1 Underrun error */ + .word TIMER6_IRQHandler /* Vector Number 71,TIMER6 */ + .word DMA1_Channel0_IRQHandler /* Vector Number 72,DMA1 Channel0 */ + .word DMA1_Channel1_IRQHandler /* Vector Number 73,DMA1 Channel1 */ + .word DMA1_Channel2_IRQHandler /* Vector Number 74,DMA1 Channel2 */ + .word DMA1_Channel3_IRQHandler /* Vector Number 75,DMA1 Channel3 */ + .word DMA1_Channel4_IRQHandler /* Vector Number 76,DMA1 Channel4 */ + .word 0 /* Vector Number 77,Reserved */ + .word 0 /* Vector Number 78,Reserved */ + .word CAN1_TX_IRQHandler /* Vector Number 79,CAN1 TX */ + .word CAN1_RX0_IRQHandler /* Vector Number 80,CAN1 RX0 */ + .word CAN1_RX1_IRQHandler /* Vector Number 81,CAN1 RX1 */ + .word CAN1_EWMC_IRQHandler /* Vector Number 82,CAN1 EWMC */ + .word USBFS_IRQHandler /* Vector Number 83,USBFS */ + .word DMA1_Channel5_IRQHandler /* Vector Number 84,DMA1 Channel5 */ + .word DMA1_Channel6_IRQHandler /* Vector Number 85,DMA1 Channel6 */ + .word DMA1_Channel7_IRQHandler /* Vector Number 86,DMA1 Channel7 */ + .word USART5_IRQHandler /* Vector Number 87,USART5 */ + .word I2C2_EV_IRQHandler /* Vector Number 88,I2C2 Event */ + .word I2C2_ER_IRQHandler /* Vector Number 89,I2C2 Error */ + .word USBHS_EP1_Out_IRQHandler /* Vector Number 90,USBHS Endpoint 1 Out */ + .word USBHS_EP1_In_IRQHandler /* Vector Number 91,USBHS Endpoint 1 in */ + .word USBHS_WKUP_IRQHandler /* Vector Number 92,USBHS Wakeup through EXTI Line */ + .word USBHS_IRQHandler /* Vector Number 93,USBHS */ + .word DCI_IRQHandler /* Vector Number 94,DCI */ + .word 0 /* Vector Number 95,Reserved */ + .word TRNG_IRQHandler /* Vector Number 96,TRNG */ + .word FPU_IRQHandler /* Vector Number 97,FPU */ + + .size __gVectors, . - __gVectors + + .weak NMI_Handler + .thumb_set NMI_Handler,Default_Handler + + .weak HardFault_Handler + .thumb_set HardFault_Handler,Default_Handler + + .weak MemManage_Handler + .thumb_set MemManage_Handler,Default_Handler + + .weak BusFault_Handler + .thumb_set BusFault_Handler,Default_Handler + + .weak UsageFault_Handler + .thumb_set UsageFault_Handler,Default_Handler + + .weak SVC_Handler + .thumb_set SVC_Handler,Default_Handler + + .weak DebugMon_Handler + .thumb_set DebugMon_Handler,Default_Handler + + .weak PendSV_Handler + .thumb_set PendSV_Handler,Default_Handler + + .weak SysTick_Handler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDGT_IRQHandler + .thumb_set WWDGT_IRQHandler,Default_Handler + + .weak LVD_IRQHandler + .thumb_set LVD_IRQHandler,Default_Handler + + .weak TAMPER_STAMP_IRQHandler + .thumb_set TAMPER_STAMP_IRQHandler,Default_Handler + + .weak RTC_WKUP_IRQHandler + .thumb_set RTC_WKUP_IRQHandler,Default_Handler + + .weak FMC_IRQHandler + .thumb_set FMC_IRQHandler,Default_Handler + + .weak RCU_CTC_IRQHandler + .thumb_set RCU_CTC_IRQHandler,Default_Handler + + .weak EXTI0_IRQHandler + .thumb_set EXTI0_IRQHandler,Default_Handler + + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA0_Channel0_IRQHandler + .thumb_set DMA0_Channel0_IRQHandler,Default_Handler + + .weak DMA0_Channel1_IRQHandler + .thumb_set DMA0_Channel1_IRQHandler,Default_Handler + + .weak DMA0_Channel2_IRQHandler + .thumb_set DMA0_Channel2_IRQHandler,Default_Handler + + .weak DMA0_Channel3_IRQHandler + .thumb_set DMA0_Channel3_IRQHandler,Default_Handler + + .weak DMA0_Channel4_IRQHandler + .thumb_set DMA0_Channel4_IRQHandler,Default_Handler + + .weak DMA0_Channel5_IRQHandler + .thumb_set DMA0_Channel5_IRQHandler,Default_Handler + + .weak DMA0_Channel6_IRQHandler + .thumb_set DMA0_Channel6_IRQHandler,Default_Handler + + .weak ADC_IRQHandler + .thumb_set ADC_IRQHandler,Default_Handler + + .weak CAN0_TX_IRQHandler + .thumb_set CAN0_TX_IRQHandler,Default_Handler + + .weak CAN0_RX0_IRQHandler + .thumb_set CAN0_RX0_IRQHandler,Default_Handler + + .weak CAN0_RX1_IRQHandler + .thumb_set CAN0_RX1_IRQHandler,Default_Handler + + .weak CAN0_EWMC_IRQHandler + .thumb_set CAN0_EWMC_IRQHandler,Default_Handler + + .weak EXTI5_9_IRQHandler + .thumb_set EXTI5_9_IRQHandler,Default_Handler + + .weak TIMER0_BRK_TIMER8_IRQHandler + .thumb_set TIMER0_BRK_TIMER8_IRQHandler,Default_Handler + + .weak TIMER0_UP_TIMER9_IRQHandler + .thumb_set TIMER0_UP_TIMER9_IRQHandler,Default_Handler + + .weak TIMER0_TRG_CMT_TIMER10_IRQHandler + .thumb_set TIMER0_TRG_CMT_TIMER10_IRQHandler,Default_Handler + + .weak TIMER0_Channel_IRQHandler + .thumb_set TIMER0_Channel_IRQHandler,Default_Handler + + .weak TIMER1_IRQHandler + .thumb_set TIMER1_IRQHandler,Default_Handler + + .weak TIMER2_IRQHandler + .thumb_set TIMER2_IRQHandler,Default_Handler + + .weak TIMER3_IRQHandler + .thumb_set TIMER3_IRQHandler,Default_Handler + + .weak I2C0_EV_IRQHandler + .thumb_set I2C0_EV_IRQHandler,Default_Handler + + .weak I2C0_ER_IRQHandler + .thumb_set I2C0_ER_IRQHandler,Default_Handler + + .weak I2C1_EV_IRQHandler + .thumb_set I2C1_EV_IRQHandler,Default_Handler + + .weak I2C1_ER_IRQHandler + .thumb_set I2C1_ER_IRQHandler,Default_Handler + + .weak SPI0_IRQHandler + .thumb_set SPI0_IRQHandler,Default_Handler + + .weak SPI1_IRQHandler + .thumb_set SPI1_IRQHandler,Default_Handler + + .weak USART0_IRQHandler + .thumb_set USART0_IRQHandler,Default_Handler + + .weak USART1_IRQHandler + .thumb_set USART1_IRQHandler,Default_Handler + + .weak USART2_IRQHandler + .thumb_set USART2_IRQHandler,Default_Handler + + .weak EXTI10_15_IRQHandler + .thumb_set EXTI10_15_IRQHandler,Default_Handler + + .weak RTC_Alarm_IRQHandler + .thumb_set RTC_Alarm_IRQHandler,Default_Handler + + .weak USBFS_WKUP_IRQHandler + .thumb_set USBFS_WKUP_IRQHandler,Default_Handler + + .weak TIMER7_BRK_TIMER11_IRQHandler + .thumb_set TIMER7_BRK_TIMER11_IRQHandler,Default_Handler + + .weak TIMER7_UP_TIMER12_IRQHandler + .thumb_set TIMER7_UP_TIMER12_IRQHandler,Default_Handler + + .weak TIMER7_TRG_CMT_TIMER13_IRQHandler + .thumb_set TIMER7_TRG_CMT_TIMER13_IRQHandler,Default_Handler + + .weak TIMER7_Channel_IRQHandler + .thumb_set TIMER7_Channel_IRQHandler,Default_Handler + + .weak DMA0_Channel7_IRQHandler + .thumb_set DMA0_Channel7_IRQHandler,Default_Handler + + .weak SDIO_IRQHandler + .thumb_set SDIO_IRQHandler,Default_Handler + + .weak TIMER4_IRQHandler + .thumb_set TIMER4_IRQHandler,Default_Handler + + .weak SPI2_IRQHandler + .thumb_set SPI2_IRQHandler,Default_Handler + + .weak UART3_IRQHandler + .thumb_set UART3_IRQHandler,Default_Handler + + .weak UART4_IRQHandler + .thumb_set UART4_IRQHandler,Default_Handler + + .weak TIMER5_DAC_IRQHandler + .thumb_set TIMER5_DAC_IRQHandler,Default_Handler + + .weak TIMER6_IRQHandler + .thumb_set TIMER6_IRQHandler,Default_Handler + + .weak DMA1_Channel0_IRQHandler + .thumb_set DMA1_Channel0_IRQHandler,Default_Handler + + .weak DMA1_Channel1_IRQHandler + .thumb_set DMA1_Channel1_IRQHandler,Default_Handler + + .weak DMA1_Channel2_IRQHandler + .thumb_set DMA1_Channel2_IRQHandler,Default_Handler + + .weak DMA1_Channel3_IRQHandler + .thumb_set DMA1_Channel3_IRQHandler,Default_Handler + + .weak DMA1_Channel4_IRQHandler + .thumb_set DMA1_Channel4_IRQHandler,Default_Handler + + .weak CAN1_TX_IRQHandler + .thumb_set CAN1_TX_IRQHandler,Default_Handler + + .weak CAN1_RX0_IRQHandler + .thumb_set CAN1_RX0_IRQHandler,Default_Handler + + .weak CAN1_RX1_IRQHandler + .thumb_set CAN1_RX1_IRQHandler,Default_Handler + + .weak CAN1_EWMC_IRQHandler + .thumb_set CAN1_EWMC_IRQHandler,Default_Handler + + .weak USBFS_IRQHandler + .thumb_set USBFS_IRQHandler,Default_Handler + + .weak DMA1_Channel5_IRQHandler + .thumb_set DMA1_Channel5_IRQHandler,Default_Handler + + .weak DMA1_Channel6_IRQHandler + .thumb_set DMA1_Channel6_IRQHandler,Default_Handler + + .weak DMA1_Channel7_IRQHandler + .thumb_set DMA1_Channel7_IRQHandler,Default_Handler + + .weak USART5_IRQHandler + .thumb_set USART5_IRQHandler,Default_Handler + + .weak I2C2_EV_IRQHandler + .thumb_set I2C2_EV_IRQHandler,Default_Handler + + .weak I2C2_ER_IRQHandler + .thumb_set I2C2_ER_IRQHandler,Default_Handler + + .weak USBHS_EP1_Out_IRQHandler + .thumb_set USBHS_EP1_Out_IRQHandler,Default_Handler + + .weak USBHS_EP1_In_IRQHandler + .thumb_set USBHS_EP1_In_IRQHandler,Default_Handler + + .weak USBHS_WKUP_IRQHandler + .thumb_set USBHS_WKUP_IRQHandler,Default_Handler + + .weak USBHS_IRQHandler + .thumb_set USBHS_IRQHandler,Default_Handler + + .weak DCI_IRQHandler + .thumb_set DCI_IRQHandler,Default_Handler + + .weak TRNG_IRQHandler + .thumb_set TRNG_IRQHandler,Default_Handler + + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler diff --git a/src/platform/GD32/startup/startup_gd32f460.S b/src/platform/GD32/startup/startup_gd32f460.S new file mode 100755 index 0000000000..5f4eb6119d --- /dev/null +++ b/src/platform/GD32/startup/startup_gd32f460.S @@ -0,0 +1,466 @@ + + .syntax unified + .cpu cortex-m4 + .fpu softvfp + .thumb + +.global Default_Handler + +/* necessary symbols defined in linker script to initialize data */ +.word _sidata +.word _sdata +.word _edata +.word _sbss +.word _ebss +.word __fastram_bss_start__ +.word __fastram_bss_end__ + + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function + +/* reset Handler */ +Reset_Handler: + movs r1, #0 + b DataInit + +CopyData: + ldr r3, =_sidata + ldr r3, [r3, r1] + str r3, [r0, r1] + adds r1, r1, #4 + +DataInit: + ldr r0, =_sdata + ldr r3, =_edata + adds r2, r0, r1 + cmp r2, r3 + bcc CopyData + ldr r2, =_sbss + b Zerobss + +FillZerobss: + movs r3, #0 + str r3, [r2], #4 + +Zerobss: + ldr r3, = _ebss + cmp r2, r3 + bcc FillZerobss + + + ldr r2, =__fastram_bss_start__ + b Zerofastram_bss + +FillZerofastram_bss: + movs r3, #0 + str r3, [r2], #4 + +Zerofastram_bss: + ldr r3, = __fastram_bss_end__ + cmp r2, r3 + bcc FillZerofastram_bss + + +/* Call SystemInit function */ + bl SystemInit +/* Call static constructors */ + bl __libc_init_array +/*Call the main function */ + bl main + bx lr +.size Reset_Handler, .-Reset_Handler + + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop + .size Default_Handler, .-Default_Handler + + .section .vectors,"a",%progbits + .global __gVectors + +__gVectors: + .word _estack /* Top of Stack */ + .word Reset_Handler /* Reset Handler */ + .word NMI_Handler /* NMI Handler */ + .word HardFault_Handler /* Hard Fault Handler */ + .word MemManage_Handler /* MPU Fault Handler */ + .word BusFault_Handler /* Bus Fault Handler */ + .word UsageFault_Handler /* Usage Fault Handler */ + .word 0 /* Reserved */ + .word 0 /* Reserved */ + .word 0 /* Reserved */ + .word 0 /* Reserved */ + .word SVC_Handler /* SVCall Handler */ + .word DebugMon_Handler /* Debug Monitor Handler */ + .word 0 /* Reserved */ + .word PendSV_Handler /* PendSV Handler */ + .word SysTick_Handler /* SysTick Handler */ + + /* External interrupts handler */ + .word WWDGT_IRQHandler /* Vector Number 16,Window Watchdog Timer */ + .word LVD_IRQHandler /* Vector Number 17,LVD through EXTI Line detect */ + .word TAMPER_STAMP_IRQHandler /* Vector Number 18,Tamper and TimeStamp through EXTI Line detect */ + .word RTC_WKUP_IRQHandler /* Vector Number 19,RTC Wakeup through EXTI Line */ + .word FMC_IRQHandler /* Vector Number 20,FMC */ + .word RCU_CTC_IRQHandler /* Vector Number 21,RCU and CTC */ + .word EXTI0_IRQHandler /* Vector Number 22,EXTI Line 0 */ + .word EXTI1_IRQHandler /* Vector Number 23,EXTI Line 1 */ + .word EXTI2_IRQHandler /* Vector Number 24,EXTI Line 2 */ + .word EXTI3_IRQHandler /* Vector Number 25,EXTI Line 3 */ + .word EXTI4_IRQHandler /* Vector Number 26,EXTI Line 4 */ + .word DMA0_Channel0_IRQHandler /* Vector Number 27,DMA0 Channel0 */ + .word DMA0_Channel1_IRQHandler /* Vector Number 28,DMA0 Channel1 */ + .word DMA0_Channel2_IRQHandler /* Vector Number 29,DMA0 Channel2 */ + .word DMA0_Channel3_IRQHandler /* Vector Number 30,DMA0 Channel3 */ + .word DMA0_Channel4_IRQHandler /* Vector Number 31,DMA0 Channel4 */ + .word DMA0_Channel5_IRQHandler /* Vector Number 32,DMA0 Channel5 */ + .word DMA0_Channel6_IRQHandler /* Vector Number 33,DMA0 Channel6 */ + .word ADC_IRQHandler /* Vector Number 34,ADC */ + .word CAN0_TX_IRQHandler /* Vector Number 35,CAN0 TX */ + .word CAN0_RX0_IRQHandler /* Vector Number 36,CAN0 RX0 */ + .word CAN0_RX1_IRQHandler /* Vector Number 37,CAN0 RX1 */ + .word CAN0_EWMC_IRQHandler /* Vector Number 38,CAN0 EWMC */ + .word EXTI5_9_IRQHandler /* Vector Number 39,EXTI5 to EXTI9 */ + .word TIMER0_BRK_TIMER8_IRQHandler /* Vector Number 40,TIMER0 Break and TIMER8 */ + .word TIMER0_UP_TIMER9_IRQHandler /* Vector Number 41,TIMER0 Update and TIMER9 */ + .word TIMER0_TRG_CMT_TIMER10_IRQHandler /* Vector Number 42,TIMER0 Trigger and Commutation and TIMER10 */ + .word TIMER0_Channel_IRQHandler /* Vector Number 43,TIMER0 Channel Capture Compare */ + .word TIMER1_IRQHandler /* Vector Number 44,TIMER1 */ + .word TIMER2_IRQHandler /* Vector Number 45,TIMER2 */ + .word TIMER3_IRQHandler /* Vector Number 46,TIMER3 */ + .word I2C0_EV_IRQHandler /* Vector Number 47,I2C0 Event */ + .word I2C0_ER_IRQHandler /* Vector Number 48,I2C0 Error */ + .word I2C1_EV_IRQHandler /* Vector Number 49,I2C1 Event */ + .word I2C1_ER_IRQHandler /* Vector Number 50,I2C1 Error */ + .word SPI0_IRQHandler /* Vector Number 51,SPI0 */ + .word SPI1_IRQHandler /* Vector Number 52,SPI1 */ + .word USART0_IRQHandler /* Vector Number 53,USART0 */ + .word USART1_IRQHandler /* Vector Number 54,USART1 */ + .word USART2_IRQHandler /* Vector Number 55,USART2 */ + .word EXTI10_15_IRQHandler /* Vector Number 56,EXTI10 to EXTI15 */ + .word RTC_Alarm_IRQHandler /* Vector Number 57,RTC Alarm */ + .word USBFS_WKUP_IRQHandler /* Vector Number 58,USBFS Wakeup */ + .word TIMER7_BRK_TIMER11_IRQHandler /* Vector Number 59,TIMER7 Break and TIMER11 */ + .word TIMER7_UP_TIMER12_IRQHandler /* Vector Number 60,TIMER7 Update and TIMER12 */ + .word TIMER7_TRG_CMT_TIMER13_IRQHandler /* Vector Number 61,TIMER7 Trigger and Commutation and TIMER13 */ + .word TIMER7_Channel_IRQHandler /* Vector Number 62,TIMER7 Channel Capture Compare */ + .word DMA0_Channel7_IRQHandler /* Vector Number 63,DMA0 Channel7 */ + .word 0 /* Vector Number 64,Reserved */ + .word SDIO_IRQHandler /* Vector Number 65,SDIO */ + .word TIMER4_IRQHandler /* Vector Number 66,TIMER4 */ + .word SPI2_IRQHandler /* Vector Number 67,SPI2 */ + .word UART3_IRQHandler /* Vector Number 68,UART3 */ + .word UART4_IRQHandler /* Vector Number 69,UART4 */ + .word TIMER5_DAC_IRQHandler /* Vector Number 70,TIMER5 and DAC0 DAC1 Underrun error */ + .word TIMER6_IRQHandler /* Vector Number 71,TIMER6 */ + .word DMA1_Channel0_IRQHandler /* Vector Number 72,DMA1 Channel0 */ + .word DMA1_Channel1_IRQHandler /* Vector Number 73,DMA1 Channel1 */ + .word DMA1_Channel2_IRQHandler /* Vector Number 74,DMA1 Channel2 */ + .word DMA1_Channel3_IRQHandler /* Vector Number 75,DMA1 Channel3 */ + .word DMA1_Channel4_IRQHandler /* Vector Number 76,DMA1 Channel4 */ + .word ENET_IRQHandler /* Vector Number 77,Ethernet */ + .word ENET_WKUP_IRQHandler /* Vector Number 78,Ethernet Wakeup through EXTI Line */ + .word CAN1_TX_IRQHandler /* Vector Number 79,CAN1 TX */ + .word CAN1_RX0_IRQHandler /* Vector Number 80,CAN1 RX0 */ + .word CAN1_RX1_IRQHandler /* Vector Number 81,CAN1 RX1 */ + .word CAN1_EWMC_IRQHandler /* Vector Number 82,CAN1 EWMC */ + .word USBFS_IRQHandler /* Vector Number 83,USBFS */ + .word DMA1_Channel5_IRQHandler /* Vector Number 84,DMA1 Channel5 */ + .word DMA1_Channel6_IRQHandler /* Vector Number 85,DMA1 Channel6 */ + .word DMA1_Channel7_IRQHandler /* Vector Number 86,DMA1 Channel7 */ + .word USART5_IRQHandler /* Vector Number 87,USART5 */ + .word I2C2_EV_IRQHandler /* Vector Number 88,I2C2 Event */ + .word I2C2_ER_IRQHandler /* Vector Number 89,I2C2 Error */ + .word USBHS_EP1_Out_IRQHandler /* Vector Number 90,USBHS Endpoint 1 Out */ + .word USBHS_EP1_In_IRQHandler /* Vector Number 91,USBHS Endpoint 1 in */ + .word USBHS_WKUP_IRQHandler /* Vector Number 92,USBHS Wakeup through EXTI Line */ + .word USBHS_IRQHandler /* Vector Number 93,USBHS */ + .word DCI_IRQHandler /* Vector Number 94,DCI */ + .word 0 /* Vector Number 95,Reserved */ + .word TRNG_IRQHandler /* Vector Number 96,TRNG */ + .word FPU_IRQHandler /* Vector Number 97,FPU */ + .word SPI3_IRQHandler /* Vector Number 100,SPI3 */ + .word SPI4_IRQHandler /* Vector Number 101,SPI4 */ + .word SPI5_IRQHandler /* Vector Number 102,SPI5 */ + .word 0 /* Vector Number 103,Reserved */ + + .size __gVectors, . - __gVectors + + .weak NMI_Handler + .thumb_set NMI_Handler,Default_Handler + + .weak HardFault_Handler + .thumb_set HardFault_Handler,Default_Handler + + .weak MemManage_Handler + .thumb_set MemManage_Handler,Default_Handler + + .weak BusFault_Handler + .thumb_set BusFault_Handler,Default_Handler + + .weak UsageFault_Handler + .thumb_set UsageFault_Handler,Default_Handler + + .weak SVC_Handler + .thumb_set SVC_Handler,Default_Handler + + .weak DebugMon_Handler + .thumb_set DebugMon_Handler,Default_Handler + + .weak PendSV_Handler + .thumb_set PendSV_Handler,Default_Handler + + .weak SysTick_Handler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDGT_IRQHandler + .thumb_set WWDGT_IRQHandler,Default_Handler + + .weak LVD_IRQHandler + .thumb_set LVD_IRQHandler,Default_Handler + + .weak TAMPER_STAMP_IRQHandler + .thumb_set TAMPER_STAMP_IRQHandler,Default_Handler + + .weak RTC_WKUP_IRQHandler + .thumb_set RTC_WKUP_IRQHandler,Default_Handler + + .weak FMC_IRQHandler + .thumb_set FMC_IRQHandler,Default_Handler + + .weak RCU_CTC_IRQHandler + .thumb_set RCU_CTC_IRQHandler,Default_Handler + + .weak EXTI0_IRQHandler + .thumb_set EXTI0_IRQHandler,Default_Handler + + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA0_Channel0_IRQHandler + .thumb_set DMA0_Channel0_IRQHandler,Default_Handler + + .weak DMA0_Channel1_IRQHandler + .thumb_set DMA0_Channel1_IRQHandler,Default_Handler + + .weak DMA0_Channel2_IRQHandler + .thumb_set DMA0_Channel2_IRQHandler,Default_Handler + + .weak DMA0_Channel3_IRQHandler + .thumb_set DMA0_Channel3_IRQHandler,Default_Handler + + .weak DMA0_Channel4_IRQHandler + .thumb_set DMA0_Channel4_IRQHandler,Default_Handler + + .weak DMA0_Channel5_IRQHandler + .thumb_set DMA0_Channel5_IRQHandler,Default_Handler + + .weak DMA0_Channel6_IRQHandler + .thumb_set DMA0_Channel6_IRQHandler,Default_Handler + + .weak ADC_IRQHandler + .thumb_set ADC_IRQHandler,Default_Handler + + .weak CAN0_TX_IRQHandler + .thumb_set CAN0_TX_IRQHandler,Default_Handler + + .weak CAN0_RX0_IRQHandler + .thumb_set CAN0_RX0_IRQHandler,Default_Handler + + .weak CAN0_RX1_IRQHandler + .thumb_set CAN0_RX1_IRQHandler,Default_Handler + + .weak CAN0_EWMC_IRQHandler + .thumb_set CAN0_EWMC_IRQHandler,Default_Handler + + .weak EXTI5_9_IRQHandler + .thumb_set EXTI5_9_IRQHandler,Default_Handler + + .weak TIMER0_BRK_TIMER8_IRQHandler + .thumb_set TIMER0_BRK_TIMER8_IRQHandler,Default_Handler + + .weak TIMER0_UP_TIMER9_IRQHandler + .thumb_set TIMER0_UP_TIMER9_IRQHandler,Default_Handler + + .weak TIMER0_TRG_CMT_TIMER10_IRQHandler + .thumb_set TIMER0_TRG_CMT_TIMER10_IRQHandler,Default_Handler + + .weak TIMER0_Channel_IRQHandler + .thumb_set TIMER0_Channel_IRQHandler,Default_Handler + + .weak TIMER1_IRQHandler + .thumb_set TIMER1_IRQHandler,Default_Handler + + .weak TIMER2_IRQHandler + .thumb_set TIMER2_IRQHandler,Default_Handler + + .weak TIMER3_IRQHandler + .thumb_set TIMER3_IRQHandler,Default_Handler + + .weak I2C0_EV_IRQHandler + .thumb_set I2C0_EV_IRQHandler,Default_Handler + + .weak I2C0_ER_IRQHandler + .thumb_set I2C0_ER_IRQHandler,Default_Handler + + .weak I2C1_EV_IRQHandler + .thumb_set I2C1_EV_IRQHandler,Default_Handler + + .weak I2C1_ER_IRQHandler + .thumb_set I2C1_ER_IRQHandler,Default_Handler + + .weak SPI0_IRQHandler + .thumb_set SPI0_IRQHandler,Default_Handler + + .weak SPI1_IRQHandler + .thumb_set SPI1_IRQHandler,Default_Handler + + .weak USART0_IRQHandler + .thumb_set USART0_IRQHandler,Default_Handler + + .weak USART1_IRQHandler + .thumb_set USART1_IRQHandler,Default_Handler + + .weak USART2_IRQHandler + .thumb_set USART2_IRQHandler,Default_Handler + + .weak EXTI10_15_IRQHandler + .thumb_set EXTI10_15_IRQHandler,Default_Handler + + .weak RTC_Alarm_IRQHandler + .thumb_set RTC_Alarm_IRQHandler,Default_Handler + + .weak USBFS_WKUP_IRQHandler + .thumb_set USBFS_WKUP_IRQHandler,Default_Handler + + .weak TIMER7_BRK_TIMER11_IRQHandler + .thumb_set TIMER7_BRK_TIMER11_IRQHandler,Default_Handler + + .weak TIMER7_UP_TIMER12_IRQHandler + .thumb_set TIMER7_UP_TIMER12_IRQHandler,Default_Handler + + .weak TIMER7_TRG_CMT_TIMER13_IRQHandler + .thumb_set TIMER7_TRG_CMT_TIMER13_IRQHandler,Default_Handler + + .weak TIMER7_Channel_IRQHandler + .thumb_set TIMER7_Channel_IRQHandler,Default_Handler + + .weak DMA0_Channel7_IRQHandler + .thumb_set DMA0_Channel7_IRQHandler,Default_Handler + + .weak SDIO_IRQHandler + .thumb_set SDIO_IRQHandler,Default_Handler + + .weak TIMER4_IRQHandler + .thumb_set TIMER4_IRQHandler,Default_Handler + + .weak SPI2_IRQHandler + .thumb_set SPI2_IRQHandler,Default_Handler + + .weak UART3_IRQHandler + .thumb_set UART3_IRQHandler,Default_Handler + + .weak UART4_IRQHandler + .thumb_set UART4_IRQHandler,Default_Handler + + .weak TIMER5_DAC_IRQHandler + .thumb_set TIMER5_DAC_IRQHandler,Default_Handler + + .weak TIMER6_IRQHandler + .thumb_set TIMER6_IRQHandler,Default_Handler + + .weak DMA1_Channel0_IRQHandler + .thumb_set DMA1_Channel0_IRQHandler,Default_Handler + + .weak DMA1_Channel1_IRQHandler + .thumb_set DMA1_Channel1_IRQHandler,Default_Handler + + .weak DMA1_Channel2_IRQHandler + .thumb_set DMA1_Channel2_IRQHandler,Default_Handler + + .weak DMA1_Channel3_IRQHandler + .thumb_set DMA1_Channel3_IRQHandler,Default_Handler + + .weak DMA1_Channel4_IRQHandler + .thumb_set DMA1_Channel4_IRQHandler,Default_Handler + + .weak ENET_IRQHandler + .thumb_set ENET_IRQHandler,Default_Handler + + .weak ENET_WKUP_IRQHandler + .thumb_set ENET_WKUP_IRQHandler,Default_Handler + + .weak CAN1_TX_IRQHandler + .thumb_set CAN1_TX_IRQHandler,Default_Handler + + .weak CAN1_RX0_IRQHandler + .thumb_set CAN1_RX0_IRQHandler,Default_Handler + + .weak CAN1_RX1_IRQHandler + .thumb_set CAN1_RX1_IRQHandler,Default_Handler + + .weak CAN1_EWMC_IRQHandler + .thumb_set CAN1_EWMC_IRQHandler,Default_Handler + + .weak USBFS_IRQHandler + .thumb_set USBFS_IRQHandler,Default_Handler + + .weak DMA1_Channel5_IRQHandler + .thumb_set DMA1_Channel5_IRQHandler,Default_Handler + + .weak DMA1_Channel6_IRQHandler + .thumb_set DMA1_Channel6_IRQHandler,Default_Handler + + .weak DMA1_Channel7_IRQHandler + .thumb_set DMA1_Channel7_IRQHandler,Default_Handler + + .weak USART5_IRQHandler + .thumb_set USART5_IRQHandler,Default_Handler + + .weak I2C2_EV_IRQHandler + .thumb_set I2C2_EV_IRQHandler,Default_Handler + + .weak I2C2_ER_IRQHandler + .thumb_set I2C2_ER_IRQHandler,Default_Handler + + .weak USBHS_EP1_Out_IRQHandler + .thumb_set USBHS_EP1_Out_IRQHandler,Default_Handler + + .weak USBHS_EP1_In_IRQHandler + .thumb_set USBHS_EP1_In_IRQHandler,Default_Handler + + .weak USBHS_WKUP_IRQHandler + .thumb_set USBHS_WKUP_IRQHandler,Default_Handler + + .weak USBHS_IRQHandler + .thumb_set USBHS_IRQHandler,Default_Handler + + .weak DCI_IRQHandler + .thumb_set DCI_IRQHandler,Default_Handler + + .weak TRNG_IRQHandler + .thumb_set TRNG_IRQHandler,Default_Handler + + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler + + .weak SPI3_IRQHandler + .thumb_set SPI3_IRQHandler,Default_Handler + + .weak SPI4_IRQHandler + .thumb_set SPI4_IRQHandler,Default_Handler + + .weak SPI5_IRQHandler + .thumb_set SPI5_IRQHandler,Default_Handler diff --git a/src/platform/GD32/startup/system_gd32f4xx.c b/src/platform/GD32/startup/system_gd32f4xx.c new file mode 100755 index 0000000000..01d367ae2f --- /dev/null +++ b/src/platform/GD32/startup/system_gd32f4xx.c @@ -0,0 +1,1305 @@ +/*! + \file system_gd32f4xx.c + \brief CMSIS Cortex-M4 Device Peripheral Access Layer Source File for + GD32F4xx Device Series +*/ + +/* Copyright (c) 2012 ARM LIMITED + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + - Neither the name of ARM nor the names of its contributors may be used + to endorse or promote products derived from this software without + specific prior written permission. + * + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + ---------------------------------------------------------------------------*/ + +/* This file refers the CMSIS standard, some adjustments are made according to GigaDevice chips */ + +#include "gd32f4xx.h" + +/* system frequency define */ +#define __IRC16M (IRC16M_VALUE) /* internal 16 MHz RC oscillator frequency */ +#define __HXTAL (HXTAL_VALUE) /* high speed crystal oscillator frequency */ +#define __SYS_OSC_CLK (__IRC16M) /* main oscillator frequency */ + +/* select a system clock by uncommenting the following line */ +//#define __SYSTEM_CLOCK_IRC16M (uint32_t)(__IRC16M) +//#define __SYSTEM_CLOCK_HXTAL (uint32_t)(__HXTAL) +//#define __SYSTEM_CLOCK_120M_PLL_IRC16M (uint32_t)(120000000) +//#define __SYSTEM_CLOCK_120M_PLL_8M_HXTAL (uint32_t)(120000000) +//#define __SYSTEM_CLOCK_120M_PLL_25M_HXTAL (uint32_t)(120000000) +//#define __SYSTEM_CLOCK_168M_PLL_IRC16M (uint32_t)(168000000) +// #define __SYSTEM_CLOCK_168M_PLL_8M_HXTAL (uint32_t)(168000000) +// #define __SYSTEM_CLOCK_168M_PLL_25M_HXTAL (uint32_t)(168000000) + +#if defined (GD32F450) || defined (GD32F425) || defined (GD32F427) +#if (HSE_VALUE==25000000) +#define __SYSTEM_CLOCK_200M_PLL_25M_HXTAL (uint32_t)(200000000) +#elif (HSE_VALUE==8000000) +#define __SYSTEM_CLOCK_200M_PLL_8M_HXTAL (uint32_t)(200000000) +#else +#define __SYSTEM_CLOCK_200M_PLL_8M_HXTAL (uint32_t)(200000000) +#endif +#endif // GD32F450 || GD32F425 || GD32F427 + +#if defined (GD32F470) || defined (GD32F460) +#if (HSE_VALUE==25000000) +#define __SYSTEM_CLOCK_240M_PLL_25M_HXTAL (uint32_t)(240000000) +#elif (HSE_VALUE==8000000) +#define __SYSTEM_CLOCK_240M_PLL_8M_HXTAL (uint32_t)(240000000) +#else +#define __SYSTEM_CLOCK_240M_PLL_8M_HXTAL (uint32_t)(240000000) +#endif +#endif // GD32F470 || GD32F460 + +/* The following is to prevent Vcore fluctuations caused by frequency switching. + It is strongly recommended to include it to avoid issues caused by self-removal. */ +#define RCU_MODIFY_4(__delay) do{ \ + volatile uint32_t i, reg; \ + if(0 != __delay){ \ + /* Insert a software delay */ \ + for(i=0; i<__delay; i++){ \ + } \ + reg = RCU_CFG0; \ + reg &= ~(RCU_CFG0_AHBPSC); \ + reg |= RCU_AHB_CKSYS_DIV2; \ + /* AHB = SYSCLK/2 */ \ + RCU_CFG0 = reg; \ + /* Insert a software delay */ \ + for(i=0; i<__delay; i++){ \ + } \ + reg = RCU_CFG0; \ + reg &= ~(RCU_CFG0_AHBPSC); \ + reg |= RCU_AHB_CKSYS_DIV4; \ + /* AHB = SYSCLK/4 */ \ + RCU_CFG0 = reg; \ + /* Insert a software delay */ \ + for(i=0; i<__delay; i++){ \ + } \ + reg = RCU_CFG0; \ + reg &= ~(RCU_CFG0_AHBPSC); \ + reg |= RCU_AHB_CKSYS_DIV8; \ + /* AHB = SYSCLK/8 */ \ + RCU_CFG0 = reg; \ + /* Insert a software delay */ \ + for(i=0; i<__delay; i++){ \ + } \ + reg = RCU_CFG0; \ + reg &= ~(RCU_CFG0_AHBPSC); \ + reg |= RCU_AHB_CKSYS_DIV16; \ + /* AHB = SYSCLK/16 */ \ + RCU_CFG0 = reg; \ + /* Insert a software delay */ \ + for(i=0; i<__delay; i++){ \ + } \ + } \ + }while(0) + +#define SEL_IRC16M 0x00U +#define SEL_HXTAL 0x01U +#define SEL_PLLP 0x02U + +/* set the system clock frequency and declare the system clock configuration function */ +#ifdef __SYSTEM_CLOCK_IRC16M +uint32_t SystemCoreClock = __SYSTEM_CLOCK_IRC16M; +static void system_clock_16m_irc16m(void); +#elif defined (__SYSTEM_CLOCK_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_HXTAL; +static void system_clock_hxtal(void); +#elif defined (__SYSTEM_CLOCK_120M_PLL_IRC16M) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_120M_PLL_IRC16M; +static void system_clock_120m_irc16m(void); +#elif defined (__SYSTEM_CLOCK_120M_PLL_8M_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_120M_PLL_8M_HXTAL; +static void system_clock_120m_8m_hxtal(void); +#elif defined (__SYSTEM_CLOCK_120M_PLL_25M_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_120M_PLL_25M_HXTAL; +static void system_clock_120m_25m_hxtal(void); +#elif defined (__SYSTEM_CLOCK_168M_PLL_IRC16M) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_168M_PLL_IRC16M; +static void system_clock_168m_irc16m(void); +#elif defined (__SYSTEM_CLOCK_168M_PLL_8M_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_168M_PLL_8M_HXTAL; +static void system_clock_168m_8m_hxtal(void); +#elif defined (__SYSTEM_CLOCK_168M_PLL_25M_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_168M_PLL_25M_HXTAL; +static void system_clock_168m_25m_hxtal(void); +#elif defined (__SYSTEM_CLOCK_200M_PLL_IRC16M) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_200M_PLL_IRC16M; +static void system_clock_200m_irc16m(void); +#elif defined (__SYSTEM_CLOCK_200M_PLL_8M_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_200M_PLL_8M_HXTAL; +static void system_clock_200m_8m_hxtal(void); +#elif defined (__SYSTEM_CLOCK_200M_PLL_25M_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_200M_PLL_25M_HXTAL; +static void system_clock_200m_25m_hxtal(void); +#elif defined (__SYSTEM_CLOCK_240M_PLL_IRC16M) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_240M_PLL_IRC16M; +static void system_clock_240m_irc16m(void); +#elif defined (__SYSTEM_CLOCK_240M_PLL_8M_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_240M_PLL_8M_HXTAL; +static void system_clock_240m_8m_hxtal(void); +#elif defined (__SYSTEM_CLOCK_240M_PLL_25M_HXTAL) +uint32_t SystemCoreClock = __SYSTEM_CLOCK_240M_PLL_25M_HXTAL; +static void system_clock_240m_25m_hxtal(void); +#endif /* __SYSTEM_CLOCK_IRC16M */ + +/* configure the system clock */ +static void system_clock_config(void); + +/* software delay to prevent the impact of Vcore fluctuations. + It is strongly recommended to include it to avoid issues caused by self-removal. */ +static void _soft_delay_(uint32_t time) +{ + __IO uint32_t i; + for(i=0; iCPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ +#endif + /* Reset the RCU clock configuration to the default reset state */ + /* Set IRC16MEN bit */ + RCU_CTL |= RCU_CTL_IRC16MEN; + while(0U == (RCU_CTL & RCU_CTL_IRC16MSTB)){ + } + if(((RCU_CFG0 & RCU_CFG0_SCSS) == RCU_SCSS_PLLP)){ + RCU_MODIFY_4(0x50); + } + _soft_delay_(200); + + RCU_CFG0 &= ~RCU_CFG0_SCS; + + /* Reset HXTALEN, CKMEN and PLLEN bits */ + RCU_CTL &= ~(RCU_CTL_PLLEN | RCU_CTL_CKMEN | RCU_CTL_HXTALEN); + + /* Reset HSEBYP bit */ + RCU_CTL &= ~(RCU_CTL_HXTALBPS); + + /* Reset CFG0 register */ + RCU_CFG0 = 0x00000000U; + + /* wait until IRC16M is selected as system clock */ + while(0 != (RCU_CFG0 & RCU_SCSS_IRC16M)){ + } + + /* Reset PLLCFGR register */ + RCU_PLL = 0x24003010U; + + /* Disable all interrupts */ + RCU_INT = 0x00000000U; + +#ifdef VECT_TAB_SRAM + + extern uint8_t isr_vector_table_flash_base; + extern uint8_t isr_vector_table_base; + extern uint8_t isr_vector_table_end; + + memcpy(&isr_vector_table_base, &isr_vector_table_flash_base, &isr_vector_table_end - &isr_vector_table_base); + SCB->VTOR = (uint32_t)&isr_vector_table_base; +#else + extern uint8_t isr_vector_table_flash_base; + + SCB->VTOR = (uint32_t)&isr_vector_table_flash_base; +#endif + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings */ + system_clock_config(); +} + + +extern void _init(void) {;} + + +/*! + \brief configure the system clock + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_config(void) +{ +#ifdef __SYSTEM_CLOCK_IRC16M + system_clock_16m_irc16m(); +#elif defined (__SYSTEM_CLOCK_HXTAL) + system_clock_hxtal(); +#elif defined (__SYSTEM_CLOCK_120M_PLL_IRC16M) + system_clock_120m_irc16m(); +#elif defined (__SYSTEM_CLOCK_120M_PLL_8M_HXTAL) + system_clock_120m_8m_hxtal(); +#elif defined (__SYSTEM_CLOCK_120M_PLL_25M_HXTAL) + system_clock_120m_25m_hxtal(); +#elif defined (__SYSTEM_CLOCK_168M_PLL_IRC16M) + system_clock_168m_irc16m(); +#elif defined (__SYSTEM_CLOCK_168M_PLL_8M_HXTAL) + system_clock_168m_8m_hxtal(); +#elif defined (__SYSTEM_CLOCK_168M_PLL_25M_HXTAL) + system_clock_168m_25m_hxtal(); +#elif defined (__SYSTEM_CLOCK_200M_PLL_IRC16M) + system_clock_200m_irc16m(); +#elif defined (__SYSTEM_CLOCK_200M_PLL_8M_HXTAL) + system_clock_200m_8m_hxtal(); +#elif defined (__SYSTEM_CLOCK_200M_PLL_25M_HXTAL) + system_clock_200m_25m_hxtal(); +#elif defined (__SYSTEM_CLOCK_240M_PLL_IRC16M) + system_clock_240m_irc16m(); +#elif defined (__SYSTEM_CLOCK_240M_PLL_8M_HXTAL) + system_clock_240m_8m_hxtal(); +#elif defined (__SYSTEM_CLOCK_240M_PLL_25M_HXTAL) + system_clock_240m_25m_hxtal(); +#endif /* __SYSTEM_CLOCK_IRC16M */ +} + +#ifdef __SYSTEM_CLOCK_IRC16M +/*! + \brief configure the system clock to 16M by IRC16M + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_16m_irc16m(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable IRC16M */ + RCU_CTL |= RCU_CTL_IRC16MEN; + + /* wait until IRC16M is stable or the startup time is longer than IRC16M_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_IRC16MSTB); + }while((0U == stab_flag) && (IRC16M_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_IRC16MSTB)){ + while(1){ + } + } + + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; + /* APB1 = AHB */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; + + reg_temp = RCU_CFG0; + /* select IRC16M as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_IRC16M; + RCU_CFG0 = reg_temp; + + /* wait until IRC16M is selected as system clock */ + while(0 != (RCU_CFG0 & RCU_SCSS_IRC16M)){ + } +} + +#elif defined (__SYSTEM_CLOCK_HXTAL) +/*! + \brief configure the system clock to HXTAL + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_hxtal(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_HXTALSTB); + }while((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; + /* APB1 = AHB */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; + + reg_temp = RCU_CFG0; + /* select HXTAL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_HXTAL; + RCU_CFG0 = reg_temp; + + /* wait until HXTAL is selected as system clock */ + while(0 == (RCU_CFG0 & RCU_SCSS_HXTAL)){ + } +} + +#elif defined (__SYSTEM_CLOCK_120M_PLL_IRC16M) +/*! + \brief configure the system clock to 120M by PLL which selects IRC16M as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_120m_irc16m(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable IRC16M */ + RCU_CTL |= RCU_CTL_IRC16MEN; + + /* wait until IRC16M is stable or the startup time is longer than IRC16M_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_IRC16MSTB); + }while((0U == stab_flag) && (IRC16M_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_IRC16MSTB)){ + while(1){ + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* IRC16M is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 16, PLL_N = 240, PLL_P = 2, PLL_Q = 5 */ + RCU_PLL = (16U | (240U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_IRC16M) | (5U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 120 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_120M_PLL_8M_HXTAL) +/*! + \brief configure the system clock to 120M by PLL which selects HXTAL(8M) as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_120m_8m_hxtal(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_HXTALSTB); + }while((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 8, PLL_N = 240, PLL_P = 2, PLL_Q = 5 */ + RCU_PLL = (8U | (240U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_HXTAL) | (5U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 120 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_120M_PLL_25M_HXTAL) +/*! + \brief configure the system clock to 120M by PLL which selects HXTAL(25M) as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_120m_25m_hxtal(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_HXTALSTB); + }while((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 25, PLL_N = 240, PLL_P = 2, PLL_Q = 5 */ + RCU_PLL = (25U | (240U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_HXTAL) | (5U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 120 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_168M_PLL_IRC16M) +/*! + \brief configure the system clock to 168M by PLL which selects IRC16M as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_168m_irc16m(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable IRC16M */ + RCU_CTL |= RCU_CTL_IRC16MEN; + + /* wait until IRC16M is stable or the startup time is longer than IRC16M_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_IRC16MSTB); + }while((0U == stab_flag) && (IRC16M_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_IRC16MSTB)){ + while(1){ + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* IRC16M is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 16, PLL_N = 336, PLL_P = 2, PLL_Q = 7 */ + RCU_PLL = (16U | (336U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_IRC16M) | (7U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 168 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + FMC_WS = 0x00000705; + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_168M_PLL_8M_HXTAL) +/*! + \brief configure the system clock to 168M by PLL which selects HXTAL(8M) as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_168m_8m_hxtal(void) +{ + uint32_t timeout = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + while((0U == (RCU_CTL & RCU_CTL_HXTALSTB)) && (HXTAL_STARTUP_TIMEOUT != timeout++)){ + } + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 8, PLL_N = 336, PLL_P = 2, PLL_Q = 7 */ + RCU_PLL = (8U | (336 << 6U) | (((2 >> 1U) -1U) << 16U) | + (RCU_PLLSRC_HXTAL) | (7 << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + +#if 1 + /* Enable the high-drive to extend the clock frequency to 168 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + +#else + FMC_WS = 0x00000705; +#endif + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_168M_PLL_25M_HXTAL) +/*! + \brief configure the system clock to 168M by PLL which selects HXTAL(25M) as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_168m_25m_hxtal(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_HXTALSTB); + }while((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 25, PLL_N = 336, PLL_P = 2, PLL_Q = 7 */ + RCU_PLL = (25U | (336U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_HXTAL) | (7U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + +#if 1 + /* Enable the high-drive to extend the clock frequency to 168 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + +#else + FMC_WS = 0x00000705; +#endif + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_200M_PLL_IRC16M) +/*! + \brief configure the system clock to 200M by PLL which selects IRC16M as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_200m_irc16m(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable IRC16M */ + RCU_CTL |= RCU_CTL_IRC16MEN; + + /* wait until IRC16M is stable or the startup time is longer than IRC16M_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_IRC16MSTB); + }while((0U == stab_flag) && (IRC16M_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_IRC16MSTB)){ + while(1){ + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* IRC16M is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 16, PLL_N = 400, PLL_P = 2, PLL_Q = 9 */ + RCU_PLL = (16U | (400U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_IRC16M) | (9U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 200 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_200M_PLL_8M_HXTAL) +/*! + \brief configure the system clock to 200M by PLL which selects HXTAL(8M) as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_200m_8m_hxtal(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_HXTALSTB); + }while((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 8, PLL_N = 400, PLL_P = 2, PLL_Q = 9 */ + RCU_PLL = (8U | (400U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_HXTAL) | (9U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 200 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_200M_PLL_25M_HXTAL) +/*! + \brief configure the system clock to 200M by PLL which selects HXTAL(25M) as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_200m_25m_hxtal(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_HXTALSTB); + }while((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 25, PLL_N = 400, PLL_P = 2, PLL_Q = 9 */ + RCU_PLL = (25U | (400U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_HXTAL) | (9U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 200 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_240M_PLL_IRC16M) +/*! + \brief configure the system clock to 240M by PLL which selects IRC16M as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_240m_irc16m(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable IRC16M */ + RCU_CTL |= RCU_CTL_IRC16MEN; + + /* wait until IRC16M is stable or the startup time is longer than IRC16M_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_IRC16MSTB); + }while((0U == stab_flag) && (IRC16M_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_IRC16MSTB)){ + while(1){ + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* IRC16M is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 16, PLL_N = 480, PLL_P = 2, PLL_Q = 10 */ + RCU_PLL = (16U | (480U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_IRC16M) | (10U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 240 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_240M_PLL_8M_HXTAL) +/*! + \brief configure the system clock to 240M by PLL which selects HXTAL(8M) as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_240m_8m_hxtal(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_HXTALSTB); + }while((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 8, PLL_N = 480, PLL_P = 2, PLL_Q = 10 */ + RCU_PLL = (8U | (480U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_HXTAL) | (10U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 240 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} + +#elif defined (__SYSTEM_CLOCK_240M_PLL_25M_HXTAL) +/*! + \brief configure the system clock to 240M by PLL which selects HXTAL(25M) as its clock source + \param[in] none + \param[out] none + \retval none +*/ +static void system_clock_240m_25m_hxtal(void) +{ + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; + __IO uint32_t reg_temp; + + /* enable HXTAL */ + RCU_CTL |= RCU_CTL_HXTALEN; + + /* wait until HXTAL is stable or the startup time is longer than HXTAL_STARTUP_TIMEOUT */ + do{ + timeout++; + stab_flag = (RCU_CTL & RCU_CTL_HXTALSTB); + }while((0U == stab_flag) && (HXTAL_STARTUP_TIMEOUT != timeout)); + + /* if fail */ + if(0U == (RCU_CTL & RCU_CTL_HXTALSTB)){ + while(0U == (RCU_CTL & RCU_CTL_HXTALSTB)) + { + } + } + + RCU_APB1EN |= RCU_APB1EN_PMUEN; + PMU_CTL |= PMU_CTL_LDOVS; + + /* HXTAL is stable */ + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB/2 */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV2; + /* APB1 = AHB/4 */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV4; + + /* Configure the main PLL, PSC = 25, PLL_N = 480, PLL_P = 2, PLL_Q = 10 */ + RCU_PLL = (25U | (480U << 6U) | (((2U >> 1U) - 1U) << 16U) | + (RCU_PLLSRC_HXTAL) | (10U << 24U)); + + /* enable PLL */ + RCU_CTL |= RCU_CTL_PLLEN; + + /* wait until PLL is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSTB)){ + } + + /* Enable the high-drive to extend the clock frequency to 240 Mhz */ + PMU_CTL |= PMU_CTL_HDEN; + while(0U == (PMU_CS & PMU_CS_HDRF)){ + } + + /* select the high-drive mode */ + PMU_CTL |= PMU_CTL_HDS; + while(0U == (PMU_CS & PMU_CS_HDSRF)){ + } + + reg_temp = RCU_CFG0; + /* select PLL as system clock */ + reg_temp &= ~RCU_CFG0_SCS; + reg_temp |= RCU_CKSYSSRC_PLLP; + RCU_CFG0 = reg_temp; + + /* wait until PLL is selected as system clock */ + while(0U == (RCU_CFG0 & RCU_SCSS_PLLP)){ + } +} +#endif /* __SYSTEM_CLOCK_IRC16M */ +/*! + \brief update the SystemCoreClock with current core clock retrieved from cpu registers + \param[in] none + \param[out] none + \retval none +*/ +void SystemCoreClockUpdate(void) +{ + uint32_t sws; + uint32_t pllpsc, plln, pllsel, pllp, ck_src, idx, clk_exp; + + /* exponent of AHB, APB1 and APB2 clock divider */ + const uint8_t ahb_exp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + + sws = GET_BITS(RCU_CFG0, 2, 3); + switch(sws){ + /* IRC16M is selected as CK_SYS */ + case SEL_IRC16M: + SystemCoreClock = IRC16M_VALUE; + break; + /* HXTAL is selected as CK_SYS */ + case SEL_HXTAL: + SystemCoreClock = HXTAL_VALUE; + break; + /* PLLP is selected as CK_SYS */ + case SEL_PLLP: + /* get the value of PLLPSC[5:0] */ + pllpsc = GET_BITS(RCU_PLL, 0U, 5U); + plln = GET_BITS(RCU_PLL, 6U, 14U); + pllp = (GET_BITS(RCU_PLL, 16U, 17U) + 1U) * 2U; + /* PLL clock source selection, HXTAL or IRC8M/2 */ + pllsel = (RCU_PLL & RCU_PLL_PLLSEL); + if (RCU_PLLSRC_HXTAL == pllsel) { + ck_src = HXTAL_VALUE; + } else { + ck_src = IRC16M_VALUE; + } + SystemCoreClock = ((ck_src / pllpsc) * plln) / pllp; + break; + /* IRC16M is selected as CK_SYS */ + default: + SystemCoreClock = IRC16M_VALUE; + break; + } + /* calculate AHB clock frequency */ + idx = GET_BITS(RCU_CFG0, 4, 7); + clk_exp = ahb_exp[idx]; + SystemCoreClock = SystemCoreClock >> clk_exp; +} + +#ifdef __FIRMWARE_VERSION_DEFINE +/*! + \brief get firmware version + \param[in] none + \param[out] none + \retval firmware version +*/ +uint32_t gd32f4xx_firmware_version_get(void) +{ + return __GD32F4xx_STDPERIPH_VERSION; +} +#endif /* __FIRMWARE_VERSION_DEFINE */ diff --git a/src/platform/GD32/startup/system_gd32f4xx.h b/src/platform/GD32/startup/system_gd32f4xx.h new file mode 100755 index 0000000000..7c22afa418 --- /dev/null +++ b/src/platform/GD32/startup/system_gd32f4xx.h @@ -0,0 +1,67 @@ +/*! + \file system_gd32f4xx.h + \brief CMSIS Cortex-M4 Device Peripheral Access Layer Header File for + GD32F4xx Device Series +*/ + +/* Copyright (c) 2012 ARM LIMITED + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + - Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + - Neither the name of ARM nor the names of its contributors may be used + to endorse or promote products derived from this software without + specific prior written permission. + * + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + ---------------------------------------------------------------------------*/ + +/* This file refers the CMSIS standard, some adjustments are made according to GigaDevice chips */ + +#ifndef SYSTEM_GD32F4XX_H +#define SYSTEM_GD32F4XX_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +/* firmware version can be aquired by uncommenting the macro */ +#define __FIRMWARE_VERSION_DEFINE + +/* system clock frequency (core clock) */ +extern uint32_t SystemCoreClock; + +/* function declarations */ +/* initialize the system and update the SystemCoreClock variable */ +extern void SystemInit (void); +/* update the SystemCoreClock with current core clock retrieved from cpu registers */ +extern void SystemCoreClockUpdate (void); +#ifdef __FIRMWARE_VERSION_DEFINE +/* get firmware version */ +extern uint32_t gd32f4xx_firmware_version_get(void); +#endif /* __FIRMWARE_VERSION_DEFINE */ + +extern void systemClockSetHSEValue(uint32_t frequency); + +#ifdef __cplusplus +} +#endif + +#endif /* SYSTEM_GD32F4XX_H */ diff --git a/src/platform/GD32/system_gd32f4xx.c b/src/platform/GD32/system_gd32f4xx.c new file mode 100755 index 0000000000..8e7d0be83e --- /dev/null +++ b/src/platform/GD32/system_gd32f4xx.c @@ -0,0 +1,183 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "drivers/accgyro/accgyro_mpu.h" +#include "drivers/exti.h" +#include "drivers/nvic.h" +#include "drivers/system.h" +#include "drivers/persistent.h" + +void sys_clock_config(void); + +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; + +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); + + rcu_periph_clock_enable(RCU_SYSCFG); + syscfg_bootmode_config(SYSCFG_BOOTMODE_BOOTLOADER); + + extern isrVector_t system_isr_vector_table_base; + + SCB->VTOR = (uint32_t)&system_isr_vector_table_base; + __DSB(); + __DSB(); + + __set_MSP(system_isr_vector_table_base.stackEnd); + system_isr_vector_table_base.resetHandler(); + while (1); +} + +void enableGPIOPowerUsageAndNoiseReductions(void) +{ + /* enable AHB1 peripherals clock */ + rcu_periph_clock_enable(RCU_BKPSRAM); + rcu_periph_clock_enable(RCU_DMA0); + rcu_periph_clock_enable(RCU_DMA1); + + /* enable APB1 peripherals clock */ + rcu_periph_clock_enable(RCU_TIMER1); + rcu_periph_clock_enable(RCU_TIMER2); + rcu_periph_clock_enable(RCU_TIMER3); + rcu_periph_clock_enable(RCU_TIMER4); + rcu_periph_clock_enable(RCU_TIMER5); + rcu_periph_clock_enable(RCU_TIMER6); + rcu_periph_clock_enable(RCU_TIMER11); + rcu_periph_clock_enable(RCU_TIMER12); + rcu_periph_clock_enable(RCU_TIMER13); + rcu_periph_clock_enable(RCU_WWDGT); + rcu_periph_clock_enable(RCU_SPI1); + rcu_periph_clock_enable(RCU_SPI2); + rcu_periph_clock_enable(RCU_USART1); + rcu_periph_clock_enable(RCU_USART1); + rcu_periph_clock_enable(RCU_UART3); + rcu_periph_clock_enable(RCU_UART4); + rcu_periph_clock_enable(RCU_I2C0); + rcu_periph_clock_enable(RCU_I2C1); + rcu_periph_clock_enable(RCU_I2C2); + rcu_periph_clock_enable(RCU_CAN0); + rcu_periph_clock_enable(RCU_CAN1); + rcu_periph_clock_enable(RCU_PMU); + rcu_periph_clock_enable(RCU_DAC); + + rcu_periph_clock_enable(RCU_TIMER0); + rcu_periph_clock_enable(RCU_TIMER7); + rcu_periph_clock_enable(RCU_USART0); + rcu_periph_clock_enable(RCU_USART5); + rcu_periph_clock_enable(RCU_ADC0); + rcu_periph_clock_enable(RCU_ADC1); + rcu_periph_clock_enable(RCU_ADC2); + rcu_periph_clock_enable(RCU_SDIO); + rcu_periph_clock_enable(RCU_SPI0); + rcu_periph_clock_enable(RCU_SYSCFG); + rcu_periph_clock_enable(RCU_TIMER8); + rcu_periph_clock_enable(RCU_TIMER9); + rcu_periph_clock_enable(RCU_TIMER10); + +} + +void sys_clock_config(void) +{ + // The system clock has been configured in /startup/system_gd32f4xx.c + // file. The HSE_VALUE can be 25MHz or 8MHz, and the default is 8MHz. + // The HSE_VALUE can be set when you compile the firmware. + // Here we just update the value SystemCoreClock. + SystemCoreClockUpdate(); +} + +bool isMPUSoftReset(void) +{ + if (cachedRccCsrValue & RCU_RSTSCK_SWRSTF) + return true; + else + return false; +} + +void systemInit(void) +{ + persistentObjectInit(); + + checkForBootLoaderRequest(); + + sys_clock_config(); + + // Configure NVIC preempt/priority groups + nvic_priority_group_set(NVIC_PRIORITY_GROUPING); + + // cache RCU RSTSCK register value to use it in isMPUSoftReset() and others + cachedRccCsrValue = RCU_RSTSCK; + + // Although VTOR is already loaded with a possible vector table in RAM, + // removing the call to NVIC_SetVectorTable causes USB not to become active, + +#ifdef VECT_TAB_SRAM + extern uint8_t isr_vector_table_base; + nvic_vector_table_set((uint32_t)&isr_vector_table_base, 0x0); +#endif + + rcu_periph_clock_disable(RCU_USBFS); + + rcu_all_reset_flag_clear(); + + enableGPIOPowerUsageAndNoiseReductions(); + + // Init cycle counter + cycleCounterInit(); + + // SysTick + SysTick_Config(SystemCoreClock / 1000); +} diff --git a/src/platform/GD32/target/GD32F425/target.h b/src/platform/GD32/target/GD32F425/target.h new file mode 100755 index 0000000000..6f16c1f14b --- /dev/null +++ b/src/platform/GD32/target/GD32F425/target.h @@ -0,0 +1,137 @@ +/* + * 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 "G425" +#endif + +#ifndef USBD_PRODUCT_STRING +#define USBD_PRODUCT_STRING "Betaflight GD32F425" +#endif + +#ifndef GD32F425 +#define GD32F425 +#endif + +#define USE_I2C_DEVICE_0 +#define USE_I2C_DEVICE_1 +#define USE_I2C_DEVICE_2 + +#define I2C0_CLOCKSPEED 400 +#define I2C1_CLOCKSPEED 400 +#define I2C2_CLOCKSPEED 400 + +#if defined(USE_I2C_PULLUP) +#define I2C0_PULLUP true +#else +#define I2C0_PULLUP false +#endif + +#define USE_VCP + +#define USE_SOFTSERIAL + +#ifdef USE_SOFTSERIAL +#define UNIFIED_SERIAL_PORT_COUNT 3 +#else +#define UNIFIED_SERIAL_PORT_COUNT 1 +#endif + +#define USE_UART0 +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 + +#ifdef USE_UART0 +#define SERIAL_UART_FIRST_INDEX 0 +#endif + +#define USE_INVERTER + +#define USE_SPI_DEVICE_0 +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +#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 TARGET_IO_PORTG 0xffff + +#define USE_I2C +#define I2C_FULL_RECONFIGURABILITY + +#define USE_DSHOT_BITBAND + +#define USE_BEEPER + +#ifdef USE_SDCARD +#ifndef USE_SDCARD_SDIO +#define USE_SDCARD_SPI +#define USE_SDCARD_SDIO +#endif +#endif + +#define USE_SPI +#define SPI_FULL_RECONFIGURABILITY +#define USE_SPI_DMA_ENABLE_EARLY + +#define USE_USB_DETECT + +#define USE_ESCSERIAL + +#define USE_ADC +#ifndef ADC_INSTANCE +#define ADC_INSTANCE ADC0 +#endif + +#define USE_SPI_DEVICE_0 + +#define USE_EXTI + +#define USE_PID_DENOM_CHECK +#define USE_PID_DENOM_OVERCLOCK_LEVEL 2 + +#define FLASH_PAGE_SIZE ((uint32_t)0x4000) // 16K sectors + + +#ifdef USE_SPI +#ifdef USE_SPI_DEVICE_0 +#ifndef SPI0_TX_DMA_OPT +#define SPI0_TX_DMA_OPT (DMA_OPT_UNUSED) +#endif +#ifndef SPI0_RX_DMA_OPT +#define SPI0_RX_DMA_OPT (DMA_OPT_UNUSED) +#endif +#endif +#endif + +#if !defined(ADC0_DMA_OPT) +#define ADC0_DMA_OPT (DMA_OPT_UNUSED) +#endif + + diff --git a/src/platform/GD32/target/GD32F425/target.mk b/src/platform/GD32/target/GD32F425/target.mk new file mode 100755 index 0000000000..7f913bfd21 --- /dev/null +++ b/src/platform/GD32/target/GD32F425/target.mk @@ -0,0 +1,3 @@ +TARGET_MCU := GD32F425xx +TARGET_MCU_FAMILY := GD32F4 + diff --git a/src/platform/GD32/target/GD32F460RG/target.h b/src/platform/GD32/target/GD32F460RG/target.h new file mode 100755 index 0000000000..63c32c821d --- /dev/null +++ b/src/platform/GD32/target/GD32F460RG/target.h @@ -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 . + */ + +#pragma once + +#ifndef TARGET_BOARD_IDENTIFIER +#define TARGET_BOARD_IDENTIFIER "G460" +#endif + +#ifndef USBD_PRODUCT_STRING +#define USBD_PRODUCT_STRING "Betaflight GD32F460" +#endif + +#ifndef GD32F460 +#define GD32F460 +#endif + +#define USE_I2C_DEVICE_0 +#define USE_I2C_DEVICE_1 +#define USE_I2C_DEVICE_2 + +#define I2C0_CLOCKSPEED 400 +#define I2C1_CLOCKSPEED 400 +#define I2C2_CLOCKSPEED 400 + +#if defined(USE_I2C_PULLUP) +#define I2C0_PULLUP true +#else +#define I2C0_PULLUP false +#endif + +#define USE_VCP + +#define USE_SOFTSERIAL + +#ifdef USE_SOFTSERIAL +#define UNIFIED_SERIAL_PORT_COUNT 3 +#else +#define UNIFIED_SERIAL_PORT_COUNT 1 +#endif + +#define USE_UART0 +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 + +#ifdef USE_UART0 +#define SERIAL_UART_FIRST_INDEX 0 +#endif + +#define USE_INVERTER + +#define USE_SPI_DEVICE_0 +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +#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 + +#ifdef USE_SDCARD +#ifndef USE_SDCARD_SDIO +#define USE_SDCARD_SPI +#define USE_SDCARD_SDIO +#endif +#endif + +#define USE_SPI +#define SPI_FULL_RECONFIGURABILITY +#define USE_SPI_DMA_ENABLE_EARLY + +#define USE_USB_DETECT + +#define USE_ESCSERIAL + +#define USE_ADC +#ifndef ADC_INSTANCE +#define ADC_INSTANCE ADC0 +#endif + +#define USE_SPI_DEVICE_0 + +#define USE_EXTI + +#define USE_PID_DENOM_CHECK +#define USE_PID_DENOM_OVERCLOCK_LEVEL 2 + +#define FLASH_PAGE_SIZE ((uint32_t)0x4000) // 16K sectors + + +#ifdef USE_SPI +#ifdef USE_SPI_DEVICE_0 +#ifndef SPI0_TX_DMA_OPT +#define SPI0_TX_DMA_OPT (DMA_OPT_UNUSED) +#endif +#ifndef SPI0_RX_DMA_OPT +#define SPI0_RX_DMA_OPT (DMA_OPT_UNUSED) +#endif +#endif +#endif + +#if !defined(ADC0_DMA_OPT) +#define ADC0_DMA_OPT (DMA_OPT_UNUSED) +#endif + + diff --git a/src/platform/GD32/target/GD32F460RG/target.mk b/src/platform/GD32/target/GD32F460RG/target.mk new file mode 100755 index 0000000000..3cd030c5f2 --- /dev/null +++ b/src/platform/GD32/target/GD32F460RG/target.mk @@ -0,0 +1,3 @@ +TARGET_MCU := GD32F460xg +TARGET_MCU_FAMILY := GD32F4 + diff --git a/src/platform/GD32/timer_def.h b/src/platform/GD32/timer_def.h new file mode 100755 index 0000000000..2d8bd2520d --- /dev/null +++ b/src/platform/GD32/timer_def.h @@ -0,0 +1,364 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#include "platform.h" +#include "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_TIMER0_CH0N BTCH_TIMER0_CH0 +#define BTCH_TIMER0_CH1N BTCH_TIMER0_CH1 +#define BTCH_TIMER0_CH2N BTCH_TIMER0_CH2 + +#define BTCH_TIMER7_CH0N BTCH_TIMER7_CH0 +#define BTCH_TIMER7_CH1N BTCH_TIMER7_CH1 +#define BTCH_TIMER7_CH2N BTCH_TIMER7_CH2 + +#define BTCH_TIMER12_CH0N BTCH_TIMER12_CH0 +#define BTCH_TIMER13_CH0N BTCH_TIMER13_CH0 + +// channel table D(chan_n, n_type) +#define DEF_TIM_CH_GET(ch) CONCAT2(DEF_TIM_CH__, ch) +#define DEF_TIM_CH__CH_CH0 D(0, 0) +#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_CH0N D(0, 1) +#define DEF_TIM_CH__CH_CH1N D(1, 1) +#define DEF_TIM_CH__CH_CH2N D(2, 1) + +// timer table D(tim_n) +#define DEF_TIM_TIM_GET(tim) CONCAT2(DEF_TIM_TIM__, tim) +#define DEF_TIM_TIM__TIMER_TIMER0 D(0) +#define DEF_TIM_TIM__TIMER_TIMER1 D(1) +#define DEF_TIM_TIM__TIMER_TIMER2 D(2) +#define DEF_TIM_TIM__TIMER_TIMER3 D(3) +#define DEF_TIM_TIM__TIMER_TIMER4 D(4) +#define DEF_TIM_TIM__TIMER_TIMER5 D(5) +#define DEF_TIM_TIM__TIMER_TIMER6 D(6) +#define DEF_TIM_TIM__TIMER_TIMER7 D(7) +#define DEF_TIM_TIM__TIMER_TIMER8 D(8) +#define DEF_TIM_TIM__TIMER_TIMER9 D(9) +#define DEF_TIM_TIM__TIMER_TIMER10 D(10) +#define DEF_TIM_TIM__TIMER_TIMER11 D(11) +#define DEF_TIM_TIM__TIMER_TIMER12 D(12) +#define DEF_TIM_TIM__TIMER_TIMER13 D(13) + +#define DEF_TIM_TIM__TIM_TIM15 D(15) +#define DEF_TIM_TIM__TIM_TIM16 D(16) +#define DEF_TIM_TIM__TIM_TIM17 D(17) +#define DEF_TIM_TIM__TIM_TIM18 D(18) +#define DEF_TIM_TIM__TIM_TIM19 D(19) +#define DEF_TIM_TIM__TIM_TIM20 D(20) +#define DEF_TIM_TIM__TIM_TIM21 D(21) +#define DEF_TIM_TIM__TIM_TIM22 D(22) + +// 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) + +#if defined(GD32F4) + +#define DEF_TIMER(tim, chan, pin, out, dmaopt) { \ + (void *)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) TIMER_CH_ ## 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 + +#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_SUBPERI ## chan_n +#define DEF_TIM_DMA_CHANNEL__NONE DMA_SUBPERI0 + +#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 ## _CH ## stream_n ## _BASE +#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 ## _CH ## stream_n ## _HANDLER +#define DEF_TIM_DMA_HANDLER__NONE 0 + +/* F4 Channel Mappings */ +// D(DMAx, Channel, Sub_Channel) +#define DEF_TIM_DMA__BTCH_TIMER0_CH0 D(1, 6, 0),D(1, 1, 6),D(1, 3, 6) +#define DEF_TIM_DMA__BTCH_TIMER0_CH1 D(1, 6, 0),D(1, 2, 6) +#define DEF_TIM_DMA__BTCH_TIMER0_CH2 D(1, 6, 0),D(1, 6, 6) +#define DEF_TIM_DMA__BTCH_TIMER0_CH3 D(1, 4, 6) +#define DEF_TIM_DMA__BTCH_TIMER0_CH0N NONE + +#define DEF_TIM_DMA__BTCH_TIMER1_CH0 D(0, 5, 3) +#define DEF_TIM_DMA__BTCH_TIMER1_CH1 D(0, 6, 3) +#define DEF_TIM_DMA__BTCH_TIMER1_CH2 D(0, 1, 3) +#define DEF_TIM_DMA__BTCH_TIMER1_CH3 D(0, 7, 3),D(0, 6, 3) + +#define DEF_TIM_DMA__BTCH_TIMER2_CH0 D(0, 4, 5) +#define DEF_TIM_DMA__BTCH_TIMER2_CH1 D(0, 5, 5) +#define DEF_TIM_DMA__BTCH_TIMER2_CH2 D(0, 7, 5) +#define DEF_TIM_DMA__BTCH_TIMER2_CH3 D(0, 2, 5) + +#define DEF_TIM_DMA__BTCH_TIMER3_CH0 D(0, 0, 2) +#define DEF_TIM_DMA__BTCH_TIMER3_CH1 D(0, 3, 2) +#define DEF_TIM_DMA__BTCH_TIMER3_CH2 D(0, 7, 2) + +#define DEF_TIM_DMA__BTCH_TIMER4_CH0 D(0, 2, 6) +#define DEF_TIM_DMA__BTCH_TIMER4_CH1 D(0, 4, 6) +#define DEF_TIM_DMA__BTCH_TIMER4_CH2 D(0, 0, 6) +#define DEF_TIM_DMA__BTCH_TIMER4_CH3 D(0, 1, 6),D(0, 3, 6) + +#define DEF_TIM_DMA__BTCH_TIMER7_CH0 D(1, 2, 0),D(1, 2, 7) +#define DEF_TIM_DMA__BTCH_TIMER7_CH1 D(1, 2, 0),D(1, 3, 7) +#define DEF_TIM_DMA__BTCH_TIMER7_CH2 D(1, 2, 0),D(1, 4, 7) +#define DEF_TIM_DMA__BTCH_TIMER7_CH3 D(1, 7, 7) + +#define DEF_TIM_DMA__BTCH_TIMER3_CH3 NONE + +#define DEF_TIM_DMA__BTCH_TIMER8_CH0 NONE +#define DEF_TIM_DMA__BTCH_TIMER8_CH1 NONE + +#define DEF_TIM_DMA__BTCH_TIMER9_CH0 NONE + +#define DEF_TIM_DMA__BTCH_TIMER10_CH0 NONE + +#define DEF_TIM_DMA__BTCH_TIMER11_CH0 NONE +#define DEF_TIM_DMA__BTCH_TIMER11_CH1 NONE + +#define DEF_TIM_DMA__BTCH_TIMER12_CH0 NONE + +#define DEF_TIM_DMA__BTCH_TIMER13_CH0 NONE + +// TIM_UP table +#define DEF_TIM_DMA__BTCH_TIMER0_UP D(1, 5, 6) +#define DEF_TIM_DMA__BTCH_TIMER1_UP D(0, 7, 3) +#define DEF_TIM_DMA__BTCH_TIMER2_UP D(0, 2, 5) +#define DEF_TIM_DMA__BTCH_TIMER3_UP D(0, 6, 2) +#define DEF_TIM_DMA__BTCH_TIMER4_UP D(0, 0, 6) +#define DEF_TIM_DMA__BTCH_TIMER5_UP D(0, 1, 7) +#define DEF_TIM_DMA__BTCH_TIMER6_UP D(0, 4, 1) +#define DEF_TIM_DMA__BTCH_TIMER7_UP D(1, 1, 7) +#define DEF_TIM_DMA__BTCH_TIMER8_UP NONE +#define DEF_TIM_DMA__BTCH_TIMER9_UP NONE +#define DEF_TIM_DMA__BTCH_TIMER10_UP NONE +#define DEF_TIM_DMA__BTCH_TIMER11_UP NONE +#define DEF_TIM_DMA__BTCH_TIMER12_UP NONE +#define DEF_TIM_DMA__BTCH_TIMER13_UP NONE + +// AF table +// D(afn, timern) +// NONE +#define DEF_TIM_AF__NONE__TCH_TIMER0_CH0 D(1, 0) +#define DEF_TIM_AF__NONE__TCH_TIMER0_CH1 D(1, 0) +#define DEF_TIM_AF__NONE__TCH_TIMER0_CH2 D(1, 0) +#define DEF_TIM_AF__NONE__TCH_TIMER0_CH3 D(1, 0) +#define DEF_TIM_AF__NONE__TCH_TIMER7_CH0 D(3, 7) +#define DEF_TIM_AF__NONE__TCH_TIMER7_CH1 D(3, 7) +#define DEF_TIM_AF__NONE__TCH_TIMER7_CH2 D(3, 7) +#define DEF_TIM_AF__NONE__TCH_TIMER7_CH3 D(3, 7) + +//PORTA +#define DEF_TIM_AF__PA0__TCH_TIMER1_CH0 D(1, 1) +#define DEF_TIM_AF__PA1__TCH_TIMER1_CH1 D(1, 1) +#define DEF_TIM_AF__PA2__TCH_TIMER1_CH2 D(1, 1) +#define DEF_TIM_AF__PA3__TCH_TIMER1_CH3 D(1, 1) +#define DEF_TIM_AF__PA5__TCH_TIMER1_CH0 D(1, 1) +#define DEF_TIM_AF__PA7__TCH_TIMER0_CH0N D(1, 0) +#define DEF_TIM_AF__PA8__TCH_TIMER0_CH0 D(1, 0) +#define DEF_TIM_AF__PA9__TCH_TIMER0_CH1 D(1, 0) +#define DEF_TIM_AF__PA10__TCH_TIMER0_CH2 D(1, 0) +#define DEF_TIM_AF__PA11__TCH_TIMER0_CH3 D(1, 0) +#define DEF_TIM_AF__PA15__TCH_TIMER1_CH0 D(1, 1) + +#define DEF_TIM_AF__PA0__TCH_TIMER4_CH0 D(2, 4) +#define DEF_TIM_AF__PA1__TCH_TIMER4_CH1 D(2, 4) +#define DEF_TIM_AF__PA2__TCH_TIMER4_CH2 D(2, 4) +#define DEF_TIM_AF__PA3__TCH_TIMER4_CH3 D(2, 4) +#define DEF_TIM_AF__PA6__TCH_TIMER2_CH0 D(2, 2) +#define DEF_TIM_AF__PA7__TCH_TIMER2_CH1 D(2, 2) + +#define DEF_TIM_AF__PA2__TCH_TIMER8_CH0 D(3, 8) +#define DEF_TIM_AF__PA3__TCH_TIMER8_CH1 D(3, 8) +#define DEF_TIM_AF__PA5__TCH_TIMER7_CH0N D(3, 7) +#define DEF_TIM_AF__PA7__TCH_TIMER7_CH0N D(3, 7) + +#define DEF_TIM_AF__PA6__TCH_TIMER12_CH0 D(9, 12) +#define DEF_TIM_AF__PA7__TCH_TIMER13_CH0 D(9, 13) + +//PORTB +#define DEF_TIM_AF__PB0__TCH_TIMER0_CH1N D(1, 0) +#define DEF_TIM_AF__PB1__TCH_TIMER0_CH2N D(1, 0) +#define DEF_TIM_AF__PB2__TCH_TIMER1_CH3 D(1, 1) +#define DEF_TIM_AF__PB3__TCH_TIMER1_CH1 D(1, 1) +#define DEF_TIM_AF__PB8__TCH_TIMER1_CH0 D(1, 1) +#define DEF_TIM_AF__PB9__TCH_TIMER1_CH1 D(1, 1) +#define DEF_TIM_AF__PB10__TCH_TIMER1_CH2 D(1, 1) +#define DEF_TIM_AF__PB11__TCH_TIMER1_CH3 D(1, 1) +#define DEF_TIM_AF__PB13__TCH_TIMER0_CH0N D(1, 0) +#define DEF_TIM_AF__PB14__TCH_TIMER0_CH1N D(1, 0) +#define DEF_TIM_AF__PB15__TCH_TIMER0_CH2N D(1, 0) + +#define DEF_TIM_AF__PB0__TCH_TIMER2_CH2 D(2, 2) +#define DEF_TIM_AF__PB1__TCH_TIMER2_CH3 D(2, 2) +#define DEF_TIM_AF__PB4__TCH_TIMER2_CH0 D(2, 2) +#define DEF_TIM_AF__PB5__TCH_TIMER2_CH1 D(2, 2) +#define DEF_TIM_AF__PB6__TCH_TIMER3_CH0 D(2, 3) +#define DEF_TIM_AF__PB7__TCH_TIMER3_CH1 D(2, 3) +#define DEF_TIM_AF__PB8__TCH_TIMER3_CH2 D(2, 3) +#define DEF_TIM_AF__PB9__TCH_TIMER3_CH3 D(2, 3) + +#define DEF_TIM_AF__PB0__TCH_TIMER7_CH1N D(3, 7) +#define DEF_TIM_AF__PB1__TCH_TIMER7_CH2N D(3, 7) +#define DEF_TIM_AF__PB8__TCH_TIMER9_CH0 D(3, 9) +#define DEF_TIM_AF__PB9__TCH_TIMER10_CH0 D(3, 10) +#define DEF_TIM_AF__PB14__TCH_TIMER7_CH1N D(3, 7) +#define DEF_TIM_AF__PB15__TCH_TIMER7_CH2N D(3, 7) + +#define DEF_TIM_AF__PB14__TCH_TIMER11_CH0 D(9, 11) +#define DEF_TIM_AF__PB15__TCH_TIMER11_CH1 D(9, 11) + +//PORTC +#define DEF_TIM_AF__PC6__TCH_TIMER2_CH0 D(2, 2) +#define DEF_TIM_AF__PC7__TCH_TIMER2_CH1 D(2, 2) +#define DEF_TIM_AF__PC8__TCH_TIMER2_CH2 D(2, 2) +#define DEF_TIM_AF__PC9__TCH_TIMER2_CH3 D(2, 2) + +#define DEF_TIM_AF__PC6__TCH_TIMER7_CH0 D(3, 7) +#define DEF_TIM_AF__PC7__TCH_TIMER7_CH1 D(3, 7) +#define DEF_TIM_AF__PC8__TCH_TIMER7_CH2 D(3, 7) +#define DEF_TIM_AF__PC9__TCH_TIMER7_CH3 D(3, 7) + +//PORTD +#define DEF_TIM_AF__PD12__TCH_TIMER3_CH0 D(2, 3) +#define DEF_TIM_AF__PD13__TCH_TIMER3_CH1 D(2, 3) +#define DEF_TIM_AF__PD14__TCH_TIMER3_CH2 D(2, 3) +#define DEF_TIM_AF__PD15__TCH_TIMER3_CH3 D(2, 3) + +//PORTE +#define DEF_TIM_AF__PE8__TCH_TIMER0_CH0N D(1, 0) +#define DEF_TIM_AF__PE9__TCH_TIMER0_CH0 D(1, 0) +#define DEF_TIM_AF__PE10__TCH_TIMER0_CH1N D(1, 0) +#define DEF_TIM_AF__PE11__TCH_TIMER0_CH1 D(1, 0) +#define DEF_TIM_AF__PE12__TCH_TIMER0_CH2N D(1, 0) +#define DEF_TIM_AF__PE13__TCH_TIMER0_CH2 D(1, 0) +#define DEF_TIM_AF__PE14__TCH_TIMER0_CH3 D(1, 0) + +#define DEF_TIM_AF__PE5__TCH_TIMER8_CH0 D(3, 8) +#define DEF_TIM_AF__PE6__TCH_TIMER8_CH1 D(3, 8) + +//PORTF +#define DEF_TIM_AF__PF6__TCH_TIMER9_CH0 D(3, 9) +#define DEF_TIM_AF__PF7__TCH_TIMER10_CH0 D(3, 10) + +//PORTH +#define DEF_TIM_AF__PH10__TCH_TIMER4_CH0 D(2, 4) +#define DEF_TIM_AF__PH11__TCH_TIMER4_CH1 D(2, 4) +#define DEF_TIM_AF__PH12__TCH_TIMER4_CH2 D(2, 4) + +#define DEF_TIM_AF__PH13__TCH_TIMER7_CH0N D(3, 7) +#define DEF_TIM_AF__PH14__TCH_TIMER7_CH1N D(3, 7) +#define DEF_TIM_AF__PH15__TCH_TIMER7_CH2N D(3, 7) + +#define DEF_TIM_AF__PH6__TCH_TIMER11_CH0 D(9, 11) +#define DEF_TIM_AF__PH9__TCH_TIMER11_CH1 D(9, 11) + +//PORTI +#define DEF_TIM_AF__PI0__TCH_TIMER4_CH3 D(2, 4) + +#define DEF_TIM_AF__PI2__TCH_TIMER7_CH3 D(3, 7) +#define DEF_TIM_AF__PI5__TCH_TIMER7_CH0 D(3, 7) +#define DEF_TIM_AF__PI6__TCH_TIMER7_CH1 D(3, 7) +#define DEF_TIM_AF__PI7__TCH_TIMER7_CH2 D(3, 7) + +#else + +#endif + +#if defined(GD32F4) + +#define FULL_TIMER_CHANNEL_COUNT 78 +#define USED_TIMERS ( TIM_N(0) | 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)) +#define HARDWARE_TIMER_DEFINITION_COUNT 14 + +#else + +#endif diff --git a/src/platform/GD32/timer_gd32f4xx.c b/src/platform/GD32/timer_gd32f4xx.c new file mode 100755 index 0000000000..b42d687097 --- /dev/null +++ b/src/platform/GD32/timer_gd32f4xx.c @@ -0,0 +1,204 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#ifdef USE_TIMER + +#include "common/utils.h" + +#include "drivers/dma.h" +#include "drivers/io.h" +#include "timer_def.h" + +#include "gd32f4xx.h" +#include "platform/rcc.h" +#include "drivers/timer.h" + +const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { + { .TIMx = (void *)TIMER0, .rcc = RCC_APB2(TIMER0), .inputIrq = TIMER0_Channel_IRQn}, + { .TIMx = (void *)TIMER1, .rcc = RCC_APB1(TIMER1), .inputIrq = TIMER1_IRQn}, + { .TIMx = (void *)TIMER2, .rcc = RCC_APB1(TIMER2), .inputIrq = TIMER2_IRQn}, + { .TIMx = (void *)TIMER3, .rcc = RCC_APB1(TIMER3), .inputIrq = TIMER3_IRQn}, + +#if defined (GD32F450) || defined (GD32F460) || defined (GD32F470) || defined (GD32F405) || defined (GD32F425) || defined (GD32F407) || defined (GD32F427) + { .TIMx = (void *)TIMER4, .rcc = RCC_APB1(TIMER4), .inputIrq = TIMER4_IRQn}, + { .TIMx = (void *)TIMER5, .rcc = RCC_APB1(TIMER5), .inputIrq = TIMER5_DAC_IRQn}, + { .TIMx = (void *)TIMER6, .rcc = RCC_APB1(TIMER6), .inputIrq = TIMER6_IRQn}, +#else + { .TIMx = (void *)TIMER4, .rcc = RCC_APB1(TIMER4), .inputIrq = 0}, + { .TIMx = (void *)TIMER5, .rcc = RCC_APB1(TIMER5), .inputIrq = 0}, + { .TIMx = (void *)TIMER6, .rcc = RCC_APB1(TIMER6), .inputIrq = 0}, +#endif /* GD32F450 || GD32F460 || GD32F470 || (GD32F405) || defined (GD32F425) defined (GD32F407) || defined (GD32F427) */ + + { .TIMx = (void *)TIMER7, .rcc = RCC_APB2(TIMER7), .inputIrq = TIMER7_Channel_IRQn}, + { .TIMx = (void *)TIMER8, .rcc = RCC_APB2(TIMER8), .inputIrq = TIMER0_BRK_TIMER8_IRQn}, + { .TIMx = (void *)TIMER9, .rcc = RCC_APB2(TIMER9), .inputIrq = TIMER0_UP_TIMER9_IRQn}, + { .TIMx = (void *)TIMER10, .rcc = RCC_APB2(TIMER10), .inputIrq = TIMER0_TRG_CMT_TIMER10_IRQn}, + + { .TIMx = (void *)TIMER11, .rcc = RCC_APB1(TIMER11), .inputIrq = TIMER7_BRK_TIMER11_IRQn}, + { .TIMx = (void *)TIMER12, .rcc = RCC_APB1(TIMER12), .inputIrq = TIMER7_UP_TIMER12_IRQn}, + { .TIMx = (void *)TIMER13, .rcc = RCC_APB1(TIMER13), .inputIrq = TIMER7_TRG_CMT_TIMER13_IRQn}, +}; + +#if defined(USE_TIMER_MGMT) +const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = { + // Auto-generated from 'timer_def.h' +//PORTA + DEF_TIMER(TIMER1, CH0, PA0, 0, 0), + DEF_TIMER(TIMER1, CH1, PA1, 0, 0), + DEF_TIMER(TIMER1, CH2, PA2, 0, 0), + DEF_TIMER(TIMER1, CH3, PA3, 0, 0), + DEF_TIMER(TIMER1, CH0, PA5, 0, 0), + DEF_TIMER(TIMER0, CH0N, PA7, 0, 0), + DEF_TIMER(TIMER0, CH0, PA8, 0, 0), + DEF_TIMER(TIMER0, CH1, PA9, 0, 0), + DEF_TIMER(TIMER0, CH2, PA10, 0, 0), + DEF_TIMER(TIMER0, CH3, PA11, 0, 0), + DEF_TIMER(TIMER1, CH0, PA15, 0, 0), + + DEF_TIMER(TIMER4, CH0, PA0, 0, 0), + DEF_TIMER(TIMER4, CH1, PA1, 0, 0), + DEF_TIMER(TIMER4, CH2, PA2, 0, 0), + DEF_TIMER(TIMER4, CH3, PA3, 0, 0), + DEF_TIMER(TIMER2, CH0, PA6, 0, 0), + DEF_TIMER(TIMER2, CH1, PA7, 0, 0), + + DEF_TIMER(TIMER8, CH0, PA2, 0, 0), + DEF_TIMER(TIMER8, CH1, PA3, 0, 0), + DEF_TIMER(TIMER7, CH0N, PA5, 0, 0), + DEF_TIMER(TIMER7, CH0N, PA7, 0, 0), + + DEF_TIMER(TIMER12, CH0, PA6, 0, 0), + DEF_TIMER(TIMER13, CH0, PA7, 0, 0), + +//PORTB + DEF_TIMER(TIMER0, CH1N, PB0, 0, 0), + DEF_TIMER(TIMER0, CH2N, PB1, 0, 0), + DEF_TIMER(TIMER1, CH3, PB2, 0, 0), + DEF_TIMER(TIMER1, CH1, PB3, 0, 0), + DEF_TIMER(TIMER1, CH2, PB10, 0, 0), + DEF_TIMER(TIMER1, CH3, PB11, 0, 0), + DEF_TIMER(TIMER0, CH0N, PB13, 0, 0), + DEF_TIMER(TIMER0, CH1N, PB14, 0, 0), + DEF_TIMER(TIMER0, CH2N, PB15, 0, 0), + + DEF_TIMER(TIMER2, CH2, PB0, 0, 0), + DEF_TIMER(TIMER2, CH3, PB1, 0, 0), + DEF_TIMER(TIMER2, CH0, PB4, 0, 0), + DEF_TIMER(TIMER2, CH1, PB5, 0, 0), + DEF_TIMER(TIMER3, CH0, PB6, 0, 0), + DEF_TIMER(TIMER3, CH1, PB7, 0, 0), + DEF_TIMER(TIMER3, CH2, PB8, 0, 0), + DEF_TIMER(TIMER3, CH3, PB9, 0, 0), + + DEF_TIMER(TIMER7, CH1N, PB0, 0, 0), + DEF_TIMER(TIMER7, CH2N, PB1, 0, 0), + DEF_TIMER(TIMER9, CH0, PB8, 0, 0), + DEF_TIMER(TIMER10, CH0, PB9, 0, 0), + DEF_TIMER(TIMER7, CH1N, PB14, 0, 0), + DEF_TIMER(TIMER7, CH2N, PB15, 0, 0), + + DEF_TIMER(TIMER11, CH0, PB14, 0, 0), + DEF_TIMER(TIMER11, CH1, PB15, 0, 0), + +//PORTC + DEF_TIMER(TIMER2, CH0, PC6, 0, 0), + DEF_TIMER(TIMER2, CH1, PC7, 0, 0), + DEF_TIMER(TIMER2, CH2, PC8, 0, 0), + DEF_TIMER(TIMER2, CH3, PC9, 0, 0), + + DEF_TIMER(TIMER7, CH0, PC6, 0, 0), + DEF_TIMER(TIMER7, CH1, PC7, 0, 0), + DEF_TIMER(TIMER7, CH2, PC8, 0, 0), + DEF_TIMER(TIMER7, CH3, PC9, 0, 0), + +//PORTD + DEF_TIMER(TIMER3, CH0, PD12, 0, 0), + DEF_TIMER(TIMER3, CH1, PD13, 0, 0), + DEF_TIMER(TIMER3, CH2, PD14, 0, 0), + DEF_TIMER(TIMER3, CH3, PD15, 0, 0), + +//PORTE + DEF_TIMER(TIMER0, CH0N, PE8, 0, 0), + DEF_TIMER(TIMER0, CH0, PE9, 0, 0), + DEF_TIMER(TIMER0, CH1N, PE10, 0, 0), + DEF_TIMER(TIMER0, CH1, PE11, 0, 0), + DEF_TIMER(TIMER0, CH2N, PE12, 0, 0), + DEF_TIMER(TIMER0, CH2, PE13, 0, 0), + DEF_TIMER(TIMER0, CH3, PE14, 0, 0), + + DEF_TIMER(TIMER8, CH0, PE5, 0, 0), + DEF_TIMER(TIMER8, CH1, PE6, 0, 0), + +//PORTF + DEF_TIMER(TIMER9, CH0, PF6, 0, 0), + DEF_TIMER(TIMER10, CH0, PF7, 0, 0), +}; +#endif + +uint32_t timerClock(const TIM_TypeDef *tim) +{ + uint32_t timer = (uint32_t)tim; + if (timer == TIMER7 || timer == TIMER0 || timer == TIMER8 || timer == TIMER9 || timer == TIMER10) { + return SystemCoreClock; + } else { + return SystemCoreClock / 2; + } +} + +void gd32_timer_input_capture_config(void* timer, uint16_t channel, uint8_t state) +{ + switch(channel) { + case TIMER_CH_0: + if(state) { + TIMER_CHCTL2((uint32_t)timer) |= (uint32_t)TIMER_CHCTL2_CH0EN; + } else { + TIMER_CHCTL2((uint32_t)timer) &= (~(uint32_t)TIMER_CHCTL2_CH0EN); + } + break; + case TIMER_CH_1: + if(state) { + TIMER_CHCTL2((uint32_t)timer) |= (uint32_t)TIMER_CHCTL2_CH1EN; + } else { + TIMER_CHCTL2((uint32_t)timer) &= (~(uint32_t)TIMER_CHCTL2_CH1EN); + } + break; + case TIMER_CH_2: + if(state) { + TIMER_CHCTL2((uint32_t)timer) |= (uint32_t)TIMER_CHCTL2_CH2EN; + } else { + TIMER_CHCTL2((uint32_t)timer) &= (~(uint32_t)TIMER_CHCTL2_CH2EN); + } + break; + case TIMER_CH_3: + if(state) { + TIMER_CHCTL2((uint32_t)timer) |= (uint32_t)TIMER_CHCTL2_CH3EN; + } else { + TIMER_CHCTL2((uint32_t)timer) &= (~(uint32_t)TIMER_CHCTL2_CH3EN); + } + break; + default: + break; + } +} + +#endif diff --git a/src/platform/GD32/timer_stdperiph.c b/src/platform/GD32/timer_stdperiph.c new file mode 100755 index 0000000000..e5bbc9b42a --- /dev/null +++ b/src/platform/GD32/timer_stdperiph.c @@ -0,0 +1,926 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_TIMER + +#include "build/atomic.h" + +#include "common/utils.h" + +#include "drivers/nvic.h" + +#include "drivers/io.h" +#include "platform/rcc.h" +#include "drivers/system.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): + TIMER0 2 channels + TIMER1 4 channels + TIMER2 4 channels + TIMER3 4 channels +*/ + +#define USED_TIMER_COUNT BITCOUNT(USED_TIMERS) +#define CC_CHANNELS_PER_TIMER 4 + +#define TIMER_INT_CHx(ch) (TIMER_INT_CH0 << (ch)) + +typedef struct timerConfig_s { + 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 linked list of active overflow callbacks + uint32_t forcedOverflowTimerValue; +} timerConfig_t; + +timerConfig_t timerConfig[USED_TIMER_COUNT]; + +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]; + +// 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 TIM_TypeDef *tim) // TIM_TypeDef只有 +{ +#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_(TIMER##i, TIMER_INDEX(i)) + +// let gcc do the work, switch should be quite optimized + switch ((unsigned)tim >> _CASE_SHF) { +#if USED_TIMERS & TIM_N(0) + _CASE(0); +#endif +#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 + default: return ~1; // make sure final index is out of range + } +#undef _CASE +#undef _CASE_ +} + +uint32_t usedTimers[USED_TIMER_COUNT] = { +#define _DEF(i) (uint32_t)(TIMER##i) + +#if USED_TIMERS & TIM_N(0) + _DEF(0), +#endif +#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 +#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(0) + _DEF(0), +#endif +#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 +#undef _DEF +}; + +int8_t timerGetNumberByIndex(uint8_t index) +{ + if (index < USED_TIMER_COUNT) { + return timerNumbers[index]; + } else { + return 0; + } +} + +int8_t timerGetTIMNumber(const TIM_TypeDef *tim) +{ + const uint8_t index = lookupTimerIndex(tim); + + return timerGetNumberByIndex(index); +} + +static inline uint8_t lookupChannelIndex(const uint16_t channel) +{ + return channel; +} + +uint8_t timerLookupChannelIndex(const uint16_t channel) +{ + return lookupChannelIndex(channel); +} + +rccPeriphTag_t timerRCC(const TIM_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 TIM_TypeDef *tim) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].inputIrq; + } + } + return 0; +} + +static void timerNVICConfigure(uint8_t irq) +{ + nvic_irq_enable(irq , NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER)); +} + +void timerReconfigureTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) +{ + configTimeBase(tim, period, hz); +} + +void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) +{ + timer_parameter_struct timer_initpara; + + timer_struct_para_init(&timer_initpara); + + timer_initpara.period = (period - 1) & 0xFFFF; + timer_initpara.prescaler = (timerClock(tim) / hz) - 1; + timer_initpara.clockdivision = TIMER_CKDIV_DIV1; + timer_initpara.alignedmode = TIMER_COUNTER_EDGE; + timer_initpara.counterdirection = TIMER_COUNTER_UP; + timer_initpara.repetitioncounter = 0; + timer_init((uint32_t)tim, &timer_initpara); +} + +// old interface for PWM inputs. It should be replaced +void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz) +{ + configTimeBase(timerHardwarePtr->tim, period, hz); + timer_enable((uint32_t)timerHardwarePtr->tim); + + uint8_t irq = timerInputIrq(timerHardwarePtr->tim); + timerNVICConfigure(irq); + + switch (irq) { + case TIMER0_Channel_IRQn: + timerNVICConfigure(TIMER0_UP_TIMER9_IRQn); + break; + + case TIMER7_Channel_IRQn: + timerNVICConfigure(TIMER7_UP_TIMER12_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) +{ + 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((void *)usedTimers[timer], 0, 1); + timer_enable(usedTimers[timer]); + + nvic_irq_enable(irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority)); + + 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 TIM_TypeDef *tim) +{ + 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) { + timer_interrupt_enable((uint32_t)tim, TIMER_INT_UP); + } else { + timer_interrupt_disable((uint32_t)tim, TIMER_INT_UP); + } +} + +// config edge and overflow callback for channel. Try to avoid per-channel 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 + timer_interrupt_disable((uint32_t)(timHw->tim), TIMER_INT_CHx(timHw->channel)); + + // setup callback info + timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback; + timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback; + // enable channel IRQ + if (edgeCallback) + timer_interrupt_enable((uint32_t)(timHw->tim), TIMER_INT_CHx(timHw->channel)); + + timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim); +} + +void timerConfigUpdateCallback(const TIM_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 & ~TIMER_CH_1; // lower channel + uint16_t chHi = timHw->channel | TIMER_CH_1; // upper channel + + uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel + + if (edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL + timer_interrupt_disable((uint32_t)(timHw->tim), TIMER_INT_CHx(chLo)); + + if (edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL + timer_interrupt_disable((uint32_t)(timHw->tim), TIMER_INT_CHx(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) { + timer_flag_clear((uint32_t)(timHw->tim), TIMER_INT_CHx(chLo)); + timer_interrupt_enable((uint32_t)(timHw->tim), TIMER_INT_CHx(chLo)); + } + + if (edgeCallbackHi) { + timer_flag_clear((uint32_t)(timHw->tim), TIMER_INT_CHx(chHi)); + timer_interrupt_enable((uint32_t)(timHw->tim), TIMER_INT_CHx(chHi)); + } + + timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim); +} + +// enable/disable IRQ for low channel in dual configuration +void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newState) +{ + if (newState) { + timer_interrupt_enable((uint32_t)(timHw->tim), TIMER_INT_CHx(timHw->channel &~ TIMER_CH_1)); + } else { + timer_interrupt_disable((uint32_t)(timHw->tim), TIMER_INT_CHx(timHw->channel &~ TIMER_CH_1)); + } +} + +// enable or disable IRQ +void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState) +{ + if (newState) { + timer_interrupt_enable((uint32_t)(timHw->tim), TIMER_INT_CHx(timHw->channel)); + } else { + timer_interrupt_disable((uint32_t)(timHw->tim), TIMER_INT_CHx(timHw->channel)); + } +} + +// clear Compare/Capture flag for channel +void timerChClearCCFlag(const timerHardware_t *timHw) +{ + timer_flag_clear((uint32_t)(timHw->tim), TIMER_INT_CHx(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 capture +void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) +{ + timer_ic_parameter_struct timer_icinitpara; + timer_channel_input_struct_para_init(&timer_icinitpara); + timer_icinitpara.icpolarity = polarityRising ? TIMER_IC_POLARITY_RISING : TIMER_IC_POLARITY_FALLING; + timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI; + timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1; + timer_icinitpara.icfilter = getFilter(inputFilterTicks); + timer_input_capture_config((uint32_t)(timHw->tim), timHw->channel, &timer_icinitpara); +} + +// 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) +{ + timer_ic_parameter_struct timer_icinitpara; + bool directRising = (timHw->channel & TIMER_CH_1) ? !polarityRising : polarityRising; + // configure direct channel + timer_channel_input_struct_para_init(&timer_icinitpara); + + + timer_icinitpara.icpolarity = directRising ? TIMER_IC_POLARITY_RISING : TIMER_IC_POLARITY_FALLING; + timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI; + timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1; + timer_icinitpara.icfilter = getFilter(inputFilterTicks); + timer_input_capture_config((uint32_t)(timHw->tim), timHw->channel, &timer_icinitpara); + // configure indirect channel + timer_icinitpara.icpolarity = directRising ? TIMER_IC_POLARITY_FALLING : TIMER_IC_POLARITY_RISING; + timer_icinitpara.icselection = TIMER_IC_SELECTION_INDIRECTTI; + timer_input_capture_config((uint32_t)(timHw->tim), (timHw->channel ^ TIMER_CH_1), &timer_icinitpara); // get opposite channel no +} + +void timerChICPolarity(const timerHardware_t *timHw, bool polarityRising) +{ + timCCER_t tmpccer = TIMER_CHCTL2((uint32_t)(timHw->tim)); + tmpccer &= ~(TIMER_CHCTL2_CH0P << (timHw->channel << 2)); + tmpccer |= polarityRising ? (TIMER_IC_POLARITY_RISING << (timHw->channel << 2)) : (TIMER_IC_POLARITY_FALLING << (timHw->channel << 2)); + TIMER_CHCTL2((uint32_t)(timHw->tim)) = tmpccer; +} + +volatile timCCR_t* timerChCCRHi(const timerHardware_t *timHw) +{ + return (volatile timCCR_t*)((volatile uint32_t*)((uintptr_t)&TIMER_CH0CV(&(timHw->tim))) + (timHw->channel | TIMER_CH_1)); +} + +volatile timCCR_t* timerChCCRLo(const timerHardware_t *timHw) +{ + return (volatile timCCR_t*)((volatile uint32_t*)((uintptr_t)&TIMER_CH0CV(&(timHw->tim))) + (timHw->channel & ~TIMER_CH_1)); +} + +volatile timCCR_t* timerChCCR(const timerHardware_t *timHw) +{ + return (volatile timCCR_t*)((volatile uint32_t*)((uintptr_t)&TIMER_CH0CV((uint32_t)(timHw->tim))) + (timHw->channel)); +} + +void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh) +{ + timer_oc_parameter_struct timer_ocintpara; + timer_channel_output_struct_para_init(&timer_ocintpara); + + uint16_t ocmode; + if (outEnable) { + ocmode = TIMER_OC_MODE_INACTIVE; + timer_ocintpara.outputstate = TIMER_CCX_ENABLE; + + if (timHw->output & TIMER_OUTPUT_INVERTED) { + stateHigh = !stateHigh; + } + timer_ocintpara.ocpolarity = stateHigh ? TIMER_OC_POLARITY_HIGH : TIMER_OC_POLARITY_LOW; + } else { + ocmode = TIMER_OC_MODE_TIMING; + } + + timer_channel_output_config((uint32_t)(timHw->tim), timHw->channel, &timer_ocintpara); + timer_channel_output_mode_config((uint32_t)(timHw->tim), timHw->channel, ocmode); + timer_channel_output_shadow_config((uint32_t)(timHw->tim), timHw->channel, TIMER_OC_SHADOW_DISABLE); +} + + +static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) +{ + uint16_t capture; + unsigned tim_status; + tim_status = TIMER_INTF((uint32_t)tim) & TIMER_DMAINTEN((uint32_t)tim); + + while (tim_status) { + // flags will be cleared by reading CCR in dual capture, make sure we call handler correctly + // current 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); + TIMER_INTF((uint32_t)tim) = mask; + tim_status &= mask; + + switch (bit) { + case __builtin_clz(TIMER_INT_UP): { + + if (timerConfig->forcedOverflowTimerValue != 0) { + capture = timerConfig->forcedOverflowTimerValue - 1; + timerConfig->forcedOverflowTimerValue = 0; + } else { + capture = TIMER_CAR((uint32_t)tim); + } + + timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive; + while (cb) { + cb->fn(cb, capture); + cb = cb->next; + } + break; + } + case __builtin_clz(TIMER_INT_CH0): + if (timerConfig->edgeCallback[0]) { + timerConfig->edgeCallback[0]->fn(timerConfig->edgeCallback[0], TIMER_CH0CV((uint32_t)tim)); + } + break; + case __builtin_clz(TIMER_INT_CH1): + if (timerConfig->edgeCallback[1]) { + timerConfig->edgeCallback[1]->fn(timerConfig->edgeCallback[1], TIMER_CH1CV((uint32_t)tim)); + } + break; + case __builtin_clz(TIMER_INT_CH2): + if (timerConfig->edgeCallback[2]) { + timerConfig->edgeCallback[2]->fn(timerConfig->edgeCallback[2], TIMER_CH2CV((uint32_t)tim)); + } + break; + case __builtin_clz(TIMER_INT_CH3): + if (timerConfig->edgeCallback[3]) { + timerConfig->edgeCallback[3]->fn(timerConfig->edgeCallback[3], TIMER_CH3CV((uint32_t)tim)); + } + break; + } + } +} + +static inline void timUpdateHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig) +{ + uint16_t capture; + unsigned tim_status; + tim_status = TIMER_INTF((uint32_t)tim) & TIMER_DMAINTEN((uint32_t)tim); + 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); + TIMER_INTF((uint32_t)tim) = mask; + tim_status &= mask; + switch (bit) { + case __builtin_clz(TIMER_INT_UP): { + + if (timerConfig->forcedOverflowTimerValue != 0) { + capture = timerConfig->forcedOverflowTimerValue - 1; + timerConfig->forcedOverflowTimerValue = 0; + } else { + capture = TIMER_CAR((uint32_t)tim); + } + + 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((void *)(TIMER ## i), &timerConfig[TIMER_INDEX(i)]); \ + timCCxHandler((void *)(TIMER ## j), &timerConfig[TIMER_INDEX(j)]); \ + } struct dummy + +#define _TIM_IRQ_HANDLER(name, i) \ + void name(void) \ + { \ + timCCxHandler((void *)(TIMER ## i), &timerConfig[TIMER_INDEX(i)]); \ + } struct dummy + +#define _TIM_IRQ_HANDLER_UPDATE_ONLY(name, i) \ + void name(void) \ + { \ + timUpdateHandler((void *)(TIMER ## i), &timerConfig[TIMER_INDEX(i)]); \ + } struct dummy + +#if USED_TIMERS & TIM_N(0) +_TIM_IRQ_HANDLER(TIMER0_Channel_IRQHandler, 0); +# if USED_TIMERS & TIM_N(9) +_TIM_IRQ_HANDLER2(TIMER0_UP_TIMER9_IRQHandler, 0, 9); // both timers are in use +# else +_TIM_IRQ_HANDLER(TIMER0_UP_TIMER9_IRQHandler, 0); // timer9 is not used +# endif +#endif + +#if USED_TIMERS & TIM_N(1) +_TIM_IRQ_HANDLER(TIMER1_IRQHandler, 1); +#endif +#if USED_TIMERS & TIM_N(2) +_TIM_IRQ_HANDLER(TIMER2_IRQHandler, 2); +#endif +#if USED_TIMERS & TIM_N(3) +_TIM_IRQ_HANDLER(TIMER3_IRQHandler, 3); +#endif +#if USED_TIMERS & TIM_N(4) +_TIM_IRQ_HANDLER(TIMER4_IRQHandler, 4); +#endif + +#if USED_TIMERS & TIM_N(5) +# if !(defined(USE_PID_AUDIO)) +_TIM_IRQ_HANDLER(TIMER5_DAC_IRQHandler, 5); +# endif +#endif + +#if USED_TIMERS & TIM_N(6) +# if !(defined(USE_VCP) && (defined(GD32F4))) +_TIM_IRQ_HANDLER_UPDATE_ONLY(TIMER6_IRQHandler, 6); +# endif +#endif + +#if USED_TIMERS & TIM_N(7) +_TIM_IRQ_HANDLER(TIMER7_Channel_IRQHandler, 7); + +# if USED_TIMERS & TIM_N(12) +_TIM_IRQ_HANDLER2(TIMER7_UP_TIMER12_IRQHandler, 7, 12); // both timers are in use +# else +_TIM_IRQ_HANDLER(TIMER7_UP_TIMER12_IRQHandler, 7); // timer12 is not used +# endif +#endif + +#if USED_TIMERS & TIM_N(8) +_TIM_IRQ_HANDLER(TIMER0_BRK_TIMER8_IRQHandler, 8); +#endif +#if USED_TIMERS & TIM_N(10) +_TIM_IRQ_HANDLER(TIMER0_TRG_CMT_TIMER10_IRQHandler, 10); +#endif +#if USED_TIMERS & TIM_N(11) +_TIM_IRQ_HANDLER(TIMER7_BRK_TIMER11_IRQHandler, 11); +#endif +#if USED_TIMERS & TIM_N(13) +_TIM_IRQ_HANDLER(TIMER7_TRG_CMT_TIMER13_IRQHandler, 13); +#endif + +void timerInit(void) +{ + memset(timerConfig, 0, sizeof(timerConfig)); + + /* 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(TIM_TypeDef *tim) +{ + timer_enable((uint32_t)tim); +} + +/*! + \brief force an overflow for a given timer + \param[in] tim: The timer to overflow + \param[out] none + \retval none +*/ +void timerForceOverflow(TIM_TypeDef *tim) +{ + uint8_t timerIndex = lookupTimerIndex((const TIM_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 = TIMER_CNT((uint32_t)tim) + 1; + + // Force an overflow by setting the UG bit + TIMER_SWEVG((uint32_t)tim) |= TIMER_SWEVG_UPG; + } +} + +void timerOCInit(TIM_TypeDef *tim, uint8_t channel, TIM_OCInitTypeDef *init) +{ + timer_channel_output_config((uint32_t)tim, (uint16_t)channel, init); +} + +void timerOCModeConfig(void *tim, uint8_t channel, uint16_t ocmode) +{ + timer_channel_output_mode_config((uint32_t)tim, (uint16_t)channel, ocmode); +} + +void timerOCPreloadConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t ocshadow) +{ + timer_channel_output_shadow_config((uint32_t)tim, channel, ocshadow); +} + +volatile timCCR_t* timerCCR(TIM_TypeDef *tim, uint8_t channel) +{ + return (volatile timCCR_t*)((volatile uint32_t*)(&TIMER_CH0CV((uint32_t)tim)) + (channel)); +} + +uint16_t timerDmaSource(uint8_t channel) +{ + switch (channel) { + case TIMER_CH_0: + return TIMER_DMA_CH0D; + case TIMER_CH_1: + return TIMER_DMA_CH1D; + case TIMER_CH_2: + return TIMER_DMA_CH2D; + case TIMER_CH_3: + return TIMER_DMA_CH3D; + } + return 0; +} + +uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz) +{ + return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz)); +} + +uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz) +{ + return (uint16_t)((timerClock(tim) / (prescaler + 1)) / hz); +} + +uint16_t timerGetPrescalerByDesiredHertz(TIM_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; +} + +void timerReset(TIM_TypeDef *timer) +{ + timer_deinit((uint32_t)timer); +} + +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period) +{ + TIMER_CAR((uint32_t)timer) = period; +} + +uint32_t timerGetPeriod(TIM_TypeDef *timer) +{ + return TIMER_CAR((uint32_t)timer); +} + +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter) +{ + TIMER_CNT((uint32_t)timer) = counter; +} + +void timerDisable(TIM_TypeDef *timer) +{ + timer_interrupt_disable((uint32_t)timer, TIMER_INT_UP); + timer_disable((uint32_t)timer); +} + +void timerEnable(TIM_TypeDef *timer) +{ + timer_enable((uint32_t)timer); + timer_event_software_generate((uint32_t)timer, TIMER_EVENT_SRC_UPG); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + timer_flag_clear((uint32_t)timer, TIMER_FLAG_UP); + timer_interrupt_enable((uint32_t)timer, TIMER_INT_UP); +} + +#endif diff --git a/src/platform/GD32/transponder_ir_io_stdperiph.c b/src/platform/GD32/transponder_ir_io_stdperiph.c new file mode 100755 index 0000000000..e82e7d1af6 --- /dev/null +++ b/src/platform/GD32/transponder_ir_io_stdperiph.c @@ -0,0 +1,249 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_TRANSPONDER + +#include "drivers/dma.h" +#include "drivers/dma_reqmap.h" +#include "drivers/io.h" +#include "drivers/nvic.h" +#include "platform/rcc.h" +#include "drivers/timer.h" +#include "drivers/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 TIM_TypeDef *timer = NULL; +uint8_t alternateFunction; +static dmaResource_t *dmaRef = NULL; + +transponder_t transponder; + +static void TRANSPONDER_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) +{ + if (DMA_GET_FLAG_STATUS(descriptor, DMA_INT_FLAG_FTF)) { + transponderIrDataTransferInProgress = 0; + + xDMA_Cmd(descriptor->ref, DISABLE); + DMA_CLEAR_FLAG(descriptor, DMA_INT_FLAG_FTF); + } +} + +void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder) +{ + (void) transponder; + if (!ioTag) { + return; + } + + timer_parameter_struct timer_initpara; + timer_oc_parameter_struct timer_ocintpara; + dma_single_data_parameter_struct dma_init_struct; + + const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_TRANSPONDER, 0); + timer = timerHardware->tim; + alternateFunction = timerHardware->alternateFunction; + +#if defined(USE_DMA_SPEC) + const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware); + + if (dmaSpec == NULL) { + return; + } + + 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_TRANSPONDER, 0)) { + return; + } + + transponderIO = IOGetByTag(ioTag); + IOInit(transponderIO, OWNER_TRANSPONDER, 0); + + IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLDOWN), timerHardware->alternateFunction); + + dmaEnable(dmaGetIdentifier(dmaRef)); + dmaSetHandler(dmaGetIdentifier(dmaRef), TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, 0); + + RCC_ClockCmd(timerRCC(timer), ENABLE); + + uint16_t prescaler = timerGetPrescalerByDesiredMhz(timer, transponder->timer_hz); + uint16_t period = timerGetPeriodByPrescaler(timer, prescaler, transponder->timer_carrier_hz); + + transponder->bitToggleOne = period / 2; + + timer_struct_para_init(&timer_initpara); + timer_initpara.period = period; + timer_initpara.prescaler = prescaler; + timer_initpara.clockdivision = TIMER_CKDIV_DIV1; + timer_initpara.alignedmode = TIMER_COUNTER_EDGE; + timer_initpara.counterdirection = TIMER_COUNTER_UP; + timer_initpara.repetitioncounter = 0; + timer_init((uint32_t)timer, &timer_initpara); + + timer_channel_output_struct_para_init(&timer_ocintpara); + timer_channel_output_mode_config((uint32_t)timer, timerHardware->channel, TIMER_OC_MODE_PWM0); + + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + timer_ocintpara.outputnstate = TIMER_CCXN_ENABLE; + timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; + } else { + timer_ocintpara.outputstate = TIMER_CCX_ENABLE; + timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_HIGH; + } + + timer_ocintpara.ocpolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIMER_OC_IDLE_STATE_LOW : TIMER_OC_POLARITY_HIGH; + timer_channel_output_pulse_value_config((uint32_t)timer, timerHardware->channel, 0); + + timerOCInit(timer, timerHardware->channel, &timer_ocintpara); + timerOCModeConfig(timer, timerHardware->channel, TIMER_OC_MODE_PWM0); + timerOCPreloadConfig(timer, timerHardware->channel, TIMER_OC_SHADOW_ENABLE); + timer_primary_output_config((uint32_t)timer, ENABLE); + + xDMA_Cmd(dmaRef, DISABLE); + xDMA_DeInit(dmaRef); + + dma_single_data_para_struct_init(&dma_init_struct); + dma_init_struct.periph_addr = (uint32_t)timerCCR(timer, timerHardware->channel); + + uint32_t temp_dma_periph; + int temp_dma_channel; + gd32_dma_chbase_parse((uint32_t)dmaRef, &temp_dma_periph, &temp_dma_channel); + + dma_channel_subperipheral_select(temp_dma_periph, temp_dma_channel, dmaChannel); + + dma_init_struct.memory0_addr =(uint32_t)&(transponder->transponderIrDMABuffer); + dma_init_struct.direction = DMA_MEMORY_TO_PERIPH; + dma_init_struct.number = transponder->dma_buffer_size; + dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + +#if defined(GD32F4) + dma_init_struct.periph_memory_width = DMA_PERIPH_WIDTH_32BIT; +#endif + + dma_init_struct.circular_mode = DMA_CIRCULAR_MODE_DISABLE; + dma_init_struct.priority = DMA_PRIORITY_HIGH; + + gd32_dma_init((uint32_t)dmaRef, &dma_init_struct); + + timer_dma_enable((uint32_t)timer, timerDmaSource(timerHardware->channel)); + + xDMA_ITConfig(dmaRef, DMA_INT_FTF, ENABLE); +} + +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) +{ + xDMA_SetCurrDataCounter(dmaRef, transponder->dma_buffer_size); + timer_counter_value_config((uint32_t)timer, 0); + timer_enable((uint32_t)timer); + xDMA_Cmd(dmaRef, ENABLE); +} + +void transponderIrDisable(void) +{ + xDMA_Cmd(dmaRef, DISABLE); + timer_disable((uint32_t)timer); + + IOInit(transponderIO, OWNER_TRANSPONDER, 0); + + IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_PULLDOWN), alternateFunction); + +#ifdef TRANSPONDER_INVERTED + IOHi(transponderIO); +#else + IOLo(transponderIO); +#endif +} + +void transponderIrTransmit(void) +{ + transponderIrWaitForTransmitComplete(); + + transponderIrDataTransferInProgress = 1; + transponderIrDMAEnable(&transponder); +} +#endif diff --git a/src/platform/GD32/usb_f4/gd32f4xx_it.c b/src/platform/GD32/usb_f4/gd32f4xx_it.c new file mode 100755 index 0000000000..39365171f8 --- /dev/null +++ b/src/platform/GD32/usb_f4/gd32f4xx_it.c @@ -0,0 +1,233 @@ +/*! + \file gd32f4xx_it.c + \brief main interrupt service routines + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#include "gd32f4xx_it.h" +#include "drv_usbd_int.h" + +extern usb_core_driver USB_OTG_dev; + +//extern void usb_timer_irq(void); + +/* local function prototypes ('static') */ +static void resume_mcu_clk(void); + +/*! + \brief this function handles NMI exception + \param[in] none + \param[out] none + \retval none +*/ +void NMI_Handler(void) +{ + /* if NMI exception occurs, go to infinite loop */ + while(1) { + } +} + +/*! + \brief this function handles SVC exception + \param[in] none + \param[out] none + \retval none +*/ +void SVC_Handler(void) +{ + /* if SVC exception occurs, go to infinite loop */ + while(1) { + } +} + +/*! + \brief this function handles DebugMon exception + \param[in] none + \param[out] none + \retval none +*/ +void DebugMon_Handler(void) +{ + /* if DebugMon exception occurs, go to infinite loop */ + while(1) { + } +} + +/*! + \brief this function handles PendSV exception + \param[in] none + \param[out] none + \retval none +*/ +void PendSV_Handler(void) +{ + /* if PendSV exception occurs, go to infinite loop */ + while(1) { + } +} + +#ifdef USE_USB_FS + +/*! + \brief this function handles USBFS wakeup interrupt request + \param[in] none + \param[out] none + \retval none +*/ +void USBFS_WKUP_IRQHandler(void) +{ + if(USB_OTG_dev.bp.low_power) { + resume_mcu_clk(); + + rcu_pll48m_clock_config(RCU_PLL48MSRC_PLLQ); + rcu_ck48m_clock_config(RCU_CK48MSRC_PLL48M); + + rcu_periph_clock_enable(RCU_USBFS); + + usb_clock_active(&USB_OTG_dev); + } + + exti_interrupt_flag_clear(EXTI_18); +} + +#elif defined(USE_USB_HS) + +/*! + \brief this function handles USBHS wakeup interrupt request + \param[in] none + \param[out] none + \retval none +*/ +void USBHS_WKUP_IRQHandler(void) +{ + if(cdc_acm.bp.low_power) { + resume_mcu_clk(); + +#ifdef USE_EMBEDDED_PHY + rcu_pll48m_clock_config(RCU_PLL48MSRC_PLLQ); + rcu_ck48m_clock_config(RCU_CK48MSRC_PLL48M); +#elif defined(USE_ULPI_PHY) + rcu_periph_clock_enable(RCU_USBHSULPI); +#endif + + rcu_periph_clock_enable(RCU_USBHS); + + usb_clock_active(&USB_OTG_dev); + } + + exti_interrupt_flag_clear(EXTI_20); +} + +#endif /* USE_USBFS */ + +#ifdef USE_USB_FS + +/*! + \brief this function handles USBFS global interrupt request + \param[in] none + \param[out] none + \retval none +*/ +void USBFS_IRQHandler(void) +{ + usbd_isr(&USB_OTG_dev); +} + +#elif defined(USE_USB_HS) + +/*! + \brief this function handles USBHS global interrupt request + \param[in] none + \param[out] none + \retval none +*/ +void USBHS_IRQHandler(void) +{ + usbd_isr(&USB_OTG_dev); +} + +#endif /* USE_USBFS */ + +#ifdef USB_HS_DEDICATED_EP1_ENABLED + +/*! + \brief this function handles EP1_IN interrupt request + \param[in] none + \param[out] none + \retval none +*/ +void USBHS_EP1_In_IRQHandler(void) +{ + usbd_int_dedicated_ep1in(&USB_OTG_dev); +} + +/*! + \brief this function handles EP1_OUT interrupt request + \param[in] none + \param[out] none + \retval none +*/ +void USBHS_EP1_Out_IRQHandler(void) +{ + usbd_int_dedicated_ep1out(&USB_OTG_dev); +} + +#endif /* USB_HS_DEDICATED_EP1_ENABLED */ + +/*! + \brief resume mcu clock + \param[in] none + \param[out] none + \retval none +*/ +static void resume_mcu_clk(void) +{ + /* enable HXTAL */ + rcu_osci_on(RCU_HXTAL); + + /* wait till HXTAL is ready */ + while(RESET == rcu_flag_get(RCU_FLAG_HXTALSTB)) { + } + + /* enable PLL */ + rcu_osci_on(RCU_PLL_CK); + + /* wait till PLL is ready */ + while(RESET == rcu_flag_get(RCU_FLAG_PLLSTB)) { + } + + /* select PLL as system clock source */ + rcu_system_clock_source_config(RCU_CKSYSSRC_PLLP); + + /* wait till PLL is used as system clock source */ + while(RCU_SCSS_PLLP != rcu_system_clock_source_get()) { + } +} diff --git a/src/platform/GD32/usb_f4/gd32f4xx_it.h b/src/platform/GD32/usb_f4/gd32f4xx_it.h new file mode 100755 index 0000000000..c06c263509 --- /dev/null +++ b/src/platform/GD32/usb_f4/gd32f4xx_it.h @@ -0,0 +1,78 @@ +/*! + \file gd32f4xx_it.h + \brief the header file of the ISR + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#ifndef GD32F4XX_IT_H +#define GD32F4XX_IT_H + +#include "gd32f4xx.h" + +/* function declarations */ +/* this function handles NMI exception */ +void NMI_Handler(void); + +void HardFault_Handler(void); +void MemManage_Handler(void); +void BusFault_Handler(void); +void UsageFault_Handler(void); + +/* this function handles SVC exception */ +void SVC_Handler(void); +/* this function handles DebugMon exception */ +void DebugMon_Handler(void); +/* this function handles PendSV exception */ +void PendSV_Handler(void); + +void SysTick_Handler(void); + +#ifdef USE_USB_FS +/* this function handles USBFS wakeup interrupt request */ +void USBFS_WKUP_IRQHandler(void); +/* this function handles USBFS global interrupt request */ +void USBFS_IRQHandler(void); + +#endif /* USE_USB_FS */ +#ifdef USE_USB_HS +/* this function handles USBHS wakeup interrupt request */ +void USBHS_WKUP_IRQHandler(void); +/* this function handles USBHS global interrupt request */ +void USBHS_IRQHandler(void); +#endif /* USE_USB_HS */ +#ifdef USB_HS_DEDICATED_EP1_ENABLED +/* this function handles EP1_IN IRQ interrupt request */ +void USBHS_EP1_In_IRQHandler(void); +/* this function handles EP1_OUT IRQ interrupt request */ +void USBHS_EP1_Out_IRQHandler(void); +#endif /* USB_HS_DEDICATED_EP1_ENABLED */ + +#endif /* GD32F4XX_IT_H */ diff --git a/src/platform/GD32/usb_f4/sram_msd.h b/src/platform/GD32/usb_f4/sram_msd.h new file mode 100755 index 0000000000..1cb7707dd9 --- /dev/null +++ b/src/platform/GD32/usb_f4/sram_msd.h @@ -0,0 +1,55 @@ +/*! + \file sram_msd.h + \brief the header file of sram_msd.c + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#ifndef SRAM_MSD_H +#define SRAM_MSD_H + +#include "usbd_conf.h" + +#define ISRAM_BLOCK_SIZE 512U +#define ISRAM_BLOCK_NUM 80U + +/* function declarations */ +/* read data from multiple blocks of SRAM */ +uint32_t sram_read_multi_blocks(uint8_t* pbuf, \ + uint32_t read_addr, \ + uint16_t blk_size, \ + uint32_t blk_num); +/* write data to multiple blocks of SRAM */ +uint32_t sram_write_multi_blocks(uint8_t* pbuf, \ + uint32_t write_addr, \ + uint16_t blk_size, \ + uint32_t blk_num); + +#endif /* SRAM_MSD_H */ diff --git a/src/platform/GD32/usb_f4/usb_bsp.c b/src/platform/GD32/usb_f4/usb_bsp.c new file mode 100755 index 0000000000..925d963c82 --- /dev/null +++ b/src/platform/GD32/usb_f4/usb_bsp.c @@ -0,0 +1,311 @@ +/*! + \file gd32f4xx_hw.c + \brief USB hardware configuration for GD32F4xx + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#include "drv_usb_hw.h" + +#define TIM_MSEC_DELAY 0x01U +#define TIM_USEC_DELAY 0x02U + +__IO uint32_t delay_time = 0U; +__IO uint16_t timer_prescaler = 5U; + +/*! + \brief configure USB clock + \param[in] none + \param[out] none + \retval none +*/ +void usb_rcu_config(void) +{ +#ifdef USE_USB_FS + + /* configure the PLLSAIP = 48MHz, PLLSAI_N = 288, PLLSAI_P = 6, PLLSAI_R = 2 */ + rcu_pllsai_config(288U, 6U, 2U); + /* enable PLLSAI */ + RCU_CTL |= RCU_CTL_PLLSAIEN; + /* wait until PLLSAI is stable */ + while(0U == (RCU_CTL & RCU_CTL_PLLSAISTB)){ + } + + rcu_pll48m_clock_config(RCU_PLL48MSRC_PLLSAIP); + rcu_ck48m_clock_config(RCU_CK48MSRC_PLL48M); + + rcu_periph_clock_enable(RCU_USBFS); + +#elif defined(USE_USB_HS) + +#ifdef USE_EMBEDDED_PHY + rcu_pll48m_clock_config(RCU_PLL48MSRC_PLLQ); + rcu_ck48m_clock_config(RCU_CK48MSRC_PLL48M); +#elif defined(USE_ULPI_PHY) + rcu_periph_clock_enable(RCU_USBHSULPI); +#endif /* USE_EMBEDDED_PHY */ + + rcu_periph_clock_enable(RCU_USBHS); + +#endif /* USB_USBFS */ +} + +/*! + \brief configure USB data line GPIO + \param[in] none + \param[out] none + \retval none +*/ +void usb_gpio_config(void) +{ + rcu_periph_clock_enable(RCU_SYSCFG); + +#ifdef USE_USB_FS + rcu_periph_clock_enable(RCU_GPIOA); + + /* USBFS_DM(PA11) and USBFS_DP(PA12) GPIO pin configuration */ + gpio_mode_set(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_11 | GPIO_PIN_12); + gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_MAX, GPIO_PIN_11 | GPIO_PIN_12); + + gpio_af_set(GPIOA, GPIO_AF_10, GPIO_PIN_11 | GPIO_PIN_12); + +#elif defined(USE_USB_HS) + +#ifdef USE_ULPI_PHY + rcu_periph_clock_enable(RCU_GPIOA); + rcu_periph_clock_enable(RCU_GPIOB); + rcu_periph_clock_enable(RCU_GPIOC); + rcu_periph_clock_enable(RCU_GPIOH); + rcu_periph_clock_enable(RCU_GPIOI); + + /* ULPI_STP(PC0) GPIO pin configuration */ + gpio_mode_set(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_0); + gpio_output_options_set(GPIOC, GPIO_OTYPE_PP, GPIO_OSPEED_MAX, GPIO_PIN_0); + + /* ULPI_CK(PA5) GPIO pin configuration */ + gpio_mode_set(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_5); + gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_MAX, GPIO_PIN_5); + + /* ULPI_NXT(PH4) GPIO pin configuration */ + gpio_mode_set(GPIOH, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_4); + gpio_output_options_set(GPIOH, GPIO_OTYPE_PP, GPIO_OSPEED_MAX, GPIO_PIN_4); + + /* ULPI_DIR(PI11) GPIO pin configuration */ + gpio_mode_set(GPIOI, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_11); + gpio_output_options_set(GPIOI, GPIO_OTYPE_PP, GPIO_OSPEED_MAX, GPIO_PIN_11); + + /* ULPI_D1(PB0), ULPI_D2(PB1), ULPI_D3(PB10), ULPI_D4(PB11) \ + ULPI_D5(PB12), ULPI_D6(PB13) and ULPI_D7(PB5) GPIO pin configuration */ + gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, \ + GPIO_PIN_5 | GPIO_PIN_13 | GPIO_PIN_12 | \ + GPIO_PIN_11 | GPIO_PIN_10 | GPIO_PIN_1 | GPIO_PIN_0); + gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_MAX, \ + GPIO_PIN_5 | GPIO_PIN_13 | GPIO_PIN_12 | \ + GPIO_PIN_11 | GPIO_PIN_10 | GPIO_PIN_1 | GPIO_PIN_0); + + /* ULPI_D0(PA3) GPIO pin configuration */ + gpio_mode_set(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_3); + gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_MAX, GPIO_PIN_3); + + gpio_af_set(GPIOC, GPIO_AF_10, GPIO_PIN_0); + gpio_af_set(GPIOH, GPIO_AF_10, GPIO_PIN_4); + gpio_af_set(GPIOI, GPIO_AF_10, GPIO_PIN_11); + gpio_af_set(GPIOA, GPIO_AF_10, GPIO_PIN_5 | GPIO_PIN_3); + gpio_af_set(GPIOB, GPIO_AF_10, GPIO_PIN_5 | GPIO_PIN_13 | GPIO_PIN_12 | \ + GPIO_PIN_11 | GPIO_PIN_10 | GPIO_PIN_1 | GPIO_PIN_0); +#elif defined(USE_EMBEDDED_PHY) + rcu_periph_clock_enable(RCU_GPIOB); + + /* USBHS_DM(PB14) and USBHS_DP(PB15) GPIO pin configuration */ + gpio_mode_set(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_14 | GPIO_PIN_15); + gpio_output_options_set(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_MAX, GPIO_PIN_14 | GPIO_PIN_15); + gpio_af_set(GPIOB, GPIO_AF_12, GPIO_PIN_14 | GPIO_PIN_15); +#endif /* USE_ULPI_PHY */ + +#endif /* USE_USBFS */ +} + +/*! + \brief configure USB interrupt + \param[in] none + \param[out] none + \retval none +*/ +void usb_intr_config(void) +{ + nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2); + +#ifdef USE_USB_FS + nvic_irq_enable(USBFS_IRQn, 2U, 0U); + +#if USBFS_LOW_POWER + /* enable the power module clock */ + rcu_periph_clock_enable(RCU_PMU); + + /* USB wakeup EXTI line configuration */ + exti_interrupt_flag_clear(EXTI_18); + exti_init(EXTI_18, EXTI_INTERRUPT, EXTI_TRIG_RISING); + exti_interrupt_enable(EXTI_18); + + nvic_irq_enable(USBFS_WKUP_IRQn, 0U, 0U); +#endif /* USBFS_LOW_POWER */ +#elif defined(USE_USB_HS) + nvic_irq_enable(USBHS_IRQn, 2U, 0U); + +#if USBHS_LOW_POWER + /* enable the power module clock */ + rcu_periph_clock_enable(RCU_PMU); + + /* USB wakeup EXTI line configuration */ + exti_interrupt_flag_clear(EXTI_20); + exti_init(EXTI_20, EXTI_INTERRUPT, EXTI_TRIG_RISING); + exti_interrupt_enable(EXTI_20); + + nvic_irq_enable(USBHS_WKUP_IRQn, 0U, 0U); +#endif /* USBHS_LOW_POWER */ +#endif /* USE_USB_FS */ + +#ifdef USB_HS_DEDICATED_EP1_ENABLED + nvic_irq_enable(USBHS_EP1_Out_IRQn, 1U, 0U); + nvic_irq_enable(USBHS_EP1_In_IRQn, 1U, 0U); +#endif /* USB_HS_DEDICATED_EP1_ENABLED */ +} + +/*! + \brief delay in microseconds + \param[in] usec: value of delay required in microseconds + \param[out] none + \retval none +*/ +void usb_udelay(const uint32_t usec) +{ + uint32_t count = 0; + + const uint32_t utime = (120 * usec / 7); + do { + if ( ++count > utime ) + { + return ; + } + } while (1); +} + +/*! + \brief delay in milliseconds + \param[in] msec: value of delay required in milliseconds + \param[out] none + \retval none +*/ +void usb_mdelay(const uint32_t msec) +{ + usb_udelay(msec*1000); +} + +#if 0 +/*! + \brief timer base IRQ + \param[in] none + \param[out] none + \retval none +*/ +void usb_timer_irq(void) +{ + if(RESET != timer_interrupt_flag_get(TIMER2, TIMER_INT_FLAG_UP)) { + timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_UP); + + if(delay_time > 0x00U) { + delay_time--; + } else { + timer_disable(TIMER2); + } + } +} + +/*! + \brief delay routine based on TIMER2 + \param[in] nTime: delay Time + \param[in] unit: delay Time unit = milliseconds / microseconds + \param[out] none + \retval none +*/ +static void hw_delay(uint32_t ntime, uint8_t unit) +{ + delay_time = ntime; + + hw_time_set(unit); + + while(0U != delay_time) { + } + + timer_disable(TIMER2); +} + +/*! + \brief configures TIMER for delay routine based on Timer2 + \param[in] unit: msec /usec + \param[out] none + \retval none +*/ +static void hw_time_set(uint8_t unit) +{ + timer_parameter_struct timer_basestructure; + + timer_prescaler = ((rcu_clock_freq_get(CK_APB1) / 1000000U * 2U) / 12U) - 1U; + + timer_disable(TIMER2); + timer_interrupt_disable(TIMER2, TIMER_INT_UP); + + if(TIM_USEC_DELAY == unit) { + timer_basestructure.period = 11U; + } else if(TIM_MSEC_DELAY == unit) { + timer_basestructure.period = 11999U; + } else { + /* no operation */ + } + + timer_basestructure.prescaler = timer_prescaler; + timer_basestructure.alignedmode = TIMER_COUNTER_EDGE; + timer_basestructure.counterdirection = TIMER_COUNTER_UP; + timer_basestructure.clockdivision = TIMER_CKDIV_DIV1; + timer_basestructure.repetitioncounter = 0U; + + timer_init(TIMER2, &timer_basestructure); + + timer_interrupt_flag_clear(TIMER2, TIMER_INT_FLAG_UP); + + timer_auto_reload_shadow_enable(TIMER2); + + /* TIMER2 interrupt enable */ + timer_interrupt_enable(TIMER2, TIMER_INT_UP); + + /* TIMER2 enable counter */ + timer_enable(TIMER2); +} +#endif diff --git a/src/platform/GD32/usb_f4/usb_cdc_hid.c b/src/platform/GD32/usb_f4/usb_cdc_hid.c new file mode 100755 index 0000000000..5bd9552bf3 --- /dev/null +++ b/src/platform/GD32/usb_f4/usb_cdc_hid.c @@ -0,0 +1,538 @@ +/*! + \file cdc_hid_wrapper.c + \brief this file calls to the separate CDC and HID class layer handlers + + \version 2018-06-01, V1.0.0, application for GD32 USBD +*/ + +/* + Copyright (c) 2018, GigaDevice Semiconductor Inc. + + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#include "usb_cdc_hid.h" +#include "standard_hid_core.h" +#include +#include "usbd_cdc_vcp.h" +#define USBD_VID 0x28E9U +#define USBD_PID 0x028BU + +/* local function prototypes ('static') */ +static uint8_t cdc_hid_init (usb_dev *udev, uint8_t config_index); +static uint8_t cdc_hid_deinit (usb_dev *udev, uint8_t config_index); +static uint8_t cdc_hid_req_handler (usb_dev *udev, usb_req *req); +static uint8_t cdc_hid_ctlx_out (usb_dev *udev); +static uint8_t cdc_hid_data_in (usb_dev *udev, uint8_t ep_num); +static uint8_t cdc_hid_data_out (usb_dev *udev, uint8_t ep_num); + +usb_class_core bf_usbd_cdc_hid_cb = { + .init = cdc_hid_init, + .deinit = cdc_hid_deinit, + .req_proc = cdc_hid_req_handler, + .ctlx_out = cdc_hid_ctlx_out, + .data_in = cdc_hid_data_in, + .data_out = cdc_hid_data_out, +}; + +/* note:it should use the C99 standard when compiling the below codes */ +/* USB standard device descriptor */ +const usb_desc_dev cdc_hid_dev_desc = +{ + .header = + { + .bLength = USB_DEV_DESC_LEN, + .bDescriptorType = USB_DESCTYPE_DEV, + }, + .bcdUSB = 0x0200U, + .bDeviceClass = 0xEFU, + .bDeviceSubClass = 0x02U, + .bDeviceProtocol = 0x01U, + .bMaxPacketSize0 = USB_FS_EP0_MAX_LEN, + .idVendor = USBD_VID, + .idProduct = USBD_PID, + .bcdDevice = 0x0100U, + .iManufacturer = STR_IDX_MFC, + .iProduct = STR_IDX_PRODUCT, + .iSerialNumber = STR_IDX_SERIAL, + .bNumberConfigurations = USBD_CFG_MAX_NUM, +}; + +/* USB device configuration descriptor */ +const usb_cdc_hid_desc_config_set cdc_hid_config_desc = +{ + .config = + { + .header = + { + .bLength = sizeof(usb_desc_config), + .bDescriptorType = USB_DESCTYPE_CONFIG, + }, + .wTotalLength = HID_CDC_CONFIG_DESC_SIZE, + .bNumInterfaces = 0x03U, + .bConfigurationValue = 0x01U, + .iConfiguration = 0x00U, + .bmAttributes = 0xE0U, + .bMaxPower = 0x32U + }, + + .hid_interface = + { + .header = + { + .bLength = sizeof(usb_desc_itf), + .bDescriptorType = USB_DESCTYPE_ITF + }, + .bInterfaceNumber = USBD_HID_INTERFACE, + .bAlternateSetting = 0x00U, + .bNumEndpoints = 0x01U, + .bInterfaceClass = 0x03U, + .bInterfaceSubClass = 0x00U, + .bInterfaceProtocol = 0x00U, + .iInterface = 0x00U + }, + + .hid_vendor_hid = + { + .header = + { + .bLength = sizeof(usb_desc_hid), + .bDescriptorType = USB_DESCTYPE_HID + }, + .bcdHID = 0x0111U, + .bCountryCode = 0x00U, + .bNumDescriptors = 0x01U, + .bDescriptorType = USB_DESCTYPE_REPORT, + .wDescriptorLength = HID_MOUSE_REPORT_DESC_SIZE, + }, + + .hid_report_in_endpoint = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = HID_IN_EP, + .bmAttributes = 0x03U, + .wMaxPacketSize = HID_IN_PACKET, + .bInterval = 0x20U + }, + + .iad = + { + .header = + { + .bLength = sizeof(usb_desc_IAD), + .bDescriptorType = 0x0BU + }, + .bFirstInterface = 0x01U, + .bInterfaceCount = 0x02U, + .bFunctionClass = 0x02U, + .bFunctionSubClass = 0x02U, + .bFunctionProtocol = 0x01U, + .iFunction = 0x00U + }, + + .cmd_itf = + { + .header = + { + .bLength = sizeof(usb_desc_itf), + .bDescriptorType = USB_DESCTYPE_ITF + }, + .bInterfaceNumber = 0x01U, + .bAlternateSetting = 0x00U, + .bNumEndpoints = 0x01U, + .bInterfaceClass = USB_CLASS_CDC, + .bInterfaceSubClass = 0x02U, + .bInterfaceProtocol = 0x01U, + .iInterface = 0x00U + }, + + .cdc_header = + { + .header = + { + .bLength = sizeof(usb_desc_header_func), + .bDescriptorType = USB_DESCTYPE_CS_INTERFACE + }, + .bDescriptorSubtype = 0x00U, + .bcdCDC = 0x0110U + }, + + .cdc_call_managment = + { + .header = + { + .bLength = sizeof(usb_desc_call_managment_func), + .bDescriptorType = USB_DESCTYPE_CS_INTERFACE + }, + .bDescriptorSubtype = 0x01U, + .bmCapabilities = 0x00U, + .bDataInterface = 0x02U + }, + + .cdc_acm = + { + .header = + { + .bLength = sizeof(usb_desc_acm_func), + .bDescriptorType = USB_DESCTYPE_CS_INTERFACE + }, + .bDescriptorSubtype = 0x02U, + .bmCapabilities = 0x02U, + }, + + .cdc_union = + { + .header = + { + .bLength = sizeof(usb_desc_union_func), + .bDescriptorType = USB_DESCTYPE_CS_INTERFACE + }, + .bDescriptorSubtype = 0x06U, + .bMasterInterface = 0x01U, + .bSlaveInterface0 = 0x02U, + }, + + .cdc_cmd_endpoint = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP, + }, + .bEndpointAddress = CDC_CMD_EP, + .bmAttributes = 0x03U, + .wMaxPacketSize = USB_CDC_CMD_PACKET_SIZE, + .bInterval = 0x0AU + }, + + .cdc_data_interface = + { + .header = + { + .bLength = sizeof(usb_desc_itf), + .bDescriptorType = USB_DESCTYPE_ITF, + }, + .bInterfaceNumber = 0x02U, + .bAlternateSetting = 0x00U, + .bNumEndpoints = 0x02U, + .bInterfaceClass = 0x0AU, + .bInterfaceSubClass = 0x00U, + .bInterfaceProtocol = 0x00U, + .iInterface = 0x00U + }, + + .cdc_out_endpoint = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP, + }, + .bEndpointAddress = CDC_DATA_OUT_EP, + .bmAttributes = USB_EP_ATTR_BULK, + .wMaxPacketSize = USB_CDC_DATA_PACKET_SIZE, + .bInterval = 0x00U + }, + + .cdc_in_endpoint = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = CDC_DATA_IN_EP, + .bmAttributes = USB_EP_ATTR_BULK, + .wMaxPacketSize = USB_CDC_DATA_PACKET_SIZE, + .bInterval = 0x00U + } +}; + +/* USB language ID Descriptor */ +static const usb_desc_LANGID usbd_language_id_desc = +{ + .header = + { + .bLength = sizeof(usb_desc_LANGID), + .bDescriptorType = USB_DESCTYPE_STR, + }, + .wLANGID = ENG_LANGID +}; + +/* USB manufacture string */ +static const usb_desc_str manufacturer_string = +{ + .header = + { + .bLength = USB_STRING_LEN(10), + .bDescriptorType = USB_DESCTYPE_STR, + }, + .unicode_string = {'G', 'i', 'g', 'a', 'D', 'e', 'v', 'i', 'c', 'e'} +}; + +/* USB product string */ +static const usb_desc_str product_string = +{ + .header = + { + .bLength = USB_STRING_LEN(12), + .bDescriptorType = USB_DESCTYPE_STR, + }, + .unicode_string = {'G', 'D', '3', '2', '-', 'C', 'D', 'C', '_', 'H', 'I', 'D'} +}; + +/* USBD serial string */ +static usb_desc_str serial_string = +{ + .header = + { + .bLength = USB_STRING_LEN(12), + .bDescriptorType = USB_DESCTYPE_STR, + } +}; + +/* USB string descriptor set */ +void *const usbd_cdc_hid_strings[] = +{ + [STR_IDX_LANGID] = (uint8_t *)&usbd_language_id_desc, + [STR_IDX_MFC] = (uint8_t *)&manufacturer_string, + [STR_IDX_PRODUCT] = (uint8_t *)&product_string, + [STR_IDX_SERIAL] = (uint8_t *)&serial_string +}; + +usb_desc bf_cdc_hid_desc = { + .dev_desc = (uint8_t *)&cdc_hid_dev_desc, + .config_desc = (uint8_t *)&cdc_hid_config_desc, + .strings = usbd_cdc_hid_strings +}; + +__ALIGN_BEGIN static uint8_t HID_MOUSE_ReportDesc[HID_MOUSE_REPORT_DESC_SIZE] __ALIGN_END = +{ + 0x05, 0x01, + 0x09, 0x02, + 0xA1, 0x01, + 0x09, 0x01, + + 0xA1, 0x00, + 0x05, 0x09, + 0x19, 0x01, + 0x29, 0x03, + + 0x15, 0x00, + 0x25, 0x01, + 0x95, 0x03, + 0x75, 0x01, + + 0x81, 0x02, + 0x95, 0x01, + 0x75, 0x05, + 0x81, 0x01, + + 0x05, 0x01, + 0x09, 0x30, + 0x09, 0x31, + 0x09, 0x38, + + 0x15, 0x81, + 0x25, 0x7F, + 0x75, 0x08, + 0x95, 0x03, + + 0x81, 0x06, + 0xC0, 0x09, + 0x3c, 0x05, + 0xff, 0x09, + + 0x01, 0x15, + 0x00, 0x25, + 0x01, 0x75, + 0x01, 0x95, + + 0x02, 0xb1, + 0x22, 0x75, + 0x06, 0x95, + 0x01, 0xb1, + + 0x01, 0xc0 +}; + +/*! + \brief initialize the HID/CDC device + \param[in] udev: pointer to USB device instance + \param[in] config_index: configuration index + \param[out] none + \retval USB device operation status +*/ +static uint8_t cdc_hid_init (usb_dev *udev, uint8_t config_index) +{ + (void)(config_index); + + /* HID initialization */ + usbd_hid_cb.init(udev, config_index); + + /* CDC initialization */ + bf_cdc_class.init(udev, config_index); + + return USBD_OK; +} + +/*! + \brief de-initialize the HID/CDC device + \param[in] udev: pointer to USB device instance + \param[in] config_index: configuration index + \param[out] none + \retval USB device operation status +*/ +static uint8_t cdc_hid_deinit (usb_dev *udev, uint8_t config_index) +{ + (void)(config_index); + + /* HID De-initialization */ + usbd_hid_cb.deinit(udev, config_index); + + /* CDC De-initialization */ + bf_cdc_class.deinit(udev, config_index); + + return USBD_OK; +} + +/*! + \brief handle the custom HID/CDC class-specific request + \param[in] pudev: pointer to USB device instance + \param[in] req: device class request + \param[out] none + \retval USB device operation status +*/ +static uint8_t cdc_hid_req_handler (usb_dev *udev, usb_req *req) +{ + if (req->wIndex == USBD_HID_INTERFACE) { + usb_transc *transc = NULL; + + standard_hid_handler *hid = (standard_hid_handler *)udev->dev.class_data[USBD_HID_INTERFACE]; + + switch (req->bRequest) { + case GET_REPORT: + break; + + case GET_IDLE: + transc = &udev->dev.transc_in[0]; + transc->xfer_buf = (uint8_t *)&hid->idle_state; + transc->remain_len = 1U; + break; + + case GET_PROTOCOL: + transc = &udev->dev.transc_in[0]; + transc->xfer_buf = (uint8_t *)&hid->protocol; + transc->remain_len = 1U; + break; + + case SET_REPORT: + break; + + case SET_IDLE: + hid->idle_state = (uint8_t)(req->wValue >> 8U); + break; + + case SET_PROTOCOL: + hid->protocol = (uint8_t)(req->wValue); + break; + + case USB_GET_DESCRIPTOR: + if (USB_DESCTYPE_REPORT == (req->wValue >> 8U)) { + transc = &udev->dev.transc_in[0]; + transc->remain_len = USB_MIN(HID_MOUSE_REPORT_DESC_SIZE, req->wLength); + transc->xfer_buf = (uint8_t *)HID_MOUSE_ReportDesc; + } else { + return USBD_FAIL; + } + break; + + default: + return USBD_FAIL; + } + + + return USBD_OK; + } else { + return bf_cdc_class.req_proc(udev, req); + } +} + +static uint8_t cdc_hid_ctlx_out (usb_dev *udev) +{ + usb_req req = udev->dev.control.req; + + if (0x01 == req.wIndex) { + return bf_cdc_class.ctlx_out(udev); + } + + return 0; +} + +/*! + \brief handle data stage + \param[in] pudev: pointer to USB device instance + \param[in] rx_tx: data transfer direction: + \arg USBD_TX + \arg USBD_RX + \param[in] ep_num: endpoint identifier + \param[out] none + \retval USB device operation status +*/ +static uint8_t cdc_hid_data_in (usb_dev *udev, uint8_t ep_num) +{ + if((HID_IN_EP & 0x7FU) == ep_num) { + return usbd_hid_cb.data_in(udev, ep_num); + } else { + return bf_cdc_class.data_in(udev, ep_num); + } +} + +/*! + \brief handle data stage + \param[in] pudev: pointer to USB device instance + \param[in] rx_tx: data transfer direction: + \arg USBD_TX + \arg USBD_RX + \param[in] ep_num: endpoint identifier + \param[out] none + \retval USB device operation status +*/ +static uint8_t cdc_hid_data_out (usb_dev *udev, uint8_t ep_num) +{ + + return bf_cdc_class.data_out(udev, ep_num); + +} + +void sendReport(uint8_t *report, uint8_t len) +{ + hid_report_send(&USB_OTG_dev, report, len); +} diff --git a/src/platform/GD32/usb_f4/usb_cdc_hid.h b/src/platform/GD32/usb_f4/usb_cdc_hid.h new file mode 100755 index 0000000000..dd498c3b46 --- /dev/null +++ b/src/platform/GD32/usb_f4/usb_cdc_hid.h @@ -0,0 +1,83 @@ +/*! + \file cdc_hid_wrapper.h + \brief the header file of cdc hid wrapper driver + + \version 2018-06-01, V1.0.0, application for GD32 USBD +*/ + +/* + Copyright (c) 2018, GigaDevice Semiconductor Inc. + + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#ifndef __USB_HID_CORE_H_ +#define __USB_HID_CORE_H_ + +#include "usbd_desc.h" +#include "standard_hid_core.h" +#include "usbd_enum.h" + +#define HID_CDC_CONFIG_DESC_SIZE 0x64U +#define HID_MOUSE_REPORT_DESC_SIZE 74 + +#pragma pack(1) + +typedef struct +{ + usb_desc_header header; /*!< regular descriptor header containing the descriptor's type and length */ + uint8_t bFirstInterface; /*!< bFirstInterface */ + uint8_t bInterfaceCount; /*!< bInterfaceCount */ + uint8_t bFunctionClass; /*!< bFunctionClass */ + uint8_t bFunctionSubClass; /*!< bFunctionSubClass */ + uint8_t bFunctionProtocol; /*!< bFunctionProtocol */ + uint8_t iFunction; /*!< iFunction */ +} usb_desc_IAD; + +#pragma pack() + +typedef struct +{ + usb_desc_config config; + usb_desc_itf hid_interface; + usb_desc_hid hid_vendor_hid; + usb_desc_ep hid_report_in_endpoint; + usb_desc_IAD iad; + usb_desc_itf cmd_itf; + usb_desc_header_func cdc_header; + usb_desc_call_managment_func cdc_call_managment; + usb_desc_acm_func cdc_acm; + usb_desc_union_func cdc_union; + usb_desc_ep cdc_cmd_endpoint; + usb_desc_itf cdc_data_interface; + usb_desc_ep cdc_out_endpoint; + usb_desc_ep cdc_in_endpoint; +} usb_cdc_hid_desc_config_set; + +extern usb_desc bf_cdc_hid_desc; +extern usb_class_core bf_usbd_cdc_hid_cb; + +#endif /* __CDC_HID_WRAPPER_H */ diff --git a/src/platform/GD32/usb_f4/usb_conf.h b/src/platform/GD32/usb_f4/usb_conf.h new file mode 100755 index 0000000000..707d30a14b --- /dev/null +++ b/src/platform/GD32/usb_f4/usb_conf.h @@ -0,0 +1,141 @@ +/*! + \file usb_conf.h + \brief USB core driver basic configuration + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#ifndef USB_CONF_H +#define USB_CONF_H + +#include "gd32f4xx.h" + +/* USB FS/HS PHY CONFIGURATION */ +#ifdef USE_USB_FS + #define USB_FS_CORE +#endif /* USE_USB_FS */ + +#ifdef USE_USB_HS + #define USB_HS_CORE +#endif /* USE_USB_HS */ + +/* USB FIFO size config */ +#ifdef USB_FS_CORE + #define RX_FIFO_FS_SIZE 128U + #define TX0_FIFO_FS_SIZE 64U + #define TX1_FIFO_FS_SIZE 64U + #define TX2_FIFO_FS_SIZE 64U + #define TX3_FIFO_FS_SIZE 0U + + #define USBFS_SOF_OUTPUT 0U + #define USBFS_LOW_POWER 0U +#endif /* USB_FS_CORE */ + +#ifdef USB_HS_CORE + #define RX_FIFO_HS_SIZE 512U + #define TX0_FIFO_HS_SIZE 128U + #define TX1_FIFO_HS_SIZE 128U + #define TX2_FIFO_HS_SIZE 128U + #define TX3_FIFO_HS_SIZE 0U + #define TX4_FIFO_HS_SIZE 0U + #define TX5_FIFO_HS_SIZE 0U + + #ifdef USE_ULPI_PHY + #define USB_ULPI_PHY_ENABLED + #endif + + #ifdef USE_EMBEDDED_PHY + #define USB_EMBEDDED_PHY_ENABLED + #endif + +// #define USB_HS_INTERNAL_DMA_ENABLED +// #define USB_HS_DEDICATED_EP1_ENABLED + + #define USBHS_SOF_OUTPUT 0U + #define USBHS_LOW_POWER 0U +#endif /* USB_HS_CORE */ + +/* if uncomment it, need jump to USB JP */ +//#define VBUS_SENSING_ENABLED + +//#define USE_HOST_MODE +#define USE_DEVICE_MODE +//#define USE_OTG_MODE + +#ifndef USB_FS_CORE + #ifndef USB_HS_CORE + #error "USB_HS_CORE or USB_FS_CORE should be defined!" + #endif +#endif + +#ifndef USE_DEVICE_MODE + #ifndef USE_HOST_MODE + #error "USE_DEVICE_MODE or USE_HOST_MODE should be defined!" + #endif +#endif + +#ifndef USE_USB_HS + #ifndef USE_USB_FS + #error "USE_USB_HS or USE_USB_FS should be defined!" + #endif +#endif + +/* all variables and data structures during the transaction process should be 4-bytes aligned */ + +#ifdef USB_HS_INTERNAL_DMA_ENABLED + #if defined (__GNUC__) /* GNU Compiler */ + #define __ALIGN_END __attribute__ ((aligned (4U))) + #define __ALIGN_BEGIN + #else + #define __ALIGN_END + + #if defined (__CC_ARM) /* ARM Compiler */ + #define __ALIGN_BEGIN __align(4U) + #elif defined (__ICCARM__) /* IAR Compiler */ + #define __ALIGN_BEGIN + #elif defined (__TASKING__)/* TASKING Compiler */ + #define __ALIGN_BEGIN __align(4U) + #endif /* __CC_ARM */ + #endif /* __GNUC__ */ +#else + #define __ALIGN_BEGIN + #define __ALIGN_END +#endif /* USB_HS_INTERNAL_DMA_ENABLED */ + +/* __packed keyword used to decrease the data type alignment to 1-byte */ +#if defined (__GNUC__) /* GNU Compiler */ + #ifndef __packed + #define __packed __unaligned + #endif +#elif defined (__TASKING__) /* TASKING Compiler */ + #define __packed __unaligned +#endif /* __GNUC__ */ + +#endif /* USB_CONF_H */ diff --git a/src/platform/GD32/usb_f4/usbd_conf.h b/src/platform/GD32/usb_f4/usbd_conf.h new file mode 100755 index 0000000000..7415f73f8d --- /dev/null +++ b/src/platform/GD32/usb_f4/usbd_conf.h @@ -0,0 +1,103 @@ +/*! + \file usbd_conf.h + \brief the header file of USB device configuration + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#ifndef USBD_CONF_H +#define USBD_CONF_H + +#include "usb_conf.h" + +/* USB configure exported defines */ +#define USBD_CFG_MAX_NUM 1U +#define USBD_ITF_MAX_NUM 1U +#define USB_STR_DESC_MAX_SIZE 255U +#define USB_STRING_COUNT 4U + +/* CDC */ +#define CDC_DATA_IN_EP EP1_IN /* EP1 for data IN */ +#define CDC_DATA_OUT_EP EP1_OUT /* EP3 for data OUT */ +#define CDC_CMD_EP EP2_IN /* EP2 for CDC commands */ + +#define CDC_COM_INTERFACE 0U + +#ifdef USE_USB_HS + #ifdef USE_ULPI_PHY + #define USB_CDC_DATA_PACKET_SIZE 512U + #define USB_CDC_CMD_PACKET_SIZE 8U + #define CDC_IN_FRAME_INTERVAL 40U + #define APP_TX_DATA_SIZE 2048U + #define APP_RX_DATA_SIZE 2048U + #else + #define USB_CDC_DATA_PACKET_SIZE 512U + #define USB_CDC_CMD_PACKET_SIZE 8U + #define CDC_IN_FRAME_INTERVAL 5U + #define APP_TX_DATA_SIZE 2048U + #define APP_RX_DATA_SIZE 2048U + #endif +#else /*USE_USB_FS*/ + #define USB_CDC_DATA_PACKET_SIZE 64U + #define USB_CDC_CMD_PACKET_SIZE 8U + #define CDC_IN_FRAME_INTERVAL 15U + #define APP_TX_DATA_SIZE 2048U + #define APP_RX_DATA_SIZE 2048U +#endif +/* END CDC */ + +/* HID */ +#define HID_IN_EP EP3_IN +#define HID_IN_PACKET 9U +#define USBD_HID_INTERFACE 0U +/* END HID */ + +/* MSC */ +#define MSC_IN_EP EP1_IN +#define MSC_OUT_EP EP1_OUT +#define USBD_MSC_INTERFACE 0U + +#define MSC_MEDIA_PACKET_SIZE 4096U +#define MEM_LUN_NUM 1U + +#ifdef USE_USB_HS + #ifdef USE_ULPI_PHY + #define MSC_DATA_PACKET_SIZE 512U + #else + #define MSC_DATA_PACKET_SIZE 64U + #endif +#else /*USE_USB_FS*/ + #define MSC_DATA_PACKET_SIZE 64U +#endif +/* END MSC */ + +#define APP_FOPS VCP_fops + +#endif /* USBD_CONF_H */ diff --git a/src/platform/GD32/usb_f4/usbd_desc.c b/src/platform/GD32/usb_f4/usbd_desc.c new file mode 100755 index 0000000000..772153a5e7 --- /dev/null +++ b/src/platform/GD32/usb_f4/usbd_desc.c @@ -0,0 +1,640 @@ +/*! + \file cdc_acm_core.c + \brief CDC ACM driver + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#include "usbd_desc.h" + +#include "platform.h" +#include "build/version.h" + +#include "pg/pg.h" +#include "pg/usb.h" + +#define USBD_VID 0x28E9U +#define USBD_PID 0x018AU + +volatile uint32_t APP_Rx_ptr_in = 0; +volatile uint32_t APP_Rx_ptr_out = 0; +uint32_t APP_Rx_length = 0; + +uint8_t USB_Tx_State = USB_CDC_IDLE; +__ALIGN_BEGIN uint8_t USB_Rx_Buffer [USB_CDC_DATA_PACKET_SIZE] __ALIGN_END ; +__ALIGN_BEGIN uint8_t APP_Rx_Buffer [APP_RX_DATA_SIZE] __ALIGN_END ; +__ALIGN_BEGIN uint8_t CmdBuff[USB_CDC_DATA_PACKET_SIZE] __ALIGN_END ; +extern CDC_IF_Prop_TypeDef APP_FOPS; +extern LINE_CODING g_lc; +static void Handle_USBAsynchXfer (usb_dev *udev); +extern void (*ctrlLineStateCb)(void* context, uint16_t ctrlLineState); +extern void *ctrlLineStateCbContext; +extern void (*baudRateCb)(void *context, uint32_t baud); +extern void *baudRateCbContext; + +/* note:it should use the C99 standard when compiling the below codes */ +/* USB standard device descriptor */ +__ALIGN_BEGIN const usb_desc_dev bf_cdc_dev_desc __ALIGN_END = { + .header = + { + .bLength = USB_DEV_DESC_LEN, + .bDescriptorType = USB_DESCTYPE_DEV + }, + .bcdUSB = 0x0200U, + .bDeviceClass = USB_CLASS_CDC, + .bDeviceSubClass = 0x02U, + .bDeviceProtocol = 0x00U, + .bMaxPacketSize0 = USB_FS_EP0_MAX_LEN, + .idVendor = USBD_VID, + .idProduct = USBD_PID, + .bcdDevice = 0x0200U, + .iManufacturer = STR_IDX_MFC, + .iProduct = STR_IDX_PRODUCT, + .iSerialNumber = STR_IDX_SERIAL, + .bNumberConfigurations = USBD_CFG_MAX_NUM +}; + +/* USB device configuration descriptor */ +__ALIGN_BEGIN const usb_cdc_desc_config_set bf_cdc_config_desc __ALIGN_END = { + .config = + { + .header = + { + .bLength = sizeof(usb_desc_config), + .bDescriptorType = USB_DESCTYPE_CONFIG + }, + .wTotalLength = USB_CDC_ACM_CONFIG_DESC_SIZE, + .bNumInterfaces = 0x02U, + .bConfigurationValue = 0x01U, + .iConfiguration = 0x00U, + .bmAttributes = 0xC0U, + .bMaxPower = 0x32U + }, + + .cmd_itf = + { + .header = + { + .bLength = sizeof(usb_desc_itf), + .bDescriptorType = USB_DESCTYPE_ITF + }, + .bInterfaceNumber = 0x00U, + .bAlternateSetting = 0x00U, + .bNumEndpoints = 0x01U, + .bInterfaceClass = USB_CLASS_CDC, + .bInterfaceSubClass = USB_CDC_SUBCLASS_ACM, + .bInterfaceProtocol = USB_CDC_PROTOCOL_AT, + .iInterface = 0x00U + }, + + .cdc_header = + { + .header = + { + .bLength = sizeof(usb_desc_header_func), + .bDescriptorType = USB_DESCTYPE_CS_INTERFACE + }, + .bDescriptorSubtype = 0x00U, + .bcdCDC = 0x0110U + }, + + .cdc_call_managment = + { + .header = + { + .bLength = sizeof(usb_desc_call_managment_func), + .bDescriptorType = USB_DESCTYPE_CS_INTERFACE + }, + .bDescriptorSubtype = 0x01U, + .bmCapabilities = 0x00U, + .bDataInterface = 0x01U + }, + + .cdc_acm = + { + .header = + { + .bLength = sizeof(usb_desc_acm_func), + .bDescriptorType = USB_DESCTYPE_CS_INTERFACE + }, + .bDescriptorSubtype = 0x02U, + .bmCapabilities = 0x02U + }, + + .cdc_union = + { + .header = + { + .bLength = sizeof(usb_desc_union_func), + .bDescriptorType = USB_DESCTYPE_CS_INTERFACE + }, + .bDescriptorSubtype = 0x06U, + .bMasterInterface = 0x00U, + .bSlaveInterface0 = 0x01U + }, + + .cdc_cmd_endpoint = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = CDC_CMD_EP, + .bmAttributes = USB_EP_ATTR_INT, + .wMaxPacketSize = USB_CDC_CMD_PACKET_SIZE, + .bInterval = 0xFFU + }, + + .cdc_data_interface = + { + .header = + { + .bLength = sizeof(usb_desc_itf), + .bDescriptorType = USB_DESCTYPE_ITF + }, + .bInterfaceNumber = 0x01U, + .bAlternateSetting = 0x00U, + .bNumEndpoints = 0x02U, + .bInterfaceClass = USB_CLASS_DATA, + .bInterfaceSubClass = 0x00U, + .bInterfaceProtocol = USB_CDC_PROTOCOL_NONE, + .iInterface = 0x00U + }, + + .cdc_out_endpoint = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = CDC_DATA_OUT_EP, + .bmAttributes = USB_EP_ATTR_BULK, + .wMaxPacketSize = USB_CDC_DATA_PACKET_SIZE, + .bInterval = 0x00U + }, + + .cdc_in_endpoint = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = CDC_DATA_IN_EP, + .bmAttributes = USB_EP_ATTR_BULK, + .wMaxPacketSize = USB_CDC_DATA_PACKET_SIZE, + .bInterval = 0x00U + } +}; + +/* USB language ID Descriptor */ +static __ALIGN_BEGIN const usb_desc_LANGID usbd_language_id_desc __ALIGN_END = { + .header = + { + .bLength = sizeof(usb_desc_LANGID), + .bDescriptorType = USB_DESCTYPE_STR + }, + .wLANGID = ENG_LANGID +}; + +/* USB manufacture string */ +static __ALIGN_BEGIN usb_desc_str manufacturer_string __ALIGN_END = { + .header = + { + .bLength = USB_STRING_LEN(10U), + .bDescriptorType = USB_DESCTYPE_STR + }, + .unicode_string = {'G', 'i', 'g', 'a', 'D', 'e', 'v', 'i', 'c', 'e'} +}; + +/* USB product string */ +static __ALIGN_BEGIN usb_desc_str product_string __ALIGN_END = { + .header = + { + .bLength = USB_STRING_LEN(12U), + .bDescriptorType = USB_DESCTYPE_STR + }, + .unicode_string = {'G', 'D', '3', '2', '-', 'C', 'D', 'C', '_', 'A', 'C', 'M'} +}; + +/* USBD serial string */ +static __ALIGN_BEGIN usb_desc_str serial_string __ALIGN_END = { + .header = + { + .bLength = USB_STRING_LEN(12U), + .bDescriptorType = USB_DESCTYPE_STR + } +}; + +/* USB string descriptor set */ +void *const usbd_bf_cdc_strings[] = { + [STR_IDX_LANGID] = (uint8_t *)&usbd_language_id_desc, + [STR_IDX_MFC] = (uint8_t *)&manufacturer_string, + [STR_IDX_PRODUCT] = (uint8_t *)&product_string, + [STR_IDX_SERIAL] = (uint8_t *)&serial_string +}; + +usb_desc bf_cdc_desc = { + .dev_desc = (uint8_t *)&bf_cdc_dev_desc, + .config_desc = (uint8_t *)&bf_cdc_config_desc, + .strings = usbd_bf_cdc_strings +}; + +/* local function prototypes ('static') */ +static uint8_t bf_cdc_acm_init(usb_dev *udev, uint8_t config_index); +static uint8_t bf_cdc_acm_deinit(usb_dev *udev, uint8_t config_index); +static uint8_t bf_cdc_acm_req(usb_dev *udev, usb_req *req); +static uint8_t bf_cdc_acm_ctlx_out(usb_dev *udev); +static uint8_t bf_cdc_acm_in(usb_dev *udev, uint8_t ep_num); +static uint8_t bf_cdc_acm_out(usb_dev *udev, uint8_t ep_num); +static uint8_t bf_cdc_acm_sof(usb_dev *udev); + +/* USB CDC device class callbacks structure */ +usb_class_core bf_cdc_class = { + .command = NO_CMD, + .alter_set = 0U, + + .init = bf_cdc_acm_init, + .deinit = bf_cdc_acm_deinit, + .req_proc = bf_cdc_acm_req, + .ctlx_out = bf_cdc_acm_ctlx_out, + .data_in = bf_cdc_acm_in, + .data_out = bf_cdc_acm_out, + .SOF = bf_cdc_acm_sof +}; + +/*! + \brief usbd string get + \param[in] src: pointer to source string + \param[in] unicode : formatted string buffer (unicode) + \param[in] len : descriptor length + \param[out] none + \retval USB device operation status +*/ +static void usbd_string_buf_get(const char *src, uint8_t *unicode) +{ + uint8_t idx = 0; + uint8_t len = 0; + + if (src != NULL) + { + len = USB_STRING_LEN(strlen(src)); + unicode[idx++] = len; + unicode[idx++] = USB_DESCTYPE_STR; + + while (*src != '\0') + { + unicode[idx++] = *src++; + unicode[idx++] = 0x00; + } + } +} + + +void usbd_desc_string_update(void) +{ +#if defined(USBD_PRODUCT_STRING) + usbd_string_buf_get(USBD_PRODUCT_STRING, (uint8_t *)&product_string); +#endif +#if defined(FC_FIRMWARE_NAME) + usbd_string_buf_get(FC_FIRMWARE_NAME, (uint8_t *)&manufacturer_string); +#endif +} + + +/*! + \brief initialize the CDC ACM device + \param[in] udev: pointer to USB device instance + \param[in] config_index: configuration index + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_cdc_acm_init(usb_dev *udev, uint8_t config_index) +{ + (void)(config_index); + + static __ALIGN_BEGIN usb_cdc_handler cdc_handler __ALIGN_END; + + /* initialize the data TX endpoint */ + usbd_ep_setup(udev, &(bf_cdc_config_desc.cdc_in_endpoint)); + + /* initialize the data RX endpoint */ + usbd_ep_setup(udev, &(bf_cdc_config_desc.cdc_out_endpoint)); + + /* initialize the command TX endpoint */ + usbd_ep_setup(udev, &(bf_cdc_config_desc.cdc_cmd_endpoint)); + + udev->dev.class_data[CDC_COM_INTERFACE] = (void *)&cdc_handler; + + APP_FOPS.pIf_Init(); + + usbd_ep_recev(udev, CDC_DATA_OUT_EP, (uint8_t*)(USB_Rx_Buffer), USB_CDC_DATA_PACKET_SIZE); + return USBD_OK; +} + +/*! + \brief deinitialize the CDC ACM device + \param[in] udev: pointer to USB device instance + \param[in] config_index: configuration index + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_cdc_acm_deinit(usb_dev *udev, uint8_t config_index) +{ + (void)(config_index); + + /* deinitialize the data TX/RX endpoint */ + usbd_ep_clear(udev, CDC_DATA_IN_EP); + usbd_ep_clear(udev, CDC_DATA_OUT_EP); + + /* deinitialize the command TX endpoint */ + usbd_ep_clear(udev, CDC_CMD_EP); + + APP_FOPS.pIf_DeInit(); + + return USBD_OK; +} + +/*! + \brief handle the CDC ACM class-specific requests + \param[in] udev: pointer to USB device instance + \param[in] req: device class-specific request + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_cdc_acm_req(usb_dev *udev, usb_req *req) +{ + usb_cdc_handler *cdc = (usb_cdc_handler *)udev->dev.class_data[0]; + usb_transc *transc = NULL; + + switch(req->bRequest) { + case SEND_ENCAPSULATED_COMMAND: + /* no operation for this driver */ + break; + + case GET_ENCAPSULATED_RESPONSE: + /* no operation for this driver */ + break; + + case SET_COMM_FEATURE: + /* no operation for this driver */ + break; + + case GET_COMM_FEATURE: + /* no operation for this driver */ + break; + + case CLEAR_COMM_FEATURE: + /* no operation for this driver */ + break; + + case SET_LINE_CODING: + transc = &udev->dev.transc_out[0]; + + /* set the value of the current command to be processed */ + udev->dev.class_core->alter_set = req->bRequest; + + /* enable EP0 prepare to receive command data packet */ + transc->remain_len = req->wLength; + transc->xfer_buf = cdc->cmd; + break; + + case GET_LINE_CODING: + transc = &udev->dev.transc_in[0]; + + cdc->cmd[0] = (uint8_t)(cdc->line_coding.dwDTERate); + cdc->cmd[1] = (uint8_t)(cdc->line_coding.dwDTERate >> 8); + cdc->cmd[2] = (uint8_t)(cdc->line_coding.dwDTERate >> 16); + cdc->cmd[3] = (uint8_t)(cdc->line_coding.dwDTERate >> 24); + cdc->cmd[4] = cdc->line_coding.bCharFormat; + cdc->cmd[5] = cdc->line_coding.bParityType; + cdc->cmd[6] = cdc->line_coding.bDataBits; + + transc->xfer_buf = cdc->cmd; + transc->remain_len = 7U; + break; + + case SET_CONTROL_LINE_STATE: + transc = &udev->dev.transc_out[0]; + if (ctrlLineStateCb) { + ctrlLineStateCb(ctrlLineStateCbContext, *((uint16_t *)CmdBuff)); + } + transc->remain_len = req->wLength; + transc->xfer_buf = CmdBuff; + break; + + case SEND_BREAK: + /* no operation for this driver */ + break; + + default: + break; + } + + return USBD_OK; +} + +/*! + \brief command data received on control endpoint + \param[in] udev: pointer to USB device instance + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_cdc_acm_ctlx_out(usb_dev *udev) +{ + usb_cdc_handler *cdc = (usb_cdc_handler *)udev->dev.class_data[0]; + + if(NO_CMD != udev->dev.class_core->alter_set) { + /* process the command data */ + cdc->line_coding.dwDTERate = (uint32_t)((uint32_t)cdc->cmd[0] | \ + ((uint32_t)cdc->cmd[1] << 8) | \ + ((uint32_t)cdc->cmd[2] << 16) | \ + ((uint32_t)cdc->cmd[3] << 24)); + + cdc->line_coding.bCharFormat = cdc->cmd[4]; + cdc->line_coding.bParityType = cdc->cmd[5]; + cdc->line_coding.bDataBits = cdc->cmd[6]; + + udev->dev.class_core->alter_set = NO_CMD; + } + if (baudRateCb) { + baudRateCb(baudRateCbContext, cdc->line_coding.dwDTERate); + } + + return USBD_OK; +} + +/*! + \brief handle CDC ACM data IN stage + \param[in] udev: pointer to USB device instance + \param[in] ep_num: endpoint identifier + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_cdc_acm_in(usb_dev *udev, uint8_t ep_num) +{ + (void)(ep_num); + uint16_t USB_Tx_length; + + if (USB_Tx_State == USB_CDC_BUSY) + { + if (APP_Rx_length == 0) + { + USB_Tx_State = USB_CDC_IDLE; + } else { + if (APP_Rx_length > USB_CDC_DATA_PACKET_SIZE) { + USB_Tx_length = USB_CDC_DATA_PACKET_SIZE; + } else { + USB_Tx_length = APP_Rx_length; + + if (USB_Tx_length == USB_CDC_DATA_PACKET_SIZE) { + USB_Tx_State = USB_CDC_ZLP; + } + } + + /* Prepare the available data buffer to be sent on IN endpoint */ + usbd_ep_send(udev, CDC_DATA_IN_EP, (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out], USB_Tx_length); + + // Advance the out pointer + APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE; + APP_Rx_length -= USB_Tx_length; + + return USBD_OK; + } + } + + /* Avoid any asynchronous transfer during ZLP */ + if (USB_Tx_State == USB_CDC_ZLP) { + /*Send ZLP to indicate the end of the current transfer */ + usbd_ep_send(udev, CDC_DATA_IN_EP, NULL, 0); + + USB_Tx_State = USB_CDC_IDLE; + } + return USBD_OK; +} + +/*! + \brief handle CDC ACM data OUT stage + \param[in] udev: pointer to USB device instance + \param[in] ep_num: endpoint identifier + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_cdc_acm_out(usb_dev *udev, uint8_t ep_num) +{ + uint16_t USB_Rx_Cnt; + + /* Get the received data buffer and update the counter */ + USB_Rx_Cnt = ((usb_core_driver *)udev)->dev.transc_out[ep_num].xfer_count; + + /* USB data will be immediately processed, this allow next USB traffic being + NAKed till the end of the application Xfer */ + APP_FOPS.pIf_DataRx(USB_Rx_Buffer, USB_Rx_Cnt); + + /* Prepare Out endpoint to receive next packet */ + usbd_ep_recev(udev, CDC_DATA_OUT_EP, (uint8_t *)(uint8_t*)(USB_Rx_Buffer), USB_CDC_DATA_PACKET_SIZE); + + return USBD_OK; +} + +/** + * @brief usbd_audio_SOF + * Start Of Frame event management + * @param pdev: instance + * @param epnum: endpoint number + * @retval status + */ +uint8_t bf_cdc_acm_sof(usb_dev *udev) +{ + static uint32_t FrameCount = 0; + + if (FrameCount++ == CDC_IN_FRAME_INTERVAL) + { + /* Reset the frame counter */ + FrameCount = 0; + + /* Check the data to be sent through IN pipe */ + Handle_USBAsynchXfer(udev); + } + + return USBD_OK; +} + +/** + * @brief Handle_USBAsynchXfer + * Send data to USB + * @param pdev: instance + * @retval None + */ +static void Handle_USBAsynchXfer (usb_dev *udev) +{ + uint16_t USB_Tx_length; + + if (USB_Tx_State == USB_CDC_IDLE) { + if (APP_Rx_ptr_out == APP_Rx_ptr_in) { + // Ring buffer is empty + return; + } + + if (APP_Rx_ptr_out > APP_Rx_ptr_in) { + // Transfer bytes up to the end of the ring buffer + APP_Rx_length = APP_RX_DATA_SIZE - APP_Rx_ptr_out; + } else { + // Transfer all bytes in ring buffer + APP_Rx_length = APP_Rx_ptr_in - APP_Rx_ptr_out; + } + +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED + // Only transfer whole 32 bit words of data + APP_Rx_length &= ~0x03; +#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ + + if (APP_Rx_length > USB_CDC_DATA_PACKET_SIZE) { + USB_Tx_length = USB_CDC_DATA_PACKET_SIZE; + + USB_Tx_State = USB_CDC_BUSY; + } else { + USB_Tx_length = APP_Rx_length; + + if (USB_Tx_length == USB_CDC_DATA_PACKET_SIZE) { + USB_Tx_State = USB_CDC_ZLP; + } else { + USB_Tx_State = USB_CDC_BUSY; + } + } + + usbd_ep_send(udev, CDC_DATA_IN_EP, (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out], USB_Tx_length); + + APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE; + APP_Rx_length -= USB_Tx_length; + } +} diff --git a/src/platform/GD32/usb_f4/usbd_desc.h b/src/platform/GD32/usb_f4/usbd_desc.h new file mode 100755 index 0000000000..eea101a42c --- /dev/null +++ b/src/platform/GD32/usb_f4/usbd_desc.h @@ -0,0 +1,63 @@ +/*! + \file usbd_desc.h + \brief the header file of cdc acm driver + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ +#ifndef __USBD_DESC_H +#define __USBD_DESC_H + +#include "usbd_enum.h" +#include "usb_cdc.h" +#include "cdc_acm_core.h" +#include "usbd_cdc_vcp.h" +#include "usbd_conf.h" + + +#define USB_CDC_RX_LEN USB_CDC_DATA_PACKET_SIZE /*< CDC data packet size */ +#define USB_CDC_IDLE 0 +#define USB_CDC_BUSY 1 +#define USB_CDC_ZLP 2 + +extern usb_desc bf_cdc_desc; +extern usb_class_core bf_cdc_class; +extern volatile uint32_t APP_Rx_ptr_in; +extern volatile uint32_t APP_Rx_ptr_out; +extern uint32_t APP_Rx_length; + +/* function declarations */ +/* check CDC ACM is ready for data transfer */ +uint8_t bf_cdc_acm_check_ready(usb_dev *udev); +/* send CDC ACM data */ +void bf_cdc_acm_data_send(usb_dev *udev); +/* receive CDC ACM data */ +void bf_cdc_acm_data_receive(usb_dev *udev); + +#endif /* __USBD_DESC_H */ diff --git a/src/platform/GD32/usb_f4/usbd_msc_desc.c b/src/platform/GD32/usb_f4/usbd_msc_desc.c new file mode 100755 index 0000000000..2237899e9d --- /dev/null +++ b/src/platform/GD32/usb_f4/usbd_msc_desc.c @@ -0,0 +1,427 @@ +/*! + \file usbd_msc_core.c + \brief USB MSC device class core functions + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#include "usbd_core.h" +#include "usbd_enum.h" +#include "usbd_msc_bbb.h" +#include "usbd_msc_core.h" +#include "usbd_msc_mem.h" + +#define USBD_VID 0x28E9U +#define USBD_PID 0x028FU + +#define USBD_PRODUCT_FS_STRING "Betaflight FC Mass Storage (FS Mode)" + +/* local function prototypes ('static') */ +static uint8_t bf_msc_core_init(usb_dev *udev, uint8_t config_index); +static uint8_t bf_msc_core_deinit(usb_dev *udev, uint8_t config_index); +static uint8_t bf_msc_core_req(usb_dev *udev, usb_req *req); +static uint8_t bf_msc_core_in(usb_dev *udev, uint8_t ep_num); +static uint8_t bf_msc_core_out(usb_dev *udev, uint8_t ep_num); + +usb_class_core bf_msc_class = { + .init = bf_msc_core_init, + .deinit = bf_msc_core_deinit, + + .req_proc = bf_msc_core_req, + + .data_in = bf_msc_core_in, + .data_out = bf_msc_core_out +}; + +/* note: it should use the C99 standard when compiling the below codes */ +/* USB standard device descriptor */ +__ALIGN_BEGIN const usb_desc_dev bf_msc_dev_desc __ALIGN_END = { + .header = + { + .bLength = USB_DEV_DESC_LEN, + .bDescriptorType = USB_DESCTYPE_DEV + }, + .bcdUSB = 0x0200U, + .bDeviceClass = 0x00U, + .bDeviceSubClass = 0x00U, + .bDeviceProtocol = 0x00U, + .bMaxPacketSize0 = USB_FS_EP0_MAX_LEN, + .idVendor = USBD_VID, + .idProduct = USBD_PID, + .bcdDevice = 0x0200U, + .iManufacturer = STR_IDX_MFC, + .iProduct = STR_IDX_PRODUCT, + .iSerialNumber = STR_IDX_SERIAL, + .bNumberConfigurations = USBD_CFG_MAX_NUM +}; + +/* USB device configuration descriptor */ +__ALIGN_BEGIN const usb_desc_config_set bf_msc_config_desc __ALIGN_END = { + .config = + { + .header = + { + .bLength = sizeof(usb_desc_config), + .bDescriptorType = USB_DESCTYPE_CONFIG + }, + .wTotalLength = USB_MSC_CONFIG_DESC_SIZE, + .bNumInterfaces = 0x01U, + .bConfigurationValue = 0x01U, + .iConfiguration = 0x04U, + .bmAttributes = 0xC0U, + .bMaxPower = 0x32U + }, + + .msc_itf = + { + .header = + { + .bLength = sizeof(usb_desc_itf), + .bDescriptorType = USB_DESCTYPE_ITF + }, + .bInterfaceNumber = 0x00U, + .bAlternateSetting = 0x00U, + .bNumEndpoints = 0x02U, + .bInterfaceClass = USB_CLASS_MSC, + .bInterfaceSubClass = USB_MSC_SUBCLASS_SCSI, + .bInterfaceProtocol = USB_MSC_PROTOCOL_BBB, + .iInterface = 0x05U + }, + + .msc_epin = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = MSC_IN_EP, + .bmAttributes = USB_EP_ATTR_BULK, + .wMaxPacketSize = MSC_EPIN_SIZE, + .bInterval = 0x00U + }, + + .msc_epout = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = MSC_OUT_EP, + .bmAttributes = USB_EP_ATTR_BULK, + .wMaxPacketSize = MSC_EPOUT_SIZE, + .bInterval = 0x00U + } +}; + +/* USB device configuration descriptor */ +__ALIGN_BEGIN const usb_desc_config_set bf_other_speed_msc_config_desc __ALIGN_END = { + .config = + { + .header = + { + .bLength = sizeof(usb_desc_config), + .bDescriptorType = USB_DESCTYPE_OTHER_SPD_CONFIG + }, + .wTotalLength = USB_MSC_CONFIG_DESC_SIZE, + .bNumInterfaces = 0x01U, + .bConfigurationValue = 0x01U, + .iConfiguration = 0x00U, + .bmAttributes = 0xC0U, + .bMaxPower = 0x32U + }, + + .msc_itf = + { + .header = + { + .bLength = sizeof(usb_desc_itf), + .bDescriptorType = USB_DESCTYPE_ITF + }, + .bInterfaceNumber = 0x00U, + .bAlternateSetting = 0x00U, + .bNumEndpoints = 0x02U, + .bInterfaceClass = USB_CLASS_MSC, + .bInterfaceSubClass = USB_MSC_SUBCLASS_SCSI, + .bInterfaceProtocol = USB_MSC_PROTOCOL_BBB, + .iInterface = 0x05U + }, + + .msc_epin = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = MSC_IN_EP, + .bmAttributes = USB_EP_ATTR_BULK, + .wMaxPacketSize = 64U, + .bInterval = 0x00U + }, + + .msc_epout = + { + .header = + { + .bLength = sizeof(usb_desc_ep), + .bDescriptorType = USB_DESCTYPE_EP + }, + .bEndpointAddress = MSC_OUT_EP, + .bmAttributes = USB_EP_ATTR_BULK, + .wMaxPacketSize = 64U, + .bInterval = 0x00U + } +}; + +/* USB language ID descriptor */ +static __ALIGN_BEGIN const usb_desc_LANGID usbd_language_id_desc __ALIGN_END = { + .header = + { + .bLength = sizeof(usb_desc_LANGID), + .bDescriptorType = USB_DESCTYPE_STR + }, + .wLANGID = ENG_LANGID +}; + +/* USB manufacture string */ +static __ALIGN_BEGIN const usb_desc_str manufacturer_string __ALIGN_END = { + .header = + { + .bLength = USB_STRING_LEN(10U), + .bDescriptorType = USB_DESCTYPE_STR + }, + .unicode_string = {'G', 'i', 'g', 'a', 'D', 'e', 'v', 'i', 'c', 'e'} +}; + +/* USB product string */ +static __ALIGN_BEGIN const usb_desc_str product_string __ALIGN_END = { + .header = + { + .bLength = USB_STRING_LEN(34U), + .bDescriptorType = USB_DESCTYPE_STR + }, + .unicode_string = {'B', 'e', 't', 'a', 'f', 'l', 'i', 'g', 'h', 't', ' ', + 'F', 'C', ' ', 'M', 'a', 's', 's', ' ', 'S', 't', 'o', 'r', 'a', 'g', 'e', ' ', + 'F', 'S', ' ', 'M', 'o', 'd', 'e' + } + // .unicode_string = {'G', 'D', '3', '2', '-', 'U', 'S', 'B', '_', 'M', 'S', 'C'} +}; + +/* USBD serial string */ +static __ALIGN_BEGIN usb_desc_str serial_string __ALIGN_END = { + .header = + { + .bLength = USB_STRING_LEN(12U), + .bDescriptorType = USB_DESCTYPE_STR + } +}; + +/* USB string descriptor */ +static void *const bf_usbd_msc_strings[] = { + [STR_IDX_LANGID] = (uint8_t *)&usbd_language_id_desc, + [STR_IDX_MFC] = (uint8_t *)&manufacturer_string, + [STR_IDX_PRODUCT] = (uint8_t *)&product_string, + [STR_IDX_SERIAL] = (uint8_t *)&serial_string +}; + +usb_desc bf_msc_desc = { + .dev_desc = (uint8_t *)&bf_msc_dev_desc, + .config_desc = (uint8_t *)&bf_msc_config_desc, + +#if defined(USE_USB_HS) && defined(USE_ULPI_PHY) + .other_speed_config_desc = (uint8_t *)&bf_other_speed_msc_config_desc, + .qualifier_desc = (uint8_t *)&usbd_qualifier_desc, +#endif /* USE_USB_HS && USE_ULPI_PHY */ + + .strings = bf_usbd_msc_strings +}; + +static __ALIGN_BEGIN uint8_t usbd_msc_maxlun __ALIGN_END = 0U ; + +/*! + \brief usbd string get + \param[in] src: pointer to source string + \param[in] unicode : formatted string buffer (unicode) + \param[in] len : descriptor length + \param[out] none + \retval USB device operation status +*/ +static void usbd_msc_string_buf_get(const char *src, uint8_t *unicode) +{ + uint8_t idx = 0; + uint8_t len = 0; + + if (src != NULL) { + len = USB_STRING_LEN(strlen(src)); + unicode[idx++] = len; + unicode[idx++] = USB_DESCTYPE_STR; + + while (*src != '\0') + { + unicode[idx++] = *src++; + unicode[idx++] = 0x00; + } + } +} + + +void usbd_msc_desc_string_update(void) +{ +#if defined(USBD_PRODUCT_FS_STRING) + usbd_msc_string_buf_get(USBD_PRODUCT_FS_STRING, (uint8_t *)&product_string); +#endif +} + + +/*! + \brief initialize the MSC device + \param[in] udev: pointer to USB device instance + \param[in] config_index: configuration index + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_msc_core_init(usb_dev *udev, uint8_t config_index) +{ + (void)(config_index); + + static __ALIGN_BEGIN usbd_msc_handler msc_handler __ALIGN_END; + + memset((void *)&msc_handler, 0U, sizeof(usbd_msc_handler)); + + udev->dev.class_data[USBD_MSC_INTERFACE] = (void *)&msc_handler; + + /* configure MSC TX endpoint */ + usbd_ep_setup(udev, &(bf_msc_config_desc.msc_epin)); + + /* configure MSC RX endpoint */ + usbd_ep_setup(udev, &(bf_msc_config_desc.msc_epout)); + + /* initialize the BBB layer */ + msc_bbb_init(udev); + + return USBD_OK; +} + +/*! + \brief deinitialize the MSC device + \param[in] udev: pointer to USB device instance + \param[in] config_index: configuration index + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_msc_core_deinit(usb_dev *udev, uint8_t config_index) +{ + (void)(config_index); + + /* clear MSC endpoints */ + usbd_ep_clear(udev, MSC_IN_EP); + usbd_ep_clear(udev, MSC_OUT_EP); + + /* deinitialize the BBB layer */ + msc_bbb_deinit(udev); + + return USBD_OK; +} + +/*! + \brief handle the MSC class-specific and standard requests + \param[in] udev: pointer to USB device instance + \param[in] req: device class-specific request + \param[out] none + \retval USB device operation status +*/ +static uint8_t bf_msc_core_req(usb_dev *udev, usb_req *req) +{ + usb_transc *transc = &udev->dev.transc_in[0]; + + switch(req->bRequest) { + case BBB_GET_MAX_LUN : + if((0U == req->wValue) && \ + (1U == req->wLength) && \ + (0x80U == (req->bmRequestType & 0x80U))) { + usbd_msc_maxlun = (uint8_t)usbd_mem_fops->mem_maxlun(); + + transc->xfer_buf = &usbd_msc_maxlun; + transc->remain_len = 1U; + } else { + return USBD_FAIL; + } + break; + + case BBB_RESET : + if((0U == req->wValue) && \ + (0U == req->wLength) && \ + (0x80U != (req->bmRequestType & 0x80U))) { + msc_bbb_reset(udev); + } else { + return USBD_FAIL; + } + break; + + case USB_CLEAR_FEATURE: + msc_bbb_clrfeature(udev, (uint8_t)req->wIndex); + break; + + default: + return USBD_FAIL; + } + + return USBD_OK; +} + +/*! + \brief handle data IN stage + \param[in] udev: pointer to USB device instance + \param[in] ep_num: the endpoint number + \param[out] none + \retval none +*/ +static uint8_t bf_msc_core_in(usb_dev *udev, uint8_t ep_num) +{ + msc_bbb_data_in(udev, ep_num); + + return USBD_OK; +} + +/*! + \brief handle data OUT stage + \param[in] udev: pointer to USB device instance + \param[in] ep_num: the endpoint number + \param[out] none + \retval none +*/ +static uint8_t bf_msc_core_out(usb_dev *udev, uint8_t ep_num) +{ + msc_bbb_data_out(udev, ep_num); + + return USBD_OK; +} diff --git a/src/platform/GD32/usb_f4/usbd_msc_desc.h b/src/platform/GD32/usb_f4/usbd_msc_desc.h new file mode 100755 index 0000000000..ccb6eed720 --- /dev/null +++ b/src/platform/GD32/usb_f4/usbd_msc_desc.h @@ -0,0 +1,49 @@ +/*! + \file usbd_msc_core.h + \brief the header file of USB MSC device class core functions + + \version 2024-12-20, V3.3.1, firmware for GD32F4xx +*/ + +/* + Copyright (c) 2024, GigaDevice Semiconductor Inc. + + Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software without + specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +OF SUCH DAMAGE. +*/ + +#ifndef __USB_DESC_H +#define __USB_DESC_H + +#include "usbd_msc_core.h" + +#define USB_MSC_CONFIG_DESC_SIZE 32U /*!< MSC configuration descriptor size */ + +#define MSC_EPIN_SIZE MSC_DATA_PACKET_SIZE /*!< MSC endpoint IN size */ +#define MSC_EPOUT_SIZE MSC_DATA_PACKET_SIZE /*!< MSC endpoint OUT size */ + +extern usb_desc bf_msc_desc; +extern usb_class_core bf_msc_class; + +#endif /* __USBD_DESC_H */ + diff --git a/src/platform/GD32/usb_msc_f4xx.c b/src/platform/GD32/usb_msc_f4xx.c new file mode 100755 index 0000000000..1e1ed505ef --- /dev/null +++ b/src/platform/GD32/usb_msc_f4xx.c @@ -0,0 +1,126 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" +#define USE_USB_MSC +#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_conf.h" +#include "drivers/usb_io.h" +#include "usbd_msc_desc.h" +#include "usbd_msc_mem.h" +#include "drv_usb_hw.h" + +extern usb_core_driver USB_OTG_dev; +usbd_mem_cb *usbd_mem_fops; + +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_STORAGE_fops = &USBD_MSC_MICRO_SDIO_fops; + break; +#endif +#ifdef USE_SDCARD_SPI + case SDCARD_MODE_SPI: + USBD_STORAGE_fops = &USBD_MSC_MICRO_SD_SPI_fops; + break; +#endif + default: + return 1; + } + break; +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + USBD_STORAGE_fops = &USBD_MSC_EMFAT_fops; + break; +#endif + default: + return 1; + } + + static usbd_mem_cb usbd_internal_storage_fops; + { + usbd_internal_storage_fops.mem_init = USBD_STORAGE_fops->Init; + usbd_internal_storage_fops.mem_ready = USBD_STORAGE_fops->IsReady; + usbd_internal_storage_fops.mem_protected = USBD_STORAGE_fops->IsWriteProtected; + usbd_internal_storage_fops.mem_read = USBD_STORAGE_fops->Read; + usbd_internal_storage_fops.mem_write = USBD_STORAGE_fops->Write; + usbd_internal_storage_fops.mem_maxlun = USBD_STORAGE_fops->GetMaxLun; + usbd_internal_storage_fops.mem_inquiry_data[0] = (uint8_t *)(USBD_STORAGE_fops->pInquiry); + usbd_internal_storage_fops.mem_getcapacity = USBD_STORAGE_fops->GetCapacity; + + usbd_internal_storage_fops.mem_block_size[0] = 0; + usbd_internal_storage_fops.mem_block_len[0] = 0; + } + + usbd_mem_fops = &usbd_internal_storage_fops; + usb_gpio_config(); + usb_rcu_config(); + //(void)(usbd_mem_fops); + usbd_init(&USB_OTG_dev, USB_CORE_ENUM_FS, &bf_msc_desc, &bf_msc_class); + usb_intr_config(); + //(void)(usbd_mem_fops); + usbd_init(&USB_OTG_dev, USB_CORE_ENUM_FS, &bf_msc_desc, &bf_msc_class); + usb_intr_config(); + // NVIC configuration for SYSTick + NVIC_DisableIRQ(SysTick_IRQn); + NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0)); + NVIC_EnableIRQ(SysTick_IRQn); + + return 0; +} + +#endif diff --git a/src/platform/GD32/usbd_cdc_vcp.c b/src/platform/GD32/usbd_cdc_vcp.c new file mode 100755 index 0000000000..3c8fcb66ac --- /dev/null +++ b/src/platform/GD32/usbd_cdc_vcp.c @@ -0,0 +1,340 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" + +#include "build/atomic.h" + +#include "usbd_cdc_vcp.h" +#include "gd32f4xx.h" +#include "drivers/nvic.h" +#include "drivers/time.h" + +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED +#pragma data_alignment = 4 +#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ + +__ALIGN_BEGIN usb_core_driver USB_OTG_dev __ALIGN_END; + +LINE_CODING g_lc; + +extern __IO uint8_t USB_Tx_State; +__IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ + +/* These are external variables imported from CDC core to be used for IN transfer management. */ + +/* This is the buffer for data received from the MCU to APP (i.e. MCU TX, APP RX) */ +extern uint8_t APP_Rx_Buffer[APP_RX_DATA_SIZE]; +extern volatile uint32_t APP_Rx_ptr_out; + +/* Increment this buffer position or roll it back to + start address when writing received data + in the buffer APP_Rx_Buffer. */ +extern volatile uint32_t APP_Rx_ptr_in; + +/* + APP TX is the circular buffer for data that is transmitted from the APP (host) + to the USB device (flight controller). +*/ +static uint8_t APP_Tx_Buffer[APP_TX_DATA_SIZE]; +static uint32_t APP_Tx_ptr_out = 0; +static uint32_t APP_Tx_ptr_in = 0; + +static uint16_t VCP_Init(void); +static uint16_t VCP_DeInit(void); +static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len); +static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len); +static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len); + +void (*ctrlLineStateCb)(void* context, uint16_t ctrlLineState); +void *ctrlLineStateCbContext; +void (*baudRateCb)(void *context, uint32_t baud); +void *baudRateCbContext; + +CDC_IF_Prop_TypeDef VCP_fops = {VCP_Init, VCP_DeInit, VCP_Ctrl, VCP_DataTx, VCP_DataRx }; + +/*! + \brief VCP_Init + Initializes the Media + \param[in] none + \param[out] none + \retval Result of the operation (USBD_OK in all cases) +*/ +static uint16_t VCP_Init(void) +{ + bDeviceState = CONFIGURED; + ctrlLineStateCb = NULL; + baudRateCb = NULL; + return USBD_OK; +} + +/*! + \brief VCP_DeInit + DeInitializes the Media + \param[in] none + \param[out] none + \retval Result of the operation (USBD_OK in all cases) +*/ +static uint16_t VCP_DeInit(void) +{ + bDeviceState = UNCONNECTED; + return USBD_OK; +} + +static void ust_cpy(LINE_CODING* plc2, const LINE_CODING* plc1) +{ + plc2->bitrate = plc1->bitrate; + plc2->format = plc1->format; + plc2->paritytype = plc1->paritytype; + plc2->datatype = plc1->datatype; +} + +/*! + \brief VCP_Ctrl + Manage the CDC class requests + \param[in] Cmd: Command code + \param[in] Buf: Buffer containing command data (request parameters) + \param[in] Len: Number of data to be sent (in bytes) + \param[out] none + \retval Result of the operation (USBD_OK in all cases) +*/ +static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) +{ + LINE_CODING* plc = (LINE_CODING*)Buf; + + // assert_param(Len>=sizeof(LINE_CODING)); + + switch (Cmd) { + /* Not needed for this driver, AT modem commands */ + case SEND_ENCAPSULATED_COMMAND: + case GET_ENCAPSULATED_RESPONSE: + break; + + // Not needed for this driver + case SET_COMM_FEATURE: + case GET_COMM_FEATURE: + case CLEAR_COMM_FEATURE: + break; + + //Note - hw flow control on UART 1-3 and 6 only + case SET_LINE_CODING: + // If a callback is provided, tell the upper driver of changes in baud rate + if (plc && (Len == sizeof(*plc))) { + if (baudRateCb) { + baudRateCb(baudRateCbContext, plc->bitrate); + } + ust_cpy(&g_lc, plc); //Copy into structure to save for later + } + break; + + case GET_LINE_CODING: + if (plc && (Len == sizeof(*plc))) { + ust_cpy(plc, &g_lc); + } + break; + + case SET_CONTROL_LINE_STATE: + // If a callback is provided, tell the upper driver of changes in DTR/RTS state + if (plc && (Len == sizeof(uint16_t))) { + if (ctrlLineStateCb) { + ctrlLineStateCb(ctrlLineStateCbContext, *((uint16_t *)Buf)); + } + } + break; + + case SEND_BREAK: + /* Not needed for this driver */ + break; + + default: + break; + } + + return USBD_OK; +} + +/*! + \brief Send DATA + send the data received from the board to the PC through USB + \param[in] ptrBuffer: buffer to send + \param[in] sendLength: the length of the buffer + \param[out] none + \retval sent data length +*/ +uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) +{ + VCP_DataTx(ptrBuffer, sendLength); + return sendLength; +} + +uint32_t CDC_Send_FreeBytes(void) +{ + return APP_RX_DATA_SIZE - CDC_Receive_BytesAvailable(); +} + +/*! + \brief VCP_DataTx + CDC data to be sent to the Host (app) over USB + \param[in] Buf: Buffer of data to be sent + \param[in] Len: Number of data to be sent (in bytes) + \param[out] none + \retval Result of the operation: USBD_OK if all operations are OK else VCP_FAIL +*/ +static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) +{ + /* + make sure that any paragraph end frame is not in play + could just check for: USB_CDC_ZLP, but better to be safe + and wait for any existing transmission to complete. + */ + while (USB_Tx_State != 0); + + for (uint32_t i = 0; i < Len; i++) { + // Stall if the ring buffer is full + while (((APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE) == APP_Rx_ptr_out) { + delay(1); + } + + APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; + APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; + } + + return USBD_OK; +} + +/*! + \brief Receive DATA + receive the data from the PC to the board and send it through USB + \param[in] recvBuf: buffer to receive data + \param[in] len: maximum length to receive + \param[out] none + \retval actual received data length +*/ +uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) +{ + uint32_t count = 0; + + while (APP_Tx_ptr_out != APP_Tx_ptr_in && (count < len)) { + recvBuf[count] = APP_Tx_Buffer[APP_Tx_ptr_out]; + APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % APP_TX_DATA_SIZE; + count++; + } + return count; +} + +uint32_t CDC_Receive_BytesAvailable(void) +{ + /* return the bytes available in the receive circular buffer */ + return (APP_Tx_ptr_in + APP_TX_DATA_SIZE - APP_Tx_ptr_out) % APP_TX_DATA_SIZE; +} + +/*! + \brief VCP_DataRx + Data received over USB OUT endpoint are sent over CDC interface + through this function. + \param[in] Buf: Buffer of data to be received + \param[in] Len: Number of data received (in bytes) + \param[out] none + \retval Result of the operation: USBD_OK if all operations are OK else VCP_FAIL + \note This function will block any OUT packet reception on USB endpoint + until exiting this function. If you exit this function before transfer + is complete on CDC interface (ie. using DMA controller) it will result + in receiving more data while previous ones are still not sent. +*/ +static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) +{ + if (CDC_Receive_BytesAvailable() + Len > APP_TX_DATA_SIZE) { + return USBD_FAIL; + } + + for (uint32_t i = 0; i < Len; i++) { + APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; + APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % APP_TX_DATA_SIZE; + } + + return USBD_OK; +} + +/*! + \brief usbIsConfigured + Determines if USB VCP is configured or not + \param[in] none + \param[out] none + \retval True if configured +*/ +uint8_t usbIsConfigured(void) +{ + return (bDeviceState == CONFIGURED); +} + +/*! + \brief usbIsConnected + Determines if USB VCP is connected or not + \param[in] none + \param[out] none + \retval True if connected +*/ +uint8_t usbIsConnected(void) +{ + return (bDeviceState != UNCONNECTED); +} + +/*! + \brief CDC_BaudRate + Get the current baud rate + \param[in] none + \param[out] none + \retval Baud rate in bps +*/ +uint32_t CDC_BaudRate(void) +{ + return g_lc.bitrate; +} + +/*! + \brief CDC_SetBaudRateCb + Set a callback to call when baud rate changes + \param[in] cb: callback function + \param[in] context: callback context + \param[out] none + \retval none +*/ +void CDC_SetBaudRateCb(void (*cb)(void *context, uint32_t baud), void *context) +{ + baudRateCbContext = context; + baudRateCb = cb; +} + +/*! + \brief CDC_SetCtrlLineStateCb + Set a callback to call when control line state changes + \param[in] cb: callback function + \param[in] context: callback context + \param[out] none + \retval none +*/ +void CDC_SetCtrlLineStateCb(void (*cb)(void *context, uint16_t ctrlLineState), void *context) +{ + ctrlLineStateCbContext = context; + ctrlLineStateCb = cb; +} diff --git a/src/platform/GD32/usbd_cdc_vcp.h b/src/platform/GD32/usbd_cdc_vcp.h new file mode 100755 index 0000000000..04e8c84cb0 --- /dev/null +++ b/src/platform/GD32/usbd_cdc_vcp.h @@ -0,0 +1,81 @@ +/* + * 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 . + */ + +#ifndef __USBD_CDC_VCP_H +#define __USBD_CDC_VCP_H + +#include "gd32f4xx.h" + +#include "usbd_conf.h" +#include + +#include "usbd_core.h" +#include "usbd_desc.h" + +extern usb_core_driver USB_OTG_dev; + +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); + +/* External variables --------------------------------------------------------*/ +extern __IO uint32_t bDeviceState; /* USB device status */ + +typedef enum _DEVICE_STATE { + UNCONNECTED, + ATTACHED, + POWERED, + SUSPENDED, + ADDRESSED, + CONFIGURED +} DEVICE_STATE; + +/* Exported typef ------------------------------------------------------------*/ +/* 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; + + +typedef struct _CDC_IF_PROP +{ + uint16_t (*pIf_Init) (void); + uint16_t (*pIf_DeInit) (void); + uint16_t (*pIf_Ctrl) (uint32_t Cmd, uint8_t* Buf, uint32_t Len); + uint16_t (*pIf_DataTx) (const uint8_t* Buf, uint32_t Len); + uint16_t (*pIf_DataRx) (uint8_t* Buf, uint32_t Len); +} +CDC_IF_Prop_TypeDef; + +#endif /* __USBD_CDC_VCP_H */ diff --git a/src/platform/common/stm32/bus_spi_hw.c b/src/platform/common/stm32/bus_spi_hw.c index f15e226968..cd7c1b5541 100644 --- a/src/platform/common/stm32/bus_spi_hw.c +++ b/src/platform/common/stm32/bus_spi_hw.c @@ -112,6 +112,12 @@ uint16_t spiCalculateDivider(uint32_t freq) } uint32_t spiClk = system_core_clock / 2; +#elif defined(GD32F4) + if(freq > 30000000){ + freq = 30000000; + } + + uint32_t spiClk = SystemCoreClock / 2; #else #error "Base SPI clock not defined for this architecture" #endif @@ -137,6 +143,12 @@ uint32_t spiCalculateClock(uint16_t spiClkDivisor) if ((spiClk / spiClkDivisor) > 36000000){ return 36000000; } +#elif defined(GD32F4) + uint32_t spiClk = SystemCoreClock / 2; + + if ((spiClk / spiClkDivisor) > 30000000){ + return 30000000; + } #else #error "Base SPI clock not defined for this architecture" #endif @@ -147,7 +159,7 @@ uint32_t spiCalculateClock(uint16_t spiClkDivisor) void spiInitBusDMA(void) { uint32_t device; -#if (defined(STM32F4) || defined(APM32F4)) && defined(USE_DSHOT_BITBANG) +#if (defined(STM32F4) || defined(APM32F4) || defined(GD32F4)) && 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 @@ -181,7 +193,7 @@ void spiInitBusDMA(void) if (dmaTxChannelSpec) { dmaTxIdentifier = dmaGetIdentifier(dmaTxChannelSpec->ref); -#if (defined(STM32F4) || defined(APM32F4)) && defined(USE_DSHOT_BITBANG) +#if (defined(STM32F4) || defined(APM32F4) || defined(GD32F4)) && defined(USE_DSHOT_BITBANG) if (dshotBitbangActive && (DMA_DEVICE_NO(dmaTxIdentifier) == 2)) { dmaTxIdentifier = DMA_NONE; break; @@ -192,7 +204,7 @@ void spiInitBusDMA(void) continue; } bus->dmaTx = dmaGetDescriptorByIdentifier(dmaTxIdentifier); -#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4) || defined(GD32F4) bus->dmaTx->stream = DMA_DEVICE_INDEX(dmaTxIdentifier); bus->dmaTx->channel = dmaTxChannelSpec->channel; #endif @@ -219,7 +231,7 @@ void spiInitBusDMA(void) if (dmaRxChannelSpec) { dmaRxIdentifier = dmaGetIdentifier(dmaRxChannelSpec->ref); -#if (defined(STM32F4) || defined(APM32F4)) && defined(USE_DSHOT_BITBANG) +#if (defined(STM32F4) || defined(APM32F4) || defined(GD32F4)) && defined(USE_DSHOT_BITBANG) if (dshotBitbangActive && (DMA_DEVICE_NO(dmaRxIdentifier) == 2)) { dmaRxIdentifier = DMA_NONE; break; @@ -230,7 +242,7 @@ void spiInitBusDMA(void) continue; } bus->dmaRx = dmaGetDescriptorByIdentifier(dmaRxIdentifier); -#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7) || defined(APM32F4) || defined(GD32F4) bus->dmaRx->stream = DMA_DEVICE_INDEX(dmaRxIdentifier); bus->dmaRx->channel = dmaRxChannelSpec->channel; #endif diff --git a/src/platform/common/stm32/bus_spi_pinconfig.c b/src/platform/common/stm32/bus_spi_pinconfig.c index 52d2d456c2..afad885d3d 100644 --- a/src/platform/common/stm32/bus_spi_pinconfig.c +++ b/src/platform/common/stm32/bus_spi_pinconfig.c @@ -484,6 +484,62 @@ const spiHardware_t spiHardware[] = { .dmaIrqHandler = DMA1_ST7_HANDLER, }, #endif +#ifdef GD32F4 + { + .device = SPIDEV_0, + .reg = (void *)SPI0, + .sckPins = { + { DEFIO_TAG_E(PA5) }, + { DEFIO_TAG_E(PB3) }, + }, + .misoPins = { + { DEFIO_TAG_E(PA6) }, + { DEFIO_TAG_E(PB4) }, + }, + .mosiPins = { + { DEFIO_TAG_E(PA7) }, + { DEFIO_TAG_E(PB5) }, + }, + .af = GPIO_AF_5, + .rcc = RCC_APB2(SPI0), + }, + { + .device = SPIDEV_1, + .reg = (void *)SPI1, + .sckPins = { + { DEFIO_TAG_E(PB10) }, + { DEFIO_TAG_E(PB13) }, + }, + .misoPins = { + { DEFIO_TAG_E(PB14) }, + { DEFIO_TAG_E(PC2) }, + }, + .mosiPins = { + { DEFIO_TAG_E(PB15) }, + { DEFIO_TAG_E(PC3) }, + }, + .af = GPIO_AF_5, + .rcc = RCC_APB1(SPI1), + }, + { + .device = SPIDEV_2, + .reg = (void *)SPI2, + .sckPins = { + { DEFIO_TAG_E(PB3) }, + { DEFIO_TAG_E(PC10) }, + }, + .misoPins = { + { DEFIO_TAG_E(PB4) }, + { DEFIO_TAG_E(PC11) }, + }, + .mosiPins = { + { DEFIO_TAG_E(PB5) }, + { DEFIO_TAG_E(PC12) }, + }, + .af = GPIO_AF_6, + .rcc = RCC_APB1(SPI2), + }, +#endif }; void spiPinConfigure(const spiPinConfig_t *pConfig) diff --git a/src/platform/common/stm32/config_flash.c b/src/platform/common/stm32/config_flash.c index 4e79ef7a4f..7b89036675 100644 --- a/src/platform/common/stm32/config_flash.c +++ b/src/platform/common/stm32/config_flash.c @@ -308,6 +308,54 @@ uint32_t getFLASHSectorForEEPROM(void) failureMode(FAILURE_CONFIG_STORE_FAILURE); } } +#elif defined(GD32F4) +/* +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 +*/ + +uint32_t getFLASHSectorForEEPROM(void) +{ + if ((uint32_t)&__config_start <= 0x08003FFF) + return CTL_SECTOR_NUMBER_0; + if ((uint32_t)&__config_start <= 0x08007FFF) + return CTL_SECTOR_NUMBER_1; + if ((uint32_t)&__config_start <= 0x0800BFFF) + return CTL_SECTOR_NUMBER_2; + if ((uint32_t)&__config_start <= 0x0800FFFF) + return CTL_SECTOR_NUMBER_3; + if ((uint32_t)&__config_start <= 0x0801FFFF) + return CTL_SECTOR_NUMBER_4; + if ((uint32_t)&__config_start <= 0x0803FFFF) + return CTL_SECTOR_NUMBER_5; + if ((uint32_t)&__config_start <= 0x0805FFFF) + return CTL_SECTOR_NUMBER_6; + if ((uint32_t)&__config_start <= 0x0807FFFF) + return CTL_SECTOR_NUMBER_7; + if ((uint32_t)&__config_start <= 0x0809FFFF) + return CTL_SECTOR_NUMBER_8; + if ((uint32_t)&__config_start <= 0x080DFFFF) + return CTL_SECTOR_NUMBER_9; + if ((uint32_t)&__config_start <= 0x080BFFFF) + return CTL_SECTOR_NUMBER_10; + if ((uint32_t)&__config_start <= 0x080FFFFF) + return CTL_SECTOR_NUMBER_11; + + // Not good + while (1) { + failureMode(FAILURE_CONFIG_STORE_FAILURE); + } +} #endif void configUnlock(void) @@ -318,6 +366,8 @@ void configUnlock(void) DAL_FLASH_Unlock(); #elif defined(AT32F4) flash_unlock(); +#elif defined(GD32F4) + fmc_unlock(); #else FLASH_Unlock(); #endif @@ -331,6 +381,8 @@ void configLock(void) flash_lock(); #elif defined(APM32F4) DAL_FLASH_Lock(); +#elif defined(GD32F4) + fmc_lock(); #else FLASH_Lock(); #endif @@ -350,6 +402,8 @@ void configClearFlags(void) 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(GD32F4) + fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_OPERR | FMC_FLAG_WPERR | FMC_FLAG_PGMERR | FMC_FLAG_PGSERR); #elif defined(UNIT_TEST) || defined(SIMULATOR_BUILD) // NOP #else @@ -471,6 +525,19 @@ configStreamerResult_e configWriteWord(uintptr_t address, config_streamer_buffer if (status != FLASH_COMPLETE) { return CONFIG_RESULT_ADDRESS_INVALID; } +#elif defined(GD32F4) + if (address % FLASH_PAGE_SIZE == 0) { + const fmc_state_enum status = fmc_sector_erase(getFLASHSectorForEEPROM()); + if (status != FMC_READY) { + return CONFIG_RESULT_FAILURE; + } + } + + STATIC_ASSERT(CONFIG_STREAMER_BUFFER_SIZE == sizeof(uint32_t), "CONFIG_STREAMER_BUFFER_SIZE does not match written size"); + const fmc_state_enum status = fmc_word_program(address, *buffer); + if (status != FMC_READY) { + return CONFIG_RESULT_ADDRESS_INVALID; + } #else #error "MCU not catered for in configWriteWord for config_streamer" #endif diff --git a/src/platform/common/stm32/dshot_bitbang_impl.h b/src/platform/common/stm32/dshot_bitbang_impl.h index 76b6b06196..13b9e15c72 100644 --- a/src/platform/common/stm32/dshot_bitbang_impl.h +++ b/src/platform/common/stm32/dshot_bitbang_impl.h @@ -88,6 +88,9 @@ #elif defined(AT32F435) #define BB_GPIO_PULLDOWN GPIO_PULL_DOWN #define BB_GPIO_PULLUP GPIO_PULL_UP +#elif defined(USE_GDBSP_DRIVER) +#define BB_GPIO_PULLDOWN GPIO_PUPD_PULLDOWN +#define BB_GPIO_PULLUP GPIO_PUPD_PULLUP #else #define BB_GPIO_PULLDOWN GPIO_PuPd_DOWN #define BB_GPIO_PULLUP GPIO_PuPd_UP @@ -112,6 +115,12 @@ typedef struct dmaRegCache_s { uint32_t NDATA; uint32_t PADDR; uint32_t M0ADDR; +#elif defined(GD32F4) + uint32_t CHCTL; + uint32_t CHCNT; + uint32_t CHPADDR; + uint32_t CHM0ADDR; + uint32_t CHFCTL; #else #error No MCU dependent code here #endif diff --git a/src/platform/common/stm32/dshot_bitbang_shared.c b/src/platform/common/stm32/dshot_bitbang_shared.c index 3339df045b..3317049790 100644 --- a/src/platform/common/stm32/dshot_bitbang_shared.c +++ b/src/platform/common/stm32/dshot_bitbang_shared.c @@ -65,7 +65,7 @@ IO_t bbGetMotorIO(unsigned index) #ifdef USE_DSHOT_BITBANG bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig) { -#if defined(STM32F4) || defined(APM32F4) +#if defined(STM32F4) || defined(APM32F4) || defined(GD32F4) return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON || (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000); #else diff --git a/src/platform/common/stm32/dshot_dpwm.h b/src/platform/common/stm32/dshot_dpwm.h index 0cdf3cf41b..86a0221fb1 100644 --- a/src/platform/common/stm32/dshot_dpwm.h +++ b/src/platform/common/stm32/dshot_dpwm.h @@ -70,7 +70,7 @@ uint32_t getDshotHz(motorProtocolTypes_e pwmProtocolType); #define DSHOT_DMA_BUFFER_ATTRIBUTE /* Empty */ #endif -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F435) || defined(APM32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F435) || defined(APM32F4) || defined(GD32F4) #define DSHOT_DMA_BUFFER_UNIT uint32_t #else #define DSHOT_DMA_BUFFER_UNIT uint8_t diff --git a/src/platform/common/stm32/mco.c b/src/platform/common/stm32/mco.c index 11959bd65f..31fff46d67 100644 --- a/src/platform/common/stm32/mco.c +++ b/src/platform/common/stm32/mco.c @@ -81,7 +81,7 @@ static void mcoConfigure(MCODevice_e device, const mcoConfig_t *config) #endif break; case MCODEV_2: // MCO2 on PC9 -#if defined(STM32F4) || defined(STM32F7) || defined(APM32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(APM32F4) || defined(GD32F4) io = IOGetByTag(DEFIO_TAG_E(PC9)); IOInit(io, OWNER_MCO, 2); #if defined(STM32F7) @@ -90,6 +90,9 @@ static void mcoConfigure(MCODevice_e device, const mcoConfig_t *config) #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); +#elif defined(GD32F4) + rcu_ckout1_config(RCU_CKOUT1SRC_PLLI2SR, RCU_CKOUT1_DIV4); + IOConfigGPIOAF(io, IO_CONFIG(GPIO_MODE_AF, GPIO_OSPEED_50MHZ, GPIO_OTYPE_PP, GPIO_PUPD_NONE), GPIO_AF_0); #else // All F4s RCC_MCO2Config(RCC_MCO2Source_PLLI2SCLK, RCC_MCO2Div_4); @@ -105,7 +108,7 @@ static void mcoConfigure(MCODevice_e device, const mcoConfig_t *config) void mcoInit(void) { -#if defined(STM32F4) || defined(STM32F7) || defined(APM32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(APM32F4) || defined(GD32F4) // 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/platform/common/stm32/platform/rcc.h b/src/platform/common/stm32/platform/rcc.h index 69641b5e2d..bf54d37543 100644 --- a/src/platform/common/stm32/platform/rcc.h +++ b/src/platform/common/stm32/platform/rcc.h @@ -58,6 +58,12 @@ enum rcc_reg { RCC_AHB3, RCC_APB2, RCC_APB1, +#elif defined(GD32F4) + RCC_AHB1, + RCC_AHB2, + RCC_AHB3, + RCC_APB1, + RCC_APB2, #else RCC_AHB, RCC_APB2, @@ -112,6 +118,12 @@ enum rcc_reg { #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) +#elif defined(GD32F4) +#define RCC_AHB1(periph) RCC_ENCODE(RCC_AHB1, RCU_AHB1EN_ ## periph ## EN) +#define RCC_AHB2(periph) RCC_ENCODE(RCC_AHB2, RCU_AHB2EN_ ## periph ## EN) +#define RCC_AHB3(periph) RCC_ENCODE(RCC_AHB3, RCU_AHB3EN_ ## periph ## EN) +#define RCC_APB2(periph) RCC_ENCODE(RCC_APB2, RCU_APB2EN_ ## periph ## EN) +#define RCC_APB1(periph) RCC_ENCODE(RCC_APB1, RCU_APB1EN_ ## periph ## EN) #endif void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState); diff --git a/src/platform/common/stm32/pwm_output_dshot_shared.c b/src/platform/common/stm32/pwm_output_dshot_shared.c index 1bf57ee98d..28079badf0 100644 --- a/src/platform/common/stm32/pwm_output_dshot_shared.c +++ b/src/platform/common/stm32/pwm_output_dshot_shared.c @@ -243,6 +243,8 @@ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void) LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource); #elif defined(AT32F435) tmr_dma_request_enable(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, FALSE); +#elif defined(USE_GDBSP_DRIVER) + timer_dma_disable((uint32_t)dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource); #else TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE); #endif diff --git a/src/platform/common/stm32/serial_uart_hw.c b/src/platform/common/stm32/serial_uart_hw.c index 1103a80db6..6e427e56e1 100644 --- a/src/platform/common/stm32/serial_uart_hw.c +++ b/src/platform/common/stm32/serial_uart_hw.c @@ -62,6 +62,8 @@ static void enableRxIrq(const uartHardware_t *hardware) #elif defined(APM32F4) DAL_NVIC_SetPriority(hardware->irqn, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority)); DAL_NVIC_EnableIRQ(hardware->irqn); +#elif defined(GD32F4) + nvic_irq_enable(hardware->irqn, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority)); #else # error "Unhandled MCU type" #endif @@ -147,6 +149,13 @@ uartPort_t *serialUART(uartDevice_t *uartdev, uint32_t baudRate, portMode_e mode pushPull ? GPIO_OType_PP : GPIO_OType_OD, ((const unsigned[]){GPIO_PuPd_NOPULL, GPIO_PuPd_DOWN, GPIO_PuPd_UP})[pull] ); +#elif defined(GD32F4) + const ioConfig_t ioCfg = IO_CONFIG( + GPIO_MODE_AF, + GPIO_OSPEED_2MHZ, + pushPull ? GPIO_OTYPE_PP : GPIO_OTYPE_OD, + ((const unsigned[]){GPIO_PUPD_NONE, GPIO_PUPD_PULLDOWN, GPIO_PUPD_PULLUP})[pull] + ); #endif IOInit(txIO, ownerTxRx, ownerIndex); IOConfigGPIOAF(txIO, ioCfg, txAf); @@ -308,6 +317,8 @@ void uartEnableTxInterrupt(uartPort_t *uartPort) __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE); #elif defined(USE_ATBSP_DRIVER) usart_interrupt_enable(uartPort->USARTx, USART_TDBE_INT, TRUE); +#elif defined(USE_GDBSP_DRIVER) + usart_interrupt_enable((uint32_t)uartPort->USARTx, USART_INT_TBE); #else USART_ITConfig(uartPort->USARTx, USART_IT_TXE, ENABLE); #endif diff --git a/src/platform/common/stm32/system.c b/src/platform/common/stm32/system.c index a2424f20a8..e546b76b69 100644 --- a/src/platform/common/stm32/system.c +++ b/src/platform/common/stm32/system.c @@ -35,7 +35,7 @@ #include "drivers/system.h" -#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(AT32F4) || defined(APM32F4) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(AT32F4) || defined(APM32F4) || defined(GD32F4) // 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" @@ -62,6 +62,8 @@ void cycleCounterInit(void) crm_clocks_freq_type clocks; crm_clocks_freq_get(&clocks); cpuClockFrequency = clocks.sclk_freq; +#elif defined(USE_GDBSP_DRIVER) + cpuClockFrequency = rcu_clock_freq_get(CK_SYS); #else RCC_ClocksTypeDef clocks; RCC_GetClocksFreq(&clocks); @@ -73,7 +75,7 @@ void cycleCounterInit(void) CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; #if defined(DWT_LAR_UNLOCK_VALUE) -#if defined(STM32H7) || defined(AT32F4) +#if defined(STM32H7) || defined(AT32F4) || defined(GD32F4) ITM->LAR = DWT_LAR_UNLOCK_VALUE; #elif defined(STM32F7) DWT->LAR = DWT_LAR_UNLOCK_VALUE; @@ -366,6 +368,10 @@ const mcuTypeInfo_t *getMcuTypeInfo(void) { .id = MCU_TYPE_APM32F405, .name = "APM32F405" }, #elif defined(APM32F407) { .id = MCU_TYPE_APM32F407, .name = "APM32F407" }, +#elif defined(GD32F425) + { .id = MCU_TYPE_GD32F425, .name = "GD32F425" }, +#elif defined(GD32F460) + { .id = MCU_TYPE_GD32F460, .name = "GD32F460" }, #else #error MCU Type info not defined for STM (or clone) #endif