diff options
Diffstat (limited to 'controller/fw/src')
-rw-r--r-- | controller/fw/src/adc.c | 93 | ||||
-rw-r--r-- | controller/fw/src/adc.h | 10 | ||||
-rw-r--r-- | controller/fw/src/dsss_demod.c | 16 | ||||
-rw-r--r-- | controller/fw/src/dsss_demod.h | 3 | ||||
-rw-r--r-- | controller/fw/src/freq_meas.c | 4 | ||||
-rw-r--r-- | controller/fw/src/freq_meas.h | 2 | ||||
-rw-r--r-- | controller/fw/src/main.c | 143 | ||||
-rw-r--r-- | controller/fw/src/mspdebug_wrapper.c | 18 | ||||
-rw-r--r-- | controller/fw/src/protocol.c | 7 | ||||
-rw-r--r-- | controller/fw/src/spi_flash.c | 88 | ||||
-rw-r--r-- | controller/fw/src/spi_flash.h | 6 | ||||
-rw-r--r-- | controller/fw/src/sr_global.h | 10 | ||||
-rw-r--r-- | controller/fw/src/startup_stm32f407xx.s | 521 | ||||
-rw-r--r-- | controller/fw/src/stm32f4_isr.h | 98 | ||||
-rw-r--r-- | controller/fw/src/system_stm32f4xx.c | 743 |
15 files changed, 1680 insertions, 82 deletions
diff --git a/controller/fw/src/adc.c b/controller/fw/src/adc.c new file mode 100644 index 0000000..5f6b995 --- /dev/null +++ b/controller/fw/src/adc.c @@ -0,0 +1,93 @@ + +#include <stm32f407xx.h> +#include <stm32f4_isr.h> + +#include "adc.h" +#include "sr_global.h" + +uint16_t adc_fft_buf[2][FMEAS_FFT_LEN]; +volatile int adc_fft_buf_ready_idx = -1; + +static DMA_TypeDef *const adc_dma = DMA2; +static DMA_Stream_TypeDef *const mem_stream = DMA2_Stream1; +static DMA_Stream_TypeDef *const adc_stream = DMA2_Stream0; +static const int dma_adc_channel = 0; + +/* Configure ADC1 to sample channel 0. Trigger from TIM1 CC0 every 1ms. Transfer readings into alternating buffers + * throug DMA. Enable DMA interrupts. + * + * We have two full FFT buffers. We always transfer data from the ADC to the back half of the active one, while a + * DMA memcpy'es the latter half of the inactive one to the front half of the active one. This means at the end of the + * ADC's DMA transfer, in the now-inactive buffer that the ADC results were just written to we have last half-period's + * data sitting in front of this half-period's data like so: [old_adc_data, new_adc_data] + * + * This means we can immediately start running an FFT on ADC DMA transfer complete interrupt. + */ +void adc_init() { + RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN; + RCC->APB2ENR |= RCC_APB2ENR_ADC1EN | RCC_APB2ENR_TIM1EN; + + adc_dma->LIFCR |= 0x3f; + adc_stream->CR = 0; /* disable */ + while (adc_stream->CR & DMA_SxCR_EN) + ; /* wait for stream to become available */ + adc_stream->NDTR = FMEAS_FFT_LEN/2; + adc_stream->PAR = ADC1->DR; + adc_stream->M0AR = (uint32_t) (adc_fft_buf[0] + FMEAS_FFT_LEN/2); + adc_stream->M1AR = (uint32_t) (adc_fft_buf[1] + FMEAS_FFT_LEN/2); + adc_stream->CR = (dma_adc_channel<<DMA_SxCR_CHSEL_Pos) | DMA_SxCR_DBM | (1<<DMA_SxCR_MSIZE_Pos) + | (1<<DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_PFCTRL + | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_DMEIE | DMA_SxCR_EN; + adc_stream->CR |= DMA_SxCR_EN; + + NVIC_EnableIRQ(DMA2_Stream0_IRQn); + NVIC_SetPriority(DMA2_Stream0_IRQn, 128); + + ADC1->CR1 = (0<<ADC_CR1_RES_Pos) | (0<<ADC_CR1_DISCNUM_Pos) | ADC_CR1_DISCEN | (0<<ADC_CR1_AWDCH_Pos); + ADC1->CR2 = ADC_CR2_EXTEN | (0<<ADC_CR2_EXTSEL_Pos) | ADC_CR2_DMA | ADC_CR2_ADON | ADC_CR2_DDS; + + TIM1->CR2 = (2<<TIM_CR2_MMS_Pos); /* Enable update event on TRGO to provide a 1ms reference to rest of system */ + TIM1->CR1 = TIM_CR1_CEN; + TIM1->CCMR1 = (6<<TIM_CCMR1_OC1M_Pos) | (0<<TIM_CCMR1_CC1S_Pos); + TIM1->CCER = TIM_CCER_CC1E; + TIM1->PSC = 84; /* 1us ticks @ f_APB2=84MHz */ + TIM1->ARR = 1000; /* 1ms period */ + TIM1->CCR1 = 1; + TIM1->EGR = TIM_EGR_UG; +} + +void DMA2_Stream0_IRQHandler(void) { + uint8_t isr = (DMA2->LISR >> DMA_LISR_FEIF0_Pos) & 0x3f; + + if (isr & DMA_LISR_TCIF0) { /* Transfer complete */ + /* Check we're done processing the old buffer */ + if (adc_fft_buf_ready_idx != -1) + panic(); + + /* Kickoff memory DMA into new buffer */ + if (mem_stream->CR & DMA_SxCR_EN) + panic(); /* We should be long done by now. */ + + mem_stream->NDTR = FMEAS_FFT_LEN/2; + int ct = !!(adc_stream->CR & DMA_SxCR_CT); + /* back half of old buffer (that was just written) */ + mem_stream->PAR = (uint32_t)(adc_fft_buf[!ct]); + /* front half of current buffer (whose back half is being written now) */ + mem_stream->M0AR = (uint32_t) (adc_fft_buf[ct] + 0); + mem_stream->CR = (1<<DMA_SxCR_MSIZE_Pos) | (1<<DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | DMA_SxCR_PINC + | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_DMEIE; + mem_stream->CR |= DMA_SxCR_EN; + + /* Kickoff FFT */ + adc_fft_buf_ready_idx = !ct; + } + + if (isr & DMA_LISR_DMEIF0) /* Direct mode error */ + panic(); + + if (isr & DMA_LISR_TEIF0) /* Transfer error */ + panic(); + + /* clear all flags */ + adc_dma->LIFCR = isr<<DMA_LISR_FEIF0_Pos; +} diff --git a/controller/fw/src/adc.h b/controller/fw/src/adc.h new file mode 100644 index 0000000..e5611cb --- /dev/null +++ b/controller/fw/src/adc.h @@ -0,0 +1,10 @@ +#ifndef __ADC_H__ +#define __ADC_H__ + +void adc_init(void); + +extern uint16_t adc_fft_buf[2][FMEAS_FFT_LEN]; +/* set index of ready buffer in adc.c, reset to -1 in main.c after processing */ +extern volatile int adc_fft_buf_ready_idx; + +#endif /* __ADC_H__ */ diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c index c5a2070..0758525 100644 --- a/controller/fw/src/dsss_demod.c +++ b/controller/fw/src/dsss_demod.c @@ -23,12 +23,15 @@ struct iir_biquad cwt_filter_bq[DSSS_FILTER_CLEN] = {DSSS_FILTER_COEFF}; void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug); static float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug); static float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx); +#if 0 static float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]); static float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st); +#endif static void matcher_init(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE]); static void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], uint64_t ts, int peak_ch, float peak_ampl); static void group_received(struct dsss_demod_state *st); +static uint8_t decode_peak(int peak_ch, float peak_ampl); #ifdef SIMULATION void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) { @@ -48,7 +51,8 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t DEBUG_PRINTN("]\n"); } #else -void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {} +void debug_print_vector(unused_a const char *name, unused_a size_t len, unused_a const float *data, + unused_a size_t stride, unused_a bool index, unused_a bool debug) {} #endif void dsss_demod_init(struct dsss_demod_state *st) { @@ -74,7 +78,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts) float avg = 0.0f; for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) - avg += fabs(cwt[i]); + avg += fabsf(cwt[i]); avg /= (float)DSSS_GOLD_CODE_COUNT; /* FIXME fix this filter */ //avg = run_iir(avg, ARRAY_LENGTH(cwt_filter_bq), cwt_filter_bq, st->cwt_filter.st); @@ -86,12 +90,12 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts) for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) { float val = cwt[i] / avg; - if (fabs(val) > fabs(max_val)) { + if (fabsf(val) > fabsf(max_val)) { max_val = val; max_ch = i; max_ts = ts; - if (fabs(val) > DSSS_THESHOLD_FACTOR) + if (fabsf(val) > DSSS_THESHOLD_FACTOR) found = true; } } @@ -155,7 +159,7 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u if (current_phase == states[i].last_phase) { /* Skip sampling */ - float score = fabs(peak_ampl) * (1.0f - skip_sampling_depreciation); + float score = fabsf(peak_ampl) * (1.0f - skip_sampling_depreciation); if (score > states[i].candidate_score) { /* We win, update candidate */ assert(i < DSSS_MATCHER_CACHE_SIZE); @@ -272,6 +276,7 @@ void group_received(struct dsss_demod_state *st) { } } +#if 0 float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]) { float intermediate = x; for (int i=0; i<(order+1)/2; i++) @@ -287,6 +292,7 @@ float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_st st->reg[0] = intermediate; return out; } +#endif float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) { float sum = 0.0f; diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h index 98cf590..448abb4 100644 --- a/controller/fw/src/dsss_demod.h +++ b/controller/fw/src/dsss_demod.h @@ -1,6 +1,9 @@ #ifndef __DSSS_DEMOD_H__ #define __DSSS_DEMOD_H__ +#include <stdint.h> +#include <unistd.h> + #define DSSS_GOLD_CODE_LENGTH ((1<<DSSS_GOLD_CODE_NBITS) - 1) #define DSSS_GOLD_CODE_COUNT ((1<<DSSS_GOLD_CODE_NBITS) + 1) #define DSSS_CORRELATION_LENGTH (DSSS_GOLD_CODE_LENGTH * DSSS_DECIMATION) diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c index ba7801e..83a330c 100644 --- a/controller/fw/src/freq_meas.c +++ b/controller/fw/src/freq_meas.c @@ -30,7 +30,7 @@ extern arm_status arm_rfft_4096_fast_init_f32(arm_rfft_fast_instance_f32 * S); void func_gauss_grad(float *out, float *params, int x, void *userdata); float func_gauss(float *params, int x, void *userdata); -int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out) { +int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) { int rc; float in_buf[FMEAS_FFT_LEN]; float out_buf[FMEAS_FFT_LEN]; @@ -42,7 +42,7 @@ int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out) { */ //DEBUG_PRINT("Applying window function"); for (size_t i=0; i<FMEAS_FFT_LEN; i++) - in_buf[i] = (float)adc_buf[i] / (float)FMEAS_ADC_MAX * fmeas_fft_window_table[i]; + in_buf[i] = ((float)adc_buf[i] / (float)FMEAS_ADC_MAX - 0.5) * fmeas_fft_window_table[i]; //DEBUG_PRINT("Running FFT"); arm_rfft_fast_instance_f32 fft_inst; diff --git a/controller/fw/src/freq_meas.h b/controller/fw/src/freq_meas.h index 4786c0e..1c083f8 100644 --- a/controller/fw/src/freq_meas.h +++ b/controller/fw/src/freq_meas.h @@ -2,6 +2,6 @@ #ifndef __FREQ_MEAS_H__ #define __FREQ_MEAS_H__ -int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out); +int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out); #endif /* __FREQ_MEAS_H__ */ diff --git a/controller/fw/src/main.c b/controller/fw/src/main.c index 3131df1..58977f7 100644 --- a/controller/fw/src/main.c +++ b/controller/fw/src/main.c @@ -1,45 +1,126 @@ #include <stdbool.h> +#include <stdint.h> +#include <assert.h> -#include <libopencm3/stm32/rcc.h> -#include <libopencm3/stm32/spi.h> -#include <libopencm3/stm32/gpio.h> +#include <stm32f407xx.h> +#include "sr_global.h" +#include "adc.h" #include "spi_flash.h" +#include "freq_meas.h" +#include "dsss_demod.h" static struct spi_flash_if spif; + +void __libc_init_array(void) { /* we don't need this. */ } +void __assert_func (unused_a const char *file, unused_a int line, unused_a const char *function, unused_a const char *expr) { + asm volatile ("bkpt"); + while(1) {} +} + static void clock_setup(void) { - rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]); - rcc_periph_clock_enable(RCC_GPIOA); - rcc_periph_clock_enable(RCC_GPIOB); - rcc_periph_clock_enable(RCC_SPI1); + /* 8MHz HSE clock as PLL source. + * + * Divide by 8 -> 1 MHz */ +#define PLL_M 8 + /* Multiply by 336 -> 336 MHz VCO frequency */ +#define PLL_N 336 + /* Divide by 2 -> 168 MHz (max freq for our chip) */ +#define PLL_P 2 + /* Aux clock for USB OTG, SDIO, RNG: divide VCO frequency (336 MHz) by 7 -> 48 MHz (required by USB OTG) */ +#define PLL_Q 7 + + RCC->CR |= RCC_CR_HSEON; + while(!(RCC->CR & RCC_CR_HSERDY)); + + RCC->APB1ENR |= RCC_APB1ENR_PWREN; + + /* set voltage scale to 1 for max frequency + * (0b0) scale 2 for fCLK <= 144 Mhz + * (0b1) scale 1 for 144 Mhz < fCLK <= 168 Mhz + */ + PWR->CR |= PWR_CR_VOS; + + /* set AHB prescaler to /1 (CFGR:bits 7:4) */ + RCC->CFGR |= (0 << RCC_CFGR_HPRE_Pos); + /* set ABP1 prescaler to 4 */ + RCC->CFGR |= (5 << RCC_CFGR_PPRE1_Pos); + /* set ABP2 prescaler to 2 */ + RCC->CFGR |= (0x4 << RCC_CFGR_PPRE2_Pos); + + /* Configure PLL */ + static_assert(PLL_P % 2 == 0); + static_assert(PLL_P >= 2 && PLL_P <= 8); + static_assert(PLL_N >= 50 && PLL_N <= 432); + static_assert(PLL_M >= 2 && PLL_M <= 63); + static_assert(PLL_Q >= 2 && PLL_Q <= 15); + RCC->PLLCFGR = (PLL_M<<RCC_PLLCFGR_PLLM_Pos) + | (PLL_N << RCC_PLLCFGR_PLLM_Pos) + | ((PLL_P/2 - 1) << RCC_PLLCFGR_PLLP_Pos) + | (PLL_Q << RCC_PLLCFGR_PLLQ_Pos) + | RCC_PLLCFGR_PLLSRC; /* select HSE as PLL source */ + RCC->CR |= RCC_CR_PLLON; + + /* Wait for main PLL */ + while(!(RCC->CR & RCC_CR_PLLRDY)) + ; + + /* Configure Flash: enable prefetch, insn cache, data cache; set latency = 5 wait states + * See reference manual (RM0090), Section 3.5.1, Table 10 (p. 80) + */ + FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN | FLASH_ACR_DCEN | (5<<FLASH_ACR_LATENCY); + + /* Select PLL as system clock source */ + RCC->CFGR &= ~RCC_CFGR_SW_Msk; + RCC->CFGR |= 2 << RCC_CFGR_SW_Pos; + + /* Wait for clock to switch over */ + while ((RCC->CFGR & RCC_CFGR_SWS_Msk)>>RCC_CFGR_SWS_Pos != 2) + ; } static void led_setup(void) { - gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6 | GPIO7); + GPIOA->MODER |= (1<<GPIO_MODER_MODER6_Pos) | (1<<GPIO_MODER_MODER7_Pos); } static void spi_flash_if_set_cs(bool val) { if (val) - gpio_set(GPIOB, GPIO0); + GPIOB->BSRR = 1<<0; else - gpio_clear(GPIOB, GPIO0); + GPIOB->BSRR = 1<<16; } static void spi_flash_setup(void) { - gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO3 | GPIO4 | GPIO5); /* SPI flash SCK/MISO/MOSI */ - gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO0); /* SPI flash CS */ - gpio_set_output_options(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO0 | GPIO3 | GPIO4 | GPIO5); - gpio_set_af(GPIOB, 5, GPIO3 | GPIO4 | GPIO5); + GPIOB->MODER &= ~GPIO_MODER_MODER3_Msk & ~GPIO_MODER_MODER4_Msk & ~GPIO_MODER_MODER5_Msk & ~GPIO_MODER_MODER0_Msk; + GPIOB->MODER |= (2<<GPIO_MODER_MODER3_Pos) /* SCK */ + | (2<<GPIO_MODER_MODER4_Pos) /* MISO */ + | (2<<GPIO_MODER_MODER5_Pos) /* MOSI */ + | (1<<GPIO_MODER_MODER0_Pos); /* CS */ + + GPIOB->OSPEEDR &= ~GPIO_OSPEEDR_OSPEED3_Msk & ~GPIO_OSPEEDR_OSPEED4_Msk + & ~GPIO_OSPEEDR_OSPEED5_Msk & ~GPIO_OSPEEDR_OSPEED0_Msk; + GPIOB->OSPEEDR |= (2<<GPIO_OSPEEDR_OSPEED3_Pos) /* SCK */ + | (2<<GPIO_OSPEEDR_OSPEED4_Pos) /* MISO */ + | (2<<GPIO_OSPEEDR_OSPEED5_Pos) /* MOSI */ + | (2<<GPIO_OSPEEDR_OSPEED0_Pos); /* CS */ + + GPIOB->AFR[0] &= ~GPIO_AFRL_AFSEL3_Msk & ~GPIO_AFRL_AFSEL4_Msk & ~GPIO_AFRL_AFSEL5_Msk; + GPIOB->AFR[0] |= (5<<GPIO_AFRL_AFSEL3_Pos) | (5<<GPIO_AFRL_AFSEL4_Pos) | (5<<GPIO_AFRL_AFSEL5_Pos); + + RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; + RCC->APB2RSTR |= RCC_APB2RSTR_SPI1RST; + RCC->APB2RSTR &= RCC_APB2RSTR_SPI1RST; spif_init(&spif, 256, SPI1, &spi_flash_if_set_cs); } -/* +/* SPI flash test routine to be called from gdb */ +#ifdef SPI_FLASH_TEST void spi_flash_test(void) { spif_clear_mem(&spif); @@ -48,7 +129,7 @@ void spi_flash_test(void) { for (size_t i=0; i<sizeof(buf); i+= sizeof(buf[0])) buf[i/sizeof(buf[0])] = addr + i; - spif_write(&spif, addr, sizeof(buf), (char *)buf); + spif_write(&spif, addr, sizeof(buf), (char *)buf); } for (size_t i=0; i<sizeof(buf)/sizeof(buf[0]); i++) @@ -56,21 +137,35 @@ void spi_flash_test(void) { spif_read(&spif, 0x1030, sizeof(buf), (char *)buf); asm volatile ("bkpt"); } -*/ +#endif + +static unsigned int measurement_errors = 0; +static struct dsss_demod_state demod_state; +static uint32_t freq_sample_ts = 0; int main(void) { clock_setup(); led_setup(); spi_flash_setup(); - - gpio_set(GPIOA, GPIO6); - - while (1) { - gpio_toggle(GPIOA, GPIO6 | GPIO7); - for (int i = 0; i < 6000000; i++) - __asm__("nop"); + adc_init(); + + while (23) { + if (adc_fft_buf_ready_idx != -1) { + float out; + if (adc_buf_measure_freq(adc_fft_buf[adc_fft_buf_ready_idx], &out)) { + measurement_errors++; + continue; + } + + dsss_demod_init(&demod_state); + dsss_demod_step(&demod_state, out, freq_sample_ts); + + freq_sample_ts++; /* TODO: also increase in case of freq measurement error? */ + adc_fft_buf_ready_idx = -1; + } } return 0; } + diff --git a/controller/fw/src/mspdebug_wrapper.c b/controller/fw/src/mspdebug_wrapper.c index 3a6d526..eda91cc 100644 --- a/controller/fw/src/mspdebug_wrapper.c +++ b/controller/fw/src/mspdebug_wrapper.c @@ -2,14 +2,14 @@ #include <unistd.h> #include <errno.h> -#include <libopencm3/stm32/gpio.h> - #include "output.h" #include "jtaglib.h" #include "sr_global.h" #include "mspdebug_wrapper.h" +#include <stm32f407xx.h> + #define BLOCK_SIZE 512 /* bytes */ @@ -101,7 +101,7 @@ enum sr_gpio_types { }; struct { - uint32_t port; + GPIO_TypeDef *gpio; uint16_t num; } gpios[SR_NUM_GPIOS] = { [SR_GPIO_TCK] = {GPIOD, 8}, @@ -114,9 +114,9 @@ struct { static void sr_gpio_write(int num, int out) { if (out) - gpio_set(gpios[num].port, gpios[num].num); + gpios[num].gpio->BSRR = 1<<gpios[num].num; else - gpio_clear(gpios[num].port, gpios[num].num); + gpios[num].gpio->BSRR = 1<<gpios[num].num<<16; } static void sr_jtdev_tck(struct jtdev *p, int out) { @@ -146,7 +146,7 @@ static void sr_jtdev_tst(struct jtdev *p, int out) { static int sr_jtdev_tdo_get(struct jtdev *p) { UNUSED(p); - return gpio_get(gpios[SR_GPIO_TST].port, gpios[SR_GPIO_TST].num); + return !!(gpios[SR_GPIO_TST].gpio->IDR & (1<<gpios[SR_GPIO_TST].num)); } static void sr_jtdev_tclk(struct jtdev *p, int out) { @@ -156,14 +156,14 @@ static void sr_jtdev_tclk(struct jtdev *p, int out) { static int sr_jtdev_tclk_get(struct jtdev *p) { UNUSED(p); - return gpio_get(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num); + return !!(gpios[SR_GPIO_TDI].gpio->IDR & (1<<gpios[SR_GPIO_TDI].num)); } static void sr_jtdev_tclk_strobe(struct jtdev *p, unsigned int count) { UNUSED(p); while (count--) { - gpio_set(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num); - gpio_clear(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num); + gpios[SR_GPIO_TDI].gpio->BSRR = 1<<gpios[SR_GPIO_TDI].num; + gpios[SR_GPIO_TDI].gpio->BSRR = 1<<gpios[SR_GPIO_TDI].num<<16; } } diff --git a/controller/fw/src/protocol.c b/controller/fw/src/protocol.c new file mode 100644 index 0000000..17020bd --- /dev/null +++ b/controller/fw/src/protocol.c @@ -0,0 +1,7 @@ + +#include "sr_global.h" +#include "dsss_demod.h" + +void handle_dsss_received(uint8_t unused_a data[static TRANSMISSION_SYMBOLS]) { + asm volatile ("bkpt"); +} diff --git a/controller/fw/src/spi_flash.c b/controller/fw/src/spi_flash.c index 98c773f..305be8f 100644 --- a/controller/fw/src/spi_flash.c +++ b/controller/fw/src/spi_flash.c @@ -22,7 +22,6 @@ */ #include "spi_flash.h" -#include <libopencm3/stm32/spi.h> enum { WRITE_ENABLE = 0x06, @@ -49,6 +48,10 @@ enum { }; +static uint8_t spi_xfer(volatile SPI_TypeDef *spi, uint8_t b); +static uint8_t spi_read(struct spi_flash_if *spif); +static void spi_write(struct spi_flash_if *spif, uint8_t b); + static void spif_write_page(struct spi_flash_if *spif, size_t addr, size_t len, const char* data); static uint8_t spif_read_status(struct spi_flash_if *spif); static void spif_enable_write(struct spi_flash_if *spif); @@ -58,32 +61,38 @@ static void spif_wait_for_write(struct spi_flash_if *spif); #define mid_byte(x) ((x>>8)&0xff) #define high_byte(x) ((x>>16)&0xff) +uint8_t spi_xfer(volatile SPI_TypeDef *spi, uint8_t b) { + while (!(spi->SR & SPI_SR_TXE)) + ; + (void) spi->DR; /* perform dummy read to clear RXNE flag */ + spi->DR = b; + while (!(spi->SR & SPI_SR_RXNE)) + ; + return spi->DR; +} -void spif_init(struct spi_flash_if *spif, size_t page_size, uint32_t spi_base, void (*cs)(bool val)) { +uint8_t spi_read(struct spi_flash_if *spif) { + return spi_xfer(spif->spi, 0); +} + +void spi_write(struct spi_flash_if *spif, uint8_t b) { + (void)spi_xfer(spif->spi, b); +} - spif->spi_base = spi_base; +void spif_init(struct spi_flash_if *spif, size_t page_size, SPI_TypeDef *spi, void (*cs)(bool val)) { + + spif->spi = spi; spif->page_size = page_size; spif->cs = cs; spif->cs(1); - spi_reset(spif->spi_base); - spi_init_master(spif->spi_base, - SPI_CR1_BAUDRATE_FPCLK_DIV_2, - SPI_CR1_CPOL_CLK_TO_1_WHEN_IDLE, - SPI_CR1_CPHA_CLK_TRANSITION_2, - SPI_CR1_DFF_8BIT, - SPI_CR1_MSBFIRST); - - spi_enable_software_slave_management(spif->spi_base); - spi_set_nss_high(spif->spi_base); - - spi_enable(spif->spi_base); + spi->CR1 = (0<<SPI_CR1_BR_Pos) | SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_CR1_SSM | SPI_CR1_SSI | SPI_CR1_SPE | SPI_CR1_MSTR; spif->cs(0); - spi_send(spif->spi_base, READ_IDENTIFICATION); - spif->id.mfg_id = spi_xfer(spif->spi_base, 0); - spif->id.type = spi_xfer(spif->spi_base, 0); - spif->id.size = 1<<spi_xfer(spif->spi_base, 0); + spi_write(spif, READ_IDENTIFICATION); + spif->id.mfg_id = spi_read(spif); + spif->id.type = spi_read(spif); + spif->id.size = 1<<spi_read(spif); spif->cs(1); } @@ -91,13 +100,13 @@ void spif_read(struct spi_flash_if *spif, size_t addr, size_t len, char* data) { spif_enable_write(spif); spif->cs(0); - spi_xfer(spif->spi_base, READ_DATA); - spi_xfer(spif->spi_base, high_byte(addr)); - spi_xfer(spif->spi_base, mid_byte(addr)); - spi_xfer(spif->spi_base, low_byte(addr)); + spi_write(spif, READ_DATA); + spi_write(spif, high_byte(addr)); + spi_write(spif, mid_byte(addr)); + spi_write(spif, low_byte(addr)); for (size_t i = 0; i < len; i++) - data[i] = spi_xfer(spif->spi_base, 0); + data[i] = spi_read(spif); spif->cs(1); } @@ -117,8 +126,8 @@ void spif_write(struct spi_flash_if *spif, size_t addr, size_t len, const char* static uint8_t spif_read_status(struct spi_flash_if *spif) { spif->cs(0); - spi_xfer(spif->spi_base, READ_STATUS); - uint8_t status = spi_xfer(spif->spi_base, 0); + spi_write(spif, READ_STATUS); + uint8_t status = spi_read(spif); spif->cs(1); return status; @@ -128,10 +137,10 @@ void spif_clear_sector(struct spi_flash_if *spif, size_t addr) { spif_enable_write(spif); spif->cs(0); - spi_xfer(spif->spi_base, SECTOR_ERASE); - spi_xfer(spif->spi_base, high_byte(addr)); - spi_xfer(spif->spi_base, mid_byte(addr)); - spi_xfer(spif->spi_base, low_byte(addr)); + spi_write(spif, SECTOR_ERASE); + spi_write(spif, high_byte(addr)); + spi_write(spif, mid_byte(addr)); + spi_write(spif, low_byte(addr)); spif->cs(1); spif_wait_for_write(spif); @@ -141,7 +150,7 @@ void spif_clear_mem(struct spi_flash_if *spif) { spif_enable_write(spif); spif->cs(0); - spi_xfer(spif->spi_base, BULK_ERASE); + spi_write(spif, BULK_ERASE); spif->cs(1); spif_wait_for_write(spif); @@ -151,13 +160,13 @@ static void spif_write_page(struct spi_flash_if *spif, size_t addr, size_t len, spif_enable_write(spif); spif->cs(0); - spi_xfer(spif->spi_base, PAGE_PROGRAM); - spi_xfer(spif->spi_base, high_byte(addr)); - spi_xfer(spif->spi_base, mid_byte(addr)); - spi_xfer(spif->spi_base, low_byte(addr)); + spi_write(spif, PAGE_PROGRAM); + spi_write(spif, high_byte(addr)); + spi_write(spif, mid_byte(addr)); + spi_write(spif, low_byte(addr)); for (size_t i = 0; i < len; i++) { - spi_xfer(spif->spi_base, data[i]); + spi_write(spif, data[i]); } spif->cs(1); @@ -166,7 +175,7 @@ static void spif_write_page(struct spi_flash_if *spif, size_t addr, size_t len, static void spif_enable_write(struct spi_flash_if *spif) { spif->cs(0); - spi_xfer(spif->spi_base, WRITE_ENABLE); + spi_write(spif, WRITE_ENABLE); spif->cs(1); } @@ -178,12 +187,13 @@ static void spif_wait_for_write(struct spi_flash_if *spif) { void spif_deep_power_down(struct spi_flash_if *spif) { spif->cs(0); - spi_xfer(spif->spi_base, DEEP_POWER_DOWN); + spi_write(spif, DEEP_POWER_DOWN); spif->cs(1); } void spif_wakeup(struct spi_flash_if *spif) { spif->cs(0); - spi_xfer(spif->spi_base, DEEP_POWER_DOWN_RELEASE); + spi_write(spif, DEEP_POWER_DOWN_RELEASE); spif->cs(1); } + diff --git a/controller/fw/src/spi_flash.h b/controller/fw/src/spi_flash.h index e647c6a..2be8933 100644 --- a/controller/fw/src/spi_flash.h +++ b/controller/fw/src/spi_flash.h @@ -4,6 +4,8 @@ #include <stdbool.h> #include <unistd.h> +#include <stm32f407xx.h> + struct spi_mem_id { size_t size; uint8_t mfg_id; @@ -12,12 +14,12 @@ struct spi_mem_id { struct spi_flash_if { struct spi_mem_id id; - uint32_t spi_base; + volatile SPI_TypeDef *spi; size_t page_size; void (*cs)(bool val); }; -void spif_init(struct spi_flash_if *spif, size_t page_size, uint32_t spi_base, void (*cs)(bool val)); +void spif_init(struct spi_flash_if *spif, size_t page_size, SPI_TypeDef *spi, void (*cs)(bool val)); void spif_write(struct spi_flash_if *spif, size_t addr, size_t len, const char* data); void spif_read(struct spi_flash_if *spif, size_t addr, size_t len, char* data); diff --git a/controller/fw/src/sr_global.h b/controller/fw/src/sr_global.h index abbad93..ac77359 100644 --- a/controller/fw/src/sr_global.h +++ b/controller/fw/src/sr_global.h @@ -1,9 +1,19 @@ #ifndef __SR_GLOBAL_H__ #define __SR_GLOBAL_H__ +#include <stdint.h> + #define UNUSED(x) ((void) x) #define ARRAY_LENGTH(x) (sizeof(x) / sizeof(x[0])) +#define unused_a __attribute__((unused)) + static inline uint16_t htole(uint16_t val) { return val; } +void __libc_init_array(void); + +static inline void panic(void) { + asm volatile ("bkpt"); +} + #endif /* __SR_GLOBAL_H__ */ diff --git a/controller/fw/src/startup_stm32f407xx.s b/controller/fw/src/startup_stm32f407xx.s new file mode 100644 index 0000000..aeeeb22 --- /dev/null +++ b/controller/fw/src/startup_stm32f407xx.s @@ -0,0 +1,521 @@ +/** + ****************************************************************************** + * @file startup_stm32f407xx.s + * @author MCD Application Team + * @brief STM32F407xx Devices vector table for GCC based toolchains. + * This module performs: + * - Set the initial SP + * - Set the initial PC == Reset_Handler, + * - Set the vector table entries with the exceptions ISR address + * - Branches to main in the C library (which eventually + * calls main()). + * After Reset the Cortex-M4 processor is in Thread mode, + * priority is Privileged, and the Stack is set to Main. + ****************************************************************************** + * @attention + * + * <h2><center>© COPYRIGHT 2017 STMicroelectronics</center></h2> + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + + .syntax unified + .cpu cortex-m4 + .fpu softvfp + .thumb + +.global g_pfnVectors +.global Default_Handler + +/* start address for the initialization values of the .data section. +defined in linker script */ +.word _sidata +/* start address for the .data section. defined in linker script */ +.word _sdata +/* end address for the .data section. defined in linker script */ +.word _edata +/* start address for the .bss section. defined in linker script */ +.word _sbss +/* end address for the .bss section. defined in linker script */ +.word _ebss +/* stack used for SystemInit_ExtMemCtl; always internal RAM used */ + +/** + * @brief This is the code that gets called when the processor first + * starts execution following a reset event. Only the absolutely + * necessary set is performed, after which the application + * supplied main() routine is called. + * @param None + * @retval : None +*/ + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function +Reset_Handler: + ldr sp, =_estack /* set stack pointer */ + +/* Copy the data segment initializers from flash to SRAM */ + movs r1, #0 + b LoopCopyDataInit + +CopyDataInit: + ldr r3, =_sidata + ldr r3, [r3, r1] + str r3, [r0, r1] + adds r1, r1, #4 + +LoopCopyDataInit: + ldr r0, =_sdata + ldr r3, =_edata + adds r2, r0, r1 + cmp r2, r3 + bcc CopyDataInit + ldr r2, =_sbss + b LoopFillZerobss +/* Zero fill the bss segment. */ +FillZerobss: + movs r3, #0 + str r3, [r2], #4 + +LoopFillZerobss: + ldr r3, = _ebss + cmp r2, r3 + bcc FillZerobss + +/* Call the clock system intitialization function.*/ + bl SystemInit +/* Call static constructors */ + bl __libc_init_array +/* Call the application's entry point.*/ + bl main + bx lr +.size Reset_Handler, .-Reset_Handler + +/** + * @brief This is the code that gets called when the processor receives an + * unexpected interrupt. This simply enters an infinite loop, preserving + * the system state for examination by a debugger. + * @param None + * @retval None +*/ + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop + .size Default_Handler, .-Default_Handler +/****************************************************************************** +* +* The minimal vector table for a Cortex M3. Note that the proper constructs +* must be placed on this to ensure that it ends up at physical address +* 0x0000.0000. +* +*******************************************************************************/ + .section .isr_vector,"a",%progbits + .type g_pfnVectors, %object + .size g_pfnVectors, .-g_pfnVectors + + +g_pfnVectors: + .word _estack + .word Reset_Handler + .word NMI_Handler + .word HardFault_Handler + .word MemManage_Handler + .word BusFault_Handler + .word UsageFault_Handler + .word 0 + .word 0 + .word 0 + .word 0 + .word SVC_Handler + .word DebugMon_Handler + .word 0 + .word PendSV_Handler + .word SysTick_Handler + + /* External Interrupts */ + .word WWDG_IRQHandler /* Window WatchDog */ + .word PVD_IRQHandler /* PVD through EXTI Line detection */ + .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */ + .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */ + .word FLASH_IRQHandler /* FLASH */ + .word RCC_IRQHandler /* RCC */ + .word EXTI0_IRQHandler /* EXTI Line0 */ + .word EXTI1_IRQHandler /* EXTI Line1 */ + .word EXTI2_IRQHandler /* EXTI Line2 */ + .word EXTI3_IRQHandler /* EXTI Line3 */ + .word EXTI4_IRQHandler /* EXTI Line4 */ + .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */ + .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */ + .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */ + .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */ + .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */ + .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */ + .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */ + .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */ + .word CAN1_TX_IRQHandler /* CAN1 TX */ + .word CAN1_RX0_IRQHandler /* CAN1 RX0 */ + .word CAN1_RX1_IRQHandler /* CAN1 RX1 */ + .word CAN1_SCE_IRQHandler /* CAN1 SCE */ + .word EXTI9_5_IRQHandler /* External Line[9:5]s */ + .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */ + .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */ + .word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */ + .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */ + .word TIM2_IRQHandler /* TIM2 */ + .word TIM3_IRQHandler /* TIM3 */ + .word TIM4_IRQHandler /* TIM4 */ + .word I2C1_EV_IRQHandler /* I2C1 Event */ + .word I2C1_ER_IRQHandler /* I2C1 Error */ + .word I2C2_EV_IRQHandler /* I2C2 Event */ + .word I2C2_ER_IRQHandler /* I2C2 Error */ + .word SPI1_IRQHandler /* SPI1 */ + .word SPI2_IRQHandler /* SPI2 */ + .word USART1_IRQHandler /* USART1 */ + .word USART2_IRQHandler /* USART2 */ + .word USART3_IRQHandler /* USART3 */ + .word EXTI15_10_IRQHandler /* External Line[15:10]s */ + .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */ + .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */ + .word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */ + .word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */ + .word TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */ + .word TIM8_CC_IRQHandler /* TIM8 Capture Compare */ + .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */ + .word FSMC_IRQHandler /* FSMC */ + .word SDIO_IRQHandler /* SDIO */ + .word TIM5_IRQHandler /* TIM5 */ + .word SPI3_IRQHandler /* SPI3 */ + .word UART4_IRQHandler /* UART4 */ + .word UART5_IRQHandler /* UART5 */ + .word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */ + .word TIM7_IRQHandler /* TIM7 */ + .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */ + .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */ + .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */ + .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */ + .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */ + .word ETH_IRQHandler /* Ethernet */ + .word ETH_WKUP_IRQHandler /* Ethernet Wakeup through EXTI line */ + .word CAN2_TX_IRQHandler /* CAN2 TX */ + .word CAN2_RX0_IRQHandler /* CAN2 RX0 */ + .word CAN2_RX1_IRQHandler /* CAN2 RX1 */ + .word CAN2_SCE_IRQHandler /* CAN2 SCE */ + .word OTG_FS_IRQHandler /* USB OTG FS */ + .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */ + .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */ + .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */ + .word USART6_IRQHandler /* USART6 */ + .word I2C3_EV_IRQHandler /* I2C3 event */ + .word I2C3_ER_IRQHandler /* I2C3 error */ + .word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */ + .word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */ + .word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */ + .word OTG_HS_IRQHandler /* USB OTG HS */ + .word DCMI_IRQHandler /* DCMI */ + .word 0 /* CRYP crypto */ + .word HASH_RNG_IRQHandler /* Hash and Rng */ + .word FPU_IRQHandler /* FPU */ + + +/******************************************************************************* +* +* Provide weak aliases for each Exception handler to the Default_Handler. +* As they are weak aliases, any function with the same name will override +* this definition. +* +*******************************************************************************/ + .weak NMI_Handler + .thumb_set NMI_Handler,Default_Handler + + .weak HardFault_Handler + .thumb_set HardFault_Handler,Default_Handler + + .weak MemManage_Handler + .thumb_set MemManage_Handler,Default_Handler + + .weak BusFault_Handler + .thumb_set BusFault_Handler,Default_Handler + + .weak UsageFault_Handler + .thumb_set UsageFault_Handler,Default_Handler + + .weak SVC_Handler + .thumb_set SVC_Handler,Default_Handler + + .weak DebugMon_Handler + .thumb_set DebugMon_Handler,Default_Handler + + .weak PendSV_Handler + .thumb_set PendSV_Handler,Default_Handler + + .weak SysTick_Handler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDG_IRQHandler + .thumb_set WWDG_IRQHandler,Default_Handler + + .weak PVD_IRQHandler + .thumb_set PVD_IRQHandler,Default_Handler + + .weak TAMP_STAMP_IRQHandler + .thumb_set TAMP_STAMP_IRQHandler,Default_Handler + + .weak RTC_WKUP_IRQHandler + .thumb_set RTC_WKUP_IRQHandler,Default_Handler + + .weak FLASH_IRQHandler + .thumb_set FLASH_IRQHandler,Default_Handler + + .weak RCC_IRQHandler + .thumb_set RCC_IRQHandler,Default_Handler + + .weak EXTI0_IRQHandler + .thumb_set EXTI0_IRQHandler,Default_Handler + + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA1_Stream0_IRQHandler + .thumb_set DMA1_Stream0_IRQHandler,Default_Handler + + .weak DMA1_Stream1_IRQHandler + .thumb_set DMA1_Stream1_IRQHandler,Default_Handler + + .weak DMA1_Stream2_IRQHandler + .thumb_set DMA1_Stream2_IRQHandler,Default_Handler + + .weak DMA1_Stream3_IRQHandler + .thumb_set DMA1_Stream3_IRQHandler,Default_Handler + + .weak DMA1_Stream4_IRQHandler + .thumb_set DMA1_Stream4_IRQHandler,Default_Handler + + .weak DMA1_Stream5_IRQHandler + .thumb_set DMA1_Stream5_IRQHandler,Default_Handler + + .weak DMA1_Stream6_IRQHandler + .thumb_set DMA1_Stream6_IRQHandler,Default_Handler + + .weak ADC_IRQHandler + .thumb_set ADC_IRQHandler,Default_Handler + + .weak CAN1_TX_IRQHandler + .thumb_set CAN1_TX_IRQHandler,Default_Handler + + .weak CAN1_RX0_IRQHandler + .thumb_set CAN1_RX0_IRQHandler,Default_Handler + + .weak CAN1_RX1_IRQHandler + .thumb_set CAN1_RX1_IRQHandler,Default_Handler + + .weak CAN1_SCE_IRQHandler + .thumb_set CAN1_SCE_IRQHandler,Default_Handler + + .weak EXTI9_5_IRQHandler + .thumb_set EXTI9_5_IRQHandler,Default_Handler + + .weak TIM1_BRK_TIM9_IRQHandler + .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler + + .weak TIM1_UP_TIM10_IRQHandler + .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler + + .weak TIM1_TRG_COM_TIM11_IRQHandler + .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler + + .weak TIM1_CC_IRQHandler + .thumb_set TIM1_CC_IRQHandler,Default_Handler + + .weak TIM2_IRQHandler + .thumb_set TIM2_IRQHandler,Default_Handler + + .weak TIM3_IRQHandler + .thumb_set TIM3_IRQHandler,Default_Handler + + .weak TIM4_IRQHandler + .thumb_set TIM4_IRQHandler,Default_Handler + + .weak I2C1_EV_IRQHandler + .thumb_set I2C1_EV_IRQHandler,Default_Handler + + .weak I2C1_ER_IRQHandler + .thumb_set I2C1_ER_IRQHandler,Default_Handler + + .weak I2C2_EV_IRQHandler + .thumb_set I2C2_EV_IRQHandler,Default_Handler + + .weak I2C2_ER_IRQHandler + .thumb_set I2C2_ER_IRQHandler,Default_Handler + + .weak SPI1_IRQHandler + .thumb_set SPI1_IRQHandler,Default_Handler + + .weak SPI2_IRQHandler + .thumb_set SPI2_IRQHandler,Default_Handler + + .weak USART1_IRQHandler + .thumb_set USART1_IRQHandler,Default_Handler + + .weak USART2_IRQHandler + .thumb_set USART2_IRQHandler,Default_Handler + + .weak USART3_IRQHandler + .thumb_set USART3_IRQHandler,Default_Handler + + .weak EXTI15_10_IRQHandler + .thumb_set EXTI15_10_IRQHandler,Default_Handler + + .weak RTC_Alarm_IRQHandler + .thumb_set RTC_Alarm_IRQHandler,Default_Handler + + .weak OTG_FS_WKUP_IRQHandler + .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler + + .weak TIM8_BRK_TIM12_IRQHandler + .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler + + .weak TIM8_UP_TIM13_IRQHandler + .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler + + .weak TIM8_TRG_COM_TIM14_IRQHandler + .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler + + .weak TIM8_CC_IRQHandler + .thumb_set TIM8_CC_IRQHandler,Default_Handler + + .weak DMA1_Stream7_IRQHandler + .thumb_set DMA1_Stream7_IRQHandler,Default_Handler + + .weak FSMC_IRQHandler + .thumb_set FSMC_IRQHandler,Default_Handler + + .weak SDIO_IRQHandler + .thumb_set SDIO_IRQHandler,Default_Handler + + .weak TIM5_IRQHandler + .thumb_set TIM5_IRQHandler,Default_Handler + + .weak SPI3_IRQHandler + .thumb_set SPI3_IRQHandler,Default_Handler + + .weak UART4_IRQHandler + .thumb_set UART4_IRQHandler,Default_Handler + + .weak UART5_IRQHandler + .thumb_set UART5_IRQHandler,Default_Handler + + .weak TIM6_DAC_IRQHandler + .thumb_set TIM6_DAC_IRQHandler,Default_Handler + + .weak TIM7_IRQHandler + .thumb_set TIM7_IRQHandler,Default_Handler + + .weak DMA2_Stream0_IRQHandler + .thumb_set DMA2_Stream0_IRQHandler,Default_Handler + + .weak DMA2_Stream1_IRQHandler + .thumb_set DMA2_Stream1_IRQHandler,Default_Handler + + .weak DMA2_Stream2_IRQHandler + .thumb_set DMA2_Stream2_IRQHandler,Default_Handler + + .weak DMA2_Stream3_IRQHandler + .thumb_set DMA2_Stream3_IRQHandler,Default_Handler + + .weak DMA2_Stream4_IRQHandler + .thumb_set DMA2_Stream4_IRQHandler,Default_Handler + + .weak ETH_IRQHandler + .thumb_set ETH_IRQHandler,Default_Handler + + .weak ETH_WKUP_IRQHandler + .thumb_set ETH_WKUP_IRQHandler,Default_Handler + + .weak CAN2_TX_IRQHandler + .thumb_set CAN2_TX_IRQHandler,Default_Handler + + .weak CAN2_RX0_IRQHandler + .thumb_set CAN2_RX0_IRQHandler,Default_Handler + + .weak CAN2_RX1_IRQHandler + .thumb_set CAN2_RX1_IRQHandler,Default_Handler + + .weak CAN2_SCE_IRQHandler + .thumb_set CAN2_SCE_IRQHandler,Default_Handler + + .weak OTG_FS_IRQHandler + .thumb_set OTG_FS_IRQHandler,Default_Handler + + .weak DMA2_Stream5_IRQHandler + .thumb_set DMA2_Stream5_IRQHandler,Default_Handler + + .weak DMA2_Stream6_IRQHandler + .thumb_set DMA2_Stream6_IRQHandler,Default_Handler + + .weak DMA2_Stream7_IRQHandler + .thumb_set DMA2_Stream7_IRQHandler,Default_Handler + + .weak USART6_IRQHandler + .thumb_set USART6_IRQHandler,Default_Handler + + .weak I2C3_EV_IRQHandler + .thumb_set I2C3_EV_IRQHandler,Default_Handler + + .weak I2C3_ER_IRQHandler + .thumb_set I2C3_ER_IRQHandler,Default_Handler + + .weak OTG_HS_EP1_OUT_IRQHandler + .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler + + .weak OTG_HS_EP1_IN_IRQHandler + .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler + + .weak OTG_HS_WKUP_IRQHandler + .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler + + .weak OTG_HS_IRQHandler + .thumb_set OTG_HS_IRQHandler,Default_Handler + + .weak DCMI_IRQHandler + .thumb_set DCMI_IRQHandler,Default_Handler + + .weak HASH_RNG_IRQHandler + .thumb_set HASH_RNG_IRQHandler,Default_Handler + + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/controller/fw/src/stm32f4_isr.h b/controller/fw/src/stm32f4_isr.h new file mode 100644 index 0000000..bc16421 --- /dev/null +++ b/controller/fw/src/stm32f4_isr.h @@ -0,0 +1,98 @@ + +#ifndef __STM32F4_ISR_H__ +#define __STM32F4_ISR_H__ + +void Reset_Handler(void); +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); + +void WWDG_IRQHandler(void); +void PVD_IRQHandler(void); +void TAMP_STAMP_IRQHandler(void); +void RTC_WKUP_IRQHandler(void); +void FLASH_IRQHandler(void); +void RCC_IRQHandler(void); +void EXTI0_IRQHandler(void); +void EXTI1_IRQHandler(void); +void EXTI2_IRQHandler(void); +void EXTI3_IRQHandler(void); +void EXTI4_IRQHandler(void); +void DMA1_Stream0_IRQHandler(void); +void DMA1_Stream1_IRQHandler(void); +void DMA1_Stream2_IRQHandler(void); +void DMA1_Stream3_IRQHandler(void); +void DMA1_Stream4_IRQHandler(void); +void DMA1_Stream5_IRQHandler(void); +void DMA1_Stream6_IRQHandler(void); +void ADC_IRQHandler(void); +void CAN1_TX_IRQHandler(void); +void CAN1_RX0_IRQHandler(void); +void CAN1_RX1_IRQHandler(void); +void CAN1_SCE_IRQHandler(void); +void EXTI9_5_IRQHandler(void); +void TIM1_BRK_TIM9_IRQHandler(void); +void TIM1_UP_TIM10_IRQHandler(void); +void TIM1_TRG_COM_TIM11_IRQHandler(void); +void TIM1_CC_IRQHandler(void); +void TIM2_IRQHandler(void); +void TIM3_IRQHandler(void); +void TIM4_IRQHandler(void); +void I2C1_EV_IRQHandler(void); +void I2C1_ER_IRQHandler(void); +void I2C2_EV_IRQHandler(void); +void I2C2_ER_IRQHandler(void); +void SPI1_IRQHandler(void); +void SPI2_IRQHandler(void); +void USART1_IRQHandler(void); +void USART2_IRQHandler(void); +void USART3_IRQHandler(void); +void EXTI15_10_IRQHandler(void); +void RTC_Alarm_IRQHandler(void); +void OTG_FS_WKUP_IRQHandler(void); +void TIM8_BRK_TIM12_IRQHandler(void); +void TIM8_UP_TIM13_IRQHandler(void); +void TIM8_TRG_COM_TIM14_IRQHandler(void); +void TIM8_CC_IRQHandler(void); +void DMA1_Stream7_IRQHandler(void); +void FSMC_IRQHandler(void); +void SDIO_IRQHandler(void); +void TIM5_IRQHandler(void); +void SPI3_IRQHandler(void); +void UART4_IRQHandler(void); +void UART5_IRQHandler(void); +void TIM6_DAC_IRQHandler(void); +void TIM7_IRQHandler(void); +void DMA2_Stream0_IRQHandler(void); +void DMA2_Stream1_IRQHandler(void); +void DMA2_Stream2_IRQHandler(void); +void DMA2_Stream3_IRQHandler(void); +void DMA2_Stream4_IRQHandler(void); +void ETH_IRQHandler(void); +void ETH_WKUP_IRQHandler(void); +void CAN2_TX_IRQHandler(void); +void CAN2_RX0_IRQHandler(void); +void CAN2_RX1_IRQHandler(void); +void CAN2_SCE_IRQHandler(void); +void OTG_FS_IRQHandler(void); +void DMA2_Stream5_IRQHandler(void); +void DMA2_Stream6_IRQHandler(void); +void DMA2_Stream7_IRQHandler(void); +void USART6_IRQHandler(void); +void I2C3_EV_IRQHandler(void); +void I2C3_ER_IRQHandler(void); +void OTG_HS_EP1_OUT_IRQHandler(void); +void OTG_HS_EP1_IN_IRQHandler(void); +void OTG_HS_WKUP_IRQHandler(void); +void OTG_HS_IRQHandler(void); +void DCMI_IRQHandler(void); +void HASH_RNG_IRQHandler(void); +void FPU_IRQHandler(void); + +#endif /* __STM32F4_ISR_H__ */ diff --git a/controller/fw/src/system_stm32f4xx.c b/controller/fw/src/system_stm32f4xx.c new file mode 100644 index 0000000..9037604 --- /dev/null +++ b/controller/fw/src/system_stm32f4xx.c @@ -0,0 +1,743 @@ +/** + ****************************************************************************** + * @file system_stm32f4xx.c + * @author MCD Application Team + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * + * This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): 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. + * + * + ****************************************************************************** + * @attention + * + * <h2><center>© COPYRIGHT 2017 STMicroelectronics</center></h2> + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f4xx_system + * @{ + */ + +/** @addtogroup STM32F4xx_System_Private_Includes + * @{ + */ + + +#include "stm32f4xx.h" + +#if !defined (HSE_VALUE) + #define HSE_VALUE ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined (HSI_VALUE) + #define HSI_VALUE ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ + +/** + * @} + */ + +/** @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 as data memory */ +#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\ + || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx) +/* #define DATA_IN_ExtSRAM */ +#endif /* STM32F40xxx || STM32F41xxx || STM32F42xxx || STM32F43xxx || STM32F469xx || STM32F479xx ||\ + STM32F412Zx || STM32F412Vx */ + +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx) +/* #define DATA_IN_ExtSDRAM */ +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx ||\ + STM32F479xx */ + +/*!< 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. */ +/******************************************************************************/ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Variables + * @{ + */ + /* This variable is updated in three ways: + 1) by calling CMSIS function SystemCoreClockUpdate() + 2) by calling HAL API function HAL_RCC_GetHCLKFreq() + 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency + Note: If you use this function to configure the system clock; then there + is no need to call the 2 first functions listed above, since SystemCoreClock + variable is updated automatically. + */ +uint32_t SystemCoreClock = 16000000; +const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; +const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4}; +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes + * @{ + */ + +#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 FPU setting, vector table location and External memory + * configuration. + * @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 + +#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM) + SystemInit_ExtMemCtl(); +#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */ + + /* 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_hal_conf.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_hal_conf.h file (its value + * depends on the application requirements), 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; + + /* 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 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 (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); + } + + pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; + SystemCoreClock = pllvco/pllp; + break; + default: + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK frequency --------------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK frequency */ + SystemCoreClock >>= tmp; +} + +#if defined (DATA_IN_ExtSRAM) && defined (DATA_IN_ExtSDRAM) +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F469xx) || defined(STM32F479xx) +/** + * @brief Setup the external memory controller. + * Called in startup_stm32f4xx.s before jump to main. + * This function configures the external memories (SRAM/SDRAM) + * This SRAM/SDRAM will be used as program data memory (including heap and stack). + * @param None + * @retval None + */ +void SystemInit_ExtMemCtl(void) +{ + __IO uint32_t tmp = 0x00; + + register uint32_t tmpreg = 0, timeout = 0xFFFF; + register __IO uint32_t index; + + /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface clock */ + RCC->AHB1ENR |= 0x000001F8; + + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN); + + /* Connect PDx pins to FMC Alternate function */ + GPIOD->AFR[0] = 0x00CCC0CC; + GPIOD->AFR[1] = 0xCCCCCCCC; + /* Configure PDx pins in Alternate function mode */ + GPIOD->MODER = 0xAAAA0A8A; + /* Configure PDx pins speed to 100 MHz */ + GPIOD->OSPEEDR = 0xFFFF0FCF; + /* 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] = 0xC00CC0CC; + GPIOE->AFR[1] = 0xCCCCCCCC; + /* Configure PEx pins in Alternate function mode */ + GPIOE->MODER = 0xAAAA828A; + /* Configure PEx pins speed to 100 MHz */ + GPIOE->OSPEEDR = 0xFFFFC3CF; + /* 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; + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN); + + FMC_Bank5_6->SDCR[0] = 0x000019E4; + 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); + +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx) + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001011; + FMC_Bank1->BTCR[3] = 0x00000201; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ +#if defined(STM32F469xx) || defined(STM32F479xx) + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001091; + FMC_Bank1->BTCR[3] = 0x00110212; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F469xx || STM32F479xx */ + + (void)(tmp); +} +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */ +#elif defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM) +/** + * @brief Setup the external memory controller. + * Called in startup_stm32f4xx.s before jump to main. + * This function configures the external memories (SRAM/SDRAM) + * This SRAM/SDRAM will be used as program data memory (including heap and stack). + * @param None + * @retval None + */ +void SystemInit_ExtMemCtl(void) +{ + __IO uint32_t tmp = 0x00; +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx) +#if defined (DATA_IN_ExtSDRAM) + register uint32_t tmpreg = 0, timeout = 0xFFFF; + register __IO uint32_t index; + +#if defined(STM32F446xx) + /* Enable GPIOA, GPIOC, GPIOD, GPIOE, GPIOF, GPIOG interface + clock */ + RCC->AHB1ENR |= 0x0000007D; +#else + /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface + clock */ + RCC->AHB1ENR |= 0x000001F8; +#endif /* STM32F446xx */ + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN); + +#if defined(STM32F446xx) + /* Connect PAx pins to FMC Alternate function */ + GPIOA->AFR[0] |= 0xC0000000; + GPIOA->AFR[1] |= 0x00000000; + /* Configure PDx pins in Alternate function mode */ + GPIOA->MODER |= 0x00008000; + /* Configure PDx pins speed to 50 MHz */ + GPIOA->OSPEEDR |= 0x00008000; + /* Configure PDx pins Output type to push-pull */ + GPIOA->OTYPER |= 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOA->PUPDR |= 0x00000000; + + /* Connect PCx pins to FMC Alternate function */ + GPIOC->AFR[0] |= 0x00CC0000; + GPIOC->AFR[1] |= 0x00000000; + /* Configure PDx pins in Alternate function mode */ + GPIOC->MODER |= 0x00000A00; + /* Configure PDx pins speed to 50 MHz */ + GPIOC->OSPEEDR |= 0x00000A00; + /* Configure PDx pins Output type to push-pull */ + GPIOC->OTYPER |= 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOC->PUPDR |= 0x00000000; +#endif /* STM32F446xx */ + + /* 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; + +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F469xx) || defined(STM32F479xx) + /* 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; +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */ + +/*-- FMC Configuration -------------------------------------------------------*/ + /* Enable the FMC interface clock */ + RCC->AHB3ENR |= 0x00000001; + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN); + + /* Configure and enable SDRAM bank1 */ +#if defined(STM32F446xx) + FMC_Bank5_6->SDCR[0] = 0x00001954; +#else + FMC_Bank5_6->SDCR[0] = 0x000019E4; +#endif /* STM32F446xx */ + 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 */ +#if defined(STM32F446xx) + FMC_Bank5_6->SDCMR = 0x000000F3; +#else + FMC_Bank5_6->SDCMR = 0x00000073; +#endif /* STM32F446xx */ + timeout = 0xFFFF; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* MRD register program */ +#if defined(STM32F446xx) + FMC_Bank5_6->SDCMR = 0x00044014; +#else + FMC_Bank5_6->SDCMR = 0x00046014; +#endif /* STM32F446xx */ + timeout = 0xFFFF; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Set refresh count */ + tmpreg = FMC_Bank5_6->SDRTR; +#if defined(STM32F446xx) + FMC_Bank5_6->SDRTR = (tmpreg | (0x0000050C<<1)); +#else + FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1)); +#endif /* STM32F446xx */ + + /* Disable write protection */ + tmpreg = FMC_Bank5_6->SDCR[0]; + FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF); +#endif /* DATA_IN_ExtSDRAM */ +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx || STM32F479xx */ + +#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\ + || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx) + +#if defined(DATA_IN_ExtSRAM) +/*-- GPIOs Configuration -----------------------------------------------------*/ + /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */ + RCC->AHB1ENR |= 0x00000078; + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIODEN); + + /* Connect PDx pins to FMC Alternate function */ + GPIOD->AFR[0] = 0x00CCC0CC; + GPIOD->AFR[1] = 0xCCCCCCCC; + /* Configure PDx pins in Alternate function mode */ + GPIOD->MODER = 0xAAAA0A8A; + /* Configure PDx pins speed to 100 MHz */ + GPIOD->OSPEEDR = 0xFFFF0FCF; + /* 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] = 0xC00CC0CC; + GPIOE->AFR[1] = 0xCCCCCCCC; + /* Configure PEx pins in Alternate function mode */ + GPIOE->MODER = 0xAAAA828A; + /* Configure PEx pins speed to 100 MHz */ + GPIOE->OSPEEDR = 0xFFFFC3CF; + /* 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 = 0x00085AAA; + /* Configure PGx pins speed to 100 MHz */ + GPIOG->OSPEEDR = 0x000CAFFF; + /* Configure PGx pins Output type to push-pull */ + GPIOG->OTYPER = 0x00000000; + /* No pull-up, pull-down for PGx pins */ + GPIOG->PUPDR = 0x00000000; + +/*-- FMC/FSMC Configuration --------------------------------------------------*/ + /* Enable the FMC/FSMC interface clock */ + RCC->AHB3ENR |= 0x00000001; + +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx) + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN); + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001011; + FMC_Bank1->BTCR[3] = 0x00000201; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ +#if defined(STM32F469xx) || defined(STM32F479xx) + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN); + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001091; + FMC_Bank1->BTCR[3] = 0x00110212; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F469xx || STM32F479xx */ +#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx)|| defined(STM32F417xx)\ + || defined(STM32F412Zx) || defined(STM32F412Vx) + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FSMCEN); + /* Configure and enable Bank1_SRAM2 */ + FSMC_Bank1->BTCR[2] = 0x00001011; + FSMC_Bank1->BTCR[3] = 0x00000201; + FSMC_Bank1E->BWTR[2] = 0x0FFFFFFF; +#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F412Zx || STM32F412Vx */ + +#endif /* DATA_IN_ExtSRAM */ +#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F427xx || STM32F437xx ||\ + STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx || STM32F412Zx || STM32F412Vx */ + (void)(tmp); +} +#endif /* DATA_IN_ExtSRAM && DATA_IN_ExtSDRAM */ +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ |