Skip to content

Commit

Permalink
Introduce dma_reqmap
Browse files Browse the repository at this point in the history
  • Loading branch information
ezshinoda committed Dec 30, 2018
1 parent 3631e41 commit fe182bb
Show file tree
Hide file tree
Showing 82 changed files with 1,050 additions and 187 deletions.
1 change: 1 addition & 0 deletions make/source.mk
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ COMMON_SRC = \
drivers/bus_spi_pinconfig.c \
drivers/buttons.c \
drivers/display.c \
drivers/dma_reqmap.c \
drivers/exti.c \
drivers/io.c \
drivers/light_led.c \
Expand Down
2 changes: 2 additions & 0 deletions src/main/drivers/adc_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -53,12 +53,14 @@ typedef struct adcTagMap_s {
typedef struct adcDevice_s {
ADC_TypeDef* ADCx;
rccPeriphTag_t rccADC;
#if !defined(USE_DMA_SPEC)
#if defined(STM32F4) || defined(STM32F7)
DMA_Stream_TypeDef* DMAy_Streamx;
uint32_t channel;
#else
DMA_Channel_TypeDef* DMAy_Channelx;
#endif
#endif
#if defined(STM32F7)
ADC_HandleTypeDef ADCHandle;
DMA_HandleTypeDef DmaHandle;
Expand Down
1 change: 1 addition & 0 deletions src/main/drivers/adc_stm32f30x.c
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,7 @@ void adcInit(const adcConfig_t *config)
}

ADCDevice device = ADC_CFG_TO_DEV(config->device);

if (device == ADCINVALID) {
return;
}
Expand Down
53 changes: 49 additions & 4 deletions src/main/drivers/adc_stm32f4xx.c
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include "build/debug.h"

#include "drivers/accgyro/accgyro.h"
#include "drivers/dma_reqmap.h"
#include "drivers/system.h"

#include "drivers/io.h"
Expand All @@ -49,10 +50,31 @@
#endif

const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = ADC1_DMA_STREAM, .channel = DMA_Channel_0 },
{
.ADCx = ADC1,
.rccADC = RCC_APB2(ADC1),
#if !defined(USE_DMA_SPEC)
.DMAy_Streamx = ADC1_DMA_STREAM,
.channel = DMA_Channel_0
#endif
},
#if !defined(STM32F411xE)
{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = ADC2_DMA_STREAM, .channel = DMA_Channel_1 },
{ .ADCx = ADC3, .rccADC = RCC_APB2(ADC3), .DMAy_Streamx = ADC3_DMA_STREAM, .channel = DMA_Channel_2 }
{
.ADCx = ADC2,
.rccADC = RCC_APB2(ADC2),
#if !defined(USE_DMA_SPEC)
.DMAy_Streamx = ADC2_DMA_STREAM,
.channel = DMA_Channel_1
#endif
},
{
.ADCx = ADC3,
.rccADC = RCC_APB2(ADC3),
#if !defined(USE_DMA_SPEC)
.DMAy_Streamx = ADC3_DMA_STREAM,
.channel = DMA_Channel_2
#endif
}
#endif
};

Expand Down Expand Up @@ -212,6 +234,7 @@ void adcInit(const adcConfig_t *config)
}

ADCDevice device = ADC_CFG_TO_DEV(config->device);

if (device == ADCINVALID) {
return;
}
Expand Down Expand Up @@ -280,16 +303,33 @@ void adcInit(const adcConfig_t *config)
ADC_DMACmd(adc.ADCx, ENABLE);
ADC_Cmd(adc.ADCx, ENABLE);

#ifdef USE_DMA_SPEC
const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpec(DMA_PERIPH_ADC, device, config->dmaopt[device]);

if (!dmaSpec) {
return;
}

dmaInit(dmaGetIdentifier(dmaSpec->ref), OWNER_ADC, RESOURCE_INDEX(device));

DMA_DeInit(dmaSpec->ref);
#else
dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0);

DMA_DeInit(adc.DMAy_Streamx);
#endif

DMA_InitTypeDef DMA_InitStructure;

DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&adc.ADCx->DR;

#ifdef USE_DMA_SPEC
DMA_InitStructure.DMA_Channel = dmaSpec->channel;
#else
DMA_InitStructure.DMA_Channel = adc.channel;
#endif

DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)adcValues;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
DMA_InitStructure.DMA_BufferSize = configuredAdcChannels;
Expand All @@ -299,9 +339,14 @@ void adcInit(const adcConfig_t *config)
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_Init(adc.DMAy_Streamx, &DMA_InitStructure);

#ifdef USE_DMA_SPEC
DMA_Init(dmaSpec->ref, &DMA_InitStructure);
DMA_Cmd(dmaSpec->ref, ENABLE);
#else
DMA_Init(adc.DMAy_Streamx, &DMA_InitStructure);
DMA_Cmd(adc.DMAy_Streamx, ENABLE);
#endif

ADC_SoftwareStartConv(adc.ADCx);
}
Expand Down
71 changes: 48 additions & 23 deletions src/main/drivers/adc_stm32f7xx.c
Original file line number Diff line number Diff line change
Expand Up @@ -27,29 +27,19 @@
#ifdef USE_ADC

#include "drivers/accgyro/accgyro.h"
#include "drivers/system.h"

#include "drivers/dma.h"
#include "drivers/dma_reqmap.h"
#include "drivers/io.h"
#include "io_impl.h"
#include "rcc.h"
#include "dma.h"

#include "drivers/io_impl.h"
#include "drivers/rcc.h"
#include "drivers/sensor.h"
#include "drivers/system.h"

#include "adc.h"
#include "adc_impl.h"
#include "drivers/adc.h"
#include "drivers/adc_impl.h"

#include "pg/adc.h"


#ifndef ADC_INSTANCE
#define ADC_INSTANCE ADC1
#endif

#ifndef ADC1_DMA_STREAM
#define ADC1_DMA_STREAM DMA2_Stream4
#endif

// Copied from stm32f7xx_ll_adc.h

#define VREFINT_CAL_VREF ( 3300U) /* Analog voltage reference (Vref+) value with which temperature sensor has been calibrated in production (tolerance: +-10 mV) (unit: mV). */
Expand All @@ -71,9 +61,30 @@
#endif

const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = ADC1_DMA_STREAM, .channel = DMA_CHANNEL_0 },
{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = ADC2_DMA_STREAM, .channel = DMA_CHANNEL_1 },
{ .ADCx = ADC3, .rccADC = RCC_APB2(ADC3), .DMAy_Streamx = ADC3_DMA_STREAM, .channel = DMA_CHANNEL_2 }
{
.ADCx = ADC1,
.rccADC = RCC_APB2(ADC1),
#if !defined(USE_DMA_SPEC)
.DMAy_Streamx = ADC1_DMA_STREAM,
.channel = DMA_CHANNEL_0
#endif
},
{
.ADCx = ADC2,
.rccADC = RCC_APB2(ADC2),
#if !defined(USE_DMA_SPEC)
.DMAy_Streamx = ADC2_DMA_STREAM,
.channel = DMA_CHANNEL_1
#endif
},
{
.ADCx = ADC3,
.rccADC = RCC_APB2(ADC3),
#if !defined(USE_DMA_SPEC)
.DMAy_Streamx = ADC3_DMA_STREAM,
.channel = DMA_CHANNEL_2
#endif
}
};

/* note these could be packed up for saving space */
Expand Down Expand Up @@ -236,7 +247,8 @@ void adcInit(const adcConfig_t *config)
adcOperatingConfig[ADC_CURRENT].tag = config->current.ioTag; //CURRENT_METER_ADC_CHANNEL;
}

ADCDevice device = adcDeviceByInstance(ADC_INSTANCE);
ADCDevice device = ADC_CFG_TO_DEV(config->device);

if (device == ADCINVALID) {
return;
}
Expand Down Expand Up @@ -299,9 +311,22 @@ void adcInit(const adcConfig_t *config)
}
}

dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0);
#ifdef USE_DMA_SPEC
const dmaChannelSpec_t *dmaspec = dmaGetChannelSpec(DMA_PERIPH_ADC, device, config->dmaopt[device]);

if (!dmaspec) {
return;
}

dmaInit(dmaGetIdentifier(dmaspec->ref), OWNER_ADC, 0);
adc.DmaHandle.Init.Channel = dmaspec->channel;
adc.DmaHandle.Instance = dmaspec->ref;
#else
dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0);
adc.DmaHandle.Init.Channel = adc.channel;
adc.DmaHandle.Instance = adc.DMAy_Streamx;
#endif

adc.DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
adc.DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
adc.DmaHandle.Init.MemInc = configuredAdcChannels > 1 ? DMA_MINC_ENABLE : DMA_MINC_DISABLE;
Expand All @@ -313,7 +338,7 @@ void adcInit(const adcConfig_t *config)
adc.DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
adc.DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE;
adc.DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
adc.DmaHandle.Instance = adc.DMAy_Streamx;


if (HAL_DMA_Init(&adc.DmaHandle) != HAL_OK)
{
Expand Down
149 changes: 149 additions & 0 deletions src/main/drivers/dma_reqmap.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,149 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/

#include <stdint.h>

#include "platform.h"

#ifdef USE_DMA_SPEC

#include "drivers/adc.h"
#include "drivers/bus_spi.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"

#include "drivers/dma_reqmap.h"

typedef struct dmaRequestMapping_s {
dmaPeripheral_e device;
uint8_t index;
dmaChannelSpec_t channelSpec[2];
} dmaRequestMapping_t;

#if defined(STM32F4) || defined(STM32F7)
#define D(d, s, c) { DMA_CODE(d, s, c), DMA ## d ## _Stream ## s, c }

static const dmaRequestMapping_t dmaRequestMapping[] = {
#ifdef USE_SPI
// Everything including F405 and F446
{ DMA_PERIPH_SPI_TX, SPIDEV_1, { D(2, 3, 3), D(2, 5, 3) } },
{ DMA_PERIPH_SPI_RX, SPIDEV_1, { D(2, 0, 3), D(2, 2, 3) } },
{ DMA_PERIPH_SPI_TX, SPIDEV_2, { D(1, 4, 0) } },
{ DMA_PERIPH_SPI_RX, SPIDEV_2, { D(1, 3, 0) } },
{ DMA_PERIPH_SPI_TX, SPIDEV_3, { D(1, 5, 0), D(1, 7, 0) } },
{ DMA_PERIPH_SPI_RX, SPIDEV_3, { D(1, 0, 0), D(1, 2, 0) } },

#if defined(STM32F411xE) || defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F765xx) || defined(STM32F722xx)
{ DMA_PERIPH_SPI_TX, SPIDEV_4, { D(2, 1, 4) } },
{ DMA_PERIPH_SPI_RX, SPIDEV_4, { D(2, 0, 4) } },

#ifdef USE_EXTENDED_SPI_DEVICE
{ DMA_PERIPH_SPI_TX, SPIDEV_5, { D(2, 6, 7) } },
{ DMA_PERIPH_SPI_RX, SPIDEV_5, { D(2, 5, 7) } },

#if !defined(STM32F722xx)
{ DMA_PERIPH_SPI_TX, SPIDEV_6, { D(2, 5, 1) } },
{ DMA_PERIPH_SPI_RX, SPIDEV_6, { D(2, 6, 1) } },
#endif
#endif // USE_EXTENDED_SPI_DEVICE
#endif
#endif // USE_SPI

#ifdef USE_ADC
{ DMA_PERIPH_ADC, ADCDEV_1, { D(2, 0, 0), D(2, 4, 0) } },
{ DMA_PERIPH_ADC, ADCDEV_2, { D(2, 2, 1), D(2, 3, 1) } },
{ DMA_PERIPH_ADC, ADCDEV_3, { D(2, 0, 2), D(2, 1, 2) } },
#endif

#ifdef USE_SDCARD_SDIO
{ DMA_PERIPH_SDIO, 0, { D(2, 3, 4), D(2, 6, 4) } },
#endif

#ifdef USE_UART
{ DMA_PERIPH_UART_TX, UARTDEV_1, { D(2, 7, 4) } },
{ DMA_PERIPH_UART_RX, UARTDEV_1, { D(2, 5, 4), D(2, 2, 4) } },
{ DMA_PERIPH_UART_TX, UARTDEV_2, { D(1, 6, 4) } },
{ DMA_PERIPH_UART_RX, UARTDEV_2, { D(1, 5, 4) } },
{ DMA_PERIPH_UART_TX, UARTDEV_3, { D(1, 3, 4) } },
{ DMA_PERIPH_UART_RX, UARTDEV_3, { D(1, 1, 4) } },
{ DMA_PERIPH_UART_TX, UARTDEV_4, { D(1, 4, 4) } },
{ DMA_PERIPH_UART_RX, UARTDEV_4, { D(1, 2, 4) } },
{ DMA_PERIPH_UART_TX, UARTDEV_5, { D(1, 7, 4) } },
{ DMA_PERIPH_UART_RX, UARTDEV_5, { D(1, 0, 4) } },
{ DMA_PERIPH_UART_TX, UARTDEV_6, { D(2, 6, 5), D(2, 7, 5) } },
{ DMA_PERIPH_UART_RX, UARTDEV_6, { D(2, 1, 5), D(2, 2, 5) } },
#endif
};
#undef D
#else // STM32F3
// The embedded ADC24_DMA_REMAP conditional should be removed
// when (and if) F3 is going generic.
#define D(d, c) { DMA_CODE(d, 0, c), DMA ## d ## _Channel ## c }
static const dmaRequestMapping_t dmaRequestMapping[17] = {
#ifdef USE_SPI
{ DMA_PERIPH_SPI_TX, 1, { D(1, 3) } },
{ DMA_PERIPH_SPI_RX, 1, { D(1, 2) } },
{ DMA_PERIPH_SPI_TX, 2, { D(1, 5) } },
{ DMA_PERIPH_SPI_RX, 2, { D(1, 4) } },
{ DMA_PERIPH_SPI_TX, 3, { D(2, 2) } },
{ DMA_PERIPH_SPI_RX, 3, { D(2, 1) } },
#endif

#ifdef USE_ADC
{ DMA_PERIPH_ADC, 1, { D(1, 1) } },
#ifdef ADC24_DMA_REMAP
{ DMA_PERIPH_ADC, 2, { D(2, 3) } },
#else
{ DMA_PERIPH_ADC, 2, { D(2, 1) } },
#endif
{ DMA_PERIPH_ADC, 3, { D(2, 5) } },
#endif

#ifdef USE_UART
{ DMA_PERIPH_UART_TX, 1, { D(1, 4) } },
{ DMA_PERIPH_UART_RX, 1, { D(1, 5) } },

{ DMA_PERIPH_UART_TX, 2, { D(1, 7) } },
{ DMA_PERIPH_UART_RX, 2, { D(1, 6) } },
{ DMA_PERIPH_UART_TX, 3, { D(1, 2) } },
{ DMA_PERIPH_UART_RX, 3, { D(1, 3) } },
{ DMA_PERIPH_UART_TX, 4, { D(2, 5) } },
{ DMA_PERIPH_UART_RX, 4, { D(2, 3) } },
};
#endif
#undef D
#endif

const dmaChannelSpec_t *dmaGetChannelSpec(dmaPeripheral_e device, uint8_t index, int8_t opt)
{
if (opt < 0 || opt >= 2) {
return NULL;
}

for (unsigned i = 0 ; i < ARRAYLEN(dmaRequestMapping) ; i++) {
const dmaRequestMapping_t *periph = &dmaRequestMapping[i];
if (periph->device == device && periph->index == index && periph->channelSpec[opt].ref) {
return &periph->channelSpec[opt];
}
}

return NULL;
}
#endif // USE_DMA_SPEC
Loading

0 comments on commit fe182bb

Please sign in to comment.