mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
update according to review, modify ADC, SPI define type
This commit is contained in:
parent
dbd56f6842
commit
af28f08bc7
10 changed files with 174 additions and 193 deletions
|
@ -55,25 +55,18 @@ uint8_t adcChannelByTag(ioTag_t ioTag)
|
|||
return 0;
|
||||
}
|
||||
|
||||
#if defined(USE_GDBSP_DRIVER)
|
||||
ADCDevice adcDeviceByInstance(const uint32_t instance)
|
||||
ADCDevice adcDeviceByInstance(const ADC_TypeDef *instance)
|
||||
{
|
||||
#if defined(USE_ADC_DEVICE_0)
|
||||
if (instance == ADC0) {
|
||||
return ADCDEV_0;
|
||||
}
|
||||
#endif
|
||||
|
||||
#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;
|
||||
|
|
|
@ -125,9 +125,5 @@ 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
|
||||
|
|
|
@ -130,12 +130,8 @@ 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.
|
||||
|
|
|
@ -45,14 +45,8 @@ 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;
|
||||
|
|
|
@ -70,11 +70,7 @@ 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
|
||||
|
|
|
@ -117,12 +117,13 @@ static void adcInitDevice(uint32_t adc_periph, int channelCount)
|
|||
|
||||
static void adcInitInternalInjected(const adcConfig_t *config)
|
||||
{
|
||||
uint32_t adc_periph = PERIPH_INT(ADC0);
|
||||
adc_channel_16_to_18(ADC_TEMP_VREF_CHANNEL_SWITCH, ENABLE);
|
||||
adc_discontinuous_mode_config(ADC0, ADC_CHANNEL_DISCON_DISABLE, 0);
|
||||
adc_channel_length_config(ADC0, ADC_INSERTED_CHANNEL, 2);
|
||||
adc_discontinuous_mode_config(adc_periph, ADC_CHANNEL_DISCON_DISABLE, 0);
|
||||
adc_channel_length_config(adc_periph, ADC_INSERTED_CHANNEL, 2);
|
||||
|
||||
adc_inserted_channel_config(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
|
||||
adc_inserted_channel_config(adc_periph, 1, ADC_CHANNEL_17, ADC_SAMPLETIME_480); // ADC_Channel_Vrefint
|
||||
adc_inserted_channel_config(adc_periph, 0, ADC_CHANNEL_16, ADC_SAMPLETIME_480); // ADC_Channel_TempSensor
|
||||
|
||||
adcVREFINTCAL = config->vrefIntCalibration ? config->vrefIntCalibration : VREFINT_EXPECTED;
|
||||
adcTSCAL1 = config->tempSensorCalibration1 ? config->tempSensorCalibration1 : ((TEMPSENSOR_CAL1_V * 4095.0f) / 3.3f);
|
||||
|
@ -143,7 +144,7 @@ static bool adcInternalConversionInProgress = false;
|
|||
bool adcInternalIsBusy(void)
|
||||
{
|
||||
if (adcInternalConversionInProgress) {
|
||||
if (adc_flag_get(ADC0, ADC_FLAG_EOIC) != RESET) {
|
||||
if (adc_flag_get(PERIPH_INT(ADC0), ADC_FLAG_EOIC) != RESET) {
|
||||
adcInternalConversionInProgress = false;
|
||||
}
|
||||
}
|
||||
|
@ -153,20 +154,21 @@ bool adcInternalIsBusy(void)
|
|||
|
||||
void adcInternalStartConversion(void)
|
||||
{
|
||||
adc_flag_clear(ADC0, ADC_FLAG_EOIC);
|
||||
adc_software_trigger_enable(ADC0, ADC_INSERTED_CHANNEL);
|
||||
uint32_t adc_periph = PERIPH_INT(ADC0);
|
||||
adc_flag_clear(adc_periph, ADC_FLAG_EOIC);
|
||||
adc_software_trigger_enable(adc_periph, ADC_INSERTED_CHANNEL);
|
||||
|
||||
adcInternalConversionInProgress = true;
|
||||
}
|
||||
|
||||
uint16_t adcInternalReadVrefint(void)
|
||||
{
|
||||
return adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_1);
|
||||
return adc_inserted_data_read(PERIPH_INT(ADC0), ADC_INSERTED_CHANNEL_1);
|
||||
}
|
||||
|
||||
uint16_t adcInternalReadTempsensor(void)
|
||||
{
|
||||
return adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_0);
|
||||
return adc_inserted_data_read(PERIPH_INT(ADC0), ADC_INSERTED_CHANNEL_0);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -230,11 +232,12 @@ void adcInit(const adcConfig_t *config)
|
|||
adc_sync_delay_config(ADC_SYNC_DELAY_5CYCLE);
|
||||
|
||||
#ifdef USE_ADC_INTERNAL
|
||||
uint32_t adc_periph = PERIPH_INT(ADC0);
|
||||
// If device is not ADC0 or there's no active channel, then initialize ADC0 separately
|
||||
if (device != ADCDEV_0 || !adcActive) {
|
||||
RCC_ClockCmd(adcHardware[ADCDEV_0].rccADC, ENABLE);
|
||||
adcInitDevice(ADC0, 2);
|
||||
adc_enable(ADC0);
|
||||
adcInitDevice(adc_periph, 2);
|
||||
adc_enable(adc_periph);
|
||||
}
|
||||
|
||||
// Initialize for injected conversion
|
||||
|
|
|
@ -54,7 +54,7 @@ static spi_parameter_struct defaultInit = {
|
|||
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) {
|
||||
if (instance == SPI1 || instance == SPI2) {
|
||||
divisor /= 2; // Safe for divisor == 0 or 1
|
||||
}
|
||||
|
||||
|
@ -66,8 +66,9 @@ static uint32_t spiDivisorToBRbits(const SPI_TypeDef *instance, uint16_t divisor
|
|||
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));
|
||||
uint32_t spi_periph = PERIPH_INT(instance);
|
||||
const uint32_t tempRegister = (SPI_CTL0(spi_periph) & ~BR_BITS);
|
||||
SPI_CTL0(spi_periph) = (tempRegister | (spiDivisorToBRbits(instance, divisor) & BR_BITS));
|
||||
#undef BR_BITS
|
||||
}
|
||||
|
||||
|
@ -91,14 +92,15 @@ void spiInitDevice(SPIDevice device)
|
|||
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_SDI_CFG, spi->af);
|
||||
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
|
||||
|
||||
uint32_t spi_periph = PERIPH_INT(spi->dev);
|
||||
// Init SPI hardware
|
||||
spi_i2s_deinit((uint32_t)spi->dev);
|
||||
spi_i2s_deinit(spi_periph);
|
||||
|
||||
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);
|
||||
spi_dma_disable(spi_periph, SPI_DMA_TRANSMIT);
|
||||
spi_dma_disable(spi_periph, SPI_DMA_RECEIVE);
|
||||
spi_init(spi_periph, &defaultInit);
|
||||
spi_crc_off(spi_periph);
|
||||
spi_enable(spi_periph);
|
||||
}
|
||||
|
||||
void spiInternalResetDescriptors(busDevice_t *bus)
|
||||
|
@ -108,10 +110,11 @@ void spiInternalResetDescriptors(busDevice_t *bus)
|
|||
dmaGenerInitTx->sub_periph = bus->dmaTx->channel;
|
||||
dma_single_data_parameter_struct *dmaInitTx = &dmaGenerInitTx->config.init_struct_s;
|
||||
|
||||
uint32_t spi_periph = PERIPH_INT(bus->busType_u.spi.instance);
|
||||
dma_single_data_para_struct_init(dmaInitTx);
|
||||
dmaInitTx->direction = DMA_MEMORY_TO_PERIPH;
|
||||
dmaInitTx->circular_mode = DMA_CIRCULAR_MODE_DISABLE;
|
||||
dmaInitTx->periph_addr = (uint32_t)&SPI_DATA((uint32_t)(bus->busType_u.spi.instance));
|
||||
dmaInitTx->periph_addr = (uint32_t)&SPI_DATA(spi_periph);
|
||||
dmaInitTx->priority = DMA_PRIORITY_LOW;
|
||||
dmaInitTx->periph_inc = DMA_PERIPH_INCREASE_DISABLE;
|
||||
dmaInitTx->periph_memory_width = DMA_PERIPH_WIDTH_8BIT;
|
||||
|
@ -125,7 +128,7 @@ void spiInternalResetDescriptors(busDevice_t *bus)
|
|||
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->periph_addr = (uint32_t)&SPI_DATA(spi_periph);
|
||||
dmaInitRx->priority = DMA_PRIORITY_LOW;
|
||||
dmaInitRx->periph_inc = DMA_PERIPH_INCREASE_DISABLE;
|
||||
dmaInitRx->periph_memory_width = DMA_PERIPH_WIDTH_8BIT;
|
||||
|
@ -146,14 +149,15 @@ void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
|
|||
bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
|
||||
{
|
||||
uint8_t b;
|
||||
uint32_t spi_periph = PERIPH_INT(instance);
|
||||
|
||||
while (len--) {
|
||||
b = txData ? *(txData++) : 0xFF;
|
||||
while (spi_i2s_flag_get((uint32_t)instance, I2S_FLAG_TBE) == RESET);
|
||||
spi_i2s_data_transmit((uint32_t)instance, b);
|
||||
while (spi_i2s_flag_get(spi_periph, I2S_FLAG_TBE) == RESET);
|
||||
spi_i2s_data_transmit(spi_periph, b);
|
||||
|
||||
while (spi_i2s_flag_get((uint32_t)instance, I2S_FLAG_RBNE) == RESET);
|
||||
b = spi_i2s_data_receive((uint32_t)instance);
|
||||
while (spi_i2s_flag_get(spi_periph, I2S_FLAG_RBNE) == RESET);
|
||||
b = spi_i2s_data_receive(spi_periph);
|
||||
if (rxData) {
|
||||
*(rxData++) = b;
|
||||
}
|
||||
|
@ -227,9 +231,11 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
|
|||
|
||||
void spiInternalStartDMA(const extDevice_t *dev)
|
||||
{
|
||||
uint32_t spi_periph = PERIPH_INT(dev->bus->busType_u.spi.instance);
|
||||
dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
|
||||
dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
|
||||
DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
|
||||
|
||||
if (dmaRx) {
|
||||
DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
|
||||
|
||||
|
@ -264,8 +270,8 @@ void spiInternalStartDMA(const extDevice_t *dev)
|
|||
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);
|
||||
spi_dma_enable(spi_periph, SPI_DMA_TRANSMIT);
|
||||
spi_dma_enable(spi_periph, SPI_DMA_RECEIVE);
|
||||
} else {
|
||||
// Use the correct callback argument
|
||||
dmaTx->userParam = (uint32_t)dev;
|
||||
|
@ -285,7 +291,7 @@ void spiInternalStartDMA(const extDevice_t *dev)
|
|||
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);
|
||||
spi_dma_enable(spi_periph, SPI_DMA_TRANSMIT);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -293,7 +299,7 @@ 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;
|
||||
uint32_t spi_periph = PERIPH_INT(dev->bus->busType_u.spi.instance);
|
||||
DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
|
||||
|
||||
if (dmaRx) {
|
||||
|
@ -303,21 +309,21 @@ void spiInternalStopDMA (const extDevice_t *dev)
|
|||
REG32(streamRegsTx) = 0U;
|
||||
REG32(streamRegsRx) = 0U;
|
||||
|
||||
spi_dma_disable((uint32_t)instance, SPI_DMA_TRANSMIT);
|
||||
spi_dma_disable((uint32_t)instance, SPI_DMA_RECEIVE);
|
||||
spi_dma_disable(spi_periph, SPI_DMA_TRANSMIT);
|
||||
spi_dma_disable(spi_periph, SPI_DMA_RECEIVE);
|
||||
} else {
|
||||
// Ensure the current transmission is complete
|
||||
while (spi_i2s_flag_get((uint32_t)instance, I2S_FLAG_TRANS));
|
||||
while (spi_i2s_flag_get(spi_periph, I2S_FLAG_TRANS));
|
||||
|
||||
// Drain the RX buffer
|
||||
while (spi_i2s_flag_get((uint32_t)instance, I2S_FLAG_RBNE)) {
|
||||
SPI_DATA((uint32_t)instance);
|
||||
while (spi_i2s_flag_get(spi_periph, I2S_FLAG_RBNE)) {
|
||||
SPI_DATA(spi_periph);
|
||||
}
|
||||
|
||||
// Disable stream
|
||||
REG32(streamRegsTx) = 0U;
|
||||
|
||||
spi_dma_disable((uint32_t)instance, SPI_DMA_TRANSMIT);
|
||||
spi_dma_disable(spi_periph, SPI_DMA_TRANSMIT);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -325,14 +331,14 @@ void spiInternalStopDMA (const extDevice_t *dev)
|
|||
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;
|
||||
uint32_t spi_periph = PERIPH_INT(bus->busType_u.spi.instance);
|
||||
|
||||
dev->bus->initSegment = true;
|
||||
|
||||
spi_disable((uint32_t)instance);
|
||||
spi_disable(spi_periph);
|
||||
|
||||
// Switch bus speed
|
||||
if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) {
|
||||
|
@ -342,19 +348,19 @@ void spiSequenceStart(const extDevice_t *dev)
|
|||
|
||||
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);
|
||||
SPI_CTL0(spi_periph) &= ~(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;
|
||||
SPI_CTL0(spi_periph) |= SPI_CK_PL_LOW_PH_1EDGE;
|
||||
} else
|
||||
{
|
||||
SPI_CTL0((uint32_t)instance) |= SPI_CK_PL_HIGH_PH_2EDGE;
|
||||
SPI_CTL0(spi_periph) |= SPI_CK_PL_HIGH_PH_2EDGE;
|
||||
}
|
||||
bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge;
|
||||
}
|
||||
|
||||
spi_enable((uint32_t)instance);
|
||||
spi_enable(spi_periph);
|
||||
|
||||
// Check that any there are no attempts to DMA to/from CCD SRAM
|
||||
for (busSegment_t *checkSegment = (busSegment_t *)bus->curSegment; checkSegment->len; checkSegment++) {
|
||||
|
|
|
@ -275,3 +275,42 @@ extern uint32_t timerPrescaler(const TIM_TypeDef *tim);
|
|||
#define _UART_GET_PREFIX_UARTDEV_9 UART
|
||||
#define _UART_GET_PREFIX_UARTDEV_10 USART
|
||||
#define _UART_GET_PREFIX_UARTDEV_LP1 LPUART
|
||||
|
||||
// #if defined(GD32F4)
|
||||
// We need to redefine ADC0, ADC1, etc.,
|
||||
// in the GD firmware library to be compatible with
|
||||
// such as the ADC_TypeDef * type in BF.
|
||||
#define GD_ADC0 ((ADC_TypeDef*)ADC_BASE)
|
||||
#define GD_ADC1 ((ADC_TypeDef*)(ADC_BASE + 0x100))
|
||||
#define GD_ADC2 ((ADC_TypeDef*)(ADC_BASE + 0x200))
|
||||
#undef ADC0
|
||||
#define ADC0 ((ADC_TypeDef*)GD_ADC0)
|
||||
#undef ADC1
|
||||
#define ADC1 ((ADC_TypeDef*)GD_ADC1)
|
||||
#undef ADC2
|
||||
#define ADC2 ((ADC_TypeDef*)GD_ADC2)
|
||||
|
||||
#define GD_SPI0 ((SPI_TypeDef*)(SPI_BASE + 0x0000F800U))
|
||||
#define GD_SPI1 ((SPI_TypeDef*)SPI_BASE)
|
||||
#define GD_SPI2 ((SPI_TypeDef*)(SPI_BASE + 0x00000400U))
|
||||
#define GD_SPI3 ((SPI_TypeDef*)(SPI_BASE + 0x0000FC00U))
|
||||
#define GD_SPI4 ((SPI_TypeDef*)(SPI_BASE + 0x00011800U))
|
||||
#define GD_SPI5 ((SPI_TypeDef*)(SPI_BASE + 0x00011C00U))
|
||||
#undef SPI0
|
||||
#define SPI0 ((SPI_TypeDef*)GD_SPI0)
|
||||
#undef SPI1
|
||||
#define SPI1 ((SPI_TypeDef*)GD_SPI1)
|
||||
#undef SPI2
|
||||
#define SPI2 ((SPI_TypeDef*)GD_SPI2)
|
||||
#undef SPI3
|
||||
#define SPI3 ((SPI_TypeDef*)GD_SPI3)
|
||||
#undef SPI4
|
||||
#define SPI4 ((SPI_TypeDef*)GD_SPI4)
|
||||
#undef SPI5
|
||||
#define SPI5 ((SPI_TypeDef*)GD_SPI5)
|
||||
|
||||
|
||||
// We also need to convert the pointer to the uint32_t
|
||||
// type required by the GD firmware library.
|
||||
#define PERIPH_INT(periph) ((uint32_t)periph)
|
||||
// #endif
|
||||
|
|
|
@ -66,7 +66,7 @@ bool IORead(IO_t io)
|
|||
return false;
|
||||
}
|
||||
|
||||
return (GPIO_ISTAT((uint32_t)IO_GPIO(io)) & IO_Pin(io));
|
||||
return (GPIO_ISTAT(PERIPH_INT(IO_GPIO(io))) & IO_Pin(io));
|
||||
}
|
||||
|
||||
void IOWrite(IO_t io, bool hi)
|
||||
|
@ -76,9 +76,9 @@ void IOWrite(IO_t io, bool hi)
|
|||
}
|
||||
|
||||
if (hi) {
|
||||
GPIO_BOP((uint32_t)IO_GPIO(io)) = IO_Pin(io);
|
||||
GPIO_BOP(PERIPH_INT(IO_GPIO(io))) = IO_Pin(io);
|
||||
} else {
|
||||
GPIO_BC((uint32_t)IO_GPIO(io)) = IO_Pin(io);
|
||||
GPIO_BC(PERIPH_INT(IO_GPIO(io))) = IO_Pin(io);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -88,7 +88,7 @@ void IOHi(IO_t io)
|
|||
return;
|
||||
}
|
||||
|
||||
GPIO_BOP((uint32_t)IO_GPIO(io)) = IO_Pin(io);
|
||||
GPIO_BOP(PERIPH_INT(IO_GPIO(io))) = IO_Pin(io);
|
||||
}
|
||||
|
||||
void IOLo(IO_t io)
|
||||
|
@ -97,7 +97,7 @@ void IOLo(IO_t io)
|
|||
return;
|
||||
}
|
||||
#if defined(GD32F4)
|
||||
GPIO_BC((uint32_t)IO_GPIO(io)) = IO_Pin(io);
|
||||
GPIO_BC(PERIPH_INT(IO_GPIO(io))) = IO_Pin(io);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -110,7 +110,7 @@ void IOToggle(IO_t io)
|
|||
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;
|
||||
GPIO_TG(PERIPH_INT(IO_GPIO(io))) = mask;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -125,8 +125,8 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg)
|
|||
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));
|
||||
gpio_mode_set(PERIPH_INT(IO_GPIO(io)), ((cfg >> 0) & 0x03), ((cfg >> 5) & 0x03), IO_Pin(io));
|
||||
gpio_output_options_set(PERIPH_INT(IO_GPIO(io)), ((cfg >> 4) & 0x01), ((cfg >> 2) & 0x03), IO_Pin(io));
|
||||
}
|
||||
|
||||
void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
|
||||
|
@ -138,9 +138,9 @@ void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
|
|||
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));
|
||||
gpio_af_set(PERIPH_INT(IO_GPIO(io)), af, IO_Pin(io));
|
||||
gpio_mode_set(PERIPH_INT(IO_GPIO(io)), ((cfg >> 0) & 0x03), ((cfg >> 5) & 0x03), IO_Pin(io));
|
||||
gpio_output_options_set(PERIPH_INT(IO_GPIO(io)), ((cfg >> 4) & 0x01), ((cfg >> 2) & 0x03), IO_Pin(io));
|
||||
}
|
||||
|
||||
#else
|
||||
|
|
|
@ -209,10 +209,10 @@ typedef enum {
|
|||
|
||||
|
||||
static SD_Handle_t SD_Handle;
|
||||
SD_CardInfo_t SD_CardInfo;
|
||||
static SD_CardInfo_t SD_CardInfo;
|
||||
static uint32_t SD_Status;
|
||||
static uint32_t SD_CardRCA;
|
||||
SD_CardType_t SD_CardType;
|
||||
static SD_CardType_t SD_CardType;
|
||||
static volatile uint32_t TimeOut;
|
||||
static DMA_Stream_TypeDef *dmaStream;
|
||||
static uint32_t dma_periph_sdio;
|
||||
|
@ -1455,6 +1455,71 @@ void SDIO_IRQHandler(void)
|
|||
SDIO_INTEN_RXOREIE);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief Common handler for DMA stream interrupt requests
|
||||
\param[in] dma_periph: DMA peripheral (DMA0 or DMA1)
|
||||
\param[in] dma_channel: DMA channel (DMA_CH0 ~ DMA_CH7)
|
||||
\param[out] none
|
||||
\retval none
|
||||
*/
|
||||
static void SDIO_DMA_IRQHandler_Common(uint32_t dma_periph, dma_channel_enum dma_channel)
|
||||
{
|
||||
// Transfer Error Interrupt management
|
||||
if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_TAE)) {
|
||||
if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXCTL_TAEIE)) {
|
||||
dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXCTL_TAEIE);
|
||||
dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_TAE);
|
||||
}
|
||||
}
|
||||
|
||||
// FIFO Error Interrupt management
|
||||
if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_FEE)) {
|
||||
if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXFCTL_FEEIE)) {
|
||||
dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXFCTL_FEEIE);
|
||||
dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_FEE);
|
||||
}
|
||||
}
|
||||
|
||||
// Single data mode exception flag
|
||||
if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_SDE)) {
|
||||
if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXCTL_SDEIE)) {
|
||||
dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXCTL_SDEIE);
|
||||
dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_SDE);
|
||||
}
|
||||
}
|
||||
|
||||
// Half Transfer Complete Interrupt management
|
||||
if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_HTF)) {
|
||||
if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXCTL_HTFIE)) {
|
||||
if(dma_single_data_mode_get(dma_periph, dma_channel) != RESET) {
|
||||
dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_HTF);
|
||||
} else {
|
||||
if(dma_circulation_get(dma_periph, dma_channel) == RESET) {
|
||||
dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXCTL_HTFIE);
|
||||
}
|
||||
|
||||
dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_HTF);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Transfer Complete Interrupt management
|
||||
if(dma_interrupt_flag_get(dma_periph, dma_channel, DMA_INT_FLAG_FTF)) {
|
||||
if(dma_interrupt_enable_get(dma_periph, dma_channel, DMA_CHXCTL_FTFIE)) {
|
||||
if(dma_single_data_mode_get(dma_periph, dma_channel) != RESET) {
|
||||
dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_FTF);
|
||||
} else {
|
||||
if(dma_circulation_get(dma_periph, dma_channel) == RESET) {
|
||||
dma_interrupt_disable(dma_periph, dma_channel, DMA_CHXCTL_FTFIE);
|
||||
}
|
||||
|
||||
dma_interrupt_flag_clear(dma_periph, dma_channel, DMA_INT_FLAG_FTF);
|
||||
SD_DMA_Complete(dma_periph, dma_channel);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief This function handles DMA2 Stream 3 interrupt request
|
||||
\param[in] dma: DMA channel descriptor
|
||||
|
@ -1464,60 +1529,7 @@ void SDIO_IRQHandler(void)
|
|||
void SDIO_DMA_ST3_IRQHandler(dmaChannelDescriptor_t *dma)
|
||||
{
|
||||
UNUSED(dma);
|
||||
// Transfer Error Interrupt management
|
||||
if((DMA_INTF0(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_TAE, DMA_CH3)) != 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_FLAG_ADD(DMA_INT_FLAG_TAE, DMA_CH3); // Clear the transfer error flag
|
||||
}
|
||||
}
|
||||
|
||||
// FIFO Error Interrupt management
|
||||
if((DMA_INTF0(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_FEE, DMA_CH3)) != 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_FLAG_ADD(DMA_INT_FLAG_FEE, DMA_CH3); // Clear the FIFO error flag
|
||||
}
|
||||
}
|
||||
|
||||
// Single data mode exception flag
|
||||
if((DMA_INTF0(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_SDE, DMA_CH3)) != 0) {
|
||||
if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_SDEIE) != 0) {
|
||||
DMA_CHCTL(DMA1, DMA_CH3) &= ~DMA_CHXCTL_SDEIE; // Disable the single data mode Error interrupt
|
||||
DMA_INTC0(DMA1) = DMA_FLAG_ADD(DMA_INT_FLAG_SDE, DMA_CH3);
|
||||
}
|
||||
}
|
||||
|
||||
// Half Transfer Complete Interrupt management
|
||||
if((DMA_INTF0(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_HTF, DMA_CH3)) != 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_FLAG_ADD(DMA_INT_FLAG_HTF, DMA_CH3);
|
||||
} else {
|
||||
if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_CMEN) == 0) {
|
||||
DMA_CHCTL(DMA1, DMA_CH3) &= ~DMA_CHXCTL_HTFIE;
|
||||
}
|
||||
|
||||
DMA_INTC0(DMA1) = DMA_FLAG_ADD(DMA_INT_FLAG_HTF, DMA_CH3);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Transfer Complete Interrupt management
|
||||
if((DMA_INTF0(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_FTF, DMA_CH3)) != 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_FLAG_ADD(DMA_INT_FLAG_FTF, DMA_CH3);
|
||||
} else {
|
||||
if((DMA_CHCTL(DMA1, DMA_CH3) & DMA_CHXCTL_CMEN) == 0) {
|
||||
DMA_CHCTL(DMA1, DMA_CH3) &= ~DMA_CHXCTL_FTFIE;
|
||||
}
|
||||
|
||||
DMA_INTC0(DMA1) = DMA_FLAG_ADD(DMA_INT_FLAG_FTF, DMA_CH3);
|
||||
SD_DMA_Complete(DMA1, DMA_CH3);
|
||||
}
|
||||
}
|
||||
}
|
||||
SDIO_DMA_IRQHandler_Common(DMA1, DMA_CH3);
|
||||
}
|
||||
|
||||
/*!
|
||||
|
@ -1529,61 +1541,7 @@ void SDIO_DMA_ST3_IRQHandler(dmaChannelDescriptor_t *dma)
|
|||
void SDIO_DMA_ST6_IRQHandler(dmaChannelDescriptor_t *dma)
|
||||
{
|
||||
UNUSED(dma);
|
||||
|
||||
// Transfer Error Interrupt management
|
||||
if((DMA_INTF1(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_TAE, (DMA_CH6-DMA_CH4))) != 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_FLAG_ADD(DMA_INT_FLAG_TAE, (DMA_CH6-DMA_CH4));
|
||||
}
|
||||
}
|
||||
|
||||
// FIFO Error Interrupt management
|
||||
if((DMA_INTF1(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_FEE, (DMA_CH6-DMA_CH4))) != 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_FLAG_ADD(DMA_INT_FLAG_FEE, (DMA_CH6-DMA_CH4));
|
||||
}
|
||||
}
|
||||
|
||||
// Single data mode exception flag
|
||||
if((DMA_INTF1(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_SDE, (DMA_CH6-DMA_CH4))) != 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_FLAG_ADD(DMA_INT_FLAG_SDE, (DMA_CH6-DMA_CH4));
|
||||
}
|
||||
}
|
||||
|
||||
// Half Transfer Complete Interrupt management
|
||||
if((DMA_INTF1(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_HTF, (DMA_CH6-DMA_CH4))) != 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_FLAG_ADD(DMA_INT_FLAG_HTF, (DMA_CH6-DMA_CH4));
|
||||
} 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_FLAG_ADD(DMA_INT_FLAG_HTF, (DMA_CH6-DMA_CH4));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Transfer Complete Interrupt management
|
||||
if((DMA_INTF1(DMA1) & DMA_FLAG_ADD(DMA_INT_FLAG_FTF, (DMA_CH6-DMA_CH4))) != 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_FLAG_ADD(DMA_INT_FLAG_FTF, (DMA_CH6-DMA_CH4));
|
||||
} 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_FLAG_ADD(DMA_INT_FLAG_FTF, (DMA_CH6-DMA_CH4));
|
||||
SD_DMA_Complete(DMA1, DMA_CH6);
|
||||
}
|
||||
}
|
||||
}
|
||||
SDIO_DMA_IRQHandler_Common(DMA1, DMA_CH6);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue