diff --git a/make/source.mk b/make/source.mk
index 016656edeb..6f38d98bb7 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -27,6 +27,7 @@ COMMON_SRC = \
drivers/buttons.c \
drivers/display.c \
drivers/display_canvas.c \
+ drivers/dma_common.c \
drivers/dma_reqmap.c \
drivers/exti.c \
drivers/io.c \
diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c
index 69061c78d0..ff3b5341e3 100644
--- a/src/main/drivers/adc_stm32f10x.c
+++ b/src/main/drivers/adc_stm32f10x.c
@@ -67,7 +67,6 @@ const adcTagMap_t adcTagMap[] = {
void adcInit(const adcConfig_t *config)
{
-
uint8_t configuredAdcChannels = 0;
memset(&adcOperatingConfig, 0, sizeof(adcOperatingConfig));
@@ -89,11 +88,16 @@ void adcInit(const adcConfig_t *config)
}
ADCDevice device = adcDeviceByInstance(ADC_INSTANCE);
- if (device == ADCINVALID)
+ if (device == ADCINVALID) {
return;
+ }
const adcDevice_t adc = adcHardware[device];
+ if (!dmaAllocate(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0)) {
+ return;
+ }
+
bool adcActive = false;
for (int i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcOperatingConfig[i].tag)
@@ -115,7 +119,7 @@ void adcInit(const adcConfig_t *config)
RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI)
RCC_ClockCmd(adc.rccADC, ENABLE);
- dmaInit(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0);
+ dmaEnable(dmaGetIdentifier(adc.dmaResource));
xDMA_DeInit(adc.dmaResource);
DMA_InitTypeDef DMA_InitStructure;
diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c
index b4f521722f..59a5254c18 100644
--- a/src/main/drivers/adc_stm32f30x.c
+++ b/src/main/drivers/adc_stm32f30x.c
@@ -176,15 +176,19 @@ void adcInit(const adcConfig_t *config)
#if defined(USE_DMA_SPEC)
const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, device, config->dmaopt[device]);
- if (!dmaSpec) {
+ if (!dmaSpec || !dmaAllocate(dmaGetIdentifier(dmaSpec->ref), OWNER_ADC, RESOURCE_INDEX(device))) {
return;
}
- dmaInit(dmaGetIdentifier(dmaSpec->ref), OWNER_ADC, RESOURCE_INDEX(device));
+ dmaEnable(dmaGetIdentifier(dmaSpec->ref));
DMA_DeInit((DMA_ARCH_TYPE *)dmaSpec->ref);
#else
- dmaInit(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0);
+ if (!dmaAllocate(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0)) {
+ return;
+ }
+
+ dmaEnable(dmaGetIdentifier(adc.dmaResource));
xDMA_DeInit(adc.dmaResource);
#endif
diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c
index 229eff6834..566b4bc8d0 100644
--- a/src/main/drivers/adc_stm32f4xx.c
+++ b/src/main/drivers/adc_stm32f4xx.c
@@ -304,15 +304,19 @@ void adcInit(const adcConfig_t *config)
#ifdef USE_DMA_SPEC
const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, device, config->dmaopt[device]);
- if (!dmaSpec) {
+ if (!dmaSpec || !dmaAllocate(dmaGetIdentifier(dmaSpec->ref), OWNER_ADC, RESOURCE_INDEX(device))) {
return;
}
- dmaInit(dmaGetIdentifier(dmaSpec->ref), OWNER_ADC, RESOURCE_INDEX(device));
+ dmaEnable(dmaGetIdentifier(dmaSpec->ref));
xDMA_DeInit(dmaSpec->ref);
#else
- dmaInit(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0);
+ if (!dmaAllocate(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0)) {
+ return;
+ }
+
+ dmaEnable(dmaGetIdentifier(adc.dmaResource));
xDMA_DeInit(adc.dmaResource);
#endif
diff --git a/src/main/drivers/adc_stm32f7xx.c b/src/main/drivers/adc_stm32f7xx.c
index 9b0df369ab..0170831fd0 100644
--- a/src/main/drivers/adc_stm32f7xx.c
+++ b/src/main/drivers/adc_stm32f7xx.c
@@ -288,15 +288,18 @@ void adcInit(const adcConfig_t *config)
#ifdef USE_DMA_SPEC
const dmaChannelSpec_t *dmaspec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, device, config->dmaopt[device]);
- if (!dmaspec) {
+ if (!dmaspec || !dmaAllocate(dmaGetIdentifier(dmaspec->ref), OWNER_ADC, 0)) {
return;
}
- dmaInit(dmaGetIdentifier(dmaspec->ref), OWNER_ADC, 0);
+ dmaEnable(dmaGetIdentifier(dmaspec->ref));
adc.DmaHandle.Init.Channel = dmaspec->channel;
adc.DmaHandle.Instance = (DMA_ARCH_TYPE *)dmaspec->ref;
#else
- dmaInit(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0);
+ if (!dmaAllocate(dmaGetIdentifier(adc.dmaResource), OWNER_ADC, 0)) {
+ return;
+ }
+ dmaEnable(dmaGetIdentifier(adc.dmaResource));
adc.DmaHandle.Init.Channel = adc.channel;
adc.DmaHandle.Instance = (DMA_ARCH_TYPE *)adc.dmaResource;
#endif
diff --git a/src/main/drivers/adc_stm32g4xx.c b/src/main/drivers/adc_stm32g4xx.c
index f9a308fda5..4472c79ce2 100644
--- a/src/main/drivers/adc_stm32g4xx.c
+++ b/src/main/drivers/adc_stm32g4xx.c
@@ -407,17 +407,16 @@ void adcInit(const adcConfig_t *config)
// Configure DMA for this ADC peripheral
- dmaIdentifier_e dmaIdentifier;
#ifdef USE_DMA_SPEC
const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, dev, config->dmaopt[dev]);
- if (!dmaSpec) {
+ dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaSpec->ref);
+ if (!dmaSpec || !dmaAllocate(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev))) {
return;
}
adc->DmaHandle.Instance = (DMA_ARCH_TYPE *)dmaSpec->ref;
adc->DmaHandle.Init.Request = dmaSpec->channel;
- dmaIdentifier = dmaGetIdentifier(dmaSpec->ref);
#else
dmaIdentifier = dmaGetIdentifier(adc->dmaResource);
adc->DmaHandle.Instance = (DMA_ARCH_TYPE *)adc->dmaResource;
@@ -433,9 +432,9 @@ void adcInit(const adcConfig_t *config)
// Deinitialize & Initialize the DMA for new transfer
- // dmaInit must be called before calling HAL_DMA_Init,
+ // dmaEnable must be called before calling HAL_DMA_Init,
// to enable clock for associated DMA if not already done so.
- dmaInit(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev));
+ dmaEnable(dmaIdentifier);
HAL_DMA_DeInit(&adc->DmaHandle);
HAL_DMA_Init(&adc->DmaHandle);
diff --git a/src/main/drivers/adc_stm32h7xx.c b/src/main/drivers/adc_stm32h7xx.c
index 435004f0a7..e4704e227e 100644
--- a/src/main/drivers/adc_stm32h7xx.c
+++ b/src/main/drivers/adc_stm32h7xx.c
@@ -400,19 +400,23 @@ void adcInit(const adcConfig_t *config)
// Configure DMA for this ADC peripheral
- dmaIdentifier_e dmaIdentifier;
#ifdef USE_DMA_SPEC
const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_ADC, dev, config->dmaopt[dev]);
+ dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaSpec->ref);
- if (!dmaSpec) {
+ if (!dmaSpec || !dmaAllocate(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev))) {
return;
}
adc->DmaHandle.Instance = dmaSpec->ref;
adc->DmaHandle.Init.Request = dmaSpec->channel;
- dmaIdentifier = dmaGetIdentifier(dmaSpec->ref);
#else
- dmaIdentifier = dmaGetIdentifier(adc->dmaResource);
+ dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(adc->dmaResource);
+
+ if (!dmaAllocate(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev))) {
+ return;
+ }
+
adc->DmaHandle.Instance = (DMA_ARCH_TYPE *)adc->dmaResource;
adc->DmaHandle.Init.Request = adc->channel;
#endif
@@ -426,9 +430,9 @@ void adcInit(const adcConfig_t *config)
// Deinitialize & Initialize the DMA for new transfer
- // dmaInit must be called before calling HAL_DMA_Init,
+ // dmaEnable must be called before calling HAL_DMA_Init,
// to enable clock for associated DMA if not already done so.
- dmaInit(dmaIdentifier, OWNER_ADC, RESOURCE_INDEX(dev));
+ dmaEnable(dmaIdentifier);
HAL_DMA_DeInit(&adc->DmaHandle);
HAL_DMA_Init(&adc->DmaHandle);
diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c
index 5521183adf..e033872d88 100644
--- a/src/main/drivers/dma.c
+++ b/src/main/drivers/dma.c
@@ -32,7 +32,7 @@
/*
* DMA descriptors.
*/
-static dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
+dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
DEFINE_DMA_CHANNEL(DMA1, 1, 0),
DEFINE_DMA_CHANNEL(DMA1, 2, 4),
DEFINE_DMA_CHANNEL(DMA1, 3, 8),
@@ -89,13 +89,11 @@ uint32_t dmaFlag_IT_TCIF(const dmaResource_t *channel)
}
#define DMA_RCC(x) ((x) == DMA1 ? RCC_AHBPeriph_DMA1 : RCC_AHBPeriph_DMA2)
-void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex)
+dmaEnable(dmaIdentifier_e identifier) {
{
const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
RCC_AHBPeriphClockCmd(DMA_RCC(dmaDescriptors[index].dma), ENABLE);
- dmaDescriptors[index].owner.owner = owner;
- dmaDescriptors[index].owner.resourceIndex = resourceIndex;
}
void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
@@ -115,29 +113,4 @@ void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callbac
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
-
-const resourceOwner_t *dmaGetOwner(dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].owner;
-}
-
-dmaIdentifier_e dmaGetIdentifier(const dmaResource_t* channel)
-{
- for (int i = 0; i < DMA_LAST_HANDLER; i++) {
- if (dmaDescriptors[i].ref == channel) {
- return i + 1;
- }
- }
- return 0;
-}
-
-dmaResource_t *dmaGetRefByIdentifier(const dmaIdentifier_e identifier)
-{
- return dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].ref;
-}
-
-dmaChannelDescriptor_t* dmaGetDescriptorByIdentifier(const dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)];
-}
#endif
diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h
index ba7252259b..8a70031996 100644
--- a/src/main/drivers/dma.h
+++ b/src/main/drivers/dma.h
@@ -123,11 +123,6 @@ typedef enum {
#define DMA_IT_DMEIF ((uint32_t)0x00000004)
#define DMA_IT_FEIF ((uint32_t)0x00000001)
-dmaIdentifier_e dmaGetIdentifier(const dmaResource_t *stream);
-dmaChannelDescriptor_t* dmaGetDmaDescriptor(const dmaResource_t *stream);
-dmaResource_t *dmaGetRefByIdentifier(const dmaIdentifier_e identifier);
-uint32_t dmaGetChannel(const uint8_t channel);
-
#else
#if defined(STM32G4)
@@ -221,9 +216,6 @@ typedef enum {
#define DMA_IT_HTIF ((uint32_t)0x00000004)
#define DMA_IT_TEIF ((uint32_t)0x00000008)
-dmaIdentifier_e dmaGetIdentifier(const dmaResource_t* channel);
-dmaResource_t* dmaGetRefByIdentifier(const dmaIdentifier_e identifier);
-
#endif
// Macros to avoid direct register and register bit access
@@ -254,11 +246,14 @@ dmaResource_t* dmaGetRefByIdentifier(const dmaIdentifier_e identifier);
#define DMAx_SetMemoryAddress(reg, address) ((DMA_ARCH_TYPE *)(reg))->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail]
#endif
-void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex);
+dmaIdentifier_e dmaAllocate(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex);
+void dmaEnable(dmaIdentifier_e identifier);
void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam);
+dmaIdentifier_e dmaGetIdentifier(const dmaResource_t* channel);
const resourceOwner_t *dmaGetOwner(dmaIdentifier_e identifier);
dmaChannelDescriptor_t* dmaGetDescriptorByIdentifier(const dmaIdentifier_e identifier);
+uint32_t dmaGetChannel(const uint8_t channel);
//
// Wrapper macros to cast dmaResource_t back into DMA_ARCH_TYPE
diff --git a/src/main/drivers/dma_common.c b/src/main/drivers/dma_common.c
new file mode 100644
index 0000000000..47338bc77c
--- /dev/null
+++ b/src/main/drivers/dma_common.c
@@ -0,0 +1,67 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include "platform.h"
+
+#ifdef USE_DMA
+
+#include "drivers/dma_impl.h"
+
+#include "dma.h"
+
+dmaIdentifier_e dmaAllocate(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex)
+{
+ if (dmaGetOwner(identifier)->owner != OWNER_FREE) {
+ return DMA_NONE;
+ }
+
+ const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
+ dmaDescriptors[index].owner.owner = owner;
+ dmaDescriptors[index].owner.resourceIndex = resourceIndex;
+
+ return identifier;
+}
+
+const resourceOwner_t *dmaGetOwner(dmaIdentifier_e identifier)
+{
+ return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].owner;
+}
+
+dmaIdentifier_e dmaGetIdentifier(const dmaResource_t* channel)
+{
+ for (int i = 0; i < DMA_LAST_HANDLER; i++) {
+ if (dmaDescriptors[i].ref == channel) {
+ return i + 1;
+ }
+ }
+
+ return 0;
+}
+
+dmaChannelDescriptor_t* dmaGetDescriptorByIdentifier(const dmaIdentifier_e identifier)
+{
+ return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)];
+}
+
+uint32_t dmaGetChannel(const uint8_t channel)
+{
+ return ((uint32_t)channel*2)<<24;
+}
+#endif
diff --git a/src/main/drivers/dma_impl.h b/src/main/drivers/dma_impl.h
new file mode 100644
index 0000000000..3be2f8e4b4
--- /dev/null
+++ b/src/main/drivers/dma_impl.h
@@ -0,0 +1,25 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include "drivers/dma.h"
+
+extern dmaChannelDescriptor_t dmaDescriptors[];
diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c
index 25359bc37e..e4eaa84b98 100644
--- a/src/main/drivers/dma_stm32f4xx.c
+++ b/src/main/drivers/dma_stm32f4xx.c
@@ -33,7 +33,7 @@
/*
* DMA descriptors.
*/
-static dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
+dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
DEFINE_DMA_CHANNEL(DMA1, 0, 0),
DEFINE_DMA_CHANNEL(DMA1, 1, 6),
DEFINE_DMA_CHANNEL(DMA1, 2, 16),
@@ -74,12 +74,10 @@ DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER)
#define DMA_RCC(x) ((x) == DMA1 ? RCC_AHB1Periph_DMA1 : RCC_AHB1Periph_DMA2)
-void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex)
+void dmaEnable(dmaIdentifier_e identifier)
{
const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
RCC_AHB1PeriphClockCmd(DMA_RCC(dmaDescriptors[index].dma), ENABLE);
- dmaDescriptors[index].owner.owner = owner;
- dmaDescriptors[index].owner.resourceIndex = resourceIndex;
}
#define RETURN_TCIF_FLAG(s, n) if (s == DMA1_Stream ## n || s == DMA2_Stream ## n) return DMA_IT_TCIF ## n
@@ -114,44 +112,4 @@ void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callbac
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
-
-const resourceOwner_t *dmaGetOwner(dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].owner;
-}
-
-dmaIdentifier_e dmaGetIdentifier(const dmaResource_t* instance)
-{
- for (int i = 0; i < DMA_LAST_HANDLER; i++) {
- if (dmaDescriptors[i].ref == instance) {
- return i + 1;
- }
- }
- return 0;
-}
-
-dmaChannelDescriptor_t* dmaGetDescriptor(const dmaResource_t* instance)
-{
- for (int i = 0; i < DMA_LAST_HANDLER; i++) {
- if (dmaDescriptors[i].ref == instance) {
- return &dmaDescriptors[i];
- }
- }
- return NULL;
-}
-
-dmaResource_t* dmaGetRefByIdentifier(const dmaIdentifier_e identifier)
-{
- return dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].ref;
-}
-
-dmaChannelDescriptor_t* dmaGetDescriptorByIdentifier(const dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)];
-}
-
-uint32_t dmaGetChannel(const uint8_t channel)
-{
- return ((uint32_t)channel*2)<<24;
-}
#endif
diff --git a/src/main/drivers/dma_stm32f7xx.c b/src/main/drivers/dma_stm32f7xx.c
index 9220cdbc2b..14ece8e35c 100644
--- a/src/main/drivers/dma_stm32f7xx.c
+++ b/src/main/drivers/dma_stm32f7xx.c
@@ -34,7 +34,7 @@
/*
* DMA descriptors.
*/
-static dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
+dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
DEFINE_DMA_CHANNEL(DMA1, 0, 0),
DEFINE_DMA_CHANNEL(DMA1, 1, 6),
DEFINE_DMA_CHANNEL(DMA1, 2, 16),
@@ -80,13 +80,11 @@ static void enableDmaClock(int index)
RCC_ClockCmd(dmaDescriptors[index].dma == DMA1 ? RCC_AHB1(DMA1) : RCC_AHB1(DMA2), ENABLE);
}
-void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex)
+void dmaEnable(dmaIdentifier_e identifier)
{
const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
enableDmaClock(index);
- dmaDescriptors[index].owner.owner = owner;
- dmaDescriptors[index].owner.resourceIndex = resourceIndex;
}
void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
@@ -100,34 +98,4 @@ void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callbac
HAL_NVIC_SetPriority(dmaDescriptors[index].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
HAL_NVIC_EnableIRQ(dmaDescriptors[index].irqN);
}
-
-const resourceOwner_t *dmaGetOwner(dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].owner;
-}
-
-dmaIdentifier_e dmaGetIdentifier(const dmaResource_t* stream)
-{
- for (int i = 0; i < DMA_LAST_HANDLER; i++) {
- if (dmaDescriptors[i].ref == stream) {
- return i + 1;
- }
- }
- return 0;
-}
-
-dmaResource_t *dmaGetRefByIdentifier(const dmaIdentifier_e identifier)
-{
- return dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].ref;
-}
-
-dmaChannelDescriptor_t* dmaGetDescriptorByIdentifier(const dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)];
-}
-
-uint32_t dmaGetChannel(const uint8_t channel)
-{
- return ((uint32_t)channel*2)<<24;
-}
#endif
diff --git a/src/main/drivers/dma_stm32g4xx.c b/src/main/drivers/dma_stm32g4xx.c
index 0af0c895f3..a3db8a23f8 100644
--- a/src/main/drivers/dma_stm32g4xx.c
+++ b/src/main/drivers/dma_stm32g4xx.c
@@ -34,7 +34,7 @@
/*
* DMA descriptors.
*/
-static dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
+dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
DEFINE_DMA_CHANNEL(DMA1, 1, 0),
DEFINE_DMA_CHANNEL(DMA1, 2, 4),
DEFINE_DMA_CHANNEL(DMA1, 3, 8),
@@ -81,13 +81,11 @@ static void enableDmaClock(int index)
RCC_ClockCmd(RCC_AHB1(DMAMUX1), ENABLE);
}
-void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex)
+void dmaEnable(dmaIdentifier_e identifier)
{
const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
enableDmaClock(index);
- dmaDescriptors[index].owner.owner = owner;
- dmaDescriptors[index].owner.resourceIndex = resourceIndex;
}
void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
@@ -101,34 +99,4 @@ void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callbac
HAL_NVIC_SetPriority(dmaDescriptors[index].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
HAL_NVIC_EnableIRQ(dmaDescriptors[index].irqN);
}
-
-const resourceOwner_t *dmaGetOwner(dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].owner;
-}
-
-dmaIdentifier_e dmaGetIdentifier(const dmaResource_t* stream)
-{
- for (int i = 0; i < DMA_LAST_HANDLER; i++) {
- if (dmaDescriptors[i].ref == stream) {
- return i + 1;
- }
- }
- return 0;
-}
-
-dmaResource_t* dmaGetRefByIdentifier(const dmaIdentifier_e identifier)
-{
- return dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].ref;
-}
-
-dmaChannelDescriptor_t* dmaGetDescriptorByIdentifier(const dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)];
-}
-
-uint32_t dmaGetChannel(const uint8_t channel)
-{
- return ((uint32_t)channel*2)<<24;
-}
#endif
diff --git a/src/main/drivers/dma_stm32h7xx.c b/src/main/drivers/dma_stm32h7xx.c
index ef073b1f89..52d7aa1a27 100644
--- a/src/main/drivers/dma_stm32h7xx.c
+++ b/src/main/drivers/dma_stm32h7xx.c
@@ -34,7 +34,7 @@
/*
* DMA descriptors.
*/
-static dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
+dmaChannelDescriptor_t dmaDescriptors[DMA_LAST_HANDLER] = {
DEFINE_DMA_CHANNEL(DMA1, 0, 0),
DEFINE_DMA_CHANNEL(DMA1, 1, 6),
DEFINE_DMA_CHANNEL(DMA1, 2, 16),
@@ -80,13 +80,11 @@ static void enableDmaClock(int index)
// There seems to be no explicit control for DMAMUX1 clocking
}
-void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex)
+void dmaEnable(dmaIdentifier_e identifier)
{
const int index = DMA_IDENTIFIER_TO_INDEX(identifier);
enableDmaClock(index);
- dmaDescriptors[index].owner.owner = owner;
- dmaDescriptors[index].owner.resourceIndex = resourceIndex;
}
void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
@@ -100,34 +98,4 @@ void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callbac
HAL_NVIC_SetPriority(dmaDescriptors[index].irqN, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
HAL_NVIC_EnableIRQ(dmaDescriptors[index].irqN);
}
-
-const resourceOwner_t *dmaGetOwner(dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].owner;
-}
-
-dmaIdentifier_e dmaGetIdentifier(const dmaResource_t* stream)
-{
- for (int i = 0; i < DMA_LAST_HANDLER; i++) {
- if (dmaDescriptors[i].ref == stream) {
- return i + 1;
- }
- }
- return 0;
-}
-
-dmaResource_t* dmaGetRefByIdentifier(const dmaIdentifier_e identifier)
-{
- return dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)].ref;
-}
-
-dmaChannelDescriptor_t* dmaGetDescriptorByIdentifier(const dmaIdentifier_e identifier)
-{
- return &dmaDescriptors[DMA_IDENTIFIER_TO_INDEX(identifier)];
-}
-
-uint32_t dmaGetChannel(const uint8_t channel)
-{
- return ((uint32_t)channel*2)<<24;
-}
#endif
diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/dshot_bitbang.c
index e53d48e91e..8e9335c062 100644
--- a/src/main/drivers/dshot_bitbang.c
+++ b/src/main/drivers/dshot_bitbang.c
@@ -193,7 +193,7 @@ static bbPort_t *bbFindMotorPort(int portIndex)
return NULL;
}
-static bbPort_t *bbAllocMotorPort(int portIndex)
+static bbPort_t *bbAllocateMotorPort(int portIndex)
{
if (usedMotorPorts >= MAX_SUPPORTED_MOTOR_PORTS) {
bbStatus = DSHOT_BITBANG_STATUS_TOO_MANY_PORTS;
@@ -244,25 +244,13 @@ static uint32_t getDshotBaseFrequency(motorPwmProtocolTypes_e pwmProtocolType)
}
}
-static void bbAllocDma(bbPort_t *bbPort)
+static void bbSetupDma(bbPort_t *bbPort)
{
- const timerHardware_t *timhw = bbPort->timhw;
+ const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(bbPort->dmaResource);
+ dmaEnable(dmaIdentifier);
+ bbPort->dmaSource = timerDmaSource(bbPort->timhw->channel);
-#ifdef USE_DMA_SPEC
- dmaoptValue_t dmaopt = dmaGetOptionByTimer(timhw);
- const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timhw->tim, timhw->channel, dmaopt);
- bbPort->dmaResource = dmaChannelSpec->ref;
- bbPort->dmaChannel = dmaChannelSpec->channel;
-#else
- bbPort->dmaResource = timhw->dmaRef;
- bbPort->dmaChannel = timhw->dmaChannel;
-#endif
-
- dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(bbPort->dmaResource);
- dmaInit(dmaIdentifier, OWNER_DSHOT_BITBANG, bbPort->owner.resourceIndex);
- bbPort->dmaSource = timerDmaSource(timhw->channel);
-
- bbPacer_t *bbPacer = bbFindMotorPacer(timhw->tim);
+ bbPacer_t *bbPacer = bbFindMotorPacer(bbPort->timhw->tim);
bbPacer->dmaSources |= bbPort->dmaSource;
dmaSetHandler(dmaIdentifier, bbDMAIrqHandler, NVIC_BUILD_PRIORITY(2, 1), (uint32_t)bbPort);
@@ -388,8 +376,22 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
// New port group
- bbPort = bbAllocMotorPort(portIndex);
- if (!bbPort) {
+ bbPort = bbAllocateMotorPort(portIndex);
+
+ if (bbPort) {
+ const timerHardware_t *timhw = bbPort->timhw;
+
+#ifdef USE_DMA_SPEC
+ const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timhw->tim, timhw->channel, dmaGetOptionByTimer(timhw));
+ bbPort->dmaResource = dmaChannelSpec->ref;
+ bbPort->dmaChannel = dmaChannelSpec->channel;
+#else
+ bbPort->dmaResource = timhw->dmaRef;
+ bbPort->dmaChannel = timhw->dmaChannel;
+#endif
+ }
+
+ if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.updateStart = motorUpdateStartNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
@@ -409,7 +411,7 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
bbTIM_TimeBaseInit(bbPort, bbPort->outputARR);
bbTimerChannelInit(bbPort);
- bbAllocDma(bbPort);
+ bbSetupDma(bbPort);
bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_OUTPUT);
bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_INPUT);
diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c
index 72c20f89ac..898e9e71de 100644
--- a/src/main/drivers/light_ws2811strip_hal.c
+++ b/src/main/drivers/light_ws2811strip_hal.c
@@ -80,7 +80,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag)
uint32_t dmaChannel = timerHardware->dmaChannel;
#endif
- if (dmaRef == NULL) {
+ if (dmaRef == NULL || !dmaAllocate(dmaGetIdentifier(dmaRef), OWNER_LED_STRIP, 0)) {
return false;
}
TimHandle.Instance = timer;
@@ -138,7 +138,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag)
/* Link hdma_tim to hdma[x] (channelx) */
__HAL_LINKDMA(&TimHandle, hdma[dmaIndex], hdma_tim);
- dmaInit(dmaGetIdentifier(dmaRef), OWNER_LED_STRIP, 0);
+ dmaEnable(dmaGetIdentifier(dmaRef));
dmaSetHandler(dmaGetIdentifier(dmaRef), WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, dmaIndex);
/* Initialize TIMx DMA handle */
diff --git a/src/main/drivers/light_ws2811strip_stdperiph.c b/src/main/drivers/light_ws2811strip_stdperiph.c
index 65cc3a3be9..6f7ead8d55 100644
--- a/src/main/drivers/light_ws2811strip_stdperiph.c
+++ b/src/main/drivers/light_ws2811strip_stdperiph.c
@@ -109,7 +109,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag)
#endif
#endif
- if (dmaRef == NULL) {
+ if (dmaRef == NULL || !dmaAllocate(dmaGetIdentifier(dmaRef), OWNER_LED_STRIP, 0)) {
return false;
}
@@ -171,7 +171,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag)
TIM_Cmd(timer, ENABLE);
- dmaInit(dmaGetIdentifier(dmaRef), OWNER_LED_STRIP, 0);
+ dmaEnable(dmaGetIdentifier(dmaRef));
dmaSetHandler(dmaGetIdentifier(dmaRef), WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0);
xDMA_DeInit(dmaRef);
diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c
index 5c3480dede..7b3849bc0f 100644
--- a/src/main/drivers/pwm_output_dshot.c
+++ b/src/main/drivers/pwm_output_dshot.c
@@ -269,6 +269,25 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
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(motorIndex))) {
+ return false;
+ }
+ }
+
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
TIM_TypeDef *timer = timerHardware->tim;
@@ -344,14 +363,16 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
motor->timer->timerDmaSources &= ~motor->timerDmaSource;
}
- xDMA_Cmd(dmaRef, DISABLE);
- xDMA_DeInit(dmaRef);
- DMA_StructInit(&DMAINIT);
+ if (!dmaIsConfigured) {
+ xDMA_Cmd(dmaRef, DISABLE);
+ xDMA_DeInit(dmaRef);
+ dmaEnable(dmaIdentifier);
+ }
+
+ DMA_StructInit(&DMAINIT);
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
- dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
-
motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
#if defined(STM32F3)
@@ -377,8 +398,6 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
} else
#endif
{
- dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
-
motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
#if defined(STM32F3)
@@ -415,13 +434,16 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
#else
pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
#endif
+
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
- dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
+ if (!dmaIsConfigured) {
+ dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, timerIndex);
+ }
} else
#endif
{
- dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
+ dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
}
TIM_Cmd(timer, ENABLE);
diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c
index 25d9faa84c..0d07b31a0b 100644
--- a/src/main/drivers/pwm_output_dshot_hal.c
+++ b/src/main/drivers/pwm_output_dshot_hal.c
@@ -245,6 +245,25 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
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(motorIndex))) {
+ return false;
+ }
+ }
+
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
motor->dmaRef = dmaRef;
@@ -335,14 +354,16 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
motor->timer->timerDmaSources &= ~motor->timerDmaSource;
}
- xLL_EX_DMA_DisableResource(dmaRef);
- xLL_EX_DMA_DeInit(dmaRef);
- LL_DMA_StructInit(&DMAINIT);
+ if (!dmaIsConfigured) {
+ xLL_EX_DMA_DisableResource(dmaRef);
+ xLL_EX_DMA_DeInit(dmaRef);
+ dmaEnable(dmaIdentifier);
+ }
+
+ LL_DMA_StructInit(&DMAINIT);
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
- dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
-
motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
#if defined(STM32H7) || defined(STM32G4)
@@ -358,8 +379,6 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
} else
#endif
{
- dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
-
motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
#if defined(STM32H7) || defined(STM32G4)
@@ -388,8 +407,11 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
DMAINIT.Mode = LL_DMA_MODE_NORMAL;
DMAINIT.Priority = LL_DMA_PRIORITY_HIGH;
- xLL_EX_DMA_Init(dmaRef, &DMAINIT);
- xLL_EX_DMA_EnableIT_TC(dmaRef);
+ if (!dmaIsConfigured) {
+ xLL_EX_DMA_Init(dmaRef, &DMAINIT);
+ xLL_EX_DMA_EnableIT_TC(dmaRef);
+ }
+
motor->dmaRef = dmaRef;
#ifdef USE_DSHOT_TELEMETRY
@@ -400,13 +422,16 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
#else
pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
#endif
+
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
- dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
+ if (!dmaIsConfigured) {
+ dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, timerIndex);
+ }
} else
#endif
{
- dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
+ dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
}
LL_TIM_OC_Init(timer, channel, &OCINIT);
diff --git a/src/main/drivers/pwm_output_dshot_hal_hal.c b/src/main/drivers/pwm_output_dshot_hal_hal.c
index f557a7e62a..5e484343a2 100644
--- a/src/main/drivers/pwm_output_dshot_hal_hal.c
+++ b/src/main/drivers/pwm_output_dshot_hal_hal.c
@@ -269,6 +269,25 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
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(motorIndex))) {
+ return false;
+ }
+ }
+
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
motor->timerHardware = timerHardware;
@@ -364,17 +383,16 @@ P - High - High -
motor->timerDmaIndex = timerDmaIndex(timerHardware->channel);
}
- dmaIdentifier_e identifier = dmaGetIdentifier(dmaRef);
-
+ if (!dmaIsConfigured) {
+ dmaEnable(dmaIdentifier);
#ifdef USE_DSHOT_DMAR
- if (useBurstDshot) {
- dmaInit(identifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
- dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, timerIndex);
- } else
+ if (useBurstDshot) {
+ dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, timerIndex);
+ } else
#endif
- {
- dmaInit(identifier, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
- dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motorIndex);
+ {
+ dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motorIndex);
+ }
}
#ifdef USE_DSHOT_DMAR
@@ -404,9 +422,12 @@ P - High - High -
/* Link hdma_tim to hdma[TIM_DMA_ID_UPDATE] (update) */
__HAL_LINKDMA(&motor->timer->timHandle, hdma[TIM_DMA_ID_UPDATE], motor->timer->hdma_tim);
- /* Initialize TIMx DMA handle */
- result = HAL_DMA_Init(motor->timer->timHandle.hdma[TIM_DMA_ID_UPDATE]);
-
+ if (!dmaIsConfigured) {
+ /* Initialize TIMx DMA handle */
+ result = HAL_DMA_Init(motor->timer->timHandle.hdma[TIM_DMA_ID_UPDATE]);
+ } else {
+ result = HAL_OK;
+ }
} else
#endif
{
diff --git a/src/main/drivers/sdcard_spi.c b/src/main/drivers/sdcard_spi.c
index 7ac0e95a70..1cb25f1d62 100644
--- a/src/main/drivers/sdcard_spi.c
+++ b/src/main/drivers/sdcard_spi.c
@@ -532,8 +532,12 @@ static void sdcardSpi_init(const sdcardConfig_t *config, const spiPinConfig_t *s
if (dmaIdentifier) {
sdcard.dma = dmaGetDescriptorByIdentifier(dmaIdentifier);
- dmaInit(dmaIdentifier, OWNER_SDCARD, 0);
- sdcard.useDMAForTx = true;
+ if (dmaAllocate(dmaIdentifier, OWNER_SDCARD, 0)) {
+ dmaEnable(dmaIdentifier);
+ sdcard.useDMAForTx = true;
+ } else {
+ sdcard.useDMAForTx = false;
+ }
} else {
sdcard.useDMAForTx = false;
}
diff --git a/src/main/drivers/sdio_f4xx.c b/src/main/drivers/sdio_f4xx.c
index e392b3183f..6a6b1a1499 100644
--- a/src/main/drivers/sdio_f4xx.c
+++ b/src/main/drivers/sdio_f4xx.c
@@ -238,7 +238,7 @@ static uint32_t SD_Status;
static uint32_t SD_CardRCA;
SD_CardType_t SD_CardType;
static volatile uint32_t TimeOut;
-DMA_Stream_TypeDef *dma_stream;
+DMA_Stream_TypeDef *dmaStream;
/* Private function(s) ----------------------------------------------------------------------------------------------*/
@@ -536,7 +536,7 @@ static SD_Error_t SD_InitializeCard(void)
*/
static void SD_StartBlockTransfert(uint32_t* pBuffer, uint32_t BlockSize, uint32_t NumberOfBlocks, uint8_t dir)
{
- DMA_Stream_TypeDef *pDMA = dma_stream;
+ DMA_Stream_TypeDef *pDMA = dmaStream;
SDIO->DCTRL = 0; // Initialize data control register
SD_Handle.TransferComplete = 0; // Initialize handle flags
@@ -563,7 +563,7 @@ static void SD_StartBlockTransfert(uint32_t* pBuffer, uint32_t BlockSize, uint32
} else {
pDMA->CR |= DMA_MEMORY_TO_PERIPH; // Sets memory to peripheral
}
- if (dma_stream == DMA2_Stream3) {
+ if (dmaStream == DMA2_Stream3) {
DMA2->LIFCR = DMA_LIFCR_CTEIF3 | DMA_LIFCR_CDMEIF3 |
DMA_LIFCR_CFEIF3 | DMA_LIFCR_CHTIF3 | DMA_LIFCR_CTCIF3; // Clear the transfer error flag
} else {
@@ -1556,9 +1556,14 @@ static SD_Error_t SD_IsCardProgramming(uint8_t *pStatus)
/**
* @brief Initialize the SDIO module, DMA, and IO
*/
-void SD_Initialize_LL(DMA_Stream_TypeDef *dma)
+bool SD_Initialize_LL(DMA_Stream_TypeDef *dma)
{
- // Reset SDIO Module
+ const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier((dmaResource_t *)dmaStream);
+ if (!(dma == DMA2_Stream3 || dma == DMA2_Stream6) || !dmaAllocate(dmaIdentifier, OWNER_SDCARD, 0)) {
+ return false;
+ }
+
+ // Reset SDIO Module
RCC->APB2RSTR |= RCC_APB2RSTR_SDIORST;
delay(1);
RCC->APB2RSTR &= ~RCC_APB2RSTR_SDIORST;
@@ -1612,35 +1617,30 @@ void SD_Initialize_LL(DMA_Stream_TypeDef *dma)
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
- dma_stream = dma;
+ dmaStream = dma;
RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
- if ((uint32_t)dma_stream == (uint32_t)DMA2_Stream3) {
- // Initialize DMA2 channel 3
- DMA2_Stream3->CR = 0; // Reset DMA Stream control register
- DMA2_Stream3->PAR = (uint32_t)&SDIO->FIFO;
- DMA2->LIFCR = IFCR_CLEAR_MASK_STREAM3; // Clear all interrupt flags
- DMA2_Stream3->CR = (DMA_CHANNEL_4 | DMA_SxCR_PFCTRL | // Prepare the DMA Stream configuration
- DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register
- DMA_MDATAALIGN_WORD | DMA_PRIORITY_VERY_HIGH |
- DMA_MBURST_INC4 | DMA_PBURST_INC4 |
- DMA_MEMORY_TO_PERIPH);
- DMA2_Stream3->FCR = (DMA_SxFCR_DMDIS | DMA_SxFCR_FTH); // Configuration FIFO control register
- dmaInit(dmaGetIdentifier((dmaResource_t *)DMA2_Stream3), OWNER_SDCARD, 0);
- dmaSetHandler(dmaGetIdentifier((dmaResource_t *)DMA2_Stream3), SDIO_DMA_ST3_IRQHandler, 1, 0);
+ // Initialize DMA
+ dmaStream->CR = 0; // Reset DMA Stream control register
+ dmaStream->PAR = (uint32_t)&SDIO->FIFO;
+ if (dmaStream == DMA2_Stream3) {
+ DMA2->LIFCR = IFCR_CLEAR_MASK_STREAM3; // Clear all interrupt flags
} else {
- // Initialize DMA2 channel 6
- DMA2_Stream6->CR = 0; // Reset DMA Stream control register
- DMA2_Stream6->PAR = (uint32_t)&SDIO->FIFO;
- DMA2->HIFCR = IFCR_CLEAR_MASK_STREAM6; // Clear all interrupt flags
- DMA2_Stream6->CR = (DMA_CHANNEL_4 | DMA_SxCR_PFCTRL | // Prepare the DMA Stream configuration
- DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register
- DMA_MDATAALIGN_WORD | DMA_PRIORITY_VERY_HIGH |
- DMA_MBURST_INC4 | DMA_PBURST_INC4 |
- DMA_MEMORY_TO_PERIPH);
- DMA2_Stream6->FCR = (DMA_SxFCR_DMDIS | DMA_SxFCR_FTH); // Configuration FIFO control register
- dmaInit(dmaGetIdentifier((dmaResource_t *)DMA2_Stream6), OWNER_SDCARD, 0);
- dmaSetHandler(dmaGetIdentifier((dmaResource_t *)DMA2_Stream6), SDIO_DMA_ST6_IRQHandler, 1, 0);
+ DMA2->HIFCR = IFCR_CLEAR_MASK_STREAM6; // Clear all interrupt flags
}
+ dmaStream->CR = (DMA_CHANNEL_4 | DMA_SxCR_PFCTRL | // Prepare the DMA Stream configuration
+ DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register
+ DMA_MDATAALIGN_WORD | DMA_PRIORITY_VERY_HIGH |
+ DMA_MBURST_INC4 | DMA_PBURST_INC4 |
+ DMA_MEMORY_TO_PERIPH);
+ dmaStream->FCR = (DMA_SxFCR_DMDIS | DMA_SxFCR_FTH); // Configuration FIFO control register
+ dmaEnable(dmaIdentifier);
+ if (dmaStream == DMA2_Stream3) {
+ dmaSetHandler(dmaIdentifier, SDIO_DMA_ST3_IRQHandler, 1, 0);
+ } else {
+ dmaSetHandler(dmaIdentifier, SDIO_DMA_ST6_IRQHandler, 1, 0);
+ }
+
+ return true;
}
@@ -1726,7 +1726,7 @@ void SDIO_IRQHandler(void) {
/* Currently doesn't implement multiple block write handling */
if ((SD_Handle.Operation & 0x02) == (SDIO_DIR_TX << 1)) {
/* Disable the stream */
- dma_stream->CR &= ~DMA_SxCR_EN;
+ dmaStream->CR &= ~DMA_SxCR_EN;
SDIO->DCTRL &= ~(SDIO_DCTRL_DMAEN);
/* Transfer is complete */
SD_Handle.TXCplt = 0;
diff --git a/src/main/drivers/sdio_f7xx.c b/src/main/drivers/sdio_f7xx.c
index 09308c5a4f..cbc92084a9 100644
--- a/src/main/drivers/sdio_f7xx.c
+++ b/src/main/drivers/sdio_f7xx.c
@@ -220,7 +220,7 @@ static uint32_t SD_Status;
static uint32_t SD_CardRCA;
SD_CardType_t SD_CardType;
static volatile uint32_t TimeOut;
-DMA_Stream_TypeDef *dma_stream;
+DMA_Stream_TypeDef *dmaStream;
/* Private function(s) ----------------------------------------------------------------------------------------------*/
@@ -518,7 +518,7 @@ static SD_Error_t SD_InitializeCard(void)
*/
static void SD_StartBlockTransfert(uint32_t* pBuffer, uint32_t BlockSize, uint32_t NumberOfBlocks, uint8_t dir)
{
- DMA_Stream_TypeDef *pDMA = dma_stream;
+ DMA_Stream_TypeDef *pDMA = dmaStream;
SDMMC1->DCTRL = 0; // Initialize data control register
SD_Handle.TransferComplete = 0; // Initialize handle flags
@@ -545,7 +545,7 @@ static void SD_StartBlockTransfert(uint32_t* pBuffer, uint32_t BlockSize, uint32
} else {
pDMA->CR |= DMA_MEMORY_TO_PERIPH; // Sets memory to peripheral
}
- if (dma_stream == DMA2_Stream3) {
+ if (dmaStream == DMA2_Stream3) {
DMA2->LIFCR = DMA_LIFCR_CTEIF3 | DMA_LIFCR_CDMEIF3 |
DMA_LIFCR_CFEIF3 | DMA_LIFCR_CHTIF3 | DMA_LIFCR_CTCIF3; // Clear the transfer error flag
} else {
@@ -1541,9 +1541,14 @@ static SD_Error_t SD_IsCardProgramming(uint8_t *pStatus)
/**
* @brief Initialize the SDMMC1 module, DMA, and IO
*/
-void SD_Initialize_LL(DMA_Stream_TypeDef *dma)
+bool SD_Initialize_LL(DMA_Stream_TypeDef *dma)
{
- // Reset SDMMC1 Module
+ const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier((dmaResource_t *)dmaStream);
+ if (!(dma == DMA2_Stream3 || dma == DMA2_Stream6) || !dmaAllocate(dmaIdentifier, OWNER_SDCARD, 0)) {
+ return false;
+ }
+
+ // Reset SDMMC1 Module
RCC->APB2RSTR |= RCC_APB2RSTR_SDMMC1RST;
delay(1);
RCC->APB2RSTR &= ~RCC_APB2RSTR_SDMMC1RST;
@@ -1595,35 +1600,31 @@ void SD_Initialize_LL(DMA_Stream_TypeDef *dma)
NVIC_SetPriority(SDMMC1_IRQn, NVIC_EncodePriority(PriorityGroup, 1, 0));
NVIC_EnableIRQ(SDMMC1_IRQn);
- dma_stream = dma;
+ dmaStream = dma;
RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
- if ((uint32_t)dma_stream == (uint32_t)DMA2_Stream3) {
- // Initialize DMA2 channel 3
- DMA2_Stream3->CR = 0; // Reset DMA Stream control register
- DMA2_Stream3->PAR = (uint32_t)&SDMMC1->FIFO;
- DMA2->LIFCR = IFCR_CLEAR_MASK_STREAM3; // Clear all interrupt flags
- DMA2_Stream3->CR = (DMA_CHANNEL_4 | DMA_SxCR_PFCTRL | // Prepare the DMA Stream configuration
- DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register
- DMA_MDATAALIGN_WORD | DMA_PRIORITY_VERY_HIGH |
- DMA_MBURST_INC4 | DMA_PBURST_INC4 |
- DMA_MEMORY_TO_PERIPH);
- DMA2_Stream3->FCR = (DMA_SxFCR_DMDIS | DMA_SxFCR_FTH); // Configuration FIFO control register
- dmaInit(dmaGetIdentifier((dmaResource_t *)DMA2_Stream3), OWNER_SDCARD, 0);
- dmaSetHandler(dmaGetIdentifier((dmaResource_t *)DMA2_Stream3), SDMMC_DMA_ST3_IRQHandler, 1, 0);
+
+ // Initialize DMA
+ dmaStream->CR = 0; // Reset DMA Stream control register
+ dmaStream->PAR = (uint32_t)&SDMMC1->FIFO;
+ if (dmaStream == DMA2_Stream3) {
+ DMA2->LIFCR = IFCR_CLEAR_MASK_STREAM3; // Clear all interrupt flags
} else {
- // Initialize DMA2 channel 6
- DMA2_Stream6->CR = 0; // Reset DMA Stream control register
- DMA2_Stream6->PAR = (uint32_t)&SDMMC1->FIFO;
- DMA2->HIFCR = IFCR_CLEAR_MASK_STREAM6; // Clear all interrupt flags
- DMA2_Stream6->CR = (DMA_CHANNEL_4 | DMA_SxCR_PFCTRL | // Prepare the DMA Stream configuration
- DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register
- DMA_MDATAALIGN_WORD | DMA_PRIORITY_VERY_HIGH |
- DMA_MBURST_INC4 | DMA_PBURST_INC4 |
- DMA_MEMORY_TO_PERIPH);
- DMA2_Stream6->FCR = (DMA_SxFCR_DMDIS | DMA_SxFCR_FTH); // Configuration FIFO control register
- dmaInit(dmaGetIdentifier((dmaResource_t *)DMA2_Stream6), OWNER_SDCARD, 0);
- dmaSetHandler(dmaGetIdentifier((dmaResource_t *)DMA2_Stream6), SDMMC_DMA_ST6_IRQHandler, 1, 0);
+ DMA2->HIFCR = IFCR_CLEAR_MASK_STREAM6; // Clear all interrupt flags
}
+ dmaStream->CR = (DMA_CHANNEL_4 | DMA_SxCR_PFCTRL | // Prepare the DMA Stream configuration
+ DMA_MINC_ENABLE | DMA_PDATAALIGN_WORD | // And write to DMA Stream CR register
+ DMA_MDATAALIGN_WORD | DMA_PRIORITY_VERY_HIGH |
+ DMA_MBURST_INC4 | DMA_PBURST_INC4 |
+ DMA_MEMORY_TO_PERIPH);
+ dmaStream->FCR = (DMA_SxFCR_DMDIS | DMA_SxFCR_FTH); // Configuration FIFO control register
+ dmaEnable(dmaIdentifier);
+ if (dmaStream == DMA2_Stream3) {
+ dmaSetHandler(dmaIdentifier, SDMMC_DMA_ST3_IRQHandler, 1, 0);
+ } else {
+ dmaSetHandler(dmaIdentifier, SDMMC_DMA_ST6_IRQHandler, 1, 0);
+ }
+
+ return true;
}
@@ -1706,7 +1707,7 @@ void SDMMC1_IRQHandler(void) {
/* Currently doesn't implement multiple block write handling */
if ((SD_Handle.Operation & 0x02) == (SDMMC_DIR_TX << 1)) {
/* Disable the stream */
- dma_stream->CR &= ~DMA_SxCR_EN;
+ dmaStream->CR &= ~DMA_SxCR_EN;
SDMMC1->DCTRL &= ~(SDMMC_DCTRL_DMAEN);
/* Transfer is complete */
SD_Handle.TXCplt = 0;
diff --git a/src/main/drivers/sdio_h7xx.c b/src/main/drivers/sdio_h7xx.c
index 7400797fad..d4b1f7ddf9 100644
--- a/src/main/drivers/sdio_h7xx.c
+++ b/src/main/drivers/sdio_h7xx.c
@@ -251,9 +251,11 @@ void SDIO_GPIO_Init(void)
IOConfigGPIO(cmd, IOCFG_OUT_PP);
}
-void SD_Initialize_LL(DMA_Stream_TypeDef *dma)
+bool SD_Initialize_LL(DMA_Stream_TypeDef *dma)
{
UNUSED(dma);
+
+ return true;
}
bool SD_GetState(void)
diff --git a/src/main/drivers/sdmmc_sdio.h b/src/main/drivers/sdmmc_sdio.h
index 937091b08d..8c3f0f9a48 100644
--- a/src/main/drivers/sdmmc_sdio.h
+++ b/src/main/drivers/sdmmc_sdio.h
@@ -219,7 +219,7 @@ typedef struct
extern SD_CardInfo_t SD_CardInfo;
extern SD_CardType_t SD_CardType;
-void SD_Initialize_LL (DMA_Stream_TypeDef *dma);
+bool SD_Initialize_LL (DMA_Stream_TypeDef *dma);
bool SD_Init (void);
bool SD_IsDetected (void);
bool SD_GetState (void);
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index 2ac46f13e1..6c849f04d3 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -105,28 +105,28 @@ UART_BUFFERS(9);
serialPort_t *uartOpen(UARTDevice_e device, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options)
{
- uartPort_t *s = serialUART(device, baudRate, mode, options);
+ uartPort_t *uartPort = serialUART(device, baudRate, mode, options);
- if (!s)
- return (serialPort_t *)s;
+ if (!uartPort)
+ return (serialPort_t *)uartPort;
#ifdef USE_DMA
- s->txDMAEmpty = true;
+ uartPort->txDMAEmpty = true;
#endif
// common serial initialisation code should move to serialPort::init()
- s->port.rxBufferHead = s->port.rxBufferTail = 0;
- s->port.txBufferHead = s->port.txBufferTail = 0;
+ uartPort->port.rxBufferHead = uartPort->port.rxBufferTail = 0;
+ uartPort->port.txBufferHead = uartPort->port.txBufferTail = 0;
// callback works for IRQ-based RX ONLY
- s->port.rxCallback = rxCallback;
- s->port.rxCallbackData = rxCallbackData;
- s->port.mode = mode;
- s->port.baudRate = baudRate;
- s->port.options = options;
+ uartPort->port.rxCallback = rxCallback;
+ uartPort->port.rxCallbackData = rxCallbackData;
+ uartPort->port.mode = mode;
+ uartPort->port.baudRate = baudRate;
+ uartPort->port.options = options;
- uartReconfigure(s);
+ uartReconfigure(uartPort);
- return (serialPort_t *)s;
+ return (serialPort_t *)uartPort;
}
static void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate)
@@ -145,56 +145,56 @@ static void uartSetMode(serialPort_t *instance, portMode_e mode)
static uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance)
{
- const uartPort_t *s = (const uartPort_t*)instance;
+ const uartPort_t *uartPort = (const uartPort_t*)instance;
#ifdef USE_DMA
- if (s->rxDMAResource) {
+ if (uartPort->rxDMAResource) {
// XXX Could be consolidated
#ifdef USE_HAL_DRIVER
- uint32_t rxDMAHead = __HAL_DMA_GET_COUNTER(s->Handle.hdmarx);
+ uint32_t rxDMAHead = __HAL_DMA_GET_COUNTER(uartPort->Handle.hdmarx);
#else
- uint32_t rxDMAHead = xDMA_GetCurrDataCounter(s->rxDMAResource);
+ uint32_t rxDMAHead = xDMA_GetCurrDataCounter(uartPort->rxDMAResource);
#endif
- // s->rxDMAPos and rxDMAHead represent distances from the end
+ // uartPort->rxDMAPos and rxDMAHead represent distances from the end
// of the buffer. They count DOWN as they advance.
- if (s->rxDMAPos >= rxDMAHead) {
- return s->rxDMAPos - rxDMAHead;
+ if (uartPort->rxDMAPos >= rxDMAHead) {
+ return uartPort->rxDMAPos - rxDMAHead;
} else {
- return s->port.rxBufferSize + s->rxDMAPos - rxDMAHead;
+ return uartPort->port.rxBufferSize + uartPort->rxDMAPos - rxDMAHead;
}
}
#endif
- if (s->port.rxBufferHead >= s->port.rxBufferTail) {
- return s->port.rxBufferHead - s->port.rxBufferTail;
+ if (uartPort->port.rxBufferHead >= uartPort->port.rxBufferTail) {
+ return uartPort->port.rxBufferHead - uartPort->port.rxBufferTail;
} else {
- return s->port.rxBufferSize + s->port.rxBufferHead - s->port.rxBufferTail;
+ return uartPort->port.rxBufferSize + uartPort->port.rxBufferHead - uartPort->port.rxBufferTail;
}
}
static uint32_t uartTotalTxBytesFree(const serialPort_t *instance)
{
- const uartPort_t *s = (const uartPort_t*)instance;
+ const uartPort_t *uartPort = (const uartPort_t*)instance;
uint32_t bytesUsed;
- if (s->port.txBufferHead >= s->port.txBufferTail) {
- bytesUsed = s->port.txBufferHead - s->port.txBufferTail;
+ if (uartPort->port.txBufferHead >= uartPort->port.txBufferTail) {
+ bytesUsed = uartPort->port.txBufferHead - uartPort->port.txBufferTail;
} else {
- bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail;
+ bytesUsed = uartPort->port.txBufferSize + uartPort->port.txBufferHead - uartPort->port.txBufferTail;
}
#ifdef USE_DMA
- if (s->txDMAResource) {
+ if (uartPort->txDMAResource) {
/*
* When we queue up a DMA request, we advance the Tx buffer tail before the transfer finishes, so we must add
* the remaining size of that in-progress transfer here instead:
*/
#ifdef USE_HAL_DRIVER
- bytesUsed += __HAL_DMA_GET_COUNTER(s->Handle.hdmatx);
+ bytesUsed += __HAL_DMA_GET_COUNTER(uartPort->Handle.hdmatx);
#else
- bytesUsed += xDMA_GetCurrDataCounter(s->txDMAResource);
+ bytesUsed += xDMA_GetCurrDataCounter(uartPort->txDMAResource);
#endif
/*
@@ -205,46 +205,46 @@ static uint32_t uartTotalTxBytesFree(const serialPort_t *instance)
*
* Be kind to callers and pretend like our buffer can only ever be 100% full.
*/
- if (bytesUsed >= s->port.txBufferSize - 1) {
+ if (bytesUsed >= uartPort->port.txBufferSize - 1) {
return 0;
}
}
#endif
- return (s->port.txBufferSize - 1) - bytesUsed;
+ return (uartPort->port.txBufferSize - 1) - bytesUsed;
}
static bool isUartTransmitBufferEmpty(const serialPort_t *instance)
{
- const uartPort_t *s = (const uartPort_t *)instance;
+ const uartPort_t *uartPort = (const uartPort_t *)instance;
#ifdef USE_DMA
- if (s->txDMAResource) {
- return s->txDMAEmpty;
+ if (uartPort->txDMAResource) {
+ return uartPort->txDMAEmpty;
} else
#endif
{
- return s->port.txBufferTail == s->port.txBufferHead;
+ return uartPort->port.txBufferTail == uartPort->port.txBufferHead;
}
}
static uint8_t uartRead(serialPort_t *instance)
{
uint8_t ch;
- uartPort_t *s = (uartPort_t *)instance;
+ uartPort_t *uartPort = (uartPort_t *)instance;
#ifdef USE_DMA
- if (s->rxDMAResource) {
- ch = s->port.rxBuffer[s->port.rxBufferSize - s->rxDMAPos];
- if (--s->rxDMAPos == 0)
- s->rxDMAPos = s->port.rxBufferSize;
+ if (uartPort->rxDMAResource) {
+ ch = uartPort->port.rxBuffer[uartPort->port.rxBufferSize - uartPort->rxDMAPos];
+ if (--uartPort->rxDMAPos == 0)
+ uartPort->rxDMAPos = uartPort->port.rxBufferSize;
} else
#endif
{
- ch = s->port.rxBuffer[s->port.rxBufferTail];
- if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) {
- s->port.rxBufferTail = 0;
+ ch = uartPort->port.rxBuffer[uartPort->port.rxBufferTail];
+ if (uartPort->port.rxBufferTail + 1 >= uartPort->port.rxBufferSize) {
+ uartPort->port.rxBufferTail = 0;
} else {
- s->port.rxBufferTail++;
+ uartPort->port.rxBufferTail++;
}
}
@@ -253,26 +253,26 @@ static uint8_t uartRead(serialPort_t *instance)
static void uartWrite(serialPort_t *instance, uint8_t ch)
{
- uartPort_t *s = (uartPort_t *)instance;
+ uartPort_t *uartPort = (uartPort_t *)instance;
- s->port.txBuffer[s->port.txBufferHead] = ch;
+ uartPort->port.txBuffer[uartPort->port.txBufferHead] = ch;
- if (s->port.txBufferHead + 1 >= s->port.txBufferSize) {
- s->port.txBufferHead = 0;
+ if (uartPort->port.txBufferHead + 1 >= uartPort->port.txBufferSize) {
+ uartPort->port.txBufferHead = 0;
} else {
- s->port.txBufferHead++;
+ uartPort->port.txBufferHead++;
}
#ifdef USE_DMA
- if (s->txDMAResource) {
- uartTryStartTxDMA(s);
+ if (uartPort->txDMAResource) {
+ uartTryStartTxDMA(uartPort);
} else
#endif
{
#ifdef USE_HAL_DRIVER
- __HAL_UART_ENABLE_IT(&s->Handle, UART_IT_TXE);
+ __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE);
#else
- USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE);
+ USART_ITConfig(uartPort->USARTx, USART_IT_TXE, ENABLE);
#endif
}
}
@@ -297,7 +297,7 @@ const struct serialPortVTable uartVTable[] = {
#ifdef USE_DMA
void uartConfigureDma(uartDevice_t *uartdev)
{
- uartPort_t *s = &(uartdev->port);
+ uartPort_t *uartPort = &(uartdev->port);
const uartHardware_t *hardware = uartdev->hardware;
#ifdef USE_DMA_SPEC
@@ -307,43 +307,47 @@ void uartConfigureDma(uartDevice_t *uartdev)
if (serialUartConfig(device)->txDmaopt != DMA_OPT_UNUSED) {
dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_UART_TX, device, serialUartConfig(device)->txDmaopt);
if (dmaChannelSpec) {
- s->txDMAResource = dmaChannelSpec->ref;
- s->txDMAChannel = dmaChannelSpec->channel;
+ uartPort->txDMAResource = dmaChannelSpec->ref;
+ uartPort->txDMAChannel = dmaChannelSpec->channel;
}
}
if (serialUartConfig(device)->rxDmaopt != DMA_OPT_UNUSED) {
dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_UART_RX, device, serialUartConfig(device)->txDmaopt);
if (dmaChannelSpec) {
- s->rxDMAResource = dmaChannelSpec->ref;
- s->rxDMAChannel = dmaChannelSpec->channel;
+ uartPort->rxDMAResource = dmaChannelSpec->ref;
+ uartPort->rxDMAChannel = dmaChannelSpec->channel;
}
}
#else
// Non USE_DMA_SPEC does not support configurable ON/OFF of UART DMA
if (hardware->rxDMAResource) {
- s->rxDMAResource = hardware->rxDMAResource;
- s->rxDMAChannel = hardware->rxDMAChannel;
+ uartPort->rxDMAResource = hardware->rxDMAResource;
+ uartPort->rxDMAChannel = hardware->rxDMAChannel;
}
if (hardware->txDMAResource) {
- s->txDMAResource = hardware->txDMAResource;
- s->txDMAChannel = hardware->txDMAChannel;
+ uartPort->txDMAResource = hardware->txDMAResource;
+ uartPort->txDMAChannel = hardware->txDMAChannel;
}
#endif
- if (s->txDMAResource) {
- dmaIdentifier_e identifier = dmaGetIdentifier(s->txDMAResource);
- dmaInit(identifier, OWNER_SERIAL_TX, RESOURCE_INDEX(hardware->device));
- dmaSetHandler(identifier, uartDmaIrqHandler, hardware->txPriority, (uint32_t)uartdev);
- s->txDMAPeripheralBaseAddr = (uint32_t)&UART_REG_TXD(hardware->reg);
+ if (uartPort->txDMAResource) {
+ dmaIdentifier_e identifier = dmaGetIdentifier(uartPort->txDMAResource);
+ if (dmaAllocate(identifier, OWNER_SERIAL_TX, RESOURCE_INDEX(hardware->device))) {
+ dmaEnable(identifier);
+ dmaSetHandler(identifier, uartDmaIrqHandler, hardware->txPriority, (uint32_t)uartdev);
+ uartPort->txDMAPeripheralBaseAddr = (uint32_t)&UART_REG_TXD(hardware->reg);
+ }
}
- if (s->rxDMAResource) {
- dmaIdentifier_e identifier = dmaGetIdentifier(s->rxDMAResource);
- dmaInit(identifier, OWNER_SERIAL_RX, RESOURCE_INDEX(hardware->device));
- s->rxDMAPeripheralBaseAddr = (uint32_t)&UART_REG_RXD(hardware->reg);
+ if (uartPort->rxDMAResource) {
+ dmaIdentifier_e identifier = dmaGetIdentifier(uartPort->rxDMAResource);
+ if (dmaAllocate(identifier, OWNER_SERIAL_RX, RESOURCE_INDEX(hardware->device))) {
+ dmaEnable(identifier);
+ uartPort->rxDMAPeripheralBaseAddr = (uint32_t)&UART_REG_RXD(hardware->reg);
+ }
}
}
#endif
@@ -351,8 +355,8 @@ void uartConfigureDma(uartDevice_t *uartdev)
#define UART_IRQHandler(type, number, dev) \
void type ## number ## _IRQHandler(void) \
{ \
- uartPort_t *s = &(uartDevmap[UARTDEV_ ## dev]->port); \
- uartIrqHandler(s); \
+ uartPort_t *uartPort = &(uartDevmap[UARTDEV_ ## dev]->port); \
+ uartIrqHandler(uartPort); \
}
#ifdef USE_UART1
diff --git a/src/main/drivers/transponder_ir_io_hal.c b/src/main/drivers/transponder_ir_io_hal.c
index 717792f5d4..ef6d07d113 100644
--- a/src/main/drivers/transponder_ir_io_hal.c
+++ b/src/main/drivers/transponder_ir_io_hal.c
@@ -92,7 +92,8 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
uint32_t dmaChannel = timerHardware->dmaChannel;
#endif
- if (dmaRef == NULL) {
+ dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
+ if (dmaRef == NULL || !dmaAllocate(dmaIdentifier, OWNER_TRANSPONDER, 0)) {
return;
}
@@ -153,8 +154,8 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
/* Link hdma_tim to hdma[x] (channelx) */
__HAL_LINKDMA(&TimHandle, hdma[dmaIndex], hdma_tim);
- dmaInit(dmaGetIdentifier(dmaRef), OWNER_TRANSPONDER, 0);
- dmaSetHandler(dmaGetIdentifier(dmaRef), TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, dmaIndex);
+ dmaEnable(dmaIdentifier);
+ dmaSetHandler(dmaIdentifier, TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, dmaIndex);
/* Initialize TIMx DMA handle */
if (HAL_DMA_Init(TimHandle.hdma[dmaIndex]) != HAL_OK) {
diff --git a/src/main/drivers/transponder_ir_io_stdperiph.c b/src/main/drivers/transponder_ir_io_stdperiph.c
index 9e24dd277b..453662d2b8 100644
--- a/src/main/drivers/transponder_ir_io_stdperiph.c
+++ b/src/main/drivers/transponder_ir_io_stdperiph.c
@@ -89,7 +89,7 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
#endif
#endif
- if (dmaRef == NULL) {
+ if (dmaRef == NULL || !dmaAllocate(dmaGetIdentifier(dmaRef), OWNER_TRANSPONDER, 0)) {
return;
}
@@ -97,7 +97,7 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
IOInit(transponderIO, OWNER_TRANSPONDER, 0);
IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction);
- dmaInit(dmaGetIdentifier(dmaRef), OWNER_TRANSPONDER, 0);
+ dmaEnable(dmaGetIdentifier(dmaRef));
dmaSetHandler(dmaGetIdentifier(dmaRef), TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, 0);
RCC_ClockCmd(timerRCC(timer), ENABLE);
diff --git a/src/main/msc/usbd_storage_sdio.c b/src/main/msc/usbd_storage_sdio.c
index 8fd371b83e..e424d1944a 100644
--- a/src/main/msc/usbd_storage_sdio.c
+++ b/src/main/msc/usbd_storage_sdio.c
@@ -167,21 +167,19 @@ static int8_t STORAGE_Init (uint8_t lun)
LED0_OFF;
#ifdef USE_DMA_SPEC
-#if defined(STM32H7) // H7 uses IDMA
- SD_Initialize_LL(0);
-#else
const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_SDIO, 0, sdioConfig()->dmaopt);
- if (!dmaChannelSpec) {
+ if (!dmaChannelSpec || !SD_Initialize_LL((DMA_ARCH_TYPE *)dmaChannelSpec->ref)) {
+#else
+#if defined(STM32H7) // H7 uses IDMA
+ if (!SD_Initialize_LL(0)) {
+#else
+ if (!SD_Initialize_LL(SDCARD_SDIO_DMA_OPT)) {
+#endif
+#endif // USE_DMA_SPEC
return 1;
}
- SD_Initialize_LL((DMA_ARCH_TYPE *)dmaChannelSpec->ref);
-#endif
-#else
- SD_Initialize_LL(SDCARD_SDIO_DMA_OPT);
-#endif
-
if (!sdcard_isInserted()) {
return 1;
}