From cac80f1c650a2caf248e793edede899d608b014e Mon Sep 17 00:00:00 2001 From: Markus Koch Date: Tue, 23 Feb 2021 19:05:43 +0100 Subject: [PATCH] Add initial version of firmware --- .gitmodules | 3 + firmware/.gitignore | 2 + firmware/LICENSE | 30 + firmware/Makefile | 77 ++ firmware/adc.c | 159 ++++ firmware/adc.h | 15 + firmware/ccconfig.h | 10 + firmware/colorchord | 1 + firmware/flash.cfg | 26 + firmware/lib/stm32f410.ld | 114 +++ firmware/lib/stm32f4xx_conf.h | 122 +++ firmware/lib/system_stm32f4xx.c | 1222 +++++++++++++++++++++++++++++++ firmware/lib/system_stm32f4xx.h | 105 +++ firmware/lib/systems.c | 264 +++++++ firmware/lib/systems.h | 65 ++ firmware/main.c | 195 +++++ firmware/spi2812.c | 334 +++++++++ firmware/spi2812.h | 15 + firmware/stm32f4xx_it.c | 177 +++++ firmware/stm32f4xx_it.h | 58 ++ 20 files changed, 2994 insertions(+) create mode 100644 .gitmodules create mode 100644 firmware/.gitignore create mode 100644 firmware/LICENSE create mode 100644 firmware/Makefile create mode 100644 firmware/adc.c create mode 100644 firmware/adc.h create mode 100644 firmware/ccconfig.h create mode 160000 firmware/colorchord create mode 100644 firmware/flash.cfg create mode 100644 firmware/lib/stm32f410.ld create mode 100644 firmware/lib/stm32f4xx_conf.h create mode 100644 firmware/lib/system_stm32f4xx.c create mode 100644 firmware/lib/system_stm32f4xx.h create mode 100644 firmware/lib/systems.c create mode 100644 firmware/lib/systems.h create mode 100644 firmware/main.c create mode 100644 firmware/spi2812.c create mode 100644 firmware/spi2812.h create mode 100644 firmware/stm32f4xx_it.c create mode 100644 firmware/stm32f4xx_it.h diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..649a0a6 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "firmware/colorchord"] + path = firmware/colorchord + url = https://github.com/cnlohr/colorchord.git diff --git a/firmware/.gitignore b/firmware/.gitignore new file mode 100644 index 0000000..f0e72cc --- /dev/null +++ b/firmware/.gitignore @@ -0,0 +1,2 @@ +*.bin +*.out diff --git a/firmware/LICENSE b/firmware/LICENSE new file mode 100644 index 0000000..5c49efe --- /dev/null +++ b/firmware/LICENSE @@ -0,0 +1,30 @@ +Note: This contains libraries from CM and ST that are under a different +license than this file. See those folders respectively. + + + +STM32F303-based ColorChord 2.0 Build + +Copyright (c) 2015 Charles Lohr + +Roughly based off of: https://github.com/devthrash/0xWS2812 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + + diff --git a/firmware/Makefile b/firmware/Makefile new file mode 100644 index 0000000..64d98b3 --- /dev/null +++ b/firmware/Makefile @@ -0,0 +1,77 @@ +TARGET=main +all: $(TARGET).bin + +PREFIX=arm-none-eabi + +CC=$(PREFIX)-gcc +LD=$(PREFIX)-gcc +AS=$(PREFIX)-as +CP=$(PREFIX)-objcopy +OD=$(PREFIX)-objdump + + +OBJCOPYFLAGS = -O binary + +BIN=$(CP) -O ihex + +CCBASE = colorchord/embeddedstm32f407 +DEFS = -DSTM32F40_41xxx -DHSE_VALUE=8000000 +STARTUP = $(CCBASE)/lib/startup_stm32f40_41xxx.s +STLIB = $(CCBASE)/STM32F4xx_StdPeriph_Driver +EMCOM = $(CCBASE)/../embeddedcommon +MCU = cortex-m3 +MCFLAGS = -mcpu=$(MCU) -mthumb -mlittle-endian -mthumb-interwork + +STM32_INCLUDES = -Ilib -I. -I$(STLIB)/inc -I$(CCBASE)/lib + + +OPTIMIZE = -Os + +CFLAGS = $(MCFLAGS) $(OPTIMIZE) $(DEFS) \ + -I. \ + $(STM32_INCLUDES) \ + -I$(EMCOM) \ + -Wl,-T,lib/stm32f410.ld + +CFLAGS+=-DDEBUG +CFLAGS+=-I. + +AFLAGS = $(MCFLAGS) + +SRC = main.c \ + stm32f4xx_it.c \ + lib/system_stm32f4xx.c \ + lib/systems.c \ + spi2812.c \ + adc.c \ + $(STLIB)/src/misc.c \ + $(STLIB)/src/stm32f4xx_dma.c \ + $(STLIB)/src/stm32f4xx_spi.c \ + $(STLIB)/src/stm32f4xx_syscfg.c \ + $(STLIB)/src/stm32f4xx_exti.c \ + $(STLIB)/src/stm32f4xx_rcc.c \ + $(STLIB)/src/stm32f4xx_gpio.c \ + $(STLIB)/src/stm32f4xx_usart.c \ + $(STLIB)/src/stm32f4xx_adc.c \ + $(STLIB)/src/stm32f4xx_tim.c \ + $(EMCOM)/DFT32.c \ + $(EMCOM)/embeddednf.c \ + $(EMCOM)/embeddedout.c + + +burn : $(TARGET).bin + openocd -f flash.cfg #-d3 +terminal : + openocd -f terminal.cfg + +$(TARGET).bin : $(TARGET).out + $(CP) $(OBJCOPYFLAGS) $< $@ + +$(TARGET).hex: $(EXECUTABLE) + $(CP) -O ihex $^ $@ + +$(TARGET).out : $(SRC) $(STARTUP) + $(CC) $(CFLAGS) $^ -lm -lc -lnosys -o $@ + +clean: + rm -f $(TARGET).lst $(TARGET).out $(TARGET).hex $(TARGET).bin $(TARGET).map $(EXECUTABLE) diff --git a/firmware/adc.c b/firmware/adc.c new file mode 100644 index 0000000..3b266bf --- /dev/null +++ b/firmware/adc.c @@ -0,0 +1,159 @@ +#include "adc.h" +#include +#include +#include +#include +#include "systems.h" +#include + +volatile uint16_t adc_buffer[2] = {0,}; + +void adc_init() +{ + ADC_CommonInitTypeDef ADC_CommonInitStruct; + ADC_InitTypeDef ADC_InitStructure; + GPIO_InitTypeDef GPIO_InitStruct; + + printf("ADC init\r\n"); + + // Enable Clocks + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); + RCC_AHB1PeriphResetCmd(RCC_AHB1Periph_DMA1, ENABLE); + + // Set up GPIO + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AN; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + // Set up timer + TIM_TimeBaseInitTypeDef Timer_InitStruct; + TIM_TimeBaseStructInit(&Timer_InitStruct); + Timer_InitStruct.TIM_Prescaler = 0; + Timer_InitStruct.TIM_CounterMode = TIM_CounterMode_Up; + Timer_InitStruct.TIM_Period = (ADC_F_TIM / ADC_SAMPLE_RATE); + Timer_InitStruct.TIM_ClockDivision = TIM_CKD_DIV1; + Timer_InitStruct.TIM_RepetitionCounter = 0; + TIM_TimeBaseInit(TIM5, &Timer_InitStruct); + TIM_Cmd(TIM5, ENABLE); + + TIM_OCInitTypeDef TIM_OCInitStructure; + TIM_OCStructInit (&TIM_OCInitStructure); + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCNIdleState_Reset; + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Active; + TIM_OC1Init (TIM5, &TIM_OCInitStructure); + + TIM_SetCompare1(TIM5, 0xFFFF); + TIM_ITConfig(TIM5, TIM_IT_CC1, ENABLE); + + TIM_CCxCmd(TIM5, TIM_Channel_1, TIM_CCx_Enable); + TIM_CCxNCmd(TIM5, TIM_Channel_1, TIM_CCx_Enable); + + NVIC_InitTypeDef NVIC_InitStruct; + NVIC_InitStruct.NVIC_IRQChannel = TIM5_IRQn; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStruct); + + // Set up DMA + DMA_InitTypeDef DMA_InitStruct; + + DMA_StructInit(&DMA_InitStruct); + DMA_DeInit(ADC_RX_DMA_STREAM); + + DMA_InitStruct.DMA_Channel = ADC_RX_DMA_CHANNEL; + DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t) & (ADC1->DR); + DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t)adc_buffer; + DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMA_InitStruct.DMA_BufferSize = 2; + DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMA_InitStruct.DMA_Mode = DMA_Mode_Circular; + DMA_InitStruct.DMA_Priority = DMA_Priority_High; + DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStruct.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; + DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(ADC_RX_DMA_STREAM, &DMA_InitStruct); + + DMA_ITConfig(ADC_RX_DMA_STREAM, DMA_IT_TC, ENABLE); + + NVIC_InitStruct.NVIC_IRQChannel = ADC_RX_DMA_IRQn; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStruct); + + DMA_Cmd(ADC_RX_DMA_STREAM, ENABLE); + + // Set up ADC + ADC_DeInit(); + + ADC_CommonStructInit(&ADC_CommonInitStruct); + ADC_CommonInitStruct.ADC_Mode = ADC_Mode_Independent; + ADC_CommonInitStruct.ADC_Prescaler = ADC_Prescaler_Div8; + ADC_CommonInitStruct.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + ADC_CommonInitStruct.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_20Cycles; + ADC_CommonInit(&ADC_CommonInitStruct); + + ADC_StructInit(&ADC_InitStructure); + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ScanConvMode = ENABLE; + ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; // TODO: trig via event + ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T5_CC1; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = 2; + ADC_Init(ADC1, &ADC_InitStructure); + ADC_ITConfig(ADC1, ADC_IT_EOC, ENABLE); + + NVIC_InitStruct.NVIC_IRQChannel = ADC_IRQn; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelCmd = DISABLE; // ! + NVIC_Init(&NVIC_InitStruct); + + ADC_DMARequestAfterLastTransferCmd(ADC1, ENABLE); + + ADC_EOCOnEachRegularChannelCmd(ADC1, DISABLE); + ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 1, ADC_SampleTime_28Cycles); + ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 2, ADC_SampleTime_28Cycles); + + // Turn it all on + ADC_Cmd(ADC1, ENABLE); + ADC_DMACmd(ADC1, ENABLE); + + printf("adc init done.\r\n"); + printf("ADC CR1: %08x, CR2: %08x\r\n", ADC1->CR1, ADC1->CR2); + printf("ADC SQR1: %08x, SQR3: %08x\r\n", ADC1->SQR1, ADC1->SQR3); + printf("DMA2 S0CR: %08x\r\n", ADC_RX_DMA_STREAM->CR); + printf("ADC init complete.\r\n"); +} + +void DMA2_Stream0_IRQHandler() +{ + int sample; + DMA_ClearFlag(ADC_RX_DMA_STREAM, DMA_FLAG_TCIF0); + //printf("[DMA-IRQ] %d, %d\r\n", adc_buffer[0], adc_buffer[1]); + sample = 2048 - ((adc_buffer[0] + adc_buffer[1]) / 2); + GotSample(sample); +} + +// Not used: +void ADC_IRQHandler() +{ + ADC_ClearFlag(ADC1, ADC_FLAG_EOC); + //printf("[ADC-IRQ] %d, %d\r\n", adc_buffer[0], adc_buffer[1]); +} + +void TIM5_IRQHandler() +{ + TIM_ClearFlag(TIM5, TIM_FLAG_CC1); + // I would really like to this in HW via events, but somehow it's broken... + ADC_SoftwareStartConv(ADC1); +} diff --git a/firmware/adc.h b/firmware/adc.h new file mode 100644 index 0000000..e67ec12 --- /dev/null +++ b/firmware/adc.h @@ -0,0 +1,15 @@ +#ifndef _ADC_H +#define _ADC_H + +#define ADC_F_TIM 50000000 +#define ADC_SAMPLE_RATE 44130 + + +#define ADC_RX_DMA_CHANNEL DMA_Channel_0 +#define ADC_RX_DMA_STREAM DMA2_Stream0 +#define ADC_RX_DMA_IRQn DMA2_Stream0_IRQn +// TODO: Update the IRQ handler in adc.c when updating the stream / channel. + +void adc_init(); + +#endif diff --git a/firmware/ccconfig.h b/firmware/ccconfig.h new file mode 100644 index 0000000..7927f89 --- /dev/null +++ b/firmware/ccconfig.h @@ -0,0 +1,10 @@ +#ifndef _CCCONFIG_H +#define _CCCONFIG_H + +#define CCEMBEDDED +#define NUM_LIN_LEDS 60 +#define USE_NUM_LIN_LEDS 60 +#define DFREQ 44130 + +#endif + diff --git a/firmware/colorchord b/firmware/colorchord new file mode 160000 index 0000000..9cae6dc --- /dev/null +++ b/firmware/colorchord @@ -0,0 +1 @@ +Subproject commit 9cae6dc38d288618d7a7163a0e9abcb4fb9234a2 diff --git a/firmware/flash.cfg b/firmware/flash.cfg new file mode 100644 index 0000000..a7fdc2c --- /dev/null +++ b/firmware/flash.cfg @@ -0,0 +1,26 @@ +set TRANSPORT hla_swd + +source [find interface/ftdi/olimex-arm-usb-ocd-h.cfg] +source [find target/stm32f4x.cfg] + +#reset_config none separate +#reset_config srst_push_pull +#separate trst_push_pull + +#reset_config srst_nogate + +init + +reset halt +halt 20 +reset +sleep 10 +reset halt +flash write_image erase main.bin 0x08000000 +sleep 10 +reset +sleep 10 +#arm semihosting enable +reset run +sleep 10 +shutdown diff --git a/firmware/lib/stm32f410.ld b/firmware/lib/stm32f410.ld new file mode 100644 index 0000000..8203d82 --- /dev/null +++ b/firmware/lib/stm32f410.ld @@ -0,0 +1,114 @@ +/*. Entry Point *./ +ENTRY( Reset_Handler ) + +/* Highest address of the user mode stack .*/ + +_estack = 0x20008000; /* end of 32K RAM */ + +/* Generate a link error if heap and s tack dont fit int o RAM */ + +_Min_Heap_Size = 0; /* required amount of heap .*/ +_Min_Stack_Size = 0x200; /* required amount of stack .*/ + +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 32K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + /*RAM_BACKUP (rw) : ORIGIN = 0x40024000, LENGTH = 4K*/ +} + +SECTIONS +{ + + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; + *(.data) + *(.data*) + + . = ALIGN(4); + _edata = .; + } >RAM AT> FLASH + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + _sbss = .; + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; + __bss_end__ = _ebss; + } >RAM +} + + diff --git a/firmware/lib/stm32f4xx_conf.h b/firmware/lib/stm32f4xx_conf.h new file mode 100644 index 0000000..8e473e7 --- /dev/null +++ b/firmware/lib/stm32f4xx_conf.h @@ -0,0 +1,122 @@ +/** + ****************************************************************************** + * @file ADC/ADC_DMA/stm32f4xx_conf.h + * @author MCD Application Team + * @version V1.5.0 + * @date 06-March-2015 + * @brief Library configuration file. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F4xx_CONF_H +#define __STM32F4xx_CONF_H + +/* Includes ------------------------------------------------------------------*/ +/* Uncomment the line below to enable peripheral header file inclusion */ +#include "stm32f4xx_adc.h" +#include "stm32f4xx_crc.h" +#include "stm32f4xx_dbgmcu.h" +#include "stm32f4xx_dma.h" +#include "stm32f4xx_exti.h" +#include "stm32f4xx_flash.h" +#include "stm32f4xx_gpio.h" +#include "stm32f4xx_i2c.h" +#include "stm32f4xx_iwdg.h" +#include "stm32f4xx_pwr.h" +#include "stm32f4xx_rcc.h" +#include "stm32f4xx_rtc.h" +#include "stm32f4xx_sdio.h" +#include "stm32f4xx_spi.h" +#include "stm32f4xx_syscfg.h" +#include "stm32f4xx_tim.h" +#include "stm32f4xx_usart.h" +#include "stm32f4xx_wwdg.h" +#include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ + +#if defined (STM32F429_439xx) +#include "stm32f4xx_cryp.h" +#include "stm32f4xx_hash.h" +#include "stm32f4xx_rng.h" +#include "stm32f4xx_can.h" +#include "stm32f4xx_dac.h" +#include "stm32f4xx_dcmi.h" +#include "stm32f4xx_dma2d.h" +#include "stm32f4xx_fmc.h" +#include "stm32f4xx_ltdc.h" +#include "stm32f4xx_sai.h" +#endif /* STM32F429_439xx */ + +#if defined (STM32F427_437xx) +#include "stm32f4xx_cryp.h" +#include "stm32f4xx_hash.h" +#include "stm32f4xx_rng.h" +#include "stm32f4xx_can.h" +#include "stm32f4xx_dac.h" +#include "stm32f4xx_dcmi.h" +#include "stm32f4xx_dma2d.h" +#include "stm32f4xx_fmc.h" +#include "stm32f4xx_sai.h" +#endif /* STM32F427_437xx */ + +#if defined (STM32F40_41xxx) +#include "stm32f4xx_cryp.h" +#include "stm32f4xx_hash.h" +#include "stm32f4xx_rng.h" +#include "stm32f4xx_can.h" +#include "stm32f4xx_dac.h" +#include "stm32f4xx_dcmi.h" +#include "stm32f4xx_fsmc.h" +#endif /* STM32F40_41xxx */ + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/* If an external clock source is used, then the value of the following define + should be set to the value of the external clock source, else, if no external + clock is used, keep this define commented */ +/*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */ + + +/* Uncomment the line below to expanse the "assert_param" macro in the + Standard Peripheral Library drivers code */ +/* #define USE_FULL_ASSERT 1 */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT + +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ + #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ + void assert_failed(uint8_t* file, uint32_t line); +#else + #define assert_param(expr) ((void)0) +#endif /* USE_FULL_ASSERT */ + +#endif /* __STM32F4xx_CONF_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/firmware/lib/system_stm32f4xx.c b/firmware/lib/system_stm32f4xx.c new file mode 100644 index 0000000..edfcd84 --- /dev/null +++ b/firmware/lib/system_stm32f4xx.c @@ -0,0 +1,1222 @@ +/** + ****************************************************************************** + * @file system_stm32f4xx.c + * @author MCD Application Team + * @version V1.5.0 + * @date 06-March-2015 + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * This file contains the system clock configuration for STM32F4xx devices. + * + * 1. This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier + * and Divider factors, AHB/APBx prescalers and Flash settings), + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f4xx.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * 2. After each device reset the HSI (16 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32f4xx.s" file, to + * configure the system clock before to branch to main program. + * + * 3. If the system clock source selected by user fails to startup, the SystemInit() + * function will do nothing and HSI still used as system clock source. User can + * add some code to deal with this issue inside the SetSysClock() function. + * + * 4. The default value of HSE crystal is set to 25MHz, refer to "HSE_VALUE" define + * in "stm32f4xx.h" file. When HSE is used as system clock source, directly or + * through PLL, and you are using different crystal you have to adapt the HSE + * value to your own configuration. + * + * 5. This file configures the system clock as follows: + *============================================================================= + *============================================================================= + * Supported STM32F40xxx/41xxx devices + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 168000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 168000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 4 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 25000000 + *----------------------------------------------------------------------------- + * PLL_M | 25 + *----------------------------------------------------------------------------- + * PLL_N | 336 + *----------------------------------------------------------------------------- + * PLL_P | 2 + *----------------------------------------------------------------------------- + * PLL_Q | 7 + *----------------------------------------------------------------------------- + * PLLI2S_N | NA + *----------------------------------------------------------------------------- + * PLLI2S_R | NA + *----------------------------------------------------------------------------- + * I2S input clock | NA + *----------------------------------------------------------------------------- + * VDD(V) | 3.3 + *----------------------------------------------------------------------------- + * Main regulator output voltage | Scale1 mode + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 5 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + * Instruction cache | ON + *----------------------------------------------------------------------------- + * Data cache | ON + *----------------------------------------------------------------------------- + * Require 48MHz for USB OTG FS, | Disabled + * SDIO and RNG clock | + *----------------------------------------------------------------------------- + *============================================================================= + *============================================================================= + * Supported STM32F42xxx/43xxx devices + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 180000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 180000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 4 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 25000000 + *----------------------------------------------------------------------------- + * PLL_M | 25 + *----------------------------------------------------------------------------- + * PLL_N | 360 + *----------------------------------------------------------------------------- + * PLL_P | 2 + *----------------------------------------------------------------------------- + * PLL_Q | 7 + *----------------------------------------------------------------------------- + * PLLI2S_N | NA + *----------------------------------------------------------------------------- + * PLLI2S_R | NA + *----------------------------------------------------------------------------- + * I2S input clock | NA + *----------------------------------------------------------------------------- + * VDD(V) | 3.3 + *----------------------------------------------------------------------------- + * Main regulator output voltage | Scale1 mode + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 5 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + * Instruction cache | ON + *----------------------------------------------------------------------------- + * Data cache | ON + *----------------------------------------------------------------------------- + * Require 48MHz for USB OTG FS, | Disabled + * SDIO and RNG clock | + *----------------------------------------------------------------------------- + *============================================================================= + *============================================================================= + * Supported STM32F401xx devices + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 84000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 84000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 1 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 25000000 + *----------------------------------------------------------------------------- + * PLL_M | 25 + *----------------------------------------------------------------------------- + * PLL_N | 336 + *----------------------------------------------------------------------------- + * PLL_P | 4 + *----------------------------------------------------------------------------- + * PLL_Q | 7 + *----------------------------------------------------------------------------- + * PLLI2S_N | NA + *----------------------------------------------------------------------------- + * PLLI2S_R | NA + *----------------------------------------------------------------------------- + * I2S input clock | NA + *----------------------------------------------------------------------------- + * VDD(V) | 3.3 + *----------------------------------------------------------------------------- + * Main regulator output voltage | Scale1 mode + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 2 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + * Instruction cache | ON + *----------------------------------------------------------------------------- + * Data cache | ON + *----------------------------------------------------------------------------- + * Require 48MHz for USB OTG FS, | Disabled + * SDIO and RNG clock | + *----------------------------------------------------------------------------- + *============================================================================= + *============================================================================= + * Supported STM32F411xx devices + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSI) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 100000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 100000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 2 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 1 + *----------------------------------------------------------------------------- + * HSI Frequency(Hz) | 16000000 + *----------------------------------------------------------------------------- + * PLL_M | 16 + *----------------------------------------------------------------------------- + * PLL_N | 400 + *----------------------------------------------------------------------------- + * PLL_P | 4 + *----------------------------------------------------------------------------- + * PLL_Q | 7 + *----------------------------------------------------------------------------- + * PLLI2S_N | NA + *----------------------------------------------------------------------------- + * PLLI2S_R | NA + *----------------------------------------------------------------------------- + * I2S input clock | NA + *----------------------------------------------------------------------------- + * VDD(V) | 3.3 + *----------------------------------------------------------------------------- + * Main regulator output voltage | Scale1 mode + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 3 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + * Instruction cache | ON + *----------------------------------------------------------------------------- + * Data cache | ON + *----------------------------------------------------------------------------- + * Require 48MHz for USB OTG FS, | Disabled + * SDIO and RNG clock | + *----------------------------------------------------------------------------- + *============================================================================= + *============================================================================= + * Supported STM32F446xx devices + *----------------------------------------------------------------------------- + * System Clock source | PLL (HSE) + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 180000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 180000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 4 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 2 + *----------------------------------------------------------------------------- + * HSE Frequency(Hz) | 8000000 + *----------------------------------------------------------------------------- + * PLL_M | 8 + *----------------------------------------------------------------------------- + * PLL_N | 360 + *----------------------------------------------------------------------------- + * PLL_P | 2 + *----------------------------------------------------------------------------- + * PLL_Q | 7 + *----------------------------------------------------------------------------- + * PLL_R | NA + *----------------------------------------------------------------------------- + * PLLI2S_M | NA + *----------------------------------------------------------------------------- + * PLLI2S_N | NA + *----------------------------------------------------------------------------- + * PLLI2S_P | NA + *----------------------------------------------------------------------------- + * PLLI2S_Q | NA + *----------------------------------------------------------------------------- + * PLLI2S_R | NA + *----------------------------------------------------------------------------- + * I2S input clock | NA + *----------------------------------------------------------------------------- + * VDD(V) | 3.3 + *----------------------------------------------------------------------------- + * Main regulator output voltage | Scale1 mode + *----------------------------------------------------------------------------- + * Flash Latency(WS) | 5 + *----------------------------------------------------------------------------- + * Prefetch Buffer | ON + *----------------------------------------------------------------------------- + * Instruction cache | ON + *----------------------------------------------------------------------------- + * Data cache | ON + *----------------------------------------------------------------------------- + * Require 48MHz for USB OTG FS, | Disabled + * SDIO and RNG clock | + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f4xx_system + * @{ + */ + +/** @addtogroup STM32F4xx_System_Private_Includes + * @{ + */ + +#include "stm32f4xx.h" + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Defines + * @{ + */ + +/************************* Miscellaneous Configuration ************************/ +/*!< Uncomment the following line if you need to use external SRAM or SDRAM mounted + on STM324xG_EVAL/STM324x7I_EVAL/STM324x9I_EVAL boards as data memory */ +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) +/* #define DATA_IN_ExtSRAM */ +#endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx */ + +#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) +/* #define DATA_IN_ExtSDRAM */ +#endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx */ + +#if defined(STM32F411xE) +/*!< Uncomment the following line if you need to clock the STM32F411xE by HSE Bypass + through STLINK MCO pin of STM32F103 microcontroller. The frequency cannot be changed + and is fixed at 8 MHz. + Hardware configuration needed for Nucleo Board: + \96 SB54, SB55 OFF + \96 R35 removed + \96 SB16, SB50 ON */ +/* #define USE_HSE_BYPASS */ + +#if defined(USE_HSE_BYPASS) +#define HSE_BYPASS_INPUT_FREQUENCY 8000000 +#endif /* USE_HSE_BYPASS */ +#endif /* STM32F411xE */ + +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/******************************************************************************/ + +/************************* PLL Parameters *************************************/ +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) +/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ +#define PLL_M 20 //XXX CHANGE THIS +#elif defined (STM32F446xx) +#define PLL_M 8 +#elif defined (STM32F411xE) + +#if defined(USE_HSE_BYPASS) +#define PLL_M 8 +#else +#define PLL_M 16 +#endif /* USE_HSE_BYPASS */ + +#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx */ + +/* USB OTG FS, SDIO and RNG Clock = PLL_VCO / PLLQ */ +#define PLL_Q 7 + +#if defined(STM32F446xx) +/* PLL division factor for I2S, SAI, SYSTEM and SPDIF: Clock = PLL_VCO / PLLR */ +#define PLL_R 7 +#endif /* STM32F446xx */ + +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) +#define PLL_N 500 //XXX CHANGE THIS +/* SYSCLK = PLL_VCO / PLL_P */ +#define PLL_P 2 +#endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx || STM32F446xx */ + +#if defined(STM32F401xx) +#define PLL_N 336 +/* SYSCLK = PLL_VCO / PLL_P */ +#define PLL_P 4 +#endif /* STM32F401xx */ + +#if defined(STM32F411xE) +#define PLL_N 400 +/* SYSCLK = PLL_VCO / PLL_P */ +#define PLL_P 4 +#endif /* STM32F411xx */ + +/******************************************************************************/ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Variables + * @{ + */ + +#if defined(STM32F40_41xxx) + uint32_t SystemCoreClock = 168000000; +#endif /* STM32F40_41xxx */ + +#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) + uint32_t SystemCoreClock = 180000000; +#endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx */ + +#if defined(STM32F401xx) + uint32_t SystemCoreClock = 84000000; +#endif /* STM32F401xx */ + +#if defined(STM32F411xE) + uint32_t SystemCoreClock = 100000000; +#endif /* STM32F401xx */ + +__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes + * @{ + */ + +static void SetSysClock(void); + +#if defined(DATA_IN_ExtSRAM) || defined(DATA_IN_ExtSDRAM) +static void SystemInit_ExtMemCtl(void); +#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the Embedded Flash Interface, the PLL and update the + * SystemFrequency variable. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + /* Reset the RCC clock configuration to the default reset state ------------*/ + /* Set HSION bit */ + RCC->CR |= (uint32_t)0x00000001; + + /* Reset CFGR register */ + RCC->CFGR = 0x00000000; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= (uint32_t)0xFEF6FFFF; + + /* Reset PLLCFGR register */ + RCC->PLLCFGR = 0x24003010; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Disable all interrupts */ + RCC->CIR = 0x00000000; + +#if defined(DATA_IN_ExtSRAM) || defined(DATA_IN_ExtSDRAM) + SystemInit_ExtMemCtl(); +#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */ + + /* Configure the System clock source, PLL Multiplier and Divider factors, + AHB/APBx prescalers and Flash settings ----------------------------------*/ + SetSysClock(); + + /* Configure the Vector Table location add offset address ------------------*/ +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ +#else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ +#endif +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f4xx.h file (default value + * 16 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f4xx.h file (default value + * 25 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate(void) +{ + uint32_t tmp = 0, pllvco = 0, pllp = 2, pllsource = 0, pllm = 2; +#if defined(STM32F446xx) + uint32_t pllr = 2; +#endif /* STM32F446xx */ + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock source */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock source */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL P used as system clock source */ + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N + SYSCLK = PLL_VCO / PLL_P + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; + pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; + +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F446xx) + if (pllsource != 0) + { + /* HSE used as PLL clock source */ + pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + else + { + /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } +#elif defined(STM32F411xE) +#if defined(USE_HSE_BYPASS) + if (pllsource != 0) + { + /* HSE used as PLL clock source */ + pllvco = (HSE_BYPASS_INPUT_FREQUENCY / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } +#else + if (pllsource == 0) + { + /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } +#endif /* USE_HSE_BYPASS */ +#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F446xx */ + pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; + SystemCoreClock = pllvco/pllp; + break; +#if defined(STM32F446xx) + case 0x0C: /* PLL R used as system clock source */ + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N + SYSCLK = PLL_VCO / PLL_R + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; + pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; + if (pllsource != 0) + { + /* HSE used as PLL clock source */ + pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + else + { + /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + + pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >>28) + 1 ) *2; + SystemCoreClock = pllvco/pllr; + break; +#endif /* STM32F446xx */ + default: + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK frequency --------------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK frequency */ + SystemCoreClock >>= tmp; +} + +/** + * @brief Configures the System clock source, PLL Multiplier and Divider factors, + * AHB/APBx prescalers and Flash settings + * @Note This function should be called only once the RCC clock configuration + * is reset to the default reset state (done in SystemInit() function). + * @param None + * @retval None + */ +static void SetSysClock(void) +{ +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F446xx) +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + + /* Enable HSE */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Select regulator voltage output Scale 1 mode */ + RCC->APB1ENR |= RCC_APB1ENR_PWREN; + PWR->CR |= PWR_CR_VOS; + + /* HCLK = SYSCLK / 1*/ + RCC->CFGR |= RCC_CFGR_HPRE_DIV1; + + + +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) + /* PCLK2 = HCLK / 2*/ + RCC->CFGR |= RCC_CFGR_PPRE2_DIV2; + + /* PCLK1 = HCLK / 4*/ + RCC->CFGR |= RCC_CFGR_PPRE1_DIV4; +#endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx || STM32F446xx */ + +#if defined(STM32F401xx) + /* PCLK2 = HCLK / 2*/ + RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 4*/ + RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; +#endif /* STM32F401xx */ + +#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) + /* Configure the main PLL */ + RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); +#endif /* STM32F40_41xxx || STM32F427_437x || STM32F429_439xx || STM32F401xx */ + +#if defined(STM32F446xx) + /* Configure the main PLL */ + RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24) | (PLL_R << 28); +#endif /* STM32F446xx */ + + /* Enable the main PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till the main PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + +#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) + /* Enable the Over-drive to extend the clock frequency to 180 Mhz */ + PWR->CR |= PWR_CR_ODEN; + while((PWR->CSR & PWR_CSR_ODRDY) == 0) + { + } + PWR->CR |= PWR_CR_ODSWEN; + while((PWR->CSR & PWR_CSR_ODSWRDY) == 0) + { + } + /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ + FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS; +#endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx */ + +#if defined(STM32F40_41xxx) + /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ + FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS; +#endif /* STM32F40_41xxx */ + +#if defined(STM32F401xx) + /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ + FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; +#endif /* STM32F401xx */ + + /* Select the main PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= RCC_CFGR_SW_PLL; + + /* Wait till the main PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + { + } + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } +#elif defined(STM32F411xE) +#if defined(USE_HSE_BYPASS) +/******************************************************************************/ +/* PLL (clocked by HSE) used as System clock source */ +/******************************************************************************/ + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + + /* Enable HSE and HSE BYPASS */ + RCC->CR |= ((uint32_t)RCC_CR_HSEON | RCC_CR_HSEBYP); + + /* Wait till HSE is ready and if Time out is reached exit */ + do + { + HSEStatus = RCC->CR & RCC_CR_HSERDY; + StartUpCounter++; + } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + + if ((RCC->CR & RCC_CR_HSERDY) != RESET) + { + HSEStatus = (uint32_t)0x01; + } + else + { + HSEStatus = (uint32_t)0x00; + } + + if (HSEStatus == (uint32_t)0x01) + { + /* Select regulator voltage output Scale 1 mode */ + RCC->APB1ENR |= RCC_APB1ENR_PWREN; + PWR->CR |= PWR_CR_VOS; + + /* HCLK = SYSCLK / 1*/ + RCC->CFGR |= RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 2*/ + RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 4*/ + RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; + + /* Configure the main PLL */ + RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | + (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24); + + /* Enable the main PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till the main PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ + FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; + + /* Select the main PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= RCC_CFGR_SW_PLL; + + /* Wait till the main PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + { + } + } + else + { /* If HSE fails to start-up, the application will have wrong clock + configuration. User can add here some code to deal with this error */ + } +#else /* HSI will be used as PLL clock source */ + /* Select regulator voltage output Scale 1 mode */ + RCC->APB1ENR |= RCC_APB1ENR_PWREN; + PWR->CR |= PWR_CR_VOS; + + /* HCLK = SYSCLK / 1*/ + RCC->CFGR |= RCC_CFGR_HPRE_DIV1; + + /* PCLK2 = HCLK / 2*/ + RCC->CFGR |= RCC_CFGR_PPRE2_DIV1; + + /* PCLK1 = HCLK / 4*/ + RCC->CFGR |= RCC_CFGR_PPRE1_DIV2; + + /* Configure the main PLL */ + RCC->PLLCFGR = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) | (PLL_Q << 24); + + /* Enable the main PLL */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait till the main PLL is ready */ + while((RCC->CR & RCC_CR_PLLRDY) == 0) + { + } + + /* Configure Flash prefetch, Instruction cache, Data cache and wait state */ + FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_2WS; + + /* Select the main PLL as system clock source */ + RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); + RCC->CFGR |= RCC_CFGR_SW_PLL; + + /* Wait till the main PLL is used as system clock source */ + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + { + } +#endif /* USE_HSE_BYPASS */ +#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx */ +} + +/** + * @brief Setup the external memory controller. Called in startup_stm32f4xx.s + * before jump to __main + * @param None + * @retval None + */ +#ifdef DATA_IN_ExtSRAM +/** + * @brief Setup the external memory controller. + * Called in startup_stm32f4xx.s before jump to main. + * This function configures the external SRAM mounted on STM324xG_EVAL/STM324x7I boards + * This SRAM will be used as program data memory (including heap and stack). + * @param None + * @retval None + */ +void SystemInit_ExtMemCtl(void) +{ +/*-- GPIOs Configuration -----------------------------------------------------*/ +/* + +-------------------+--------------------+------------------+--------------+ + + SRAM pins assignment + + +-------------------+--------------------+------------------+--------------+ + | PD0 <-> FMC_D2 | PE0 <-> FMC_NBL0 | PF0 <-> FMC_A0 | PG0 <-> FMC_A10 | + | PD1 <-> FMC_D3 | PE1 <-> FMC_NBL1 | PF1 <-> FMC_A1 | PG1 <-> FMC_A11 | + | PD4 <-> FMC_NOE | PE3 <-> FMC_A19 | PF2 <-> FMC_A2 | PG2 <-> FMC_A12 | + | PD5 <-> FMC_NWE | PE4 <-> FMC_A20 | PF3 <-> FMC_A3 | PG3 <-> FMC_A13 | + | PD8 <-> FMC_D13 | PE7 <-> FMC_D4 | PF4 <-> FMC_A4 | PG4 <-> FMC_A14 | + | PD9 <-> FMC_D14 | PE8 <-> FMC_D5 | PF5 <-> FMC_A5 | PG5 <-> FMC_A15 | + | PD10 <-> FMC_D15 | PE9 <-> FMC_D6 | PF12 <-> FMC_A6 | PG9 <-> FMC_NE2 | + | PD11 <-> FMC_A16 | PE10 <-> FMC_D7 | PF13 <-> FMC_A7 |-----------------+ + | PD12 <-> FMC_A17 | PE11 <-> FMC_D8 | PF14 <-> FMC_A8 | + | PD13 <-> FMC_A18 | PE12 <-> FMC_D9 | PF15 <-> FMC_A9 | + | PD14 <-> FMC_D0 | PE13 <-> FMC_D10 |-----------------+ + | PD15 <-> FMC_D1 | PE14 <-> FMC_D11 | + | | PE15 <-> FMC_D12 | + +------------------+------------------+ +*/ + /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */ + RCC->AHB1ENR |= 0x00000078; + + /* Connect PDx pins to FMC Alternate function */ + GPIOD->AFR[0] = 0x00cc00cc; + GPIOD->AFR[1] = 0xcccccccc; + /* Configure PDx pins in Alternate function mode */ + GPIOD->MODER = 0xaaaa0a0a; + /* Configure PDx pins speed to 100 MHz */ + GPIOD->OSPEEDR = 0xffff0f0f; + /* Configure PDx pins Output type to push-pull */ + GPIOD->OTYPER = 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOD->PUPDR = 0x00000000; + + /* Connect PEx pins to FMC Alternate function */ + GPIOE->AFR[0] = 0xcccccccc; + GPIOE->AFR[1] = 0xcccccccc; + /* Configure PEx pins in Alternate function mode */ + GPIOE->MODER = 0xaaaaaaaa; + /* Configure PEx pins speed to 100 MHz */ + GPIOE->OSPEEDR = 0xffffffff; + /* Configure PEx pins Output type to push-pull */ + GPIOE->OTYPER = 0x00000000; + /* No pull-up, pull-down for PEx pins */ + GPIOE->PUPDR = 0x00000000; + + /* Connect PFx pins to FMC Alternate function */ + GPIOF->AFR[0] = 0x00cccccc; + GPIOF->AFR[1] = 0xcccc0000; + /* Configure PFx pins in Alternate function mode */ + GPIOF->MODER = 0xaa000aaa; + /* Configure PFx pins speed to 100 MHz */ + GPIOF->OSPEEDR = 0xff000fff; + /* Configure PFx pins Output type to push-pull */ + GPIOF->OTYPER = 0x00000000; + /* No pull-up, pull-down for PFx pins */ + GPIOF->PUPDR = 0x00000000; + + /* Connect PGx pins to FMC Alternate function */ + GPIOG->AFR[0] = 0x00cccccc; + GPIOG->AFR[1] = 0x000000c0; + /* Configure PGx pins in Alternate function mode */ + GPIOG->MODER = 0x00080aaa; + /* Configure PGx pins speed to 100 MHz */ + GPIOG->OSPEEDR = 0x000c0fff; + /* Configure PGx pins Output type to push-pull */ + GPIOG->OTYPER = 0x00000000; + /* No pull-up, pull-down for PGx pins */ + GPIOG->PUPDR = 0x00000000; + +/*-- FMC Configuration ------------------------------------------------------*/ + /* Enable the FMC/FSMC interface clock */ + RCC->AHB3ENR |= 0x00000001; + +#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001011; + FMC_Bank1->BTCR[3] = 0x00000201; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F427_437xx || STM32F429_439xx */ + +#if defined(STM32F40_41xxx) + /* Configure and enable Bank1_SRAM2 */ + FSMC_Bank1->BTCR[2] = 0x00001011; + FSMC_Bank1->BTCR[3] = 0x00000201; + FSMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F40_41xxx */ + +/* + Bank1_SRAM2 is configured as follow: + In case of FSMC configuration + NORSRAMTimingStructure.FSMC_AddressSetupTime = 1; + NORSRAMTimingStructure.FSMC_AddressHoldTime = 0; + NORSRAMTimingStructure.FSMC_DataSetupTime = 2; + NORSRAMTimingStructure.FSMC_BusTurnAroundDuration = 0; + NORSRAMTimingStructure.FSMC_CLKDivision = 0; + NORSRAMTimingStructure.FSMC_DataLatency = 0; + NORSRAMTimingStructure.FSMC_AccessMode = FMC_AccessMode_A; + + FSMC_NORSRAMInitStructure.FSMC_Bank = FSMC_Bank1_NORSRAM2; + FSMC_NORSRAMInitStructure.FSMC_DataAddressMux = FSMC_DataAddressMux_Disable; + FSMC_NORSRAMInitStructure.FSMC_MemoryType = FSMC_MemoryType_SRAM; + FSMC_NORSRAMInitStructure.FSMC_MemoryDataWidth = FSMC_MemoryDataWidth_16b; + FSMC_NORSRAMInitStructure.FSMC_BurstAccessMode = FSMC_BurstAccessMode_Disable; + FSMC_NORSRAMInitStructure.FSMC_AsynchronousWait = FSMC_AsynchronousWait_Disable; + FSMC_NORSRAMInitStructure.FSMC_WaitSignalPolarity = FSMC_WaitSignalPolarity_Low; + FSMC_NORSRAMInitStructure.FSMC_WrapMode = FSMC_WrapMode_Disable; + FSMC_NORSRAMInitStructure.FSMC_WaitSignalActive = FSMC_WaitSignalActive_BeforeWaitState; + FSMC_NORSRAMInitStructure.FSMC_WriteOperation = FSMC_WriteOperation_Enable; + FSMC_NORSRAMInitStructure.FSMC_WaitSignal = FSMC_WaitSignal_Disable; + FSMC_NORSRAMInitStructure.FSMC_ExtendedMode = FSMC_ExtendedMode_Disable; + FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable; + FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &NORSRAMTimingStructure; + FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &NORSRAMTimingStructure; + + In case of FMC configuration + NORSRAMTimingStructure.FMC_AddressSetupTime = 1; + NORSRAMTimingStructure.FMC_AddressHoldTime = 0; + NORSRAMTimingStructure.FMC_DataSetupTime = 2; + NORSRAMTimingStructure.FMC_BusTurnAroundDuration = 0; + NORSRAMTimingStructure.FMC_CLKDivision = 0; + NORSRAMTimingStructure.FMC_DataLatency = 0; + NORSRAMTimingStructure.FMC_AccessMode = FMC_AccessMode_A; + + FMC_NORSRAMInitStructure.FMC_Bank = FMC_Bank1_NORSRAM2; + FMC_NORSRAMInitStructure.FMC_DataAddressMux = FMC_DataAddressMux_Disable; + FMC_NORSRAMInitStructure.FMC_MemoryType = FMC_MemoryType_SRAM; + FMC_NORSRAMInitStructure.FMC_MemoryDataWidth = FMC_MemoryDataWidth_16b; + FMC_NORSRAMInitStructure.FMC_BurstAccessMode = FMC_BurstAccessMode_Disable; + FMC_NORSRAMInitStructure.FMC_AsynchronousWait = FMC_AsynchronousWait_Disable; + FMC_NORSRAMInitStructure.FMC_WaitSignalPolarity = FMC_WaitSignalPolarity_Low; + FMC_NORSRAMInitStructure.FMC_WrapMode = FMC_WrapMode_Disable; + FMC_NORSRAMInitStructure.FMC_WaitSignalActive = FMC_WaitSignalActive_BeforeWaitState; + FMC_NORSRAMInitStructure.FMC_WriteOperation = FMC_WriteOperation_Enable; + FMC_NORSRAMInitStructure.FMC_WaitSignal = FMC_WaitSignal_Disable; + FMC_NORSRAMInitStructure.FMC_ExtendedMode = FMC_ExtendedMode_Disable; + FMC_NORSRAMInitStructure.FMC_WriteBurst = FMC_WriteBurst_Disable; + FMC_NORSRAMInitStructure.FMC_ContinousClock = FMC_CClock_SyncOnly; + FMC_NORSRAMInitStructure.FMC_ReadWriteTimingStruct = &NORSRAMTimingStructure; + FMC_NORSRAMInitStructure.FMC_WriteTimingStruct = &NORSRAMTimingStructure; +*/ + +} +#endif /* DATA_IN_ExtSRAM */ + +#ifdef DATA_IN_ExtSDRAM +/** + * @brief Setup the external memory controller. + * Called in startup_stm32f4xx.s before jump to main. + * This function configures the external SDRAM mounted on STM324x9I_EVAL board + * This SDRAM will be used as program data memory (including heap and stack). + * @param None + * @retval None + */ +void SystemInit_ExtMemCtl(void) +{ + register uint32_t tmpreg = 0, timeout = 0xFFFF; + register uint32_t index; + + /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface + clock */ + RCC->AHB1ENR |= 0x000001FC; + + /* Connect PCx pins to FMC Alternate function */ + GPIOC->AFR[0] = 0x0000000c; + GPIOC->AFR[1] = 0x00007700; + /* Configure PCx pins in Alternate function mode */ + GPIOC->MODER = 0x00a00002; + /* Configure PCx pins speed to 50 MHz */ + GPIOC->OSPEEDR = 0x00a00002; + /* Configure PCx pins Output type to push-pull */ + GPIOC->OTYPER = 0x00000000; + /* No pull-up, pull-down for PCx pins */ + GPIOC->PUPDR = 0x00500000; + + /* Connect PDx pins to FMC Alternate function */ + GPIOD->AFR[0] = 0x000000CC; + GPIOD->AFR[1] = 0xCC000CCC; + /* Configure PDx pins in Alternate function mode */ + GPIOD->MODER = 0xA02A000A; + /* Configure PDx pins speed to 50 MHz */ + GPIOD->OSPEEDR = 0xA02A000A; + /* Configure PDx pins Output type to push-pull */ + GPIOD->OTYPER = 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOD->PUPDR = 0x00000000; + + /* Connect PEx pins to FMC Alternate function */ + GPIOE->AFR[0] = 0xC00000CC; + GPIOE->AFR[1] = 0xCCCCCCCC; + /* Configure PEx pins in Alternate function mode */ + GPIOE->MODER = 0xAAAA800A; + /* Configure PEx pins speed to 50 MHz */ + GPIOE->OSPEEDR = 0xAAAA800A; + /* Configure PEx pins Output type to push-pull */ + GPIOE->OTYPER = 0x00000000; + /* No pull-up, pull-down for PEx pins */ + GPIOE->PUPDR = 0x00000000; + + /* Connect PFx pins to FMC Alternate function */ + GPIOF->AFR[0] = 0xcccccccc; + GPIOF->AFR[1] = 0xcccccccc; + /* Configure PFx pins in Alternate function mode */ + GPIOF->MODER = 0xAA800AAA; + /* Configure PFx pins speed to 50 MHz */ + GPIOF->OSPEEDR = 0xAA800AAA; + /* Configure PFx pins Output type to push-pull */ + GPIOF->OTYPER = 0x00000000; + /* No pull-up, pull-down for PFx pins */ + GPIOF->PUPDR = 0x00000000; + + /* Connect PGx pins to FMC Alternate function */ + GPIOG->AFR[0] = 0xcccccccc; + GPIOG->AFR[1] = 0xcccccccc; + /* Configure PGx pins in Alternate function mode */ + GPIOG->MODER = 0xaaaaaaaa; + /* Configure PGx pins speed to 50 MHz */ + GPIOG->OSPEEDR = 0xaaaaaaaa; + /* Configure PGx pins Output type to push-pull */ + GPIOG->OTYPER = 0x00000000; + /* No pull-up, pull-down for PGx pins */ + GPIOG->PUPDR = 0x00000000; + + /* Connect PHx pins to FMC Alternate function */ + GPIOH->AFR[0] = 0x00C0CC00; + GPIOH->AFR[1] = 0xCCCCCCCC; + /* Configure PHx pins in Alternate function mode */ + GPIOH->MODER = 0xAAAA08A0; + /* Configure PHx pins speed to 50 MHz */ + GPIOH->OSPEEDR = 0xAAAA08A0; + /* Configure PHx pins Output type to push-pull */ + GPIOH->OTYPER = 0x00000000; + /* No pull-up, pull-down for PHx pins */ + GPIOH->PUPDR = 0x00000000; + + /* Connect PIx pins to FMC Alternate function */ + GPIOI->AFR[0] = 0xCCCCCCCC; + GPIOI->AFR[1] = 0x00000CC0; + /* Configure PIx pins in Alternate function mode */ + GPIOI->MODER = 0x0028AAAA; + /* Configure PIx pins speed to 50 MHz */ + GPIOI->OSPEEDR = 0x0028AAAA; + /* Configure PIx pins Output type to push-pull */ + GPIOI->OTYPER = 0x00000000; + /* No pull-up, pull-down for PIx pins */ + GPIOI->PUPDR = 0x00000000; + +/*-- FMC Configuration ------------------------------------------------------*/ + /* Enable the FMC interface clock */ + RCC->AHB3ENR |= 0x00000001; + + /* Configure and enable SDRAM bank1 */ + FMC_Bank5_6->SDCR[0] = 0x000039D0; + FMC_Bank5_6->SDTR[0] = 0x01115351; + + /* SDRAM initialization sequence */ + /* Clock enable command */ + FMC_Bank5_6->SDCMR = 0x00000011; + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + while((tmpreg != 0) & (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Delay */ + for (index = 0; index<1000; index++); + + /* PALL command */ + FMC_Bank5_6->SDCMR = 0x00000012; + timeout = 0xFFFF; + while((tmpreg != 0) & (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Auto refresh command */ + FMC_Bank5_6->SDCMR = 0x00000073; + timeout = 0xFFFF; + while((tmpreg != 0) & (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* MRD register program */ + FMC_Bank5_6->SDCMR = 0x00046014; + timeout = 0xFFFF; + while((tmpreg != 0) & (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Set refresh count */ + tmpreg = FMC_Bank5_6->SDRTR; + FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1)); + + /* Disable write protection */ + tmpreg = FMC_Bank5_6->SDCR[0]; + FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF); + +/* + Bank1_SDRAM is configured as follow: + + FMC_SDRAMTimingInitStructure.FMC_LoadToActiveDelay = 2; + FMC_SDRAMTimingInitStructure.FMC_ExitSelfRefreshDelay = 6; + FMC_SDRAMTimingInitStructure.FMC_SelfRefreshTime = 4; + FMC_SDRAMTimingInitStructure.FMC_RowCycleDelay = 6; + FMC_SDRAMTimingInitStructure.FMC_WriteRecoveryTime = 2; + FMC_SDRAMTimingInitStructure.FMC_RPDelay = 2; + FMC_SDRAMTimingInitStructure.FMC_RCDDelay = 2; + + FMC_SDRAMInitStructure.FMC_Bank = SDRAM_BANK; + FMC_SDRAMInitStructure.FMC_ColumnBitsNumber = FMC_ColumnBits_Number_8b; + FMC_SDRAMInitStructure.FMC_RowBitsNumber = FMC_RowBits_Number_11b; + FMC_SDRAMInitStructure.FMC_SDMemoryDataWidth = FMC_SDMemory_Width_16b; + FMC_SDRAMInitStructure.FMC_InternalBankNumber = FMC_InternalBank_Number_4; + FMC_SDRAMInitStructure.FMC_CASLatency = FMC_CAS_Latency_3; + FMC_SDRAMInitStructure.FMC_WriteProtection = FMC_Write_Protection_Disable; + FMC_SDRAMInitStructure.FMC_SDClockPeriod = FMC_SDClock_Period_2; + FMC_SDRAMInitStructure.FMC_ReadBurst = FMC_Read_Burst_disable; + FMC_SDRAMInitStructure.FMC_ReadPipeDelay = FMC_ReadPipe_Delay_1; + FMC_SDRAMInitStructure.FMC_SDRAMTimingStruct = &FMC_SDRAMTimingInitStructure; +*/ + +} +#endif /* DATA_IN_ExtSDRAM */ + + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/firmware/lib/system_stm32f4xx.h b/firmware/lib/system_stm32f4xx.h new file mode 100644 index 0000000..5a336cd --- /dev/null +++ b/firmware/lib/system_stm32f4xx.h @@ -0,0 +1,105 @@ +/** + ****************************************************************************** + * @file system_stm32f4xx.h + * @author MCD Application Team + * @version V1.5.0 + * @date 06-March-2015 + * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f4xx_system + * @{ + */ + +/** + * @brief Define to prevent recursive inclusion + */ +#ifndef __SYSTEM_STM32F4XX_H +#define __SYSTEM_STM32F4XX_H + +#ifdef __cplusplus + extern "C" { +#endif + +/** @addtogroup STM32F4xx_System_Includes + * @{ + */ + +/** + * @} + */ + + +/** @addtogroup STM32F4xx_System_Exported_types + * @{ + */ + +extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ + + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Exported_Constants + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Exported_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Exported_Functions + * @{ + */ + +extern void SystemInit(void); +extern void SystemCoreClockUpdate(void); +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /*__SYSTEM_STM32F4XX_H */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/firmware/lib/systems.c b/firmware/lib/systems.c new file mode 100644 index 0000000..513b916 --- /dev/null +++ b/firmware/lib/systems.c @@ -0,0 +1,264 @@ +#include +#include "systems.h" +#include +#ifdef STM32F30X +#include +#include +#include +#elif defined( STM32F40_41xxx ) +#include +#include +#include +#endif +#include +#include + +extern RCC_ClocksTypeDef RCC_Clocks; + +//For precise timing. +volatile unsigned int *DWT_CYCCNT = (volatile unsigned int *)0xE0001004; //address of the register +volatile unsigned int *SCB_DEMCR = (volatile unsigned int *)0xE000EDFC; //address of the register +volatile unsigned int *DWT_CONTROL = (volatile unsigned int *)0xE0001000; //address of the register + +void send_openocd_command(int command, void *message) +{ +#ifdef DEBUG + asm("mov r0, %[cmd];" + "mov r1, %[msg];" + "bkpt #0xAB" + : + : [cmd] "r" (command), [msg] "r" (message) + : "r0", "r1", "memory"); +#endif +} + + +void send_text( const char * text ) +{ + uint32_t m[] = { 2, (uint32_t)text, strlen(text) }; + send_openocd_command(0x05, m); +} + +int __attribute__((used)) _write (int fd, const void *buf, size_t count) +{ + //uint32_t m[] = { 2, (uint32_t)buf, count }; + //send_openocd_command(0x05, m); + int i; + + for (i=count; i > 0; --i) { + while (!(USART1->SR & USART_SR_TXE)); + USART1->DR = (uint16_t)(*((char*) buf) & 0x01FF); + buf++; + } + + return count; +} + +int __attribute__((used)) _read(int fd, void *buf, size_t count) +{ + int i; + + for (i=0; i < count; ++i) { + while (!(USART1->SR & USART_SR_RXNE)); + *((char*) buf) = USART1->DR; + _write(0, buf, 1); + if (*((char*) buf) == '\r') + break; + buf++; + } + i++; + + return i; +} + +void __attribute__((used)) * _sbrk(int incr) { + extern char _ebss; // Defined by the linker + static char *heap_end; + char *prev_heap_end; + + + if (heap_end == 0) { + heap_end = &_ebss; + } + prev_heap_end = heap_end; + + char * stack = (char*) __get_MSP(); + if (heap_end + incr > stack) + { + return (void*)(-1); + } + + heap_end += incr; + + return (void*) prev_heap_end; +} + +void _delay_us(uint32_t us) { + if( us ) us--; //Approximate extra overhead time. + us *= RCC_Clocks.HCLK_Frequency/1000000; + *SCB_DEMCR = *SCB_DEMCR | 0x01000000; + *DWT_CYCCNT = 0; // reset the counter + *DWT_CONTROL = *DWT_CONTROL | 1 ; // enable the counter + while( *DWT_CYCCNT < us ); +} + +void ConfigureUART() +{ + GPIO_InitTypeDef GPIO_InitStruct; + USART_InitTypeDef USART_InitStruct; + + // Enable clock for GPIOB + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + + GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_USART1); + GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_USART1); + + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; + GPIO_Init(GPIOB, &GPIO_InitStruct); + + + + /** + * Enable clock for USART1 peripheral + */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); + + /** + * Set Baudrate to value you pass to function + * Disable Hardware Flow control + * Set Mode To TX and RX, so USART will work in full-duplex mode + * Disable parity bit + * Set 1 stop bit + * Set Data bits to 8 + * + * Initialize USART1 + * Activate USART1 + */ + USART_InitStruct.USART_BaudRate = 115200; + USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + USART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; + USART_InitStruct.USART_Parity = USART_Parity_No; + USART_InitStruct.USART_StopBits = USART_StopBits_1; + USART_InitStruct.USART_WordLength = USART_WordLength_8b; + USART_Init(USART1, &USART_InitStruct); + USART_Cmd(USART1, ENABLE); + + /** + * Enable RX interrupt + */ + //USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); + + /** + * Set Channel to USART1 + * Set Channel Cmd to enable. That will enable USART1 channel in NVIC + * Set Both priorities to 0. This means high priority + * + * Initialize NVIC + */ + /*NVIC_InitStruct.NVIC_IRQChannel = USART1_IRQn; + NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0; + NVIC_Init(&NVIC_InitStruct);*/ +} + +void ConfigureLED() +{ + ConfigureGPIO( LEDPIN, INOUT_OUT ); +} + +uint8_t GetGPIOFromString( const char * str ) +{ + int mode = 0; + int port = -1; + int pin = -1; + const char * st = str; + for( ; *st; st++ ) + { + char c = *st; + if( mode == 0 ) + { + if( c >= 'A' && c <= 'F' ) + { + port = c - 'A'; + mode = 2; + } + else if( c >= 'a' && c <= 'f' ) + { + port = c - 'a'; + mode = 2; + } + } + else if( mode == 2 ) + { + if( c >= '0' && c <= '9' ) + { + pin = 0; + mode = 3; + } + } + + if( mode == 3 ) + { + if( c >= '0' && c <= '9' ) + { + pin = pin * 10; + pin+= c - '0'; + } + else + { + break; + } + } + } + + if( port > 0 && pin > 0 && port <= 6 && pin <= 15) + { + return (port<<4)|pin; + } + else + { + return 0xff; + } +} + + +void ConfigureGPIO( uint8_t gpio, int parameters ) +{ + GPIO_InitTypeDef GPIO_InitStructure; + + /* Enable the GPIO_LED Clock */ +#ifdef STM32F30X + RCC_AHBPeriphClockCmd( 1<<(17+(gpio>>4)), ENABLE); +#elif defined( STM32F40_41xxx ) + RCC_AHB1PeriphClockCmd( 1<<((gpio>>4)), ENABLE); +#endif + + if( parameters & DEFAULT_VALUE_FLAG ) + { + GPIOOn( gpio ); + } + else + { + GPIOOff( gpio ); + } + + /* Configure the GPIO_LED pin */ + GPIO_InitStructure.GPIO_Pin = 1<<(gpio&0xf); + GPIO_InitStructure.GPIO_Mode = (parameters&INOUT_FLAG)?GPIO_Mode_OUT:GPIO_Mode_IN; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = (parameters&PUPD_FLAG)?( (parameters&PUPD_UP)?GPIO_PuPd_UP:GPIO_PuPd_DOWN ):GPIO_PuPd_NOPULL; + +#ifdef STM32F30X + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; +#elif defined( STM32F40_41xxx ) + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; +#endif + + GPIO_Init(GPIOOf(gpio), &GPIO_InitStructure); +} + diff --git a/firmware/lib/systems.h b/firmware/lib/systems.h new file mode 100644 index 0000000..bb0fa10 --- /dev/null +++ b/firmware/lib/systems.h @@ -0,0 +1,65 @@ +#ifndef _SYSTEMS_H +#define _SYSTEMS_H + +void send_openocd_command(int command, void *message); +void send_text( const char * text ); +void _delay_us(uint32_t us); + + +typedef uint8_t gpio; + +gpio GetGPIOFromString( const char * str ); + + +#define DEFAULT_VALUE_FLAG 0x00000001 +#define DEFAULT_ON 0x00000001 +#define DEFAULT_OFF 0x00000000 + +#define INOUT_FLAG 0x00000002 +#define INOUT_OUT 0x00000002 +#define INOUT_IN 0x00000000 + +#define PUPD_FLAG 0x0000000C +#define PUPD_NONE 0x00000000 +#define PUPD_UP 0x00000004 +#define PUPD_DOWN 0x00000008 + +void ConfigureGPIO( gpio gpio, int parameters ); + + + +#ifdef STM32F30X +#define GPIOOf(x) ((GPIO_TypeDef *) ((((x)>>4)<=6)?(AHB2PERIPH_BASE+0x400*((x)>>4)):0x60000000) ) +#elif defined( STM32F40_41xxx ) +#define GPIOOf(x) ((GPIO_TypeDef *) ((((x)>>4)<=6)?(AHB1PERIPH_BASE+0x400*((x)>>4)):0x60000000) ) +#endif + +#define GPIOPin(x) ((1<<((x)&0x0f))) +#define GPIOLatch(x) GPIOOf(x)->ODR + +#ifdef STM32F30X +#define GPIOOn(x) GPIOOf(x)->BSRR = (1<<((x)&0x0f)); +#define GPIOOff(x) GPIOOf(x)->BRR = (1<<((x)&0x0f)); +#elif defined( STM32F40_41xxx ) +#define GPIOOn(x) GPIOOf(x)->BSRRH = (1<<((x)&0x0f)); +#define GPIOOff(x) GPIOOf(x)->BSRRL = (1<<((x)&0x0f)); +#endif + + + +#ifdef STM32F30X +#define LEDPIN 0x18 +#elif defined( STM32F40_41xxx ) +#define LEDPIN (16+14) +#endif + +void ConfigureUART(); + +void ConfigureLED(); +#define LED_TOGGLE {GPIOOf(LEDPIN)->ODR^=(1<<((LEDPIN)&0x0f));} +#define LED_ON GPIOOn(LEDPIN) +#define LED_OFF GPIOOff(LEDPIN) + + +#endif + diff --git a/firmware/main.c b/firmware/main.c new file mode 100644 index 0000000..ac44d8f --- /dev/null +++ b/firmware/main.c @@ -0,0 +1,195 @@ +//ColorChordEmbedded implementation on the STM32F407 for the stm32f4-discovery. +//Uses on-board microphone and outputs WS2812 signal for configurable number of LEDs +//on PB5 via DMA+SPI + +#include +#include +#include +#include +#include +#include +#include +#include +#include "spi2812.h" +#include "adc.h" + +volatile int wasclicked = 0; //Used for if the root change button was clicked. + +RCC_ClocksTypeDef RCC_Clocks; + + +//Circular buffer for incoming data so we don't spend much time servicing the interrupt and we can handle colorchord in the main thread. +#define CIRCBUFSIZE 256 +volatile int last_samp_pos; +int16_t sampbuff[CIRCBUFSIZE]; +volatile int samples; + +//This gets called by the ADC/Microphone +void GotSample( int samp ) +{ + //printf("[GOT-SMP] %d\r\n", samp); + sampbuff[last_samp_pos] = samp; + last_samp_pos = ((last_samp_pos+1)%CIRCBUFSIZE); + samples++; +} + +//Call this once we've stacked together one full colorchord frame. +void NewFrame() +{ +// uint8_t led_outs[NUM_LIN_LEDS*3]; + int i; + HandleFrameInfo(); + +// UpdateLinearLEDs(); + UpdateAllSameLEDs(); + + SendSPI2812( ledOut, NUM_LIN_LEDS ); +} + + +void Configure_PA0(void) { + /* Set variables used */ + GPIO_InitTypeDef GPIO_InitStruct; + EXTI_InitTypeDef EXTI_InitStruct; + NVIC_InitTypeDef NVIC_InitStruct; + + /* Enable clock for GPIOD */ + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + /* Enable clock for SYSCFG */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); + + /* Set pin as input */ + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_DOWN; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* Tell system that you will use PA0 for EXTI_Line0 */ + SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource0); + + /* PA0 is connected to EXTI_Line0 */ + EXTI_InitStruct.EXTI_Line = EXTI_Line0; + /* Enable interrupt */ + EXTI_InitStruct.EXTI_LineCmd = ENABLE; + /* Interrupt mode */ + EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt; + /* Triggers on rising and falling edge */ + EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising; + /* Add to EXTI */ + EXTI_Init(&EXTI_InitStruct); + + /* Add IRQ vector to NVIC */ + /* PA0 is connected to EXTI_Line0, which has EXTI0_IRQn vector */ + NVIC_InitStruct.NVIC_IRQChannel = EXTI0_IRQn; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; /* Set priority */ + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; /* Set sub priority */ + NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; /* Enable interrupt */ + NVIC_Init(&NVIC_InitStruct); /* Add to NVIC */ +} + +//Handle button press on PA0. +void EXTI0_IRQHandler(void) +{ + static int rootoffset; + + if (EXTI_GetITStatus(EXTI_Line0) != RESET) + { + if( wasclicked == 0 ) + wasclicked = 10; + EXTI_ClearITPendingBit(EXTI_Line0); + } +} + +int main(void) +{ + uint32_t i = 0; + + RCC_GetClocksFreq( &RCC_Clocks ); + + ConfigureLED(); + + LED_OFF; + + // SysTick end of count event each 10ms + SysTick_Config( RCC_Clocks.HCLK_Frequency / 100); + + float fv = RCC_Clocks.HCLK_Frequency / 1000000.0f; +// We can use printf to print back through the debugging interface, but that's slow and +// it also takes up a bunch of space. No printf = no space wasted in printf. +// printf( "Operating at %.3fMHz\n", fv ); + + ConfigureUART(); + InitColorChord(); + Configure_PA0(); + //InitMP45DT02(); + InitSPI2812(); + adc_init(); + + printf("Operating at %.3fMHz\r\n", fv ); + + int this_samp = 0; + int wf = 0; + + + LED_ON; + + int c = 1; + while (1) { + for (int a=0; a < NUM_LIN_LEDS; a++) { + ledOut[a*3 + 0] = c/2; + ledOut[a*3 + 1] = (64-c)/2 % 255; + ledOut[a*3 + 2] = a/8 % 256; + } + + SendSPI2812(ledOut, NUM_LIN_LEDS); + _delay_us(10*1000); + c++; + if (c==64) + break; + } + + + SendSPI2812(ledOut, NUM_LIN_LEDS); + while(1) + { + if( this_samp != last_samp_pos ) + { + LED_OFF; //Use led on the board to show us how much CPU we're using. (You can also probe PB15) + + PushSample32( sampbuff[this_samp]/2 ); //Can't put in full volume. + + this_samp = (this_samp+1)%CIRCBUFSIZE; + + wf++; + if( wf == 128 ) + { + NewFrame(); + wf = 0; + } + LED_ON; + } + LED_ON; //Take up a little more time to make sure we don't miss this. + } +} + +void TimingDelay_Decrement() +{ + static int rootoffset; + + //A way of making sure we debounce the button. + if( wasclicked ) + { + if( wasclicked == 10 ) + { + if( rootoffset++ >= 12 ) rootoffset = 0; + RootNoteOffset = (rootoffset * NOTERANGE) / 12; + } + + wasclicked--; + } + +} + + diff --git a/firmware/spi2812.c b/firmware/spi2812.c new file mode 100644 index 0000000..7fc2f27 --- /dev/null +++ b/firmware/spi2812.c @@ -0,0 +1,334 @@ +//Parts stolen from: +// https://javakys.wordpress.com/2014/09/04/how-to-implement-full-duplex-spi-communication-using-spi-dma-mode-on-stm32f2xx-or-stm32f4xx/ +//Timing from: +// https://cpldcpu.wordpress.com/2014/01/14/light_ws2812-library-v2-0-part-i-understanding-the-ws2812/ +//Also... +// https://my.st.com/public/STe2ecommunities/mcu/Lists/STM32Discovery/Flat.aspx?RootFolder=https%3a%2f%2fmy.st.com%2fpublic%2fSTe2ecommunities%2fmcu%2fLists%2fSTM32Discovery%2fSTM32F3%20SPI%20slave%20receive%20with%20DMA&FolderCTID=0x01200200770978C69A1141439FE559EB459D75800084C20D8867EAD444A5987D47BE638E0F¤tviews=1336 +// For the '407... https://my.st.com/public/STe2ecommunities/mcu/Lists/cortex_mx_stm32/Flat.aspx?RootFolder=https%3a%2f%2fmy.st.com%2fpublic%2fSTe2ecommunities%2fmcu%2fLists%2fcortex_mx_stm32%2fSPI1%20in%20DMA2%20on%20STM32F4&FolderCTID=0x01200200770978C69A1141439FE559EB459D7580009C4E14902C3CDE46A77F0FFD06506F5B¤tviews=2447 +// Also for the '407... http://www.micromouseonline.com/2012/03/11/adding-dma-to-the-spi-driver-with-the-stm32f4/ + + +#include +#include + +#include "spi2812.h" + +//Number of 0 segments to transmit ahead of time to reset string. +#define ZERO_BUFFER 6 + +uint8_t MyBuffer[SPI2812_BUFFSIZE+ZERO_BUFFER]; + + + +#ifdef STM32F30X +#include +#include +#include +#include +#include + + //Currently, the 30X version does not have configurable ports. + + +#define SPI_PORT SPI2 +#define SPI_PORT_CLOCK RCC_APB1Periph_SPI2 +#define SPI_PORT_CLOCK_INIT RCC_APB1PeriphClockCmd +#define SPI_MOSI_PIN GPIO_Pin_15 +#define SPI_MOSI_GPIO_PORT GPIOB +#define SPI_MOSI_GPIO_CLK RCC_AHBPeriph_GPIOB +#define SPI_MOSI_SOURCE GPIO_PinSource15 +#define SPI_MOSI_AF GPIO_AF_5 +#define SPI_PORT_DR_ADDRESS SPI_PORT->DR +#define SPI_PORT_DMA DMA1 +#define SPI_PORT_DMAx_CLK RCC_AHBPeriph_DMA1 +#define SPI_PORT_TX_DMA_CHANNEL DMA1_Channel5 +#define SPI_PORT_DMA_TX_IRQn DMA1_Channel5_IRQn + +#define DMA_HANDLER_IRQFN DMA1_Channel5_IRQHandler +#define DMA_FLAG_C DMA1_FLAG_TC5 +#define DMA_FLAG_E DMA1_FLAG_TE5 +/* +#define SPI_PORT_TX_DMA_STREAM DMA1_Stream4 +#define SPI_PORT_TX_DMA_FLAG_FEIF DMA_FLAG_FEIF4 +#define SPI_PORT_TX_DMA_FLAG_DMEIF DMA_FLAG_DMEIF4 +#define SPI_PORT_TX_DMA_FLAG_TEIF DMA_FLAG_TEIF4 +#define SPI_PORT_TX_DMA_FLAG_HTIF DMA_FLAG_HTIF4 +#define SPI_PORT_TX_DMA_FLAG_TCIF DMA_FLAG_TCIF4 +#define SPI_PORT_DMA_TX_IRQn DMA1_Stream4_IRQn +#define SPI_PORT_DMA_TX_IRQHandler DMA1_Stream4_IRQHandler +*/ + +#elif defined( STM32F40_41xxx ) + +#include +#include +#include +#include +#include + +#define SPI_PORT SPI1 +#define SPI_PORT_CLOCK RCC_APB2Periph_SPI1 +#define SPI_PORT_CLOCK_INIT RCC_APB2PeriphClockCmd +#define SPI_MOSI_PIN GPIO_Pin_7 +#define SPI_MOSI_GPIO_PORT GPIOA +#define SPI_MOSI_GPIO_CLK RCC_AHB1Periph_GPIOA +#define SPI_MOSI_SOURCE GPIO_PinSource7 +#define SPI_MOSI_AF GPIO_AF_SPI1 +#define SPI_PORT_DR_ADDRESS SPI_PORT->DR +#define SPI_PORT_DMA DMA2 +#define SPI_PORT_DMAx_CLK RCC_AHB1Periph_DMA2 +#define SPI_PORT_TX_DMA_CHANNEL DMA_Channel_3 +#define SPI_PORT_TX_DMA_STREAM DMA2_Stream3 +#define SPI_PORT_TX_DMA_FLAG_FEIF DMA_FLAG_FEIF3 +#define SPI_PORT_TX_DMA_FLAG_DMEIF DMA_FLAG_DMEIF3 +#define SPI_PORT_TX_DMA_FLAG_TEIF DMA_FLAG_TEIF3 +#define SPI_PORT_TX_DMA_FLAG_HTIF DMA_FLAG_HTIF3 +#define SPI_PORT_TX_DMA_FLAG_TCIF DMA_FLAG_TCIF3 +#define SPI_PORT_DMA_TX_IRQn DMA2_Stream3_IRQn +#define SPI_PORT_DMA_TX_IRQHandler DMA2_Stream3_IRQHandler +#define DMA_FLAGE DMA_IT_TCIF3 + +#endif + + + + +#ifdef STM32F30X + +void DMA_HANDLER_IRQFN() +{ + if ( DMA_GetFlagStatus(DMA_FLAG_C) == SET ) + { + DMA_ClearITPendingBit(DMA_FLAG_C); + } + + /* Transfer Error */ + if(DMA_GetITStatus(DMA_FLAG_E) == SET) + { + if (DMA_GetFlagStatus(DMA_FLAG_E) != RESET) + { + DMA_ClearITPendingBit(DMA_FLAG_E); + } + } +} + +#elif defined( STM32F40_41xxx ) + +void SPI_PORT_DMA_TX_IRQHandler(){ + + // Test if DMA Stream Transfer Complete interrupt + if (DMA_GetITStatus(SPI_PORT_TX_DMA_STREAM, DMA_FLAGE)) { + DMA_ClearITPendingBit(SPI_PORT_TX_DMA_STREAM, DMA_FLAGE); + } + + +} + +#endif + +void InitSPI2812() +{ + SPI_InitTypeDef SPI_InitStructure; + DMA_InitTypeDef dma_init_struct; + GPIO_InitTypeDef GPIO_InitStructure; + NVIC_InitTypeDef nvic_init_struct; + + printf("Init SPI2812: sizeof(MyBuffer)=%d\r\n", sizeof(MyBuffer)); + +#ifdef STM32F30X + + //On SPI2, PORT B.15 + + RCC_AHBPeriphClockCmd(SPI_MOSI_GPIO_CLK, ENABLE); + RCC_AHBPeriphClockCmd(SPI_PORT_DMAx_CLK, ENABLE); + SPI_PORT_CLOCK_INIT(SPI_PORT_CLOCK, ENABLE); + SPI_I2S_DeInit(SPI_PORT); + + /* Configure the GPIO_LED pin */ + GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; + GPIO_Init(SPI_MOSI_GPIO_PORT, &GPIO_InitStructure); + + GPIO_PinAFConfig(SPI_MOSI_GPIO_PORT, SPI_MOSI_SOURCE, SPI_MOSI_AF); //MOSI //GPIO_AF_5??? GPIO_AF_6??? TODO + + /* SPI Config */ + SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Tx; + SPI_InitStructure.SPI_Mode = SPI_Mode_Master; + SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; + SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low; + SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge; + SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; + SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; + SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; + SPI_InitStructure.SPI_CRCPolynomial = 7; //TODO: Remove this??? + + SPI_CalculateCRC(SPI_PORT, DISABLE); + SPI_Init(SPI_PORT, &SPI_InitStructure); + RCC_I2SCLKConfig( RCC_I2S2CLKSource_SYSCLK); + SPI_Cmd(SPI_PORT, ENABLE); + + //We're on DMA1, channel 5. + + memset( MyBuffer, 0xAA, 128 ); + + DMA_DeInit(SPI_PORT_TX_DMA_CHANNEL); + DMA_StructInit(&dma_init_struct); + dma_init_struct.DMA_PeripheralBaseAddr = (uint32_t) &SPI_PORT->DR; + dma_init_struct.DMA_MemoryBaseAddr = (uint32_t)MyBuffer; //REmove this? somehow? + dma_init_struct.DMA_DIR = DMA_DIR_PeripheralDST; + dma_init_struct.DMA_BufferSize = 128; + dma_init_struct.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + dma_init_struct.DMA_MemoryInc = DMA_MemoryInc_Enable; + dma_init_struct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + dma_init_struct.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte; + dma_init_struct.DMA_Mode = DMA_Mode_Normal; + dma_init_struct.DMA_Priority = DMA_Priority_VeryHigh; + dma_init_struct.DMA_M2M = DMA_M2M_Disable; + DMA_Init(SPI_PORT_TX_DMA_CHANNEL, &dma_init_struct); + + /* Configure the Priority Group */ + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1); + + /* Initialize NVIC Struct for DMA */ + nvic_init_struct.NVIC_IRQChannel = SPI_PORT_DMA_TX_IRQn; + nvic_init_struct.NVIC_IRQChannelPreemptionPriority = 1; + nvic_init_struct.NVIC_IRQChannelSubPriority = 0; + nvic_init_struct.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic_init_struct); + + SPI_I2S_DMACmd(SPI_PORT, SPI_I2S_DMAReq_Tx, ENABLE); + +#elif defined( STM32F40_41xxx ) + + // enable the SPI peripheral clock + SPI_PORT_CLOCK_INIT(SPI_PORT_CLOCK, ENABLE); + // enable the peripheral GPIO port clocks + RCC_AHB1PeriphClockCmd(SPI_MOSI_GPIO_CLK, ENABLE); + // Connect SPI pins to AF5 - see section 3, Table 6 in the device datasheet + GPIO_PinAFConfig(SPI_MOSI_GPIO_PORT, SPI_MOSI_SOURCE, SPI_MOSI_AF); + // now configure the pins themselves + // they are all going to be fast push-pull outputs + // but the SPI pins use the alternate function + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN; + GPIO_Init(SPI_MOSI_GPIO_PORT, &GPIO_InitStructure); + // now we can set up the SPI peripheral + // Assume the target is write only and we look after the chip select ourselves + // SPI clock rate will be system frequency/4/prescaler + // so here we will go for 72/4/8 = 2.25MHz + SPI_I2S_DeInit(SPI_PORT); + SPI_StructInit(&SPI_InitStructure); + SPI_InitStructure.SPI_Mode = SPI_Mode_Master; + SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Tx; + SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; + SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16; //On F104, assume 100MHz. + SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; + SPI_Init(SPI_PORT, &SPI_InitStructure); + // Enable the SPI port + SPI_Cmd(SPI_PORT, ENABLE); + + + // first enable the clock + RCC_AHB1PeriphClockCmd(SPI_PORT_DMAx_CLK, ENABLE); + // start with a blank DMA configuration just to be sure + DMA_DeInit(SPI_PORT_TX_DMA_STREAM); + + while (DMA_GetCmdStatus (SPI_PORT_TX_DMA_STREAM) != DISABLE); + + // Configure DMA controller to manage TX DMA requests + // first make sure we are using the default values + DMA_StructInit(&dma_init_struct); + // these are the only parameters that change from the defaults + dma_init_struct.DMA_PeripheralBaseAddr = (uint32_t) & (SPI_PORT->DR); + dma_init_struct.DMA_Channel = SPI_PORT_TX_DMA_CHANNEL; + dma_init_struct.DMA_DIR = DMA_DIR_MemoryToPeripheral; + dma_init_struct.DMA_MemoryInc = DMA_MemoryInc_Enable; + /* + * It is not possible to call DMA_Init without values for the source + * address and non-zero size even though a transfer is not done here. + * These are checked only when the assert macro are active though. + */ + dma_init_struct.DMA_Memory0BaseAddr = (uint32_t)MyBuffer; + dma_init_struct.DMA_BufferSize = 1; + DMA_Init(SPI_PORT_TX_DMA_STREAM, &dma_init_struct); + // Enable the DMA transfer complete interrupt + DMA_ITConfig(SPI_PORT_TX_DMA_STREAM, DMA_IT_TC, ENABLE); + + + DMA_ITConfig (SPI_PORT_TX_DMA_STREAM, DMA_IT_TC, ENABLE); + // enable the interrupt in the NVIC + nvic_init_struct.NVIC_IRQChannel = SPI_PORT_DMA_TX_IRQn; + nvic_init_struct.NVIC_IRQChannelPreemptionPriority = 0; + nvic_init_struct.NVIC_IRQChannelSubPriority = 1; + nvic_init_struct.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init (&nvic_init_struct); + // Enable dma tx request. + SPI_I2S_DMACmd (SPI_PORT, SPI_I2S_DMAReq_Tx, ENABLE); + +#endif + +//These would fire it off... +// DMA_Cmd(DMA1_Channel5, ENABLE); +// DMA_ITConfig(DMA1_Channel5, DMA_IT_TC, ENABLE); +// DMA_ITConfig(DMA1_Channel5, DMA_IT_TE, ENABLE); + +} + +void SendSPI2812( unsigned char * lightarray, int length ) +{ + static const uint8_t aoarray[4] = { 0b10001000, 0b10001110, 0b11101000, 0b11101110 }; + + int i; + if( length > SPI2812_MAX_LEDS ) length = SPI2812_MAX_LEDS; + + MyBuffer[0] = 0; + for( i = 0; i < length; i++ ) + { + uint8_t * colorbase = &lightarray[i*3]; + uint8_t * buffbase = &MyBuffer[i*24/2+ZERO_BUFFER]; + + int j; + for( j = 0; j < 3; j++ ) + { + uint8_t c = colorbase[j];// (j==0)?1:(j==1)?0:2 ]; //Flip R and G. + + *(buffbase++) = aoarray[(c>>6)&3]; + *(buffbase++) = aoarray[(c>>4)&3]; + *(buffbase++) = aoarray[(c>>2)&3]; + *(buffbase++) = aoarray[(c>>0)&3]; + } + } + + for( i = 0; i < ZERO_BUFFER; i++ ) + MyBuffer[i] = 0; + + length *= 24/2; + length += ZERO_BUFFER; + +#ifdef STM32F30X + + DMA_Cmd(SPI_PORT_TX_DMA_CHANNEL, DISABLE); + SPI_PORT_TX_DMA_CHANNEL->CMAR = (uint32_t) MyBuffer; + SPI_PORT_TX_DMA_CHANNEL->CNDTR = (uint16_t) length; + DMA_Cmd(SPI_PORT_TX_DMA_CHANNEL, ENABLE); + DMA_ITConfig(SPI_PORT_TX_DMA_CHANNEL, DMA_IT_TC, ENABLE); + DMA_ITConfig(SPI_PORT_TX_DMA_CHANNEL, DMA_IT_TE, ENABLE); + +#elif defined( STM32F40_41xxx ) + + SPI_PORT_TX_DMA_STREAM->NDTR = (uint32_t) length; + SPI_PORT_TX_DMA_STREAM->M0AR = (uint32_t) MyBuffer+2; + DMA_Cmd (SPI_PORT_TX_DMA_STREAM, ENABLE); + +#endif + +} + + + + diff --git a/firmware/spi2812.h b/firmware/spi2812.h new file mode 100644 index 0000000..674004f --- /dev/null +++ b/firmware/spi2812.h @@ -0,0 +1,15 @@ +#ifndef _SPI_2812_H +#define _SPI_2812_H + +#define SPI2812_MAX_LEDS 300 + +//24 bits per LED, can fit two bits per byte of output. +#define SPI2812_BUFFSIZE (SPI2812_MAX_LEDS*24/2) + + +void InitSPI2812(); +void SendSPI2812( unsigned char * lightarray, int leds ); //Need one R, G, B per element. + +#endif + + diff --git a/firmware/stm32f4xx_it.c b/firmware/stm32f4xx_it.c new file mode 100644 index 0000000..16da9bf --- /dev/null +++ b/firmware/stm32f4xx_it.c @@ -0,0 +1,177 @@ +/** + ****************************************************************************** + * @file ADC/ADC_DMA/stm32f4xx_it.c + * @author MCD Application Team + * @version V1.5.0 + * @date 06-March-2015 + * @brief Main Interrupt Service Routines. + * This file provides template for all exceptions handler and + * peripherals interrupt service routine. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f4xx_it.h" + +/** @addtogroup STM32F4xx_StdPeriph_Examples + * @{ + */ + +/** @addtogroup ADC_DMA + * @{ + */ + +/* Private typedef -----------------------------------------------------------*/ +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Private functions ---------------------------------------------------------*/ + +/******************************************************************************/ +/* Cortex-M4 Processor Exceptions Handlers */ +/******************************************************************************/ + +/** + * @brief This function handles NMI exception. + * @param None + * @retval None + */ +void NMI_Handler(void) +{ + send_text( "NMI Fault.\n" ); + +} + +/** + * @brief This function handles Hard Fault exception. + * @param None + * @retval None + */ +void HardFault_Handler(void) +{ + send_text( "Hard Fault.\n" ); + /* Go to infinite loop when Hard Fault exception occurs */ + while (1) + { + } +} + +/** + * @brief This function handles Memory Manage exception. + * @param None + * @retval None + */ +void MemManage_Handler(void) +{ + send_text( "MemRange Fault.\n" ); + + /* Go to infinite loop when Memory Manage exception occurs */ + while (1) + { + } +} + +/** + * @brief This function handles Bus Fault exception. + * @param None + * @retval None + */ +void BusFault_Handler(void) +{ + send_text( "Bus Fault.\n" ); + + /* Go to infinite loop when Bus Fault exception occurs */ + while (1) + { + } +} + +/** + * @brief This function handles Usage Fault exception. + * @param None + * @retval None + */ +void UsageFault_Handler(void) +{ + send_text( "Usage Fault.\n" ); + + /* Go to infinite loop when Usage Fault exception occurs */ + while (1) + { + } +} + +/** + * @brief This function handles SVCall exception. + * @param None + * @retval None + */ +void SVC_Handler(void) +{ + send_text( "SVC Fault.\n" ); + +} + +/** + * @brief This function handles Debug Monitor exception. + * @param None + * @retval None + */ +void DebugMon_Handler(void) +{ +} + +/** + * @brief This function handles PendSVC exception. + * @param None + * @retval None + */ +void PendSV_Handler(void) +{ +} + +/** + * @brief This function handles SysTick Handler. + * @param None + * @retval None + */ +void SysTick_Handler(void) +{ + TimingDelay_Decrement(); +} + +/******************************************************************************/ +/* STM32F4xx Peripherals Interrupt Handlers */ +/* Add here the Interrupt Handler for the used peripheral(s) (PPP), for the */ +/* available peripheral interrupt handler's name please refer to the startup */ +/* file (startup_stm32f40xx.s/startup_stm32f427x.s/startup_stm32f429x.s). */ +/******************************************************************************/ + +/** + * @} + */ + +/** + * @} + */ + + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/firmware/stm32f4xx_it.h b/firmware/stm32f4xx_it.h new file mode 100644 index 0000000..d94304f --- /dev/null +++ b/firmware/stm32f4xx_it.h @@ -0,0 +1,58 @@ +/** + ****************************************************************************** + * @file ADC/ADC_DMA/stm32f4xx_it.h + * @author MCD Application Team + * @version V1.5.0 + * @date 06-March-2015 + * @brief This file contains the headers of the interrupt handlers. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2015 STMicroelectronics

+ * + * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * + * http://www.st.com/software_license_agreement_liberty_v2 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F4xx_IT_H +#define __STM32F4xx_IT_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ +/* Exported macro ------------------------------------------------------------*/ +/* Exported functions ------------------------------------------------------- */ + +void NMI_Handler(void); +void HardFault_Handler(void); +void MemManage_Handler(void); +void BusFault_Handler(void); +void UsageFault_Handler(void); +void SVC_Handler(void); +void DebugMon_Handler(void); +void PendSV_Handler(void); +void SysTick_Handler(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F4xx_IT_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/