Repo re-org
This commit is contained in:
jaseg
parent
312fee491c
commit
50998fcfb9
270 changed files with 9 additions and 9 deletions
100
reset-controller/fw/src/adc.c
Normal file
100
reset-controller/fw/src/adc.c
Normal file
|
|
@ -0,0 +1,100 @@
|
|||
|
||||
#include <assert.h>
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "adc.h"
|
||||
#include "sr_global.h"
|
||||
|
||||
static unsigned int adc_overruns = 0;
|
||||
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 adc_stream = DMA2_Stream0;
|
||||
static const int dma_adc_channel = 0;
|
||||
static const int adc_channel = 10;
|
||||
|
||||
/* 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_AHB1ENR_GPIOCEN;
|
||||
RCC->APB2ENR |= RCC_APB2ENR_ADC1EN | RCC_APB2ENR_TIM1EN;
|
||||
|
||||
/* PC0 -> ADC1.ch10 */
|
||||
GPIOC->MODER &= ~GPIO_MODER_MODER0_Msk;
|
||||
GPIOC->MODER |= (3<<GPIO_MODER_MODER0_Pos);
|
||||
|
||||
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 = (uint32_t) &(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 | (2<<DMA_SxCR_PL_Pos)
|
||||
| DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_DMEIE;
|
||||
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 = (1<<ADC_CR2_EXTEN_Pos) | (0<<ADC_CR2_EXTSEL_Pos) | ADC_CR2_DMA | ADC_CR2_ADON | ADC_CR2_DDS;
|
||||
ADC1->SQR3 = (adc_channel<<ADC_SQR3_SQ1_Pos);
|
||||
ADC1->SQR1 = (0<<ADC_SQR1_L_Pos);
|
||||
ADC1->SMPR2 = (7<<ADC_SMPR2_SMP0_Pos);
|
||||
|
||||
TIM1->CR2 = (2<<TIM_CR2_MMS_Pos); /* Enable update event on TRGO to provide a 1ms reference to rest of system */
|
||||
TIM1->CCMR1 = (6<<TIM_CCMR1_OC1M_Pos) | (0<<TIM_CCMR1_CC1S_Pos);
|
||||
TIM1->CCER = TIM_CCER_CC1E;
|
||||
assert(apb2_timer_speed%1000000 == 0);
|
||||
TIM1->PSC = 1000-1;
|
||||
TIM1->ARR = (apb2_timer_speed/1000000)-1; /* 1ms period */
|
||||
TIM1->CCR1 = 1;
|
||||
TIM1->BDTR = TIM_BDTR_MOE;
|
||||
|
||||
TIM1->CR1 = TIM_CR1_CEN;
|
||||
TIM1->EGR = TIM_EGR_UG;
|
||||
}
|
||||
|
||||
void DMA2_Stream0_IRQHandler(void) {
|
||||
uint8_t isr = (DMA2->LISR >> DMA_LISR_FEIF0_Pos) & 0x3f;
|
||||
GPIOA->BSRR = 1<<11;
|
||||
|
||||
if (isr & DMA_LISR_TCIF0) { /* Transfer complete */
|
||||
/* Check we're done processing the old buffer */
|
||||
if (adc_fft_buf_ready_idx != -1) { /* FIXME DEBUG */
|
||||
GPIOA->BSRR = 1<<11<<16;
|
||||
/* clear all flags */
|
||||
adc_dma->LIFCR = isr<<DMA_LISR_FEIF0_Pos;
|
||||
adc_overruns++;
|
||||
return;
|
||||
panic();
|
||||
}
|
||||
|
||||
/* Kickoff FFT */
|
||||
int ct = !!(adc_stream->CR & DMA_SxCR_CT);
|
||||
adc_fft_buf_ready_idx = !ct;
|
||||
}
|
||||
|
||||
if (isr & DMA_LISR_DMEIF0) /* Direct mode error */
|
||||
panic();
|
||||
|
||||
if (isr & DMA_LISR_TEIF0) /* Transfer error */
|
||||
panic();
|
||||
|
||||
GPIOA->BSRR = 1<<11<<16;
|
||||
/* clear all flags */
|
||||
adc_dma->LIFCR = isr<<DMA_LISR_FEIF0_Pos;
|
||||
}
|
||||
10
reset-controller/fw/src/adc.h
Normal file
10
reset-controller/fw/src/adc.h
Normal file
|
|
@ -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__ */
|
||||
33
reset-controller/fw/src/con_usart.c
Normal file
33
reset-controller/fw/src/con_usart.c
Normal file
|
|
@ -0,0 +1,33 @@
|
|||
|
||||
#include <stm32f4_isr.h>
|
||||
#include "con_usart.h"
|
||||
|
||||
volatile struct usart_desc con_usart = {
|
||||
.le_usart = USART1,
|
||||
.le_usart_irqn = USART1_IRQn,
|
||||
.tx_dmas = DMA2_Stream7,
|
||||
.tx_dma_sn = 7,
|
||||
.tx_dma_ch = 4,
|
||||
.tx_dma = DMA2,
|
||||
.tx_dma_irqn = DMA2_Stream7_IRQn,
|
||||
};
|
||||
|
||||
void con_usart_init() {
|
||||
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_DMA2EN;
|
||||
RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
|
||||
|
||||
/* GPIO config: A9 (TX), A10 (RX) */
|
||||
GPIOA->MODER &= ~GPIO_MODER_MODER9_Msk & ~GPIO_MODER_MODER10_Msk;
|
||||
GPIOA->MODER |= (2<<GPIO_MODER_MODER9_Pos) | (2<<GPIO_MODER_MODER10_Pos);
|
||||
GPIOA->OSPEEDR &= ~GPIO_OSPEEDR_OSPEED9_Msk & ~GPIO_OSPEEDR_OSPEED10_Msk;
|
||||
GPIOA->OSPEEDR |= (2<<GPIO_OSPEEDR_OSPEED9_Pos) | (2<<GPIO_OSPEEDR_OSPEED10_Pos);
|
||||
GPIOA->AFR[1] &= ~GPIO_AFRH_AFSEL9_Msk & ~GPIO_AFRH_AFSEL10_Msk;
|
||||
GPIOA->AFR[1] |= (7<<GPIO_AFRH_AFSEL9_Pos) | (7<<GPIO_AFRH_AFSEL10_Pos);
|
||||
|
||||
usart_dma_init(&con_usart, CON_USART_BAUDRATE);
|
||||
}
|
||||
|
||||
void DMA2_Stream7_IRQHandler(void) {
|
||||
usart_dma_stream_irq(&con_usart);
|
||||
}
|
||||
|
||||
17
reset-controller/fw/src/con_usart.h
Normal file
17
reset-controller/fw/src/con_usart.h
Normal file
|
|
@ -0,0 +1,17 @@
|
|||
#ifndef __CON_USART_H__
|
||||
#define __CON_USART_H__
|
||||
|
||||
#include "serial.h"
|
||||
|
||||
extern volatile struct usart_desc con_usart;
|
||||
|
||||
#define con_printf(...) usart_printf(&con_usart, __VA_ARGS__)
|
||||
#define con_printf_blocking(...) usart_printf_blocking(&con_usart, __VA_ARGS__)
|
||||
|
||||
#ifndef CON_USART_BAUDRATE
|
||||
#define CON_USART_BAUDRATE 500000
|
||||
#endif
|
||||
|
||||
void con_usart_init(void);
|
||||
|
||||
#endif /* __CON_USART_H__ */
|
||||
80
reset-controller/fw/src/crypto.c
Normal file
80
reset-controller/fw/src/crypto.c
Normal file
|
|
@ -0,0 +1,80 @@
|
|||
|
||||
#include <assert.h>
|
||||
#include <unistd.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <sodium.h>
|
||||
|
||||
#include "crypto.h"
|
||||
#include "simulation.h"
|
||||
|
||||
|
||||
void debug_hexdump(const char *name, const uint8_t *buf, size_t len);
|
||||
int verify_trigger_dom(const uint8_t inkey[PRESIG_MSG_LEN],
|
||||
const char *domain_string, const uint8_t refkey[PRESIG_MSG_LEN]);
|
||||
|
||||
|
||||
void debug_hexdump(const char *name, const uint8_t *buf, size_t len) {
|
||||
DEBUG_PRINTN("%20s: ", name);
|
||||
for (size_t i=0; i<len;) {
|
||||
for (size_t j=0; j<8 && i<len; i++, j++)
|
||||
DEBUG_PRINTN("%02x ", buf[i]);
|
||||
DEBUG_PRINTN(" ");
|
||||
}
|
||||
DEBUG_PRINTN("\n");
|
||||
}
|
||||
|
||||
/* Returns trigger sig height for correct trigger */
|
||||
int verify_trigger_dom(const uint8_t inkey[PRESIG_MSG_LEN],
|
||||
const char *domain_string, const uint8_t refkey[PRESIG_MSG_LEN]) {
|
||||
uint8_t key[crypto_auth_hmacsha512_KEYBYTES];
|
||||
uint8_t key_out[crypto_auth_hmacsha512_BYTES];
|
||||
|
||||
static_assert(PRESIG_MSG_LEN <= crypto_auth_hmacsha512_KEYBYTES);
|
||||
memcpy(key, inkey, PRESIG_MSG_LEN);
|
||||
memset(key + PRESIG_MSG_LEN, 0, sizeof(key) - PRESIG_MSG_LEN);
|
||||
DEBUG_PRINT("ds \"%s\"", domain_string);
|
||||
debug_hexdump("ref", refkey, PRESIG_MSG_LEN);
|
||||
|
||||
for (int i=0; i<presig_height; i++) {
|
||||
DEBUG_PRINT("Verifying height rel %d abs %d", i, presig_height-i);
|
||||
debug_hexdump("key", key, sizeof(key));
|
||||
(void)crypto_auth_hmacsha512(key_out, (uint8_t *)domain_string, strlen(domain_string), key);
|
||||
debug_hexdump("out", key_out, sizeof(key_out));
|
||||
memcpy(key, key_out, PRESIG_MSG_LEN);
|
||||
memset(key + PRESIG_MSG_LEN, 0, sizeof(key) - PRESIG_MSG_LEN);
|
||||
|
||||
if (!memcmp(key, refkey, PRESIG_MSG_LEN))
|
||||
return presig_height-i;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int verify_trigger(const uint8_t inkey[PRESIG_MSG_LEN], int *height_out, int *domain_out) {
|
||||
int res;
|
||||
for (int i=0; i<_TRIGGER_DOMAIN_COUNT; i++) {
|
||||
DEBUG_PRINT("Verifying domain %d", i);
|
||||
if ((res = verify_trigger_dom(inkey, presig_domain_strings[i], presig_keys[i]))) {
|
||||
DEBUG_PRINT("Match!");
|
||||
if (height_out)
|
||||
*height_out = res - 1;
|
||||
if (domain_out)
|
||||
*domain_out = i;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int oob_message_received(uint8_t msg[static OOB_TRIGGER_LEN]) {
|
||||
int height, domain;
|
||||
if (verify_trigger(msg, &height, &domain)) {
|
||||
oob_trigger_activated(domain, height);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
30
reset-controller/fw/src/crypto.h
Normal file
30
reset-controller/fw/src/crypto.h
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
#ifndef __CRYPTO_H__
|
||||
#define __CRYPTO_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* Presig message length: 15 byte = 120 bit ^= 20 * 6-bit symbols of 5-bit bipolar DSSS */
|
||||
#define PRESIG_MSG_LEN 15
|
||||
#define OOB_TRIGGER_LEN PRESIG_MSG_LEN
|
||||
|
||||
enum trigger_domain {
|
||||
TRIGGER_DOMAIN_ALL,
|
||||
TRIGGER_DOMAIN_VENDOR,
|
||||
TRIGGER_DOMAIN_SERIES,
|
||||
TRIGGER_DOMAIN_COUNTRY,
|
||||
TRIGGER_DOMAIN_REGION,
|
||||
_TRIGGER_DOMAIN_COUNT
|
||||
};
|
||||
|
||||
extern const char *presig_domain_strings[_TRIGGER_DOMAIN_COUNT];
|
||||
extern uint8_t presig_keys[_TRIGGER_DOMAIN_COUNT][PRESIG_MSG_LEN];
|
||||
extern int presig_height;
|
||||
extern uint8_t presig_bundle_id[16];
|
||||
extern uint64_t bundle_timestamp;
|
||||
|
||||
extern void oob_trigger_activated(enum trigger_domain domain, int height);
|
||||
|
||||
int oob_message_received(uint8_t msg[static OOB_TRIGGER_LEN]);
|
||||
int verify_trigger(const uint8_t inkey[PRESIG_MSG_LEN], int *height_out, int *domain_out);
|
||||
|
||||
#endif /* __CRYPTO_H__ */
|
||||
46
reset-controller/fw/src/dma_util.c
Normal file
46
reset-controller/fw/src/dma_util.c
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
|
||||
#include <assert.h>
|
||||
|
||||
#include "dma_util.h"
|
||||
|
||||
uint8_t dma_get_isr_and_clear(DMA_TypeDef *dma, int ch) {
|
||||
uint8_t isr_val;
|
||||
switch(ch) {
|
||||
case 0:
|
||||
isr_val = dma->LISR & 0x3f;
|
||||
dma->LIFCR = isr_val;
|
||||
return isr_val;
|
||||
case 1:
|
||||
isr_val = dma->LISR>>6 & 0x3f;
|
||||
dma->LIFCR = isr_val<<6;
|
||||
return isr_val;
|
||||
case 2:
|
||||
isr_val = dma->LISR>>16 & 0x3f;
|
||||
dma->LIFCR = isr_val<<16;
|
||||
return isr_val;
|
||||
case 3:
|
||||
isr_val = dma->LISR>>6>>16 & 0x3f;
|
||||
dma->LIFCR = isr_val<<6<<16;
|
||||
return isr_val;
|
||||
case 4:
|
||||
isr_val = dma->HISR & 0x3f;
|
||||
dma->HIFCR = isr_val;
|
||||
return isr_val;
|
||||
case 5:
|
||||
isr_val = dma->HISR>>6 & 0x3f;
|
||||
dma->HIFCR = isr_val<<6;
|
||||
return isr_val;
|
||||
case 6:
|
||||
isr_val = dma->HISR>>16 & 0x3f;
|
||||
dma->HIFCR = isr_val<<16;
|
||||
return isr_val;
|
||||
case 7:
|
||||
isr_val = dma->HISR>>6>>16 & 0x3f;
|
||||
dma->HIFCR = isr_val<<6<<16;
|
||||
return isr_val;
|
||||
default:
|
||||
assert(0);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
10
reset-controller/fw/src/dma_util.h
Normal file
10
reset-controller/fw/src/dma_util.h
Normal file
|
|
@ -0,0 +1,10 @@
|
|||
#ifndef __DMA_UTIL_H__
|
||||
#define __DMA_UTIL_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "sr_global.h"
|
||||
|
||||
uint8_t dma_get_isr_and_clear(DMA_TypeDef *dma, int ch);
|
||||
|
||||
#endif /* __DMA_UTIL_H__ */
|
||||
369
reset-controller/fw/src/dsss_demod.c
Normal file
369
reset-controller/fw/src/dsss_demod.c
Normal file
|
|
@ -0,0 +1,369 @@
|
|||
|
||||
#include <unistd.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include <arm_math.h>
|
||||
|
||||
#include "freq_meas.h"
|
||||
#include "sr_global.h"
|
||||
#include "dsss_demod.h"
|
||||
#include "simulation.h"
|
||||
|
||||
#include "generated/dsss_gold_code.h"
|
||||
// #include "generated/dsss_butter_filter.h"
|
||||
|
||||
/* Generated CWT wavelet LUT */
|
||||
extern const float * const dsss_cwt_wavelet_table;
|
||||
|
||||
//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);
|
||||
//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);
|
||||
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 symbol_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) {
|
||||
if (!debug)
|
||||
return;
|
||||
|
||||
if (index) {
|
||||
DEBUG_PRINTN(" %16s [", "");
|
||||
for (size_t i=0; i<len; i++)
|
||||
DEBUG_PRINTN("%8zu ", i);
|
||||
DEBUG_PRINTN("]\n");
|
||||
}
|
||||
|
||||
DEBUG_PRINTN(" %16s: [", name);
|
||||
for (size_t i=0; i<len; i++)
|
||||
DEBUG_PRINTN("%8.5f, ", data[i*stride]);
|
||||
DEBUG_PRINTN("]\n");
|
||||
}
|
||||
#else
|
||||
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) {
|
||||
memset(st, 0, sizeof(*st));
|
||||
matcher_init(st->matcher_cache);
|
||||
}
|
||||
|
||||
void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts) {
|
||||
//const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH;
|
||||
bool log = false;
|
||||
bool log_groups = false;
|
||||
|
||||
st->signal[st->signal_wpos] = new_value;
|
||||
st->signal_wpos = (st->signal_wpos + 1) % ARRAY_LENGTH(st->signal);
|
||||
|
||||
/* use new, incremented wpos for gold_correlate_step as first element of old data in ring buffer */
|
||||
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
|
||||
st->correlation[i][st->correlation_wpos] = gold_correlate_step(i, st->signal, st->signal_wpos, false);
|
||||
|
||||
st->correlation_wpos = (st->correlation_wpos + 1) % ARRAY_LENGTH(st->correlation[0]);
|
||||
|
||||
float cwt[DSSS_GOLD_CODE_COUNT];
|
||||
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
|
||||
cwt[i] = cwt_convolve_step(st->correlation[i], st->correlation_wpos);
|
||||
|
||||
float avg = 0.0f;
|
||||
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
|
||||
avg += fabsf(cwt[i]);
|
||||
avg /= (float)DSSS_GOLD_CODE_COUNT;
|
||||
if (log) DEBUG_PRINTN("%6zu: %f ", ts, avg);
|
||||
/* FIXME fix this filter */
|
||||
//avg = run_iir(avg, ARRAY_LENGTH(cwt_filter_bq), cwt_filter_bq, st->cwt_filter.st);
|
||||
|
||||
float max_val = st->group.max;
|
||||
int max_ch = st->group.max_ch;
|
||||
int max_ts = st->group.max_ts;
|
||||
bool found = false;
|
||||
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) {
|
||||
float val = cwt[i] / avg;
|
||||
if (log) DEBUG_PRINTN("%f ", cwt[i]);
|
||||
if (log) DEBUG_PRINTN("%f ", val);
|
||||
|
||||
if (fabsf(val) > fabsf(max_val)) {
|
||||
max_val = val;
|
||||
max_ch = i;
|
||||
max_ts = ts;
|
||||
}
|
||||
|
||||
if (fabsf(val) > DSSS_THRESHOLD_FACTOR)
|
||||
found = true;
|
||||
}
|
||||
if (log) DEBUG_PRINTN("%f %d ", max_val, found);
|
||||
if (log) DEBUG_PRINTN("\n");
|
||||
|
||||
matcher_tick(st->matcher_cache, ts, max_ch, max_val);
|
||||
|
||||
if (found) {
|
||||
/* Continue ongoing group */
|
||||
st->group.len++;
|
||||
st->group.max = max_val;
|
||||
st->group.max_ch = max_ch;
|
||||
st->group.max_ts = max_ts;
|
||||
return;
|
||||
}
|
||||
|
||||
if (st->group.len == 0)
|
||||
/* We're between groups */
|
||||
return;
|
||||
|
||||
if (log_groups) DEBUG_PRINTN("GROUP: %zu %d %f\n", st->group.max_ts, st->group.max_ch, st->group.max);
|
||||
/* A group ended. Process result. */
|
||||
group_received(st);
|
||||
|
||||
/* reset grouping state */
|
||||
st->group.len = 0;
|
||||
st->group.max_ts = 0;
|
||||
st->group.max_ch = 0;
|
||||
st->group.max = 0.0f;
|
||||
}
|
||||
|
||||
/* Map a sequence match to a data symbol. This maps the sequence's index number to the 2nd to n+2nd bit of the result,
|
||||
* and maps the polarity of detection to the LSb. 5-bit example:
|
||||
*
|
||||
* [0, S, S, S, S, S, S, P] ; S ^= symbol index (0 - 2^n+1 so we need just about n+1 bit), P ^= symbol polarity
|
||||
*
|
||||
* Symbol polarity is preserved from transmitter to receiver. The symbol index is n+1 bit instead of n bit since we have
|
||||
* 2^n+1 symbols to express, one too many for an n-bit index.
|
||||
*/
|
||||
symbol_t decode_peak(int peak_ch, float peak_ampl) {
|
||||
return (peak_ch<<1) | (peak_ampl > 0);
|
||||
}
|
||||
|
||||
void matcher_init(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE]) {
|
||||
for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++)
|
||||
states[i].last_phase = -1; /* mark as inactive */
|
||||
}
|
||||
|
||||
/* TODO make these constants configurable from Makefile */
|
||||
const int group_phase_tolerance = (int)(DSSS_CORRELATION_LENGTH * 0.10);
|
||||
|
||||
void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], uint64_t ts, int peak_ch, float peak_ampl) {
|
||||
/* TODO make these constants configurable from Makefile */
|
||||
const float skip_sampling_depreciation = 0.2f; /* 0.0 -> no depreciation, 1.0 -> complete disregard */
|
||||
const float score_depreciation = 0.1f; /* 0.0 -> no depreciation, 1.0 -> complete disregard */
|
||||
const int current_phase = ts % DSSS_CORRELATION_LENGTH;
|
||||
const int max_skips = TRANSMISSION_SYMBOLS/4*3;
|
||||
bool debug = false;
|
||||
|
||||
bool header_printed = false;
|
||||
for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) {
|
||||
if (states[i].last_phase == -1)
|
||||
continue; /* Inactive entry */
|
||||
|
||||
if (current_phase == states[i].last_phase) {
|
||||
/* Skip sampling */
|
||||
float score = fabsf(peak_ampl) * (1.0f - skip_sampling_depreciation);
|
||||
if (score > states[i].candidate_score) {
|
||||
if (debug && !header_printed) {
|
||||
header_printed = true;
|
||||
DEBUG_PRINTN("windows %zu\n", ts);
|
||||
}
|
||||
if (debug) DEBUG_PRINTN(" skip %zd old=%f new=%f\n", i, states[i].candidate_score, score);
|
||||
/* We win, update candidate */
|
||||
assert(i < DSSS_MATCHER_CACHE_SIZE);
|
||||
states[i].candidate_score = score;
|
||||
states[i].candidate_phase = current_phase;
|
||||
states[i].candidate_data = decode_peak(peak_ch, peak_ampl);
|
||||
states[i].candidate_skips = 1;
|
||||
}
|
||||
}
|
||||
|
||||
/* Note of caution on group_phase_tolerance: Group detection has some latency since a group is only considered
|
||||
* "detected" after signal levels have fallen back below the detection threshold. This means we only get to
|
||||
* process a group a couple ticks after its peak. We have to make sure the window is still open at this point.
|
||||
* This means we have to match against group_phase_tolerance should a little bit loosely.
|
||||
*/
|
||||
int phase_delta = current_phase - states[i].last_phase;
|
||||
if (phase_delta < 0)
|
||||
phase_delta += DSSS_CORRELATION_LENGTH;
|
||||
if (phase_delta == group_phase_tolerance + DSSS_DECIMATION) {
|
||||
if (debug && !header_printed) {
|
||||
header_printed = true;
|
||||
DEBUG_PRINTN("windows %zu\n", ts);
|
||||
}
|
||||
if (debug) DEBUG_PRINTN(" %zd ", i);
|
||||
/* Process window results */
|
||||
assert(i < DSSS_MATCHER_CACHE_SIZE);
|
||||
assert(0 <= states[i].data_pos && states[i].data_pos < TRANSMISSION_SYMBOLS);
|
||||
states[i].data[ states[i].data_pos ] = states[i].candidate_data;
|
||||
states[i].data_pos = states[i].data_pos + 1;
|
||||
states[i].last_score = score_depreciation * states[i].last_score +
|
||||
(1.0f - score_depreciation) * states[i].candidate_score;
|
||||
if (debug) DEBUG_PRINTN("commit pos=%d val=%d score=%f ", states[i].data_pos, states[i].candidate_data, states[i].last_score);
|
||||
states[i].candidate_score = 0.0f;
|
||||
states[i].last_skips += states[i].candidate_skips;
|
||||
|
||||
if (states[i].last_skips > max_skips) {
|
||||
if (debug) DEBUG_PRINTN("expire ");
|
||||
states[i].last_phase = -1; /* invalidate entry */
|
||||
|
||||
} else if (states[i].data_pos == TRANSMISSION_SYMBOLS) {
|
||||
if (debug) DEBUG_PRINTN("match ");
|
||||
/* Frame received completely */
|
||||
handle_dsss_received(states[i].data);
|
||||
states[i].last_phase = -1; /* invalidate entry */
|
||||
}
|
||||
if (debug) DEBUG_PRINTN("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static float gaussian(float a, float b, float c, float x) {
|
||||
float n = x-b;
|
||||
return a*expf(-n*n / (2.0f* c*c));
|
||||
}
|
||||
|
||||
|
||||
static float score_group(const struct group *g, int phase_delta) {
|
||||
/* TODO make these constants configurable from Makefile */
|
||||
const float distance_func_phase_tolerance = 10.0f;
|
||||
return fabsf(g->max) * gaussian(1.0f, 0.0f, distance_func_phase_tolerance, phase_delta);
|
||||
}
|
||||
|
||||
void group_received(struct dsss_demod_state *st) {
|
||||
bool debug = false;
|
||||
const int group_phase = st->group.max_ts % DSSS_CORRELATION_LENGTH;
|
||||
/* This is the score of a decoding starting at this group (with no context) */
|
||||
float base_score = score_group(&st->group, 0);
|
||||
|
||||
float min_score = INFINITY;
|
||||
ssize_t min_idx = -1;
|
||||
ssize_t empty_idx = -1;
|
||||
for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) {
|
||||
|
||||
/* Search for empty entries */
|
||||
if (st->matcher_cache[i].last_phase == -1) {
|
||||
empty_idx = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Search for entries with matching phase */
|
||||
/* This is the score of this group given the cached decoding at [i] */
|
||||
int phase_delta = st->matcher_cache[i].last_phase - group_phase;
|
||||
if (abs(phase_delta) <= group_phase_tolerance) {
|
||||
|
||||
float group_score = score_group(&st->group, phase_delta);
|
||||
if (st->matcher_cache[i].candidate_score < group_score) {
|
||||
assert(i < DSSS_MATCHER_CACHE_SIZE);
|
||||
if (debug) DEBUG_PRINTN(" appending %zu %d score=%f pd=%d\n", i, decode_peak(st->group.max_ch, st->group.max), group_score, phase_delta);
|
||||
/* Append to entry */
|
||||
st->matcher_cache[i].candidate_score = group_score;
|
||||
st->matcher_cache[i].candidate_phase = group_phase;
|
||||
st->matcher_cache[i].candidate_data = decode_peak(st->group.max_ch, st->group.max);
|
||||
st->matcher_cache[i].candidate_skips = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* Search for weakest entry */
|
||||
/* TODO figure out this fitness function */
|
||||
float score = st->matcher_cache[i].last_score * (1.5f + 0.1 * st->matcher_cache[i].data_pos);
|
||||
if (debug) DEBUG_PRINTN(" score %zd %f %f %d", i, score, st->matcher_cache[i].last_score, st->matcher_cache[i].data_pos);
|
||||
if (score < min_score) {
|
||||
min_idx = i;
|
||||
min_score = score;
|
||||
}
|
||||
}
|
||||
|
||||
/* If we found empty entries, replace one by a new decoding starting at this group */
|
||||
if (empty_idx >= 0) {
|
||||
if (debug) DEBUG_PRINTN(" empty %zd %d\n", empty_idx, decode_peak(st->group.max_ch, st->group.max));
|
||||
assert(0 <= empty_idx && empty_idx < DSSS_MATCHER_CACHE_SIZE);
|
||||
st->matcher_cache[empty_idx].last_phase = group_phase;
|
||||
st->matcher_cache[empty_idx].candidate_score = base_score;
|
||||
st->matcher_cache[empty_idx].last_score = base_score;
|
||||
st->matcher_cache[empty_idx].candidate_phase = group_phase;
|
||||
st->matcher_cache[empty_idx].candidate_data = decode_peak(st->group.max_ch, st->group.max);
|
||||
st->matcher_cache[empty_idx].data_pos = 0;
|
||||
st->matcher_cache[empty_idx].candidate_skips = 0;
|
||||
st->matcher_cache[empty_idx].last_skips = 0;
|
||||
|
||||
/* If the weakest decoding in cache is weaker than a new decoding starting here, replace it */
|
||||
} else if (min_score < base_score && min_idx >= 0) {
|
||||
if (debug) DEBUG_PRINTN(" min %zd %d\n", min_idx, decode_peak(st->group.max_ch, st->group.max));
|
||||
assert(0 <= min_idx && min_idx < DSSS_MATCHER_CACHE_SIZE);
|
||||
st->matcher_cache[min_idx].last_phase = group_phase;
|
||||
st->matcher_cache[min_idx].candidate_score = base_score;
|
||||
st->matcher_cache[min_idx].last_score = base_score;
|
||||
st->matcher_cache[min_idx].candidate_phase = group_phase;
|
||||
st->matcher_cache[min_idx].candidate_data = decode_peak(st->group.max_ch, st->group.max);
|
||||
st->matcher_cache[min_idx].data_pos = 0;
|
||||
st->matcher_cache[min_idx].candidate_skips = 0;
|
||||
st->matcher_cache[min_idx].last_skips = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#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++)
|
||||
intermediate = run_biquad(intermediate, &q[i], &st[i]);
|
||||
return intermediate;
|
||||
}
|
||||
|
||||
float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st) {
|
||||
/* direct form 2, see https://en.wikipedia.org/wiki/Digital_biquad_filter */
|
||||
float intermediate = x + st->reg[0] * -q->a[0] + st->reg[1] * -q->a[1];
|
||||
float out = intermediate * q->b[0] + st->reg[0] * q->b[1] + st->reg[1] * q->b[2];
|
||||
st->reg[1] = st->reg[0];
|
||||
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;
|
||||
for (ssize_t j=0; j<DSSS_WAVELET_LUT_SIZE; j++) {
|
||||
/* Our wavelet is symmetric so convolution and correlation are identical. Use correlation here for ease of
|
||||
* implementation */
|
||||
sum += v[(offx + j) % DSSS_WAVELET_LUT_SIZE] * dsss_cwt_wavelet_table[j];
|
||||
//DEBUG_PRINT(" j=%d v=%f w=%f", j, v[(offx + j) % DSSS_WAVELET_LUT_SIZE], dsss_cwt_wavelet_table[j]);
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
/* Compute last element of correlation for input [a] and hard-coded gold sequences.
|
||||
*
|
||||
* This is intened to be used once for each new incoming sample in [a]. It expects [a] to be of length
|
||||
* [dsss_correlation_length] and produces the one sample where both the reference sequence and the input fully overlap.
|
||||
* This is equivalent to "valid" mode in numpy's terminology[0].
|
||||
*
|
||||
* [0] https://docs.scipy.org/doc/numpy/reference/generated/numpy.correlate.html
|
||||
*/
|
||||
float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug) {
|
||||
|
||||
float acc_outer = 0.0f;
|
||||
uint8_t table_byte = 0;
|
||||
if (debug) DEBUG_PRINTN("Correlate n=%zd: ", ncode);
|
||||
for (size_t i=0; i<DSSS_GOLD_CODE_LENGTH; i++) {
|
||||
|
||||
if ((i&7) == 0) {
|
||||
table_byte = dsss_gold_code_table[ncode][i>>3]; /* Fetch sequence table item */
|
||||
if (debug) DEBUG_PRINTN("|");
|
||||
}
|
||||
int bv = table_byte & (0x80>>(i&7)); /* Extract bit */
|
||||
bv = !!bv*2 - 1; /* Map 0, 1 -> -1, 1 */
|
||||
if (debug) DEBUG_PRINTN("%s%d\033[0m", bv == 1 ? "\033[92m" : "\033[91m", (bv+1)/2);
|
||||
|
||||
float acc_inner = 0.0f;
|
||||
for (size_t j=0; j<DSSS_DECIMATION; j++)
|
||||
acc_inner += a[(offx + i*DSSS_DECIMATION + j) % DSSS_CORRELATION_LENGTH]; /* Multiply item */
|
||||
//if (debug) DEBUG_PRINTN("%.2f ", acc_inner);
|
||||
acc_outer += acc_inner * bv;
|
||||
}
|
||||
if (debug) DEBUG_PRINTN("\n");
|
||||
return acc_outer / DSSS_CORRELATION_LENGTH;
|
||||
}
|
||||
75
reset-controller/fw/src/dsss_demod.h
Normal file
75
reset-controller/fw/src/dsss_demod.h
Normal file
|
|
@ -0,0 +1,75 @@
|
|||
#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)
|
||||
|
||||
/* FIXME: move to makefile */
|
||||
#define DSSS_MATCHER_CACHE_SIZE 8
|
||||
|
||||
#if DSSS_GOLD_CODE_NBITS < 8
|
||||
typedef uint8_t symbol_t;
|
||||
#else
|
||||
typedef uint16_t symbol_t;
|
||||
#endif
|
||||
|
||||
struct iir_biquad {
|
||||
float a[2];
|
||||
float b[3];
|
||||
};
|
||||
|
||||
struct iir_biquad_state {
|
||||
float reg[2];
|
||||
};
|
||||
|
||||
struct cwt_iir_filter_state {
|
||||
struct iir_biquad_state st[3];
|
||||
};
|
||||
|
||||
struct group {
|
||||
int len; /* length of group in samples */
|
||||
float max; /* signed value of largest peak in group on any channel */
|
||||
uint64_t max_ts; /* absolute position of above peak */
|
||||
int max_ch; /* channel (gold sequence index) of above peak */
|
||||
};
|
||||
|
||||
struct matcher_state {
|
||||
int last_phase; /* 0 .. DSSS_CORRELATION_LENGTH */
|
||||
int candidate_phase;
|
||||
|
||||
float last_score;
|
||||
float candidate_score;
|
||||
|
||||
int last_skips;
|
||||
int candidate_skips;
|
||||
|
||||
symbol_t data[TRANSMISSION_SYMBOLS];
|
||||
int data_pos;
|
||||
symbol_t candidate_data;
|
||||
};
|
||||
|
||||
struct dsss_demod_state {
|
||||
float signal[DSSS_CORRELATION_LENGTH];
|
||||
size_t signal_wpos;
|
||||
|
||||
float correlation[DSSS_GOLD_CODE_COUNT][DSSS_WAVELET_LUT_SIZE];
|
||||
size_t correlation_wpos;
|
||||
|
||||
struct cwt_iir_filter_state cwt_filter;
|
||||
|
||||
struct group group;
|
||||
|
||||
struct matcher_state matcher_cache[DSSS_MATCHER_CACHE_SIZE];
|
||||
};
|
||||
|
||||
|
||||
extern void handle_dsss_received(symbol_t data[static TRANSMISSION_SYMBOLS]);
|
||||
|
||||
void dsss_demod_init(struct dsss_demod_state *st);
|
||||
void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts);
|
||||
|
||||
#endif /* __DSSS_DEMOD_H__ */
|
||||
168
reset-controller/fw/src/freq_meas.c
Normal file
168
reset-controller/fw/src/freq_meas.c
Normal file
|
|
@ -0,0 +1,168 @@
|
|||
|
||||
#ifndef SIMULATION
|
||||
#include <stm32f407xx.h>
|
||||
#endif
|
||||
|
||||
#include <unistd.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <arm_math.h>
|
||||
#include <levmarq.h>
|
||||
|
||||
#include "freq_meas.h"
|
||||
#include "sr_global.h"
|
||||
#include "simulation.h"
|
||||
|
||||
|
||||
/* FTT window lookup table defined in generated/fmeas_fft_window.c */
|
||||
extern const float * const fmeas_fft_window_table;
|
||||
|
||||
/* jury-rig some definitions for these functions since the ARM headers only export an over-generalized variable bin size
|
||||
* variant. */
|
||||
extern arm_status arm_rfft_32_fast_init_f32(arm_rfft_fast_instance_f32 * S);
|
||||
extern arm_status arm_rfft_64_fast_init_f32(arm_rfft_fast_instance_f32 * S);
|
||||
extern arm_status arm_rfft_128_fast_init_f32(arm_rfft_fast_instance_f32 * S);
|
||||
extern arm_status arm_rfft_256_fast_init_f32(arm_rfft_fast_instance_f32 * S);
|
||||
extern arm_status arm_rfft_512_fast_init_f32(arm_rfft_fast_instance_f32 * S);
|
||||
extern arm_status arm_rfft_1024_fast_init_f32(arm_rfft_fast_instance_f32 * S);
|
||||
extern arm_status arm_rfft_2048_fast_init_f32(arm_rfft_fast_instance_f32 * S);
|
||||
extern arm_status arm_rfft_4096_fast_init_f32(arm_rfft_fast_instance_f32 * S);
|
||||
|
||||
#define CONCAT(A, B, C) A ## B ## C
|
||||
#define arm_rfft_init_name(nbits) CONCAT(arm_rfft_, nbits, _fast_init_f32)
|
||||
|
||||
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(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
|
||||
int rc;
|
||||
float in_buf[FMEAS_FFT_LEN];
|
||||
float out_buf[FMEAS_FFT_LEN];
|
||||
/*
|
||||
DEBUG_PRINTN(" [emulated adc buf] ");
|
||||
for (size_t i=0; i<FMEAS_FFT_LEN; i++)
|
||||
DEBUG_PRINTN("%5d, ", adc_buf[i]);
|
||||
DEBUG_PRINTN("\n");
|
||||
*/
|
||||
//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 - 0.5) * fmeas_fft_window_table[i];
|
||||
|
||||
//DEBUG_PRINT("Running FFT");
|
||||
arm_rfft_fast_instance_f32 fft_inst;
|
||||
if ((rc = arm_rfft_init_name(FMEAS_FFT_LEN)(&fft_inst)) != ARM_MATH_SUCCESS) {
|
||||
*out = NAN;
|
||||
return rc;
|
||||
}
|
||||
|
||||
/*
|
||||
DEBUG_PRINTN(" [input] ");
|
||||
for (size_t i=0; i<FMEAS_FFT_LEN; i++)
|
||||
DEBUG_PRINTN("%010f, ", in_buf[i]);
|
||||
DEBUG_PRINTN("\n");
|
||||
*/
|
||||
#ifndef SIMULATION
|
||||
GPIOA->BSRR = 1<<12;
|
||||
#endif
|
||||
arm_rfft_fast_f32(&fft_inst, in_buf, out_buf, 0);
|
||||
#ifndef SIMULATION
|
||||
GPIOA->BSRR = 1<<12<<16;
|
||||
#endif
|
||||
|
||||
#define FMEAS_FFT_WINDOW_MIN_F_HZ 30.0f
|
||||
#define FMEAS_FFT_WINDOW_MAX_F_HZ 70.0f
|
||||
const float binsize_hz = (float)FMEAS_ADC_SAMPLING_RATE / FMEAS_FFT_LEN;
|
||||
const size_t first_bin = (int)(FMEAS_FFT_WINDOW_MIN_F_HZ / binsize_hz);
|
||||
const size_t last_bin = (int)(FMEAS_FFT_WINDOW_MAX_F_HZ / binsize_hz + 0.5f);
|
||||
const size_t nbins = last_bin - first_bin + 1;
|
||||
|
||||
/*
|
||||
DEBUG_PRINT("binsize_hz=%f first_bin=%zd last_bin=%zd nbins=%zd", binsize_hz, first_bin, last_bin, nbins);
|
||||
DEBUG_PRINTN(" [bins real] ");
|
||||
for (size_t i=0; i<FMEAS_FFT_LEN/2; i+=2)
|
||||
DEBUG_PRINTN("%010f, ", out_buf[i]);
|
||||
DEBUG_PRINTN("\n [bins imag] ");
|
||||
for (size_t i=1; i<FMEAS_FFT_LEN/2; i+=2)
|
||||
DEBUG_PRINTN("%010f, ", out_buf[i]);
|
||||
DEBUG_PRINT("\n");
|
||||
|
||||
DEBUG_PRINT("Repacking FFT results");
|
||||
*/
|
||||
/* Copy real values of target data to front of output buffer */
|
||||
for (size_t i=0; i<nbins; i++) {
|
||||
float real = out_buf[2 * (first_bin + i)];
|
||||
float imag = out_buf[2 * (first_bin + i) + 1];
|
||||
out_buf[i] = sqrtf(real*real + imag*imag);
|
||||
}
|
||||
|
||||
/*
|
||||
DEBUG_PRINT("Running Levenberg-Marquardt");
|
||||
*/
|
||||
LMstat lmstat;
|
||||
levmarq_init(&lmstat);
|
||||
|
||||
float a_max = 0.0f;
|
||||
int i_max = 0;
|
||||
for (size_t i=0; i<nbins; i++) {
|
||||
if (out_buf[i] > a_max) {
|
||||
a_max = out_buf[i];
|
||||
i_max = i;
|
||||
}
|
||||
}
|
||||
|
||||
float par[3] = {
|
||||
a_max, i_max, 1.0f
|
||||
};
|
||||
/*
|
||||
DEBUG_PRINT(" par_pre={%010f, %010f, %010f}", par[0], par[1], par[2]);
|
||||
*/
|
||||
|
||||
#ifndef SIMULATION
|
||||
GPIOA->BSRR = 1<<12;
|
||||
#endif
|
||||
if (levmarq(3, par, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat) < 0) {
|
||||
#ifndef SIMULATION
|
||||
GPIOA->BSRR = 1<<12<<16;
|
||||
#endif
|
||||
*out = NAN;
|
||||
return -1;
|
||||
}
|
||||
#ifndef SIMULATION
|
||||
GPIOA->BSRR = 1<<12<<16;
|
||||
#endif
|
||||
|
||||
/*
|
||||
DEBUG_PRINT(" par_post={%010f, %010f, %010f}", par[0], par[1], par[2]);
|
||||
|
||||
DEBUG_PRINT("done.");
|
||||
*/
|
||||
float res = (par[1] + first_bin) * binsize_hz;
|
||||
if (par[1] < 2 || res < 5 || res > 150 || par[0] < 1) {
|
||||
*out = NAN;
|
||||
return -1;
|
||||
}
|
||||
|
||||
*out = res;
|
||||
return 0;
|
||||
}
|
||||
|
||||
float func_gauss(float *params, int x, void *userdata) {
|
||||
UNUSED(userdata);
|
||||
float a = params[0], b = params[1], c = params[2];
|
||||
float n = x-b;
|
||||
return a*expf(-n*n / (2.0f* c*c));
|
||||
}
|
||||
|
||||
void func_gauss_grad(float *out, float *params, int x, void *userdata) {
|
||||
UNUSED(userdata);
|
||||
float a = params[0], b = params[1], c = params[2];
|
||||
float n = x-b;
|
||||
float e = expf(-n*n / (2.0f * c*c));
|
||||
|
||||
/* d/da */
|
||||
out[0] = e;
|
||||
/* d/db */
|
||||
out[1] = a*n/(c*c) * e;
|
||||
/* d/dc */
|
||||
out[2] = a*n*n/(c*c*c) * e;
|
||||
}
|
||||
7
reset-controller/fw/src/freq_meas.h
Normal file
7
reset-controller/fw/src/freq_meas.h
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
|
||||
#ifndef __FREQ_MEAS_H__
|
||||
#define __FREQ_MEAS_H__
|
||||
|
||||
int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out);
|
||||
|
||||
#endif /* __FREQ_MEAS_H__ */
|
||||
4
reset-controller/fw/src/gold_code.h
Normal file
4
reset-controller/fw/src/gold_code.h
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
|
||||
/* header file for generated gold code tables */
|
||||
|
||||
extern const uint8_t * const gold_code_table;
|
||||
46
reset-controller/fw/src/gpio_helpers.c
Normal file
46
reset-controller/fw/src/gpio_helpers.c
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
|
||||
#include "gpio_helpers.h"
|
||||
|
||||
void gpio_pin_mode(GPIO_TypeDef *gpio, int pin, int mode) {
|
||||
gpio->MODER &= ~(3 << (2*pin));
|
||||
gpio->MODER |= mode << (2*pin);
|
||||
}
|
||||
|
||||
void gpio_pin_setup(GPIO_TypeDef *gpio, int pin, int mode, int speed, int pullups, int afsel) {
|
||||
int gpio_idx = ((uint32_t)gpio>>10) & 0xf;
|
||||
RCC->AHB1ENR |= 1<<gpio_idx;
|
||||
|
||||
gpio->MODER &= ~(3 << (2*pin));
|
||||
gpio->MODER |= mode << (2*pin);
|
||||
gpio->OSPEEDR &= ~(3 << (2*pin));
|
||||
gpio->OSPEEDR |= speed << (2*pin);
|
||||
gpio->PUPDR &= ~(3 << (2*pin));
|
||||
gpio->PUPDR |= pullups << (2*pin);
|
||||
gpio->AFR[pin>>3] &= ~(0xf << (4*(pin&7)));
|
||||
gpio->AFR[pin>>3] |= afsel << (4*(pin&7));
|
||||
gpio->BSRR = 1<<pin<<16;
|
||||
}
|
||||
|
||||
void gpio_pin_output(GPIO_TypeDef *gpio, int pin, int speed) {
|
||||
gpio_pin_setup(gpio, pin, 1, speed, 0, 0);
|
||||
}
|
||||
|
||||
void gpio_pin_tristate(GPIO_TypeDef *gpio, int pin, int tristate) {
|
||||
if (tristate)
|
||||
gpio->MODER &= ~(3 << (2*pin));
|
||||
else
|
||||
gpio->MODER |= 1 << (2*pin);
|
||||
}
|
||||
|
||||
void gpio_pin_input(GPIO_TypeDef *gpio, int pin, int pullups) {
|
||||
gpio_pin_setup(gpio, pin, 0, 0, pullups, 0);
|
||||
}
|
||||
|
||||
void gpio_pin_af(GPIO_TypeDef *gpio, int pin, int speed, int pullups, int afsel) {
|
||||
gpio_pin_setup(gpio, pin, 2, speed, pullups, afsel);
|
||||
}
|
||||
|
||||
void gpio_pin_analog(GPIO_TypeDef *gpio, int pin) {
|
||||
gpio_pin_setup(gpio, pin, 3, 0, 0, 0);
|
||||
}
|
||||
|
||||
14
reset-controller/fw/src/gpio_helpers.h
Normal file
14
reset-controller/fw/src/gpio_helpers.h
Normal file
|
|
@ -0,0 +1,14 @@
|
|||
#ifndef __GPIO_HELPERS_H__
|
||||
#define __GPIO_HELPERS_H__
|
||||
|
||||
#include <stm32f407xx.h>
|
||||
|
||||
void gpio_pin_mode(GPIO_TypeDef *gpio, int pin, int mode);
|
||||
void gpio_pin_setup(GPIO_TypeDef *gpio, int pin, int mode, int speed, int pullups, int afsel);
|
||||
void gpio_pin_output(GPIO_TypeDef *gpio, int pin, int speed);
|
||||
void gpio_pin_input(GPIO_TypeDef *gpio, int pin, int pullups);
|
||||
void gpio_pin_af(GPIO_TypeDef *gpio, int pin, int speed, int pullups, int afsel);
|
||||
void gpio_pin_analog(GPIO_TypeDef *gpio, int pin);
|
||||
void gpio_pin_tristate(GPIO_TypeDef *gpio, int pin, int tristate);
|
||||
|
||||
#endif /* __GPIO_HELPERS_H__ */
|
||||
394
reset-controller/fw/src/main.c
Normal file
394
reset-controller/fw/src/main.c
Normal file
|
|
@ -0,0 +1,394 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <assert.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <stm32f407xx.h>
|
||||
|
||||
#include "sr_global.h"
|
||||
#include "adc.h"
|
||||
#include "spi_flash.h"
|
||||
#include "freq_meas.h"
|
||||
#include "dsss_demod.h"
|
||||
#include "con_usart.h"
|
||||
#include "mspdebug_wrapper.h"
|
||||
#include "crypto.h"
|
||||
|
||||
static struct spi_flash_if spif;
|
||||
|
||||
unsigned int sysclk_speed = 0;
|
||||
unsigned int apb1_speed = 0;
|
||||
unsigned int apb2_speed = 0;
|
||||
unsigned int auxclk_speed = 0;
|
||||
unsigned int apb1_timer_speed = 0;
|
||||
unsigned int apb2_timer_speed = 0;
|
||||
|
||||
struct leds leds;
|
||||
|
||||
ssize_t jt_spi_flash_read_block(void *usr, int addr, size_t len, uint8_t *out);
|
||||
static void update_image_flash_counter(void);
|
||||
|
||||
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)
|
||||
{
|
||||
/* 8MHz HSE clock as PLL source. */
|
||||
#define HSE_SPEED 8000000
|
||||
/* Divide by 8 -> 1 MHz */
|
||||
#define PLL_M 8
|
||||
/* Multiply by 336 -> 336 MHz VCO frequency */
|
||||
#define PLL_N 336
|
||||
/* Divide by 4 -> 84 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
|
||||
|
||||
if (((RCC->CFGR & RCC_CFGR_SWS_Msk) >> RCC_CFGR_SW_Pos) != 0)
|
||||
asm volatile ("bkpt");
|
||||
if (RCC->CR & RCC_CR_HSEON)
|
||||
asm volatile ("bkpt");
|
||||
|
||||
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 -> 42MHz */
|
||||
RCC->CFGR |= (5 << RCC_CFGR_PPRE1_Pos);
|
||||
/* set ABP2 prescaler to 2 -> 84MHz */
|
||||
RCC->CFGR |= (4 << RCC_CFGR_PPRE2_Pos);
|
||||
|
||||
if (RCC->CR & RCC_CR_PLLON)
|
||||
asm volatile ("bkpt");
|
||||
/* 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);
|
||||
uint32_t old = RCC->PLLCFGR & ~(RCC_PLLCFGR_PLLM_Msk
|
||||
| RCC_PLLCFGR_PLLN_Msk
|
||||
| RCC_PLLCFGR_PLLP_Msk
|
||||
| RCC_PLLCFGR_PLLQ_Msk
|
||||
| RCC_PLLCFGR_PLLSRC);
|
||||
RCC->PLLCFGR = old | (PLL_M<<RCC_PLLCFGR_PLLM_Pos)
|
||||
| (PLL_N << RCC_PLLCFGR_PLLN_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;
|
||||
|
||||
sysclk_speed = HSE_SPEED / PLL_M * PLL_N / PLL_P;
|
||||
auxclk_speed = HSE_SPEED / PLL_M * PLL_N / PLL_Q;
|
||||
apb1_speed = sysclk_speed / 4;
|
||||
apb1_timer_speed = apb1_speed * 2;
|
||||
apb2_speed = sysclk_speed / 2;
|
||||
apb2_timer_speed = apb2_speed * 2;
|
||||
|
||||
/* 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_Pos);
|
||||
|
||||
/* 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)
|
||||
{
|
||||
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN;
|
||||
/* onboard leds */
|
||||
GPIOA->MODER |= (1<<GPIO_MODER_MODER6_Pos) | (1<<GPIO_MODER_MODER7_Pos);
|
||||
GPIOB->MODER |= (1<<GPIO_MODER_MODER11_Pos) | (1<<GPIO_MODER_MODER12_Pos) | (1<<GPIO_MODER_MODER13_Pos)| (1<<GPIO_MODER_MODER14_Pos);
|
||||
GPIOB->BSRR = 0xf << 11;
|
||||
}
|
||||
|
||||
static void spi_flash_if_set_cs(bool val) {
|
||||
if (val)
|
||||
GPIOB->BSRR = 1<<0;
|
||||
else
|
||||
GPIOB->BSRR = 1<<16;
|
||||
}
|
||||
|
||||
static void spi_flash_setup(void)
|
||||
{
|
||||
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
|
||||
|
||||
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);
|
||||
|
||||
uint32_t buf[1024];
|
||||
for (size_t addr=0; addr<0x10000; addr += sizeof(buf)) {
|
||||
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);
|
||||
}
|
||||
|
||||
for (size_t i=0; i<sizeof(buf)/sizeof(buf[0]); i++)
|
||||
buf[i] = 0;
|
||||
spif_read(&spif, 0x1030, sizeof(buf), (char *)buf);
|
||||
asm volatile ("bkpt");
|
||||
}
|
||||
#endif
|
||||
|
||||
static struct jtag_img_descriptor {
|
||||
size_t devmem_img_start;
|
||||
size_t spiflash_img_start;
|
||||
size_t img_len;
|
||||
} jtag_img = {
|
||||
.devmem_img_start = 0x00c000,
|
||||
.spiflash_img_start = 0x000000,
|
||||
.img_len = 0x004000,
|
||||
};
|
||||
|
||||
char fw_dump[0x4000] = {
|
||||
#include "EasyMeter_Q3DA1002_V3.03_fw_dump_0xc000.h"
|
||||
};
|
||||
const int fw_dump_offx = 0xc000;
|
||||
const int row2_offx = 0xf438 - fw_dump_offx;
|
||||
|
||||
ssize_t jt_spi_flash_read_block(void *usr, int addr, size_t len, uint8_t *out) {
|
||||
/*
|
||||
struct jtag_img_descriptor *desc = (struct jtag_img_descriptor *)usr;
|
||||
return spif_read(&spif, desc->spiflash_img_start + addr, len, (char *)out);
|
||||
*/
|
||||
|
||||
for (size_t i=0; i<len; i++)
|
||||
out[i] = fw_dump[addr - fw_dump_offx + i];
|
||||
|
||||
return len;
|
||||
}
|
||||
|
||||
void update_image_flash_counter() {
|
||||
static int flash_counter = 0;
|
||||
flash_counter ++;
|
||||
fw_dump[row2_offx + 0] = flash_counter/10000 + '0';
|
||||
flash_counter %= 10000;
|
||||
fw_dump[row2_offx + 1] = flash_counter/1000 + '0';
|
||||
flash_counter %= 1000;
|
||||
fw_dump[row2_offx + 2] = flash_counter/100 + '0';
|
||||
flash_counter %= 100;
|
||||
fw_dump[row2_offx + 3] = flash_counter/10 + '0';
|
||||
flash_counter %= 10;
|
||||
fw_dump[row2_offx + 4] = flash_counter + '0';
|
||||
}
|
||||
|
||||
/* Callback from crypto.c:oob_message_received */
|
||||
void oob_trigger_activated(enum trigger_domain domain, int serial) {
|
||||
con_printf("oob_trigger_activated(%d, %d)\r\n", domain, serial);
|
||||
con_printf("Attempting to flash meter...\r\n");
|
||||
update_image_flash_counter();
|
||||
|
||||
int flash_tries = 0;
|
||||
while (flash_tries++ < 25) {
|
||||
mspd_jtag_init();
|
||||
if (!mspd_jtag_flash_and_reset(jtag_img.devmem_img_start, jtag_img.img_len, jt_spi_flash_read_block, &jtag_img))
|
||||
break;
|
||||
for (int j=0; j<168*1000*5; j++)
|
||||
asm volatile ("nop");
|
||||
}
|
||||
if (flash_tries == 25)
|
||||
con_printf("Giving up.\r\n");
|
||||
}
|
||||
|
||||
static unsigned int measurement_errors = 0;
|
||||
static struct dsss_demod_state demod_state;
|
||||
static uint32_t freq_sample_ts = 0;
|
||||
static float debug_last_freq = 0;
|
||||
|
||||
int main(void)
|
||||
{
|
||||
#if DEBUG
|
||||
/* PLL clock on MCO2 (pin C9) */
|
||||
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
|
||||
GPIOC->MODER &= ~GPIO_MODER_MODER9_Msk;
|
||||
GPIOC->MODER |= (2<<GPIO_MODER_MODER9_Pos);
|
||||
GPIOC->AFR[1] &= ~GPIO_AFRH_AFSEL9_Msk;
|
||||
GPIOC->OSPEEDR |= (3<<GPIO_OSPEEDR_OSPEED9_Pos);
|
||||
RCC->CFGR |= (6<<RCC_CFGR_MCO2PRE_Pos) | (3<<RCC_CFGR_MCO2_Pos);
|
||||
#endif
|
||||
|
||||
if (((SCB->CPACR>>20) & 0xf) != 0xf) {
|
||||
asm volatile ("bkpt");
|
||||
}
|
||||
|
||||
clock_setup();
|
||||
con_usart_init();
|
||||
con_printf("\033[0m\033[2J\033[HBooting...\r\n");
|
||||
|
||||
led_setup();
|
||||
spi_flash_setup();
|
||||
adc_init();
|
||||
|
||||
#if DEBUG
|
||||
/* TIM1 CC1 (ADC trigger) on pin A8 */
|
||||
GPIOA->MODER &= ~GPIO_MODER_MODER8_Msk;
|
||||
GPIOA->MODER |= (2<<GPIO_MODER_MODER8_Pos);
|
||||
GPIOA->AFR[1] &= ~GPIO_AFRH_AFSEL8_Msk;
|
||||
GPIOA->AFR[1] |= 1<<GPIO_AFRH_AFSEL8_Pos;
|
||||
|
||||
GPIOA->MODER |= (1<<GPIO_MODER_MODER11_Pos) | (1<<GPIO_MODER_MODER12_Pos) | (1<<GPIO_MODER_MODER15_Pos);
|
||||
#endif
|
||||
|
||||
dsss_demod_init(&demod_state);
|
||||
|
||||
con_printf("Booted.\r\n");
|
||||
|
||||
|
||||
/* FIXME DEBUG */
|
||||
#if 0
|
||||
uint8_t test_data[TRANSMISSION_SYMBOLS] = {
|
||||
0
|
||||
};
|
||||
con_printf("Test 0\r\n");
|
||||
handle_dsss_received(test_data);
|
||||
|
||||
uint8_t test_data2[TRANSMISSION_SYMBOLS] = {
|
||||
0x24, 0x0f, 0x3b, 0x10, 0x27, 0x0e, 0x22, 0x30, 0x01, 0x2c, 0x1c, 0x0b, 0x35, 0x0a, 0x12, 0x27, 0x11, 0x20,
|
||||
0x0c, 0x10, 0xc0, 0x08, 0xa4, 0x72, 0xa9, 0x9b, 0x7b, 0x27, 0xee, 0xcd
|
||||
};
|
||||
con_printf("Test 1\r\n");
|
||||
handle_dsss_received(test_data2);
|
||||
#endif
|
||||
/* END DEBUG */
|
||||
|
||||
while (23) {
|
||||
if (adc_fft_buf_ready_idx != -1) {
|
||||
for (int j=0; j<168*1000*2; j++)
|
||||
asm volatile ("nop");
|
||||
GPIOA->BSRR = 1<<11;
|
||||
memcpy(adc_fft_buf[!adc_fft_buf_ready_idx], adc_fft_buf[adc_fft_buf_ready_idx] + FMEAS_FFT_LEN/2, sizeof(adc_fft_buf[0][0]) * FMEAS_FFT_LEN/2);
|
||||
GPIOA->BSRR = 1<<11<<16;
|
||||
GPIOB->ODR ^= 1<<14;
|
||||
|
||||
bool clip_low=false, clip_high=false;
|
||||
const int clip_thresh = 100;
|
||||
for (size_t j=FMEAS_FFT_LEN/2; j<FMEAS_FFT_LEN; j++) {
|
||||
int val = adc_fft_buf[adc_fft_buf_ready_idx][j];
|
||||
if (val < clip_thresh)
|
||||
clip_low = true;
|
||||
if (val > FMEAS_ADC_MAX-clip_thresh)
|
||||
clip_high = true;
|
||||
}
|
||||
GPIOB->ODR = (GPIOB->ODR & ~(3<<11)) | (!clip_low<<11) | (!clip_high<<12);
|
||||
|
||||
for (int j=0; j<168*1000*2; j++)
|
||||
asm volatile ("nop");
|
||||
|
||||
GPIOA->BSRR = 1<<11;
|
||||
float out;
|
||||
if (adc_buf_measure_freq(adc_fft_buf[adc_fft_buf_ready_idx], &out)) {
|
||||
con_printf("%012d: measurement error\r\n", freq_sample_ts);
|
||||
measurement_errors++;
|
||||
GPIOB->BSRR = 1<<13;
|
||||
debug_last_freq = NAN;
|
||||
|
||||
} else {
|
||||
debug_last_freq = out;
|
||||
con_printf("%012d: %2d.%03d Hz\r\n", freq_sample_ts, (int)out, (int)(out * 1000) % 1000);
|
||||
/* frequency ok led */
|
||||
if (48 < out && out < 52)
|
||||
GPIOB->BSRR = 1<<13<<16;
|
||||
else
|
||||
GPIOB->BSRR = 1<<13;
|
||||
|
||||
GPIOA->BSRR = 1<<12;
|
||||
dsss_demod_step(&demod_state, out, freq_sample_ts);
|
||||
GPIOA->BSRR = 1<<12<<16;
|
||||
}
|
||||
GPIOA->BSRR = 1<<11<<16;
|
||||
|
||||
freq_sample_ts++; /* TODO: also increase in case of freq measurement error? */
|
||||
adc_fft_buf_ready_idx = -1;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void NMI_Handler(void) {
|
||||
asm volatile ("bkpt #1");
|
||||
}
|
||||
|
||||
void HardFault_Handler(void) {
|
||||
asm volatile ("bkpt #2");
|
||||
}
|
||||
|
||||
void MemManage_Handler(void) {
|
||||
asm volatile ("bkpt #3");
|
||||
}
|
||||
|
||||
void BusFault_Handler(void) {
|
||||
asm volatile ("bkpt #4");
|
||||
}
|
||||
|
||||
void UsageFault_Handler(void) {
|
||||
asm volatile ("bkpt #5");
|
||||
}
|
||||
|
||||
void SVC_Handler(void) {
|
||||
asm volatile ("bkpt #6");
|
||||
}
|
||||
|
||||
void DebugMon_Handler(void) {
|
||||
asm volatile ("bkpt #7");
|
||||
}
|
||||
|
||||
void PendSV_Handler(void) {
|
||||
asm volatile ("bkpt #8");
|
||||
}
|
||||
|
||||
void SysTick_Handler(void) {
|
||||
asm volatile ("bkpt #9");
|
||||
}
|
||||
|
||||
261
reset-controller/fw/src/mspdebug_wrapper.c
Normal file
261
reset-controller/fw/src/mspdebug_wrapper.c
Normal file
|
|
@ -0,0 +1,261 @@
|
|||
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "output.h"
|
||||
#include "jtaglib.h"
|
||||
|
||||
#include "sr_global.h"
|
||||
#include "gpio_helpers.h"
|
||||
#include "mspdebug_wrapper.h"
|
||||
#include "con_usart.h"
|
||||
|
||||
#include <stm32f407xx.h>
|
||||
|
||||
#define BLOCK_SIZE 512 /* bytes */
|
||||
|
||||
|
||||
static void sr_delay_inst(void);
|
||||
|
||||
static struct jtdev sr_jtdev;
|
||||
static struct jtdev sr_jtdev_default;
|
||||
|
||||
enum sr_gpio_types {
|
||||
SR_GPIO_TCK,
|
||||
SR_GPIO_TMS,
|
||||
SR_GPIO_TDI,
|
||||
SR_GPIO_RST,
|
||||
SR_GPIO_TST,
|
||||
SR_GPIO_TDO,
|
||||
SR_NUM_GPIOS
|
||||
};
|
||||
|
||||
struct {
|
||||
GPIO_TypeDef *gpio;
|
||||
int pin;
|
||||
int mode;
|
||||
} gpios[SR_NUM_GPIOS] = {
|
||||
[SR_GPIO_TCK] = {GPIOE, 10, 1},
|
||||
[SR_GPIO_TMS] = {GPIOE, 11, 1},
|
||||
[SR_GPIO_TDI] = {GPIOE, 12, 1},
|
||||
[SR_GPIO_RST] = {GPIOE, 9, 1},
|
||||
[SR_GPIO_TST] = {GPIOE, 14, 1},
|
||||
[SR_GPIO_TDO] = {GPIOE, 13, 0},
|
||||
};
|
||||
|
||||
void sr_delay_inst() {
|
||||
for (int i=0; i<10; i++)
|
||||
asm volatile("nop");
|
||||
}
|
||||
|
||||
void mspd_jtag_init() {
|
||||
for (int i=0; i<SR_NUM_GPIOS; i++)
|
||||
gpio_pin_setup(gpios[i].gpio, gpios[i].pin, gpios[i].mode, 3, 0, 0);
|
||||
}
|
||||
|
||||
static void sr_gpio_write(int num, int out) {
|
||||
if (out)
|
||||
gpios[num].gpio->BSRR = 1<<gpios[num].pin;
|
||||
else
|
||||
gpios[num].gpio->BSRR = 1<<gpios[num].pin<<16;
|
||||
}
|
||||
|
||||
static void sr_jtdev_rst(struct jtdev *p, int out) {
|
||||
UNUSED(p);
|
||||
sr_gpio_write(SR_GPIO_RST, out);
|
||||
}
|
||||
|
||||
int mspd_jtag_flash_and_reset(size_t img_start, size_t img_len, ssize_t (*read_block)(void *usr, int addr, size_t len, uint8_t *out), void *usr)
|
||||
{
|
||||
union {
|
||||
uint8_t bytes[BLOCK_SIZE];
|
||||
uint16_t words[BLOCK_SIZE/2];
|
||||
} block;
|
||||
|
||||
memcpy(&sr_jtdev, &sr_jtdev_default, sizeof(sr_jtdev));
|
||||
/* Initialize JTAG connection */
|
||||
unsigned int jtag_id = jtag_init(&sr_jtdev);
|
||||
|
||||
if (sr_jtdev.failed) {
|
||||
con_printf("Couldn't initialize device\r\n");
|
||||
return -EPIPE;
|
||||
}
|
||||
|
||||
con_printf("JTAG device ID: 0x%02x\r\n", jtag_id);
|
||||
if (jtag_id != 0x89 && jtag_id != 0x91)
|
||||
return -EINVAL;
|
||||
|
||||
#if 0
|
||||
con_printf("Memory dump:\r\n");
|
||||
for (size_t i=0x1000; i<=0x10ff;) {
|
||||
con_printf("%04x: ", i);
|
||||
for (size_t j=0; j<16; i+=1, j+=1) {
|
||||
con_printf("%02x ", jtag_read_mem(&sr_jtdev, 8, i));
|
||||
}
|
||||
con_printf("\r\n");
|
||||
}
|
||||
return 0;
|
||||
#endif
|
||||
|
||||
/* Clear flash */
|
||||
jtag_erase_flash(&sr_jtdev, JTAG_ERASE_MAIN, 0);
|
||||
if (sr_jtdev.failed)
|
||||
return -EPIPE;
|
||||
|
||||
/* Write flash */
|
||||
for (size_t p = img_start; p < img_start + img_len; p += BLOCK_SIZE) {
|
||||
con_printf("Writing block %04zx\r\n", p);
|
||||
ssize_t nin = read_block(usr, p, BLOCK_SIZE, block.bytes);
|
||||
|
||||
if (nin < 0)
|
||||
return nin;
|
||||
|
||||
if (nin & 1) { /* pad unaligned */
|
||||
block.bytes[nin] = 0;
|
||||
nin ++;
|
||||
}
|
||||
|
||||
/* Convert to little-endian */
|
||||
for (ssize_t i=0; i<nin/2; i++)
|
||||
block.words[i] = htole(block.words[i]);
|
||||
|
||||
jtag_write_flash(&sr_jtdev, p, nin/2, block.words);
|
||||
if (sr_jtdev.failed)
|
||||
return -EPIPE;
|
||||
}
|
||||
|
||||
/* TODO: Verify flash here. */
|
||||
|
||||
/* Execute power on reset */
|
||||
jtag_execute_puc(&sr_jtdev);
|
||||
if (sr_jtdev.failed)
|
||||
return -EPIPE;
|
||||
|
||||
jtag_release_device(&sr_jtdev, 0xfffe);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* mspdebug HAL shim */
|
||||
|
||||
int printc_err(const char *fmt, ...) {
|
||||
va_list va;
|
||||
va_start(va, fmt);
|
||||
int rc = usart_printf_blocking_va(&con_usart, fmt, va);
|
||||
if (rc)
|
||||
return rc;
|
||||
|
||||
size_t i;
|
||||
for (i=0; fmt[i]; i++)
|
||||
;
|
||||
if (i > 0 && fmt[i-1] == '\n')
|
||||
usart_putc_nonblocking(&con_usart, '\r');
|
||||
return rc;
|
||||
}
|
||||
|
||||
|
||||
static void sr_jtdev_power_on(struct jtdev *p) {
|
||||
UNUSED(p);
|
||||
/* ignore */
|
||||
}
|
||||
|
||||
static void sr_jtdev_connect(struct jtdev *p) {
|
||||
UNUSED(p);
|
||||
/* ignore */
|
||||
}
|
||||
|
||||
static void sr_jtdev_tck(struct jtdev *p, int out) {
|
||||
UNUSED(p);
|
||||
sr_gpio_write(SR_GPIO_TCK, out);
|
||||
}
|
||||
|
||||
static void sr_jtdev_tms(struct jtdev *p, int out) {
|
||||
UNUSED(p);
|
||||
sr_gpio_write(SR_GPIO_TMS, out);
|
||||
}
|
||||
|
||||
static void sr_jtdev_tdi(struct jtdev *p, int out) {
|
||||
UNUSED(p);
|
||||
sr_gpio_write(SR_GPIO_TDI, out);
|
||||
}
|
||||
|
||||
static void sr_jtdev_tst(struct jtdev *p, int out) {
|
||||
UNUSED(p);
|
||||
sr_gpio_write(SR_GPIO_TST, out);
|
||||
}
|
||||
|
||||
static int sr_jtdev_tdo_get(struct jtdev *p) {
|
||||
UNUSED(p);
|
||||
return !!(gpios[SR_GPIO_TDO].gpio->IDR & (1<<gpios[SR_GPIO_TDO].pin));
|
||||
}
|
||||
|
||||
static void sr_jtdev_tclk(struct jtdev *p, int out) {
|
||||
UNUSED(p);
|
||||
sr_gpio_write(SR_GPIO_TDI, out);
|
||||
}
|
||||
|
||||
static int sr_jtdev_tclk_get(struct jtdev *p) {
|
||||
UNUSED(p);
|
||||
return !!(gpios[SR_GPIO_TDI].gpio->ODR & (1<<gpios[SR_GPIO_TDI].pin));
|
||||
}
|
||||
|
||||
static void sr_jtdev_tclk_strobe(struct jtdev *p, unsigned int count) {
|
||||
UNUSED(p);
|
||||
while (count--) {
|
||||
gpios[SR_GPIO_TDI].gpio->BSRR = 1<<gpios[SR_GPIO_TDI].pin;
|
||||
sr_delay_inst();
|
||||
gpios[SR_GPIO_TDI].gpio->BSRR = 1<<gpios[SR_GPIO_TDI].pin<<16;
|
||||
}
|
||||
}
|
||||
|
||||
static void sr_jtdev_led_green(struct jtdev *p, int out) {
|
||||
UNUSED(p);
|
||||
UNUSED(out);
|
||||
/* ignore */
|
||||
}
|
||||
|
||||
static void sr_jtdev_led_red(struct jtdev *p, int out) {
|
||||
UNUSED(p);
|
||||
UNUSED(out);
|
||||
/* ignore */
|
||||
}
|
||||
|
||||
|
||||
static struct jtdev_func sr_jtdev_vtable = {
|
||||
.jtdev_open = NULL,
|
||||
.jtdev_close = NULL,
|
||||
|
||||
.jtdev_power_off = NULL,
|
||||
.jtdev_release = NULL,
|
||||
|
||||
.jtdev_power_on = sr_jtdev_power_on,
|
||||
.jtdev_connect = sr_jtdev_connect,
|
||||
|
||||
.jtdev_tck = sr_jtdev_tck,
|
||||
.jtdev_tms = sr_jtdev_tms,
|
||||
.jtdev_tdi = sr_jtdev_tdi,
|
||||
.jtdev_rst = sr_jtdev_rst,
|
||||
.jtdev_tst = sr_jtdev_tst,
|
||||
.jtdev_tdo_get = sr_jtdev_tdo_get,
|
||||
|
||||
.jtdev_tclk = sr_jtdev_tclk,
|
||||
.jtdev_tclk_get = sr_jtdev_tclk_get,
|
||||
.jtdev_tclk_strobe = sr_jtdev_tclk_strobe,
|
||||
|
||||
.jtdev_led_green = sr_jtdev_led_green,
|
||||
.jtdev_led_red = sr_jtdev_led_red,
|
||||
|
||||
};
|
||||
|
||||
static struct jtdev sr_jtdev = {
|
||||
0,
|
||||
.f = &sr_jtdev_vtable
|
||||
};
|
||||
|
||||
static struct jtdev sr_jtdev_default = {
|
||||
0,
|
||||
.f = &sr_jtdev_vtable
|
||||
};
|
||||
|
||||
|
||||
7
reset-controller/fw/src/mspdebug_wrapper.h
Normal file
7
reset-controller/fw/src/mspdebug_wrapper.h
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
#ifndef __MSPDEBUG_WRAPPER_H__
|
||||
#define __MSPDEBUG_WRAPPER_H__
|
||||
|
||||
void mspd_jtag_init(void);
|
||||
int mspd_jtag_flash_and_reset(size_t img_start, size_t img_len, ssize_t (*read_block)(void *usr, int addr, size_t len, uint8_t *out), void *usr);
|
||||
|
||||
#endif /* __MSPDEBUG_WRAPPER_H__ */
|
||||
44
reset-controller/fw/src/protocol.c
Normal file
44
reset-controller/fw/src/protocol.c
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
|
||||
#include <assert.h>
|
||||
|
||||
#include "sr_global.h"
|
||||
#include "dsss_demod.h"
|
||||
#include "con_usart.h"
|
||||
#include "rslib.h"
|
||||
#include "crypto.h"
|
||||
|
||||
void handle_dsss_received(uint8_t data[static TRANSMISSION_SYMBOLS]) {
|
||||
/* Console status output */
|
||||
con_printf("DSSS data received: ");
|
||||
for (int i=0; i<TRANSMISSION_SYMBOLS; i++) {
|
||||
int x = (data[i]>>1) * (data[i]&1 ? 1 : -1);
|
||||
con_printf("%3d ", x);
|
||||
}
|
||||
con_printf("\r\n");
|
||||
|
||||
/* Run reed-solomon error correction */
|
||||
const int sym_bits = DSSS_GOLD_CODE_NBITS + 1; /* +1 for amplitude sign bit */
|
||||
/* TODO identify erasures in DSSS demod layer */
|
||||
(void) rslib_decode(sym_bits, TRANSMISSION_SYMBOLS, (char *)data);
|
||||
/* TODO error detection & handling */
|
||||
|
||||
/* Re-bit-pack data buffer to be bit-continuous:
|
||||
* [ . . a b c d e f ] [ . . g h i j k l ] [ . . m n o p q r ] ...
|
||||
* ==> [ a b c d e f g h ] [ i j k l m n o p ] [ q r ... ] ...
|
||||
*/
|
||||
static_assert((TRANSMISSION_SYMBOLS - NPAR) * (DSSS_GOLD_CODE_NBITS + 1) == OOB_TRIGGER_LEN * 8);
|
||||
for (uint8_t i=0, j=0; i < TRANSMISSION_SYMBOLS - NPAR; i++, j += sym_bits) {
|
||||
uint32_t sym = data[i]; /* [ ... | . . X X X X X X ] for 5-bit dsss */
|
||||
data[i] = 0; /* clear for output */
|
||||
|
||||
sym <<= 8-sym_bits; /* left-align: [ ... | X X X X X X . . ] */
|
||||
sym <<= 8; /* shift to second byte: [ ... | X X X X X X . . | . . . . . . . . ]*/
|
||||
sym >>= (j%8); /* shift to bit write offset: [ ... | . . . . X X X X | X X . . . . . . ] for offset 4 */
|
||||
data[j/8] |= sym >> 8; /* write upper byte */
|
||||
data[j/8 + 1] |= sym & 0xff; /* write lower byte */
|
||||
}
|
||||
|
||||
/* hand off to crypto.c */
|
||||
oob_message_received(data);
|
||||
}
|
||||
|
||||
8
reset-controller/fw/src/rscode-config.h
Normal file
8
reset-controller/fw/src/rscode-config.h
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
/* Config header for reed-solomon library */
|
||||
|
||||
#ifndef __RSCODE_CONFIG_H__
|
||||
#define __RSCODE_CONFIG_H__
|
||||
|
||||
#define NPAR 10
|
||||
|
||||
#endif /* __RSCODE_CONFIG_H__ */
|
||||
28
reset-controller/fw/src/rslib.c
Normal file
28
reset-controller/fw/src/rslib.c
Normal file
|
|
@ -0,0 +1,28 @@
|
|||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "rscode-config.h"
|
||||
#include <ecc.h>
|
||||
|
||||
#include "rslib.h"
|
||||
|
||||
static struct rscode_driver driver;
|
||||
|
||||
void rslib_encode(int nbits, size_t msglen, char msg[static msglen], char out[msglen + NPAR]) {
|
||||
rscode_init(&driver, nbits);
|
||||
rscode_encode(&driver, (unsigned char *)msg, msglen, (unsigned char *)out);
|
||||
}
|
||||
|
||||
int rslib_decode(int nbits, size_t msglen, char msg_inout[static msglen]) {
|
||||
rscode_init(&driver, nbits);
|
||||
return rscode_decode(&driver, (unsigned char *)msg_inout, msglen);
|
||||
}
|
||||
|
||||
int rslib_gexp(int z, int nbits) {
|
||||
rscode_init(&driver, nbits);
|
||||
return gexp(&driver, z);
|
||||
}
|
||||
|
||||
size_t rslib_npar() {
|
||||
return NPAR;
|
||||
}
|
||||
12
reset-controller/fw/src/rslib.h
Normal file
12
reset-controller/fw/src/rslib.h
Normal file
|
|
@ -0,0 +1,12 @@
|
|||
#ifndef __RSLIB_H__
|
||||
#define __RSLIB_H__
|
||||
|
||||
/* parity length configuration */
|
||||
#include "rscode-config.h"
|
||||
|
||||
void rslib_encode(int nbits, size_t msglen, char msg[static msglen], char out[msglen + NPAR]);
|
||||
int rslib_decode(int nbits, size_t msglen, char msg_inout[static msglen]);
|
||||
int rslib_gexp(int z, int nbits);
|
||||
size_t rslib_npar(void);
|
||||
|
||||
#endif /* __RSLIB_H__ */
|
||||
186
reset-controller/fw/src/serial.c
Normal file
186
reset-controller/fw/src/serial.c
Normal file
|
|
@ -0,0 +1,186 @@
|
|||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "dma_util.h"
|
||||
#include "sr_global.h"
|
||||
#include "serial.h"
|
||||
|
||||
#include <tinyprintf.h>
|
||||
|
||||
static void usart_schedule_dma(volatile struct usart_desc *us);
|
||||
static void usart_dma_reset(volatile struct usart_desc *us);
|
||||
static void usart_putc_nonblocking_tpf(void *us, char c);
|
||||
static void usart_wait_chunk_free(volatile struct usart_desc *us);
|
||||
static void usart_putc_blocking_tpf(void *us, char c);
|
||||
|
||||
void usart_dma_reset(volatile struct usart_desc *us) {
|
||||
us->tx_buf.xfr_start = -1;
|
||||
us->tx_buf.xfr_end = 0;
|
||||
us->tx_buf.wr_pos = 0;
|
||||
us->tx_buf.wr_idx = 0;
|
||||
us->tx_buf.xfr_next = 0;
|
||||
us->tx_buf.wraparound = false;
|
||||
|
||||
for (size_t i=0; i<ARRAY_LENGTH(us->tx_buf.chunk_end); i++)
|
||||
us->tx_buf.chunk_end[i] = -1;
|
||||
}
|
||||
|
||||
void usart_dma_init(volatile struct usart_desc *us, unsigned int baudrate) {
|
||||
usart_dma_reset(us);
|
||||
|
||||
/* Configure DMA 1 Channel 2 to handle uart transmission */
|
||||
us->tx_dmas->PAR = (uint32_t)&(us->le_usart->DR);
|
||||
us->tx_dmas->CR =
|
||||
(us->tx_dma_ch<<DMA_SxCR_CHSEL_Pos)
|
||||
| (0<<DMA_SxCR_PL_Pos)
|
||||
| (1<<DMA_SxCR_DIR_Pos)
|
||||
| (0<<DMA_SxCR_MSIZE_Pos) /* 8 bit */
|
||||
| (0<<DMA_SxCR_PSIZE_Pos) /* 8 bit */
|
||||
| DMA_SxCR_MINC
|
||||
| DMA_SxCR_TCIE; /* Enable transfer complete interrupt. */
|
||||
|
||||
/* triggered on transfer completion. We use this to process the ADC data */
|
||||
NVIC_EnableIRQ(us->tx_dma_irqn);
|
||||
NVIC_SetPriority(us->tx_dma_irqn, 30);
|
||||
|
||||
us->le_usart->CR1 = USART_CR1_TE;
|
||||
/* Set divider for 115.2kBd @48MHz system clock. */
|
||||
us->le_usart->BRR = apb2_speed * 16 / baudrate / 16; /* 250kBd */
|
||||
us->le_usart->CR3 |= USART_CR3_DMAT; /* TX DMA enable */
|
||||
|
||||
/* And... go! */
|
||||
us->le_usart->CR1 |= USART_CR1_UE;
|
||||
}
|
||||
|
||||
void usart_schedule_dma(volatile struct usart_desc *us) {
|
||||
volatile struct dma_tx_buf *buf = &us->tx_buf;
|
||||
|
||||
ssize_t xfr_start, xfr_end, xfr_len;
|
||||
if (buf->wraparound) {
|
||||
buf->wraparound = false;
|
||||
xfr_start = 0;
|
||||
xfr_len = buf->xfr_end;
|
||||
xfr_end = buf->xfr_end;
|
||||
|
||||
} else {
|
||||
if (buf->chunk_end[buf->xfr_next] == -1)
|
||||
return; /* Nothing to trasnmit */
|
||||
|
||||
xfr_start = buf->xfr_end;
|
||||
xfr_end = buf->chunk_end[buf->xfr_next];
|
||||
buf->chunk_end[buf->xfr_next] = -1;
|
||||
buf->xfr_next = (buf->xfr_next + 1) % ARRAY_LENGTH(buf->chunk_end);
|
||||
|
||||
if (xfr_end > xfr_start) { /* no wraparound */
|
||||
xfr_len = xfr_end - xfr_start;
|
||||
|
||||
} else { /* wraparound */
|
||||
if (xfr_end != 0)
|
||||
buf->wraparound = true;
|
||||
xfr_len = sizeof(us->data) - xfr_start;
|
||||
}
|
||||
}
|
||||
|
||||
buf->xfr_start = xfr_start;
|
||||
buf->xfr_end = xfr_end;
|
||||
|
||||
us->comm_led = 100;
|
||||
|
||||
/* initiate transmission of new buffer */
|
||||
us->tx_dmas->M0AR = (uint32_t)(us->data + xfr_start);
|
||||
us->tx_dmas->NDTR = xfr_len;
|
||||
us->tx_dmas->CR |= DMA_SxCR_EN;
|
||||
}
|
||||
|
||||
void usart_dma_stream_irq(volatile struct usart_desc *us) {
|
||||
uint8_t iflags = dma_get_isr_and_clear(us->tx_dma, us->tx_dma_sn);
|
||||
|
||||
if (iflags & DMA_LISR_TCIF0) { /* Transfer complete */
|
||||
us->tx_dmas->CR &= ~DMA_SxCR_EN;
|
||||
//if (us->tx_buf.wraparound)
|
||||
usart_schedule_dma(us);
|
||||
}
|
||||
|
||||
if (iflags & DMA_LISR_FEIF0)
|
||||
us->tx_errors++;
|
||||
}
|
||||
|
||||
int usart_putc_nonblocking(volatile struct usart_desc *us, char c) {
|
||||
volatile struct dma_tx_buf *buf = &us->tx_buf;
|
||||
|
||||
if (buf->wr_pos == buf->xfr_start) {
|
||||
us->tx_byte_overruns++;
|
||||
return -EBUSY;
|
||||
}
|
||||
|
||||
buf->data[buf->wr_pos] = c;
|
||||
buf->wr_pos = (buf->wr_pos + 1) % sizeof(us->data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int usart_putc_blocking(volatile struct usart_desc *us, char c) {
|
||||
volatile struct dma_tx_buf *buf = &us->tx_buf;
|
||||
|
||||
while (buf->wr_pos == buf->xfr_start)
|
||||
;
|
||||
|
||||
buf->data[buf->wr_pos] = c;
|
||||
buf->wr_pos = (buf->wr_pos + 1) % sizeof(us->data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void usart_putc_nonblocking_tpf(void *us, char c) {
|
||||
usart_putc_nonblocking((struct usart_desc *)us, c);
|
||||
}
|
||||
|
||||
void usart_putc_blocking_tpf(void *us, char c) {
|
||||
usart_putc_blocking((struct usart_desc *)us, c);
|
||||
}
|
||||
|
||||
int usart_send_chunk_nonblocking(volatile struct usart_desc *us, const char *chunk, size_t chunk_len) {
|
||||
for (size_t i=0; i<chunk_len; i++)
|
||||
usart_putc_nonblocking(us, chunk[i]);
|
||||
|
||||
return usart_flush(us);
|
||||
}
|
||||
|
||||
void usart_wait_chunk_free(volatile struct usart_desc *us) {
|
||||
while (us->tx_buf.chunk_end[us->tx_buf.wr_idx] != -1)
|
||||
;
|
||||
}
|
||||
|
||||
int usart_flush(volatile struct usart_desc *us) {
|
||||
/* Find a free slot for this chunk */
|
||||
if (us->tx_buf.chunk_end[us->tx_buf.wr_idx] != -1) {
|
||||
us->tx_chunk_overruns++;
|
||||
return -EBUSY;
|
||||
}
|
||||
|
||||
us->tx_buf.chunk_end[us->tx_buf.wr_idx] = us->tx_buf.wr_pos;
|
||||
us->tx_buf.wr_idx = (us->tx_buf.wr_idx + 1) % ARRAY_LENGTH(us->tx_buf.chunk_end);
|
||||
|
||||
if (!(us->tx_dmas->CR & DMA_SxCR_EN))
|
||||
usart_schedule_dma(us);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int usart_printf(volatile struct usart_desc *us, const char *fmt, ...) {
|
||||
va_list va;
|
||||
va_start(va, fmt);
|
||||
tfp_format((void *)us, usart_putc_nonblocking_tpf, fmt, va);
|
||||
return usart_flush(us);
|
||||
}
|
||||
|
||||
int usart_printf_blocking_va(volatile struct usart_desc *us, const char *fmt, va_list va) {
|
||||
tfp_format((void *)us, usart_putc_blocking_tpf, fmt, va);
|
||||
usart_wait_chunk_free(us);
|
||||
return usart_flush(us);
|
||||
}
|
||||
|
||||
int usart_printf_blocking(volatile struct usart_desc *us, const char *fmt, ...) {
|
||||
va_list va;
|
||||
va_start(va, fmt);
|
||||
return usart_printf_blocking_va(us, fmt, va);
|
||||
}
|
||||
|
||||
85
reset-controller/fw/src/serial.h
Normal file
85
reset-controller/fw/src/serial.h
Normal file
|
|
@ -0,0 +1,85 @@
|
|||
/*
|
||||
* This file is part of the libusbhost library
|
||||
* hosted at http://github.com/libusbhost/libusbhost
|
||||
*
|
||||
* Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>
|
||||
*
|
||||
*
|
||||
* libusbhost is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU Lesser General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public License
|
||||
* along with this library. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __SERIAL_H__
|
||||
#define __SERIAL_H__
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdarg.h>
|
||||
#include <errno.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "sr_global.h"
|
||||
|
||||
struct dma_tx_buf {
|
||||
/* The following fields are accessed only from DMA ISR */
|
||||
ssize_t xfr_start; /* Start index of running DMA transfer */
|
||||
ssize_t xfr_end; /* End index of running DMA transfer plus one */
|
||||
bool wraparound;
|
||||
ssize_t xfr_next;
|
||||
|
||||
/* The following fields are written only from non-interrupt code */
|
||||
ssize_t wr_pos; /* Next index to be written */
|
||||
ssize_t wr_idx;
|
||||
ssize_t chunk_end[8];
|
||||
|
||||
/* Make GCC shut up about the zero-size array member. */
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wpedantic"
|
||||
/* Written outside ISR by usart_send_chunk_nonblocking, read via DMA */
|
||||
uint8_t data[0];
|
||||
#pragma GCC diagnostic pop
|
||||
};
|
||||
|
||||
struct usart_desc {
|
||||
struct dma_tx_buf tx_buf;
|
||||
uint8_t data[512];
|
||||
|
||||
uint32_t tx_chunk_overruns, tx_byte_overruns;
|
||||
uint32_t tx_errors;
|
||||
|
||||
volatile uint8_t rx_buf[32];
|
||||
|
||||
int comm_led;
|
||||
|
||||
USART_TypeDef *le_usart;
|
||||
int le_usart_irqn;
|
||||
DMA_Stream_TypeDef *tx_dmas;
|
||||
int tx_dma_sn;
|
||||
int tx_dma_ch;
|
||||
DMA_TypeDef *tx_dma;
|
||||
int tx_dma_irqn;
|
||||
};
|
||||
|
||||
void usart_dma_init(volatile struct usart_desc *us, unsigned int baudrate);
|
||||
int usart_send_chunk_nonblocking(volatile struct usart_desc *us, const char *chunk, size_t chunk_len);
|
||||
int usart_putc_nonblocking(volatile struct usart_desc *us, char c);
|
||||
int usart_putc_blocking(volatile struct usart_desc *us, char c);
|
||||
|
||||
void usart_dma_stream_irq(volatile struct usart_desc *us);
|
||||
int usart_flush(volatile struct usart_desc *us);
|
||||
int usart_printf(volatile struct usart_desc *us, const char *fmt, ...);
|
||||
int usart_printf_blocking(volatile struct usart_desc *us, const char *fmt, ...);
|
||||
int usart_printf_blocking_va(volatile struct usart_desc *us, const char *fmt, va_list va);
|
||||
|
||||
#endif // __SERIAL_H__
|
||||
0
reset-controller/fw/src/signal_processing.c
Normal file
0
reset-controller/fw/src/signal_processing.c
Normal file
14
reset-controller/fw/src/simulation.h
Normal file
14
reset-controller/fw/src/simulation.h
Normal file
|
|
@ -0,0 +1,14 @@
|
|||
#ifndef __SIMULATION_H__
|
||||
#define __SIMULATION_H__
|
||||
|
||||
#ifdef SIMULATION
|
||||
#include <stdio.h>
|
||||
#define DEBUG_PRINTN(...) printf(__VA_ARGS__)
|
||||
#define DEBUG_PRINTNF(fmt, ...) DEBUG_PRINTN("%s:%d: " fmt, __FILE__, __LINE__, ##__VA_ARGS__)
|
||||
#define DEBUG_PRINT(fmt, ...) DEBUG_PRINTNF(fmt "\n", ##__VA_ARGS__)
|
||||
#else
|
||||
#define DEBUG_PRINT(...) ((void)0)
|
||||
#define DEBUG_PRINTN(...) ((void)0)
|
||||
#endif
|
||||
|
||||
#endif /* __SIMULATION_H__ */
|
||||
200
reset-controller/fw/src/spi_flash.c
Normal file
200
reset-controller/fw/src/spi_flash.c
Normal file
|
|
@ -0,0 +1,200 @@
|
|||
/* Library for SPI flash 25* devices.
|
||||
* Copyright (c) 2014 Multi-Tech Systems
|
||||
* Copyright (c) 2020 Jan Goette <ma@jaseg.de>
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "spi_flash.h"
|
||||
|
||||
enum {
|
||||
WRITE_ENABLE = 0x06,
|
||||
WRITE_DISABLE = 0x04,
|
||||
READ_IDENTIFICATION = 0x9F,
|
||||
READ_STATUS = 0x05,
|
||||
WRITE_STATUS = 0x01,
|
||||
READ_DATA = 0x03,
|
||||
READ_DATA_FAST = 0x0B,
|
||||
PAGE_PROGRAM = 0x02,
|
||||
SECTOR_ERASE = 0xD8,
|
||||
BULK_ERASE = 0xC7,
|
||||
DEEP_POWER_DOWN = 0xB9,
|
||||
DEEP_POWER_DOWN_RELEASE = 0xAB,
|
||||
};
|
||||
|
||||
enum {
|
||||
STATUS_SRWD = 0x80, // 0b 1000 0000
|
||||
STATUS_BP2 = 0x10, // 0b 0001 0000
|
||||
STATUS_BP1 = 0x08, // 0b 0000 1000
|
||||
STATUS_BP0 = 0x04, // 0b 0000 0100
|
||||
STATUS_WEL = 0x02, // 0b 0000 0010
|
||||
STATUS_WIP = 0x01, // 0b 0000 0001
|
||||
};
|
||||
|
||||
|
||||
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);
|
||||
static void spif_wait_for_write(struct spi_flash_if *spif);
|
||||
|
||||
#define low_byte(x) (x&0xff)
|
||||
#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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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->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_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);
|
||||
}
|
||||
|
||||
ssize_t spif_read(struct spi_flash_if *spif, size_t addr, size_t len, char* data) {
|
||||
spif_enable_write(spif);
|
||||
|
||||
spif->cs(0);
|
||||
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_read(spif);
|
||||
|
||||
spif->cs(1);
|
||||
return len;
|
||||
}
|
||||
|
||||
void spif_write(struct spi_flash_if *spif, size_t addr, size_t len, const char* data) {
|
||||
size_t written = 0, write_size = 0;
|
||||
|
||||
while (written < len) {
|
||||
write_size = spif->page_size - ((addr + written) % spif->page_size);
|
||||
if (written + write_size > len)
|
||||
write_size = len - written;
|
||||
|
||||
spif_write_page(spif, addr + written, write_size, data + written);
|
||||
written += write_size;
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t spif_read_status(struct spi_flash_if *spif) {
|
||||
spif->cs(0);
|
||||
spi_write(spif, READ_STATUS);
|
||||
uint8_t status = spi_read(spif);
|
||||
spif->cs(1);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
void spif_clear_sector(struct spi_flash_if *spif, size_t addr) {
|
||||
spif_enable_write(spif);
|
||||
|
||||
spif->cs(0);
|
||||
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);
|
||||
}
|
||||
|
||||
void spif_clear_mem(struct spi_flash_if *spif) {
|
||||
spif_enable_write(spif);
|
||||
|
||||
spif->cs(0);
|
||||
spi_write(spif, BULK_ERASE);
|
||||
spif->cs(1);
|
||||
|
||||
spif_wait_for_write(spif);
|
||||
}
|
||||
|
||||
static void spif_write_page(struct spi_flash_if *spif, size_t addr, size_t len, const char* data) {
|
||||
spif_enable_write(spif);
|
||||
|
||||
spif->cs(0);
|
||||
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_write(spif, data[i]);
|
||||
}
|
||||
|
||||
spif->cs(1);
|
||||
spif_wait_for_write(spif);
|
||||
}
|
||||
|
||||
static void spif_enable_write(struct spi_flash_if *spif) {
|
||||
spif->cs(0);
|
||||
spi_write(spif, WRITE_ENABLE);
|
||||
spif->cs(1);
|
||||
}
|
||||
|
||||
static void spif_wait_for_write(struct spi_flash_if *spif) {
|
||||
while (spif_read_status(spif) & STATUS_WIP)
|
||||
for (int i = 0; i < 800; i++)
|
||||
;
|
||||
}
|
||||
|
||||
void spif_deep_power_down(struct spi_flash_if *spif) {
|
||||
spif->cs(0);
|
||||
spi_write(spif, DEEP_POWER_DOWN);
|
||||
spif->cs(1);
|
||||
}
|
||||
|
||||
void spif_wakeup(struct spi_flash_if *spif) {
|
||||
spif->cs(0);
|
||||
spi_write(spif, DEEP_POWER_DOWN_RELEASE);
|
||||
spif->cs(1);
|
||||
}
|
||||
|
||||
33
reset-controller/fw/src/spi_flash.h
Normal file
33
reset-controller/fw/src/spi_flash.h
Normal file
|
|
@ -0,0 +1,33 @@
|
|||
#ifndef __SPI_FLASH_H__
|
||||
#define __SPI_FLASH_H__
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <stm32f407xx.h>
|
||||
|
||||
struct spi_mem_id {
|
||||
size_t size;
|
||||
uint8_t mfg_id;
|
||||
uint8_t type;
|
||||
};
|
||||
|
||||
struct spi_flash_if {
|
||||
struct spi_mem_id id;
|
||||
volatile SPI_TypeDef *spi;
|
||||
size_t page_size;
|
||||
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);
|
||||
ssize_t spif_read(struct spi_flash_if *spif, size_t addr, size_t len, char* data);
|
||||
|
||||
void spif_clear_mem(struct spi_flash_if *spif);
|
||||
void spif_clear_sector(struct spi_flash_if *spif, size_t addr);
|
||||
|
||||
void spif_deep_power_down(struct spi_flash_if *spif);
|
||||
void spif_wakeup(struct spi_flash_if *spif);
|
||||
|
||||
#endif /* __SPI_FLASH_H__ */
|
||||
35
reset-controller/fw/src/sr_global.h
Normal file
35
reset-controller/fw/src/sr_global.h
Normal file
|
|
@ -0,0 +1,35 @@
|
|||
#ifndef __SR_GLOBAL_H__
|
||||
#define __SR_GLOBAL_H__
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#ifndef SIMULATION
|
||||
#include <stm32f407xx.h>
|
||||
#include <stm32f4_isr.h>
|
||||
#endif
|
||||
|
||||
#define UNUSED(x) ((void) x)
|
||||
#define ARRAY_LENGTH(x) (sizeof(x) / sizeof(x[0]))
|
||||
|
||||
#define unused_a __attribute__((unused))
|
||||
|
||||
extern unsigned int sysclk_speed;
|
||||
extern unsigned int apb1_speed;
|
||||
extern unsigned int apb2_speed;
|
||||
extern unsigned int auxclk_speed;
|
||||
extern unsigned int apb1_timer_speed;
|
||||
extern unsigned int apb2_timer_speed;
|
||||
|
||||
extern struct leds {
|
||||
unsigned int comm_tx;
|
||||
} leds;
|
||||
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__ */
|
||||
521
reset-controller/fw/src/startup_stm32f407xx.s
Normal file
521
reset-controller/fw/src/startup_stm32f407xx.s
Normal file
|
|
@ -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****/
|
||||
98
reset-controller/fw/src/stm32f4_isr.h
Normal file
98
reset-controller/fw/src/stm32f4_isr.h
Normal file
|
|
@ -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__ */
|
||||
742
reset-controller/fw/src/system_stm32f4xx.c
Normal file
742
reset-controller/fw/src/system_stm32f4xx.c
Normal file
|
|
@ -0,0 +1,742 @@
|
|||
/**
|
||||
******************************************************************************
|
||||
* @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)
|
||||
{
|
||||
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
|
||||
__DSB();
|
||||
__ISB();
|
||||
|
||||
#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****/
|
||||
168
reset-controller/fw/src/test_decoder.py
Normal file
168
reset-controller/fw/src/test_decoder.py
Normal file
|
|
@ -0,0 +1,168 @@
|
|||
"""Decoding module."""
|
||||
import numpy as np
|
||||
import warnings
|
||||
import test_pyldpc_utils as utils
|
||||
|
||||
from numba import njit, int64, types, float64
|
||||
|
||||
np.set_printoptions(linewidth=180, threshold=1000, edgeitems=20)
|
||||
|
||||
def decode(H, y, snr, maxiter=100):
|
||||
"""Decode a Gaussian noise corrupted n bits message using BP algorithm.
|
||||
|
||||
Decoding is performed in parallel if multiple codewords are passed in y.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
H: array (n_equations, n_code). Decoding matrix H.
|
||||
y: array (n_code, n_messages) or (n_code,). Received message(s) in the
|
||||
codeword space.
|
||||
maxiter: int. Maximum number of iterations of the BP algorithm.
|
||||
|
||||
Returns
|
||||
-------
|
||||
x: array (n_code,) or (n_code, n_messages) the solutions in the
|
||||
codeword space.
|
||||
|
||||
"""
|
||||
m, n = H.shape
|
||||
|
||||
bits_hist, bits_values, nodes_hist, nodes_values = utils.bitsandnodes(H)
|
||||
|
||||
var = 10 ** (-snr / 10)
|
||||
|
||||
if y.ndim == 1:
|
||||
y = y[:, None]
|
||||
# step 0: initialization
|
||||
|
||||
Lc = 2 * y / var
|
||||
_, n_messages = y.shape
|
||||
|
||||
Lq = np.zeros(shape=(m, n, n_messages))
|
||||
|
||||
Lr = np.zeros(shape=(m, n, n_messages))
|
||||
|
||||
for n_iter in range(maxiter):
|
||||
#print(f'============================ iteration {n_iter} ============================')
|
||||
Lq, Lr, L_posteriori = _logbp_numba(bits_hist, bits_values, nodes_hist,
|
||||
nodes_values, Lc, Lq, Lr, n_iter)
|
||||
#print("Lq=", Lq.flatten())
|
||||
#print("Lr=", Lr.flatten())
|
||||
#print("L_posteriori=", L_posteriori.flatten())
|
||||
#print('L_posteriori=[')
|
||||
#for row in L_posteriori.reshape([-1, 16]):
|
||||
# for val in row:
|
||||
# cc = '\033[91m' if val < 0 else ('\033[92m' if val > 0 else '\033[94m')
|
||||
# print(f"{cc}{val: 012.6g}\033[38;5;240m", end=', ')
|
||||
# print()
|
||||
x = np.array(L_posteriori <= 0).astype(int)
|
||||
|
||||
product = utils.incode(H, x)
|
||||
|
||||
if product:
|
||||
print(f'found, n_iter={n_iter}')
|
||||
break
|
||||
|
||||
if n_iter == maxiter - 1:
|
||||
warnings.warn("""Decoding stopped before convergence. You may want
|
||||
to increase maxiter""")
|
||||
return x.squeeze()
|
||||
|
||||
|
||||
output_type_log2 = types.Tuple((float64[:, :, :], float64[:, :, :],
|
||||
float64[:, :]))
|
||||
|
||||
|
||||
#@njit(output_type_log2(int64[:], int64[:], int64[:], int64[:], float64[:, :],
|
||||
# float64[:, :, :], float64[:, :, :], int64), cache=True)
|
||||
def _logbp_numba(bits_hist, bits_values, nodes_hist, nodes_values, Lc, Lq, Lr,
|
||||
n_iter):
|
||||
"""Perform inner ext LogBP solver."""
|
||||
m, n, n_messages = Lr.shape
|
||||
# step 1 : Horizontal
|
||||
|
||||
bits_counter = 0
|
||||
nodes_counter = 0
|
||||
for i in range(m):
|
||||
#print(f'=== i={i}')
|
||||
ff = bits_hist[i]
|
||||
ni = bits_values[bits_counter: bits_counter + ff]
|
||||
bits_counter += ff
|
||||
for j_iter, j in enumerate(ni):
|
||||
nij = ni[:]
|
||||
#print(f'\033[38;5;240mj={j:04d}', end=' ')
|
||||
|
||||
X = np.ones(n_messages)
|
||||
if n_iter == 0:
|
||||
for kk in range(len(nij)):
|
||||
if nij[kk] != j:
|
||||
lcv = Lc[nij[kk],0]
|
||||
lcc = '\033[91m' if lcv < 0 else ('\033[92m' if lcv > 0 else '\033[94m')
|
||||
#print(f'nij={nij[kk]:04d} Lc={lcc}{lcv:> 8f}\033[38;5;240m', end=' ')
|
||||
X *= np.tanh(0.5 * Lc[nij[kk]])
|
||||
else:
|
||||
for kk in range(len(nij)):
|
||||
if nij[kk] != j:
|
||||
X *= np.tanh(0.5 * Lq[i, nij[kk]])
|
||||
#print(f'\n==== {i:03d} {j_iter:01d} {X[0]:> 8f}')
|
||||
num = 1 + X
|
||||
denom = 1 - X
|
||||
for ll in range(n_messages):
|
||||
if num[ll] == 0:
|
||||
Lr[i, j, ll] = -1
|
||||
elif denom[ll] == 0:
|
||||
Lr[i, j, ll] = 1
|
||||
else:
|
||||
Lr[i, j, ll] = np.log(num[ll] / denom[ll])
|
||||
# step 2 : Vertical
|
||||
|
||||
for j in range(n):
|
||||
ff = nodes_hist[j]
|
||||
mj = nodes_values[bits_counter: nodes_counter + ff]
|
||||
nodes_counter += ff
|
||||
for i in mj:
|
||||
mji = mj[:]
|
||||
Lq[i, j] = Lc[j]
|
||||
|
||||
for kk in range(len(mji)):
|
||||
if mji[kk] != i:
|
||||
Lq[i, j] += Lr[mji[kk], j]
|
||||
|
||||
# LLR a posteriori:
|
||||
L_posteriori = np.zeros((n, n_messages))
|
||||
nodes_counter = 0
|
||||
for j in range(n):
|
||||
ff = nodes_hist[j]
|
||||
mj = nodes_values[bits_counter: nodes_counter + ff]
|
||||
nodes_counter += ff
|
||||
L_posteriori[j] = Lc[j] + Lr[mj, j].sum(axis=0)
|
||||
|
||||
return Lq, Lr, L_posteriori
|
||||
|
||||
|
||||
def get_message(tG, x):
|
||||
"""Compute the original `n_bits` message from a `n_code` codeword `x`.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
tG: array (n_code, n_bits) coding matrix tG.
|
||||
x: array (n_code,) decoded codeword of length `n_code`.
|
||||
|
||||
Returns
|
||||
-------
|
||||
message: array (n_bits,). Original binary message.
|
||||
|
||||
"""
|
||||
n, k = tG.shape
|
||||
|
||||
rtG, rx = utils.gausselimination(tG, x)
|
||||
|
||||
message = np.zeros(k).astype(int)
|
||||
|
||||
message[k - 1] = rx[k - 1]
|
||||
for i in reversed(range(k - 1)):
|
||||
message[i] = rx[i]
|
||||
message[i] -= utils.binaryproduct(rtG[i, list(range(i+1, k))],
|
||||
message[list(range(i+1, k))])
|
||||
|
||||
return abs(message)
|
||||
182
reset-controller/fw/src/test_pyldpc_utils.py
Normal file
182
reset-controller/fw/src/test_pyldpc_utils.py
Normal file
|
|
@ -0,0 +1,182 @@
|
|||
"""Conversion tools."""
|
||||
import math
|
||||
import numbers
|
||||
import numpy as np
|
||||
import scipy
|
||||
from scipy.stats import norm
|
||||
pi = math.pi
|
||||
|
||||
|
||||
def int2bitarray(n, k):
|
||||
"""Change an array's base from int (base 10) to binary (base 2)."""
|
||||
binary_string = bin(n)
|
||||
length = len(binary_string)
|
||||
bitarray = np.zeros(k, 'int')
|
||||
for i in range(length - 2):
|
||||
bitarray[k - i - 1] = int(binary_string[length - i - 1])
|
||||
|
||||
return bitarray
|
||||
|
||||
|
||||
def bitarray2int(bitarray):
|
||||
"""Change array's base from binary (base 2) to int (base 10)."""
|
||||
bitstring = "".join([str(i) for i in bitarray])
|
||||
|
||||
return int(bitstring, 2)
|
||||
|
||||
|
||||
def binaryproduct(X, Y):
|
||||
"""Compute a matrix-matrix / vector product in Z/2Z."""
|
||||
A = X.dot(Y)
|
||||
try:
|
||||
A = A.toarray()
|
||||
except AttributeError:
|
||||
pass
|
||||
return A % 2
|
||||
|
||||
|
||||
def gaussjordan(X, change=0):
|
||||
"""Compute the binary row reduced echelon form of X.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
X: array (m, n)
|
||||
change : boolean (default, False). If True returns the inverse transform
|
||||
|
||||
Returns
|
||||
-------
|
||||
if `change` == 'True':
|
||||
A: array (m, n). row reduced form of X.
|
||||
P: tranformations applied to the identity
|
||||
else:
|
||||
A: array (m, n). row reduced form of X.
|
||||
|
||||
"""
|
||||
A = np.copy(X)
|
||||
m, n = A.shape
|
||||
|
||||
if change:
|
||||
P = np.identity(m).astype(int)
|
||||
|
||||
pivot_old = -1
|
||||
for j in range(n):
|
||||
filtre_down = A[pivot_old+1:m, j]
|
||||
pivot = np.argmax(filtre_down)+pivot_old+1
|
||||
|
||||
if A[pivot, j]:
|
||||
pivot_old += 1
|
||||
if pivot_old != pivot:
|
||||
aux = np.copy(A[pivot, :])
|
||||
A[pivot, :] = A[pivot_old, :]
|
||||
A[pivot_old, :] = aux
|
||||
if change:
|
||||
aux = np.copy(P[pivot, :])
|
||||
P[pivot, :] = P[pivot_old, :]
|
||||
P[pivot_old, :] = aux
|
||||
|
||||
for i in range(m):
|
||||
if i != pivot_old and A[i, j]:
|
||||
if change:
|
||||
P[i, :] = abs(P[i, :]-P[pivot_old, :])
|
||||
A[i, :] = abs(A[i, :]-A[pivot_old, :])
|
||||
|
||||
if pivot_old == m-1:
|
||||
break
|
||||
|
||||
if change:
|
||||
return A, P
|
||||
return A
|
||||
|
||||
|
||||
def binaryrank(X):
|
||||
"""Compute rank of a binary Matrix using Gauss-Jordan algorithm."""
|
||||
A = np.copy(X)
|
||||
m, n = A.shape
|
||||
|
||||
A = gaussjordan(A)
|
||||
|
||||
return sum([a.any() for a in A])
|
||||
|
||||
|
||||
def f1(y, sigma):
|
||||
"""Compute normal density N(1,sigma)."""
|
||||
f = norm.pdf(y, loc=1, scale=sigma)
|
||||
return f
|
||||
|
||||
|
||||
def fm1(y, sigma):
|
||||
"""Compute normal density N(-1,sigma)."""
|
||||
|
||||
f = norm.pdf(y, loc=-1, scale=sigma)
|
||||
return f
|
||||
|
||||
|
||||
def bitsandnodes(H):
|
||||
"""Return bits and nodes of a parity-check matrix H."""
|
||||
if type(H) != scipy.sparse.csr_matrix:
|
||||
bits_indices, bits = np.where(H)
|
||||
nodes_indices, nodes = np.where(H.T)
|
||||
else:
|
||||
bits_indices, bits = scipy.sparse.find(H)[:2]
|
||||
nodes_indices, nodes = scipy.sparse.find(H.T)[:2]
|
||||
bits_histogram = np.bincount(bits_indices)
|
||||
nodes_histogram = np.bincount(nodes_indices)
|
||||
|
||||
return bits_histogram, bits, nodes_histogram, nodes
|
||||
|
||||
|
||||
def incode(H, x):
|
||||
"""Compute Binary Product of H and x."""
|
||||
return (binaryproduct(H, x) == 0).all()
|
||||
|
||||
|
||||
def gausselimination(A, b):
|
||||
"""Solve linear system in Z/2Z via Gauss Gauss elimination."""
|
||||
if type(A) == scipy.sparse.csr_matrix:
|
||||
A = A.toarray().copy()
|
||||
else:
|
||||
A = A.copy()
|
||||
b = b.copy()
|
||||
n, k = A.shape
|
||||
|
||||
for j in range(min(k, n)):
|
||||
listedepivots = [i for i in range(j, n) if A[i, j]]
|
||||
if len(listedepivots):
|
||||
pivot = np.min(listedepivots)
|
||||
else:
|
||||
continue
|
||||
if pivot != j:
|
||||
aux = (A[j, :]).copy()
|
||||
A[j, :] = A[pivot, :]
|
||||
A[pivot, :] = aux
|
||||
|
||||
aux = b[j].copy()
|
||||
b[j] = b[pivot]
|
||||
b[pivot] = aux
|
||||
|
||||
for i in range(j+1, n):
|
||||
if A[i, j]:
|
||||
A[i, :] = abs(A[i, :]-A[j, :])
|
||||
b[i] = abs(b[i]-b[j])
|
||||
|
||||
return A, b
|
||||
|
||||
|
||||
def check_random_state(seed):
|
||||
"""Turn seed into a np.random.RandomState instance
|
||||
Parameters
|
||||
----------
|
||||
seed : None | int | instance of RandomState
|
||||
If seed is None, return the RandomState singleton used by np.random.
|
||||
If seed is an int, return a new RandomState instance seeded with seed.
|
||||
If seed is already a RandomState instance, return it.
|
||||
Otherwise raise ValueError.
|
||||
"""
|
||||
if seed is None or seed is np.random:
|
||||
return np.random.mtrand._rand
|
||||
if isinstance(seed, numbers.Integral):
|
||||
return np.random.RandomState(seed)
|
||||
if isinstance(seed, np.random.RandomState):
|
||||
return seed
|
||||
raise ValueError('%r cannot be used to seed a numpy.random.RandomState'
|
||||
' instance' % seed)
|
||||
13
reset-controller/fw/src/tinyaes_adaptor.c
Normal file
13
reset-controller/fw/src/tinyaes_adaptor.c
Normal file
|
|
@ -0,0 +1,13 @@
|
|||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
void *memcpy(void *restrict dest, const void *restrict src, size_t n) {
|
||||
unsigned char *d = dest;
|
||||
const unsigned char *s = src;
|
||||
|
||||
while (n--)
|
||||
*d++ = *s++;
|
||||
|
||||
return dest;
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue