From 1b82aae4ee4138305dec0a75d128b50275b2c22b Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Sun, 23 Jul 2017 23:26:38 +0530 Subject: [PATCH 1/4] drivers: add dma based bitbanging driver --- src/drivers/boards/px4fmu-v2/board_config.h | 8 + src/drivers/drv_dma_bitbang.h | 62 ++++++ src/drivers/stm32/CMakeLists.txt | 1 + src/drivers/stm32/drv_dma_bitbang.c | 203 ++++++++++++++++++++ src/drivers/stm32/module.mk | 3 +- 5 files changed, 276 insertions(+), 1 deletion(-) create mode 100644 src/drivers/drv_dma_bitbang.h create mode 100644 src/drivers/stm32/drv_dma_bitbang.c diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 15437d55d11f..c72cf7b23e8d 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -236,6 +236,14 @@ __BEGIN_DECLS #define PWMIN_TIMER_CHANNEL 2 #define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 +/* Extra Purpose DMA */ +//Note: Only DMA2 has access to AHB bus meaning GPIO registers are only modifiable by DMA2, +//refer DMA section of stm32F4 series User manual for further details. +#define DMAMAP_BITBANG STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN6) +#define TIMER_BITBANG 1 +// Refer manual for DMA Request map to select proper timer channel +#define TIMER_BITBANG_CH 1 +#define BITBANG_GPIO_REG_BASE STM32_GPIOE_BASE /**************************************************************************************************** * Public Types ****************************************************************************************************/ diff --git a/src/drivers/drv_dma_bitbang.h b/src/drivers/drv_dma_bitbang.h new file mode 100644 index 000000000000..f15debe61f2c --- /dev/null +++ b/src/drivers/drv_dma_bitbang.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_dma_bb.c + * + * DMA based Bit Bang Support + * Author: Siddharth Bharat Purohit + * + */ + +#pragma once +#include +#include +#include + +#include +#include +#include + +#include +#include + +__BEGIN_DECLS + +/* Initialise DMA based Bitbanging scheme*/ +__EXPORT extern void dma_bb_init(uint32_t mult, uint32_t offset); + +/* Send buffer containing pin states to be sent immediately */ +__EXPORT extern void dma_bb_send_buff(uint32_t* dat, uint32_t len, dma_callback_t callback, void* arg); + +__END_DECLS diff --git a/src/drivers/stm32/CMakeLists.txt b/src/drivers/stm32/CMakeLists.txt index b7b3825aa88c..41c3edd80217 100644 --- a/src/drivers/stm32/CMakeLists.txt +++ b/src/drivers/stm32/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( drv_io_timer.c drv_pwm_servo.c drv_input_capture.c + drv_dma_bitbang.c DEPENDS platforms__common ) diff --git a/src/drivers/stm32/drv_dma_bitbang.c b/src/drivers/stm32/drv_dma_bitbang.c new file mode 100644 index 000000000000..d6af95162af9 --- /dev/null +++ b/src/drivers/stm32/drv_dma_bitbang.c @@ -0,0 +1,203 @@ +/* + This program is free software: you can redistribute it and/or modify + it 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. + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/** + * @file px4fmu_dma_bb.c + * + * DMA based Bit Bang driver + * Author: Siddharth Bharat Purohit + * + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" + +#ifdef DMAMAP_BITBANG + +#if TIMER_BITBANG == 1 +# define DMA_TIMER_BASE STM32_TIM1_BASE +# define DMA_TIMER_POWER_REG STM32_RCC_APB1ENR +# define DMA_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define DMA_TIMER_VECTOR STM32_IRQ_TIM1CC +# define DMA_TIMER_CLOCK STM32_APB2_TIM1_CLKIN +#elif TIMER_BITBANG == 8 +# define DMA_TIMER_BASE STM32_TIM8_BASE +# define DMA_TIMER_POWER_REG STM32_RCC_APB2ENR +# define DMA_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN +# define DMA_TIMER_VECTOR STM32_IRQ_TIM8CC +# define DMA_TIMER_CLOCK STM32_APB2_TIM8_CLKIN +#else +# error must select Timer 1 or Timer 8 DMA2 doesnot support any other +#endif + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(DMA_TIMER_BASE + _reg)) + +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) + + +#if TIMER_BITBANG_CH == 1 +# define rCCR_DMA rCCR1 /* compare register */ +# define DIER_DMA_INT GTIM_DIER_CC1IE /* interrupt enable */ +# define DIER_DMA_REQ GTIM_DIER_CC1DE /* DMA request enable */ +# define SR_INT_DMA GTIM_SR_CC1IF /* interrupt status */ +#elif TIMER_BITBANG_CH == 2 +# define rCCR_DMA rCCR2 /* compare register */ +# define DIER_DMA_INT GTIM_DIER_CC2IE /* interrupt enable */ +# define DIER_DMA_REQ GTIM_DIER_CC2DE /* DMA request enable */ +# define SR_INT_DMA GTIM_SR_CC2IF /* interrupt status */ +#elif TIMER_BITBANG_CH == 3 +# define rCCR_DMA rCCR3 /* compare register */ +# define DIER_DMA_INT GTIM_DIER_CC3IE /* interrupt enable */ +# define DIER_DMA_REQ GTIM_DIER_CC3DE /* DMA request enable */ +# define SR_INT_DMA GTIM_SR_CC3IF /* interrupt status */ +#elif TIMER_BITBANG_CH == 4 +# define rCCR_DMA rCCR4 /* compare register */ +# define DIER_DMA_INT GTIM_DIER_CC4IE /* interrupt enable */ +# define DIER_DMA_REQ GTIM_DIER_CC4DE /* DMA request enable */ +# define SR_INT_DMA GTIM_SR_CC4IF /* interrupt status */ +#else +# error TIMER_BITBANG_CH must be a value between 1 and 4 +#endif + +static DMA_HANDLE tx_dma; + +static void dma_reset(void); +volatile static bool dma_bitbang_initialized = false; + +/* GPIO register accessors */ +#define GPIO_REG(_x) (*(volatile uint32_t *)(BITBANG_GPIO_REG_BASE + _x)) +#define rBSSR GPIO_REG(STM32_GPIO_BSRR_OFFSET) + +// The rate of timer will be set per following equation +// Timer_Rate = {(16.8*mult + offset)/168} Mhz +// The fastest clock which can be generated would be: +// Clk_fast = Timer_Rate/4 Hz +void dma_bb_init(uint32_t mult, uint32_t offset) +{ + /* clock/power on our timer */ + modifyreg32(DMA_TIMER_POWER_REG, 0, DMA_TIMER_POWER_BIT); + + tx_dma = stm32_dmachannel(DMAMAP_BITBANG); + + /* configure the timer according to the requested factors*/ + rPSC = roundf(16.8f * mult + offset) - 1; + + /* run to only one increment*/ + rARR = 1; + + rCCR_DMA = 1; + + rCR1 = 0; + rCR2 = 0; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + dma_bitbang_initialized = true; + printf("[init] Starting DMA bitbang driver \n"); +} + +/* +Sends Set or Reset data to GPIO Register popping data from top at every timer compare + + -- "callback" is called after completion the set of requested pulse is transmitted through + to GPIO bus, + -- with "arg" being the argument to the callback + -- "dat" contains the list of pin states over time, data is sent to GPIOx_BSSR register which + has following behaviour: + + -- Bits 31:16 BRx + 0: No action + 1: Reset the corresponding GPIOx O/P bit + + -- Bits 15:0 BSx + 0: No action + 1: Set the corresponding GPIOx O/P bit + +Note: If both BSx and BRx are set, BSx has priority. +*/ +void dma_bb_send_buff(uint32_t* dat, uint32_t len, dma_callback_t callback, void* arg) +{ + if (!dma_bitbang_initialized) { + return; + } + /* unconfigure the timer request */ + rDIER &= ~DIER_DMA_REQ; + dma_reset(); + + stm32_dmasetup( + tx_dma, + (uint32_t)&rBSSR, + (uint32_t)dat, + len, + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_32BITS | + DMA_SCR_MSIZE_32BITS); + stm32_dmastart(tx_dma, callback, arg, false); + + /* Enable Timer request */ + rDIER |= DIER_DMA_REQ; +} + +static void +dma_reset(void) +{ + /* kill any pending DMA */ + stm32_dmastop(tx_dma); +} + +#endif //#ifdef DMAMAP_BITBANG diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk index 7c3d30d33532..58e4bd44d13b 100644 --- a/src/drivers/stm32/module.mk +++ b/src/drivers/stm32/module.mk @@ -40,7 +40,8 @@ SRCS = drv_hrt.c \ drv_pwm_servo.c \ drv_io_timer.c \ - drv_input_capture.c + drv_input_capture.c \ + drv_dma_bitbang.c INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common From fc09be86a61d91bd5a625e7589d0f2d507a27361 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Thu, 3 Aug 2017 17:10:54 +0530 Subject: [PATCH 2/4] add ledbin to the list of files to be ignored for pruning --- Tools/px_romfs_pruner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 67f0b57cbfd6..f0f349a4af47 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -59,7 +59,7 @@ def main(): for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file \ + if ".zip" in file or ".bin" in file or ".ledbin" in file or ".swp" in file \ or ".data" in file or ".DS_Store" in file \ or file.startswith("."): continue From c7de80aecaf6300557bbb41263234210995c9a82 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 31 Aug 2017 21:39:53 +0530 Subject: [PATCH 3/4] px4-v2: move to use TIM5 for HRT to make space for dma bitbanging on TIM8 --- nuttx-configs/px4fmu-v2/include/board.h | 2 +- src/drivers/boards/px4fmu-v2/board_config.h | 10 +++++----- src/drivers/stm32/drv_dma_bitbang.c | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index d73803b85dc5..8e5824260988 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -225,7 +225,7 @@ /* UART RX DMA configurations */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 -#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1 /* * CAN diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index c72cf7b23e8d..c2eb41b64ac6 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -67,7 +67,7 @@ __BEGIN_DECLS #define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ #define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 #define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2 -#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2 +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_1 #define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY #define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ @@ -228,7 +228,7 @@ __BEGIN_DECLS #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ -#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER 5 /* use timer5 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ @@ -239,11 +239,11 @@ __BEGIN_DECLS /* Extra Purpose DMA */ //Note: Only DMA2 has access to AHB bus meaning GPIO registers are only modifiable by DMA2, //refer DMA section of stm32F4 series User manual for further details. -#define DMAMAP_BITBANG STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN6) -#define TIMER_BITBANG 1 +#define DMAMAP_BITBANG STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN7) +#define TIMER_BITBANG 8 // Refer manual for DMA Request map to select proper timer channel #define TIMER_BITBANG_CH 1 -#define BITBANG_GPIO_REG_BASE STM32_GPIOE_BASE +#define BITBANG_GPIO_REG_BASE STM32_GPIOD_BASE /**************************************************************************************************** * Public Types ****************************************************************************************************/ diff --git a/src/drivers/stm32/drv_dma_bitbang.c b/src/drivers/stm32/drv_dma_bitbang.c index d6af95162af9..4d914366a5ce 100644 --- a/src/drivers/stm32/drv_dma_bitbang.c +++ b/src/drivers/stm32/drv_dma_bitbang.c @@ -46,7 +46,7 @@ #if TIMER_BITBANG == 1 # define DMA_TIMER_BASE STM32_TIM1_BASE -# define DMA_TIMER_POWER_REG STM32_RCC_APB1ENR +# define DMA_TIMER_POWER_REG STM32_RCC_APB2ENR # define DMA_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN # define DMA_TIMER_VECTOR STM32_IRQ_TIM1CC # define DMA_TIMER_CLOCK STM32_APB2_TIM1_CLKIN From 937aac936776faa720293f1a21c1ea9a0641a6db Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 10 Sep 2017 20:00:35 +0530 Subject: [PATCH 4/4] driver: fix order of specifiers --- src/drivers/stm32/drv_dma_bitbang.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_dma_bitbang.c b/src/drivers/stm32/drv_dma_bitbang.c index 4d914366a5ce..06697fbe655a 100644 --- a/src/drivers/stm32/drv_dma_bitbang.c +++ b/src/drivers/stm32/drv_dma_bitbang.c @@ -112,7 +112,7 @@ static DMA_HANDLE tx_dma; static void dma_reset(void); -volatile static bool dma_bitbang_initialized = false; +static volatile bool dma_bitbang_initialized = false; /* GPIO register accessors */ #define GPIO_REG(_x) (*(volatile uint32_t *)(BITBANG_GPIO_REG_BASE + _x))