diff options
-rw-r--r-- | .gitignore | 1 | ||||
-rw-r--r-- | SConscript | 7 | ||||
-rw-r--r-- | build_rules | 80 | ||||
-rw-r--r-- | gpio/gpio.h | 43 | ||||
-rw-r--r-- | gpio/pin.h | 137 | ||||
-rw-r--r-- | i2c/i2c.cpp | 145 | ||||
-rw-r--r-- | i2c/i2c.h | 62 | ||||
-rw-r--r-- | interrupt/fault.cpp | 41 | ||||
-rw-r--r-- | interrupt/interrupt.cpp | 187 | ||||
-rw-r--r-- | interrupt/interrupt.h | 151 | ||||
-rw-r--r-- | ld_scripts/arm_flash_ram.ld | 78 | ||||
-rw-r--r-- | ld_scripts/stm32_f1_8.ld | 6 | ||||
-rw-r--r-- | ld_scripts/stm32_f1_b.ld | 6 | ||||
-rw-r--r-- | ld_scripts/stm32_f4_e.ld | 7 | ||||
-rw-r--r-- | ld_scripts/stm32_f4_g.ld | 7 | ||||
-rw-r--r-- | os/mutex.h | 42 | ||||
-rw-r--r-- | os/pool.cpp | 5 | ||||
-rw-r--r-- | os/pool.h | 146 | ||||
-rw-r--r-- | os/thread.cpp | 4 | ||||
-rw-r--r-- | os/thread.h | 63 | ||||
-rw-r--r-- | os/time.cpp | 3 | ||||
-rw-r--r-- | os/time.h | 32 | ||||
-rw-r--r-- | rcc/flash.cpp | 17 | ||||
-rw-r--r-- | rcc/flash.h | 30 | ||||
-rw-r--r-- | rcc/rcc.cpp | 51 | ||||
-rw-r--r-- | rcc/rcc.h | 217 | ||||
-rw-r--r-- | spi/spi.h | 31 | ||||
-rw-r--r-- | startup/entry.cpp | 63 | ||||
-rw-r--r-- | timer/timer.h | 40 | ||||
-rw-r--r-- | usart/usart.cpp | 7 | ||||
-rw-r--r-- | usart/usart.h | 68 | ||||
-rw-r--r-- | usb/usb.h | 93 |
32 files changed, 1870 insertions, 0 deletions
diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5761abc --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +*.o diff --git a/SConscript b/SConscript new file mode 100644 index 0000000..5d706af --- /dev/null +++ b/SConscript @@ -0,0 +1,7 @@ +# Don't call this file explicitly, it is called implicitly by the build rules. + +Import('env') + +env.Append( + LIB_SOURCES = Glob('*.cpp') + Glob('*/*.cpp'), +) diff --git a/build_rules b/build_rules new file mode 100644 index 0000000..0040cd7 --- /dev/null +++ b/build_rules @@ -0,0 +1,80 @@ +laks_dir = Dir('.') +ld_dir = Dir('ld_scripts') +main_sconscript = File('SConscript') + +def select_arm(env, family): + env.SetDefault( + TOOLCHAIN = 'arm-none-eabi-', + ) + + env.Replace( + CC = '${TOOLCHAIN}gcc', + CXX = '${TOOLCHAIN}g++', + AS = '${TOOLCHAIN}gcc', + LINK = '${TOOLCHAIN}gcc', + AR = '${TOOLCHAIN}ar', + RANLIB = '${TOOLCHAIN}ranlib', + + CCFLAGS = '-O2 -Wall -ggdb -mcpu=${CPU_FAMILY} -mthumb -ffunction-sections', + CXXFLAGS = '-fno-exceptions -fno-rtti -Wno-pmf-conversions', + ASFLAGS = '-c -x assembler-with-cpp -mcpu=${CPU_FAMILY} -mthumb', + LINKFLAGS = '-Wall -mcpu=${CPU_FAMILY} -mthumb -mhard-float -nostartfiles -Wl,-T${LINK_SCRIPT}', # -Wl,--gc-sections + + CPPPATH = [laks_dir], + LIBPATH = [ld_dir], + + LIB_SOURCES = [], + + CPU_FAMILY = family, + ) + + if family == 'cortex-m4': + env.Append(CCFLAGS = ' -mhard-float') + +def select_stm32(env, variant): + family = variant[5:9] + pin_count = variant[9] + flash = variant[10] + + if family == 'f103': + select_arm(env, 'cortex-m3') + env.Append(CPPDEFINES = ['STM32F1']) + + env['LINK_SCRIPT'] = { + '8': 'stm32_f1_8.ld', + 'b': 'stm32_f1_b.ld', + }[flash] + + elif family in ('f405', 'f407'): + select_arm(env, 'cortex-m4') + env.Append(CPPDEFINES = ['STM32F4']) + + env['LINK_SCRIPT'] = { + 'e': 'stm32_f4_e.ld', + 'g': 'stm32_f4_g.ld', + }[flash] + + else: + print 'Unknown stm32 family: %s' % mcu + Exit(1) + +def SelectMCU(env, mcu): + mcu = mcu.lower() + + if mcu.startswith('stm32'): + select_stm32(env, mcu) + + else: + print 'Unknown MCU: %s' % mcu + Exit(1) + + SConscript(main_sconscript, exports = 'env') + +AddMethod(Environment, SelectMCU) + +def Firmware(env, target, sources): + firmware = env.Program(target, [sources, env['LIB_SOURCES']]) + #env.Depends(firmware, env['LINK_SCRIPT']) # TODO + return firmware + +AddMethod(Environment, Firmware) diff --git a/gpio/gpio.h b/gpio/gpio.h new file mode 100644 index 0000000..324950e --- /dev/null +++ b/gpio/gpio.h @@ -0,0 +1,43 @@ +#ifndef GPIO_H +#define GPIO_H + +struct GPIO_t { + #if defined(STM32F1) + volatile uint32_t CRL; + volatile uint32_t CRH; + volatile uint32_t IDR; + volatile uint32_t ODR; + volatile uint32_t BSRR; + volatile uint32_t BRR; + volatile uint32_t LCKR; + #elif defined(STM32F4) + volatile uint32_t MODER; + volatile uint32_t OTYPER; + volatile uint32_t OSPEEDER; + volatile uint32_t PUPDR; + volatile uint32_t IDR; + volatile uint32_t ODR; + volatile uint32_t BSRR; + volatile uint32_t LCKR; + volatile uint32_t AFRL; + volatile uint32_t AFRH; + #endif +}; + +#if defined(STM32F1) +static GPIO_t& GPIOA = *(GPIO_t*)0x40010800; +static GPIO_t& GPIOB = *(GPIO_t*)0x40010c00; +static GPIO_t& GPIOC = *(GPIO_t*)0x40011000; +#elif defined(STM32F4) +static GPIO_t& GPIOA = *(GPIO_t*)0x40020000; +static GPIO_t& GPIOB = *(GPIO_t*)0x40020400; +static GPIO_t& GPIOC = *(GPIO_t*)0x40020800; +static GPIO_t& GPIOD = *(GPIO_t*)0x40020c00; +static GPIO_t& GPIOE = *(GPIO_t*)0x40021000; +static GPIO_t& GPIOF = *(GPIO_t*)0x40021400; +static GPIO_t& GPIOG = *(GPIO_t*)0x40021800; +static GPIO_t& GPIOH = *(GPIO_t*)0x40021c00; +static GPIO_t& GPIOI = *(GPIO_t*)0x40022000; +#endif + +#endif diff --git a/gpio/pin.h b/gpio/pin.h new file mode 100644 index 0000000..1ad3ca2 --- /dev/null +++ b/gpio/pin.h @@ -0,0 +1,137 @@ +#ifndef PIN_H +#define PIN_H + +#include "gpio.h" + +class Pin { + private: + GPIO_t& g; + int n; + + public: + Pin(GPIO_t& gpio, int pin) : g(gpio), n(pin) {} + + enum Mode { + Input, + Output, + AF, + Analog, + }; + + enum Type { + PushPull, + OpenDrain, + }; + + enum Pull { + PullNone, + PullUp, + PullDown, + }; + + void set_mode(Mode m) { + g.MODER = (g.MODER & ~(3 << (n * 2))) | m << (n * 2); + } + + void set_type(Type t) { + if(t) { + g.OTYPER |= 1 << n; + } else { + g.OTYPER &= ~(1 << n); + } + } + + void set_pull(Pull p) { + g.PUPDR = (g.PUPDR & ~(3 << (n * 2))) | p << (n * 2); + } + + void set_af(int af) { + if(n < 8) { + g.AFRL = (g.AFRL & ~(0xf << (n * 4))) | af << (n * 4); + } else { + g.AFRH = (g.AFRH & ~(0xf << (n * 4 - 32))) | af << (n * 4 - 32); + } + } + + void on() { + g.BSRR = 1 << n; + } + + void off() { + g.BSRR = 1 << 16 << n; + } + + void set(bool value) { + if(value) { + on(); + } else { + off(); + } + } + + bool get() { + return g.IDR & (1 << n); + } + + void toggle() { + set(!(g.ODR & (1 << n))); + } +}; + +static Pin PA0(GPIOA, 0); +static Pin PA1(GPIOA, 1); +static Pin PA2(GPIOA, 2); +static Pin PA3(GPIOA, 3); +static Pin PA4(GPIOA, 4); +static Pin PA5(GPIOA, 5); +static Pin PA6(GPIOA, 6); +static Pin PA7(GPIOA, 7); +static Pin PA8(GPIOA, 8); +static Pin PA9(GPIOA, 9); +static Pin PA10(GPIOA, 10); +static Pin PA11(GPIOA, 11); +static Pin PA12(GPIOA, 12); +static Pin PA13(GPIOA, 13); +static Pin PA14(GPIOA, 14); +static Pin PA15(GPIOA, 15); + +static Pin PB0(GPIOB, 0); +static Pin PB1(GPIOB, 1); +static Pin PB2(GPIOB, 2); +static Pin PB3(GPIOB, 3); +static Pin PB4(GPIOB, 4); +static Pin PB5(GPIOB, 5); +static Pin PB6(GPIOB, 6); +static Pin PB7(GPIOB, 7); +static Pin PB8(GPIOB, 8); +static Pin PB9(GPIOB, 9); +static Pin PB10(GPIOB, 10); +static Pin PB11(GPIOB, 11); +static Pin PB12(GPIOB, 12); +static Pin PB13(GPIOB, 13); +static Pin PB14(GPIOB, 14); +static Pin PB15(GPIOB, 15); + +static Pin PC0(GPIOC, 0); +static Pin PC1(GPIOC, 1); +static Pin PC2(GPIOC, 2); +static Pin PC3(GPIOC, 3); +static Pin PC4(GPIOC, 4); +static Pin PC5(GPIOC, 5); +static Pin PC6(GPIOC, 6); +static Pin PC7(GPIOC, 7); +static Pin PC8(GPIOC, 8); +static Pin PC9(GPIOC, 9); +static Pin PC10(GPIOC, 10); +static Pin PC11(GPIOC, 11); +static Pin PC12(GPIOC, 12); +static Pin PC13(GPIOC, 13); +static Pin PC14(GPIOC, 14); +static Pin PC15(GPIOC, 15); + +static Pin PD12(GPIOD, 12); +static Pin PD13(GPIOD, 13); +static Pin PD14(GPIOD, 14); +static Pin PD15(GPIOD, 15); + +#endif diff --git a/i2c/i2c.cpp b/i2c/i2c.cpp new file mode 100644 index 0000000..a395b13 --- /dev/null +++ b/i2c/i2c.cpp @@ -0,0 +1,145 @@ +#include "i2c.h" + +#include <rcc/rcc.h> +#include <os/thread.h> + +#if defined(STM32F1) +I2C_t I2C1(0x40005400, 36000000, Interrupt::I2C1_EV, Interrupt::I2C1_ER); +I2C_t I2C2(0x40005800, 36000000, Interrupt::I2C2_EV, Interrupt::I2C2_ER); +#elif defined(STM32F4) +I2C_t I2C1(0x40005400, 42000000, Interrupt::I2C1_EV, Interrupt::I2C1_ER); +I2C_t I2C2(0x40005800, 42000000, Interrupt::I2C2_EV, Interrupt::I2C2_ER); +//I2C_t I2C3(0x40005c00, 42000000, Interrupt::I2C3_EV, Interrupt::I2C3_ER); +#endif + +void I2C_t::irq_ev() { + uint32_t sr1 = reg.SR1; + reg.SR2; + + // EV5, SB = 1: Start condition sent. + if(sr1 & 0x01) { + // Send address. + reg.DR = (addr << 1) | (writing ? 0 : 1); + } + + // EV6, ADDR = 1: Address sent. + if(sr1 & 0x02) { + if(writing) { + reg.DR = *write_p++; + writing--; + } else { + if(reading > 1) { + reg.CR1 |= 0x400; // Set ACK. + } else { + reg.CR1 |= 0x200; // Set STOP. + } + } + } + + // EV7, RxNE = 1: Receive buffer not empty. + if(sr1 & 0x40) { + *read_p++ = reg.DR; + reading--; + + if(reading == 1) { + // Unset ACK, set STOP. + reg.CR1 = (reg.CR1 & ~0x400) | 0x200; + } + + if(reading == 0) { + busy = 0; + } + } + + //reg.CR1 &= ~0x400; + + // EV8, TxE = 1, BTF = 0: Transmit buffer empty, still writing. + if(sr1 & 0x80 && !(sr1 & 0x04)) { + if(writing) { + // Send data. + reg.DR = *write_p++; + writing--; + } else { + // All data sent. + + if(reading) { + // Send repeat start. + reg.CR1 |= 0x100; + } else { + // Send stop. + reg.CR1 |= 0x200; + busy = 0; + } + } + } +} + +void I2C_t::irq_er() { + handle_error(); +} + +void I2C_t::handle_error() { + reg.SR1; + reg.SR2; + + //while(1); + + reg.SR1 = 0; + + reg.CR1 |= 0x200; + busy = 0; +} + +void I2C_t::enable(Pin& scl, Pin& sda) { + RCC.enable(RCC.I2C1); + asm volatile("nop"); + + scl.set_af(4); + sda.set_af(4); + scl.set_type(Pin::OpenDrain); + sda.set_type(Pin::OpenDrain); + scl.set_mode(Pin::AF); + sda.set_mode(Pin::AF); + + reg.CR1 = 0x8000; + reg.CR1 = 0; + + reg.CR2 = 0x700 | (clk / 1000000); + reg.TRISE = clk / 1000000 + 1; + reg.CCR = clk / 2 / 100000; + + Interrupt::enable(irq_ev_n, &I2C_t::irq_ev, this); + Interrupt::enable(irq_er_n, &I2C_t::irq_er, this); + + reg.CR1 = 1; +} + +void I2C_t::write_reg(uint8_t addr_, uint8_t reg_, uint8_t data) { + addr = addr_; + writing = 2; + reading = 0; + volatile uint8_t buf[] = {reg_, data}; + write_p = buf; + busy = 1; + + reg.CR1 |= 0x100; + + while(busy) { + Thread::yield(); + } +} + +void I2C_t::read_reg(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) { + addr = addr_; + writing = 1; + reading = len; + write_p = ®_; + read_p = buf; + busy = 1; + + reg.CR1 |= 0x100; + + while(busy) { + Thread::yield(); + } +} diff --git a/i2c/i2c.h b/i2c/i2c.h new file mode 100644 index 0000000..f4d6949 --- /dev/null +++ b/i2c/i2c.h @@ -0,0 +1,62 @@ +#ifndef I2C_H +#define I2C_H + +#include <stdint.h> +#include <interrupt/interrupt.h> +#include <gpio/pin.h> + +struct I2C_reg_t { + volatile uint32_t CR1; + volatile uint32_t CR2; + volatile uint32_t OAR1; + volatile uint32_t OAR2; + volatile uint32_t DR; + volatile uint32_t SR1; + volatile uint32_t SR2; + volatile uint32_t CCR; + volatile uint32_t TRISE; +}; + +class I2C_t { + private: + volatile uint8_t addr; + volatile uint8_t writing; + volatile uint8_t reading; + volatile uint8_t* write_p; + volatile uint8_t* read_p; + + volatile bool busy; + + public: + I2C_reg_t& reg; + const uint32_t clk; + Interrupt::IRQ irq_ev_n; + Interrupt::IRQ irq_er_n; + + I2C_t(uint32_t reg_addr, uint32_t bus_clk, Interrupt::IRQ ev_n, Interrupt::IRQ er_n) : reg(*(I2C_reg_t*)reg_addr), clk(bus_clk), irq_ev_n(ev_n), irq_er_n(er_n) { + reading = writing = 0; + } + + void irq_ev(); + void irq_er(); + + void handle_error(); + + void enable(Pin& scl, Pin& sda); + + void write_reg(uint8_t addr_, uint8_t reg_, uint8_t data); + void read_reg(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf); +}; + +#if defined(STM32F1) +extern I2C_t I2C1; +extern I2C_t I2C2; +#elif defined(STM32F4) +extern I2C_t I2C1; +extern I2C_t I2C2; +extern I2C_t I2C3; +#endif + +typedef I2C_t I2C; + +#endif diff --git a/interrupt/fault.cpp b/interrupt/fault.cpp new file mode 100644 index 0000000..016b74b --- /dev/null +++ b/interrupt/fault.cpp @@ -0,0 +1,41 @@ +#include "interrupt.h" +#include <os/thread.h> +#include <os/time.h> + +inline void __attribute__((naked)) switch_context() { + asm volatile ("cpsid i"); + + // Save unsaved registers. + asm volatile ("push {r4, r5, r6, r7, r8, r9, r10, r11, lr}" ::: "memory"); + + // Store stack pointer for old thread. + asm volatile ("str sp, [%0]" :: "r" (&Thread::active_thread->sp)); + + // Update running thread. + Thread::active_thread = Thread::active_thread->next; + + // Fetch stack pointer for new thread. + asm volatile ("ldr sp, [%0]" :: "r" (&Thread::active_thread->sp)); + + asm volatile ("cpsie i"); + + // Load registers and return. + asm volatile ("pop {r4, r5, r6, r7, r8, r9, r10, r11, pc}" ::: "memory"); +} + +template<> +void interrupt<Interrupt::SVCall>() { + switch_context(); +} + +template<> +void interrupt<Interrupt::SysTick>() { + Time::tick(); +} + +template<> void interrupt<Interrupt::NMI>() { while(1); } +template<> void interrupt<Interrupt::HardFault>() { while(1); } +template<> void interrupt<Interrupt::MemManage>() { while(1); } +template<> void interrupt<Interrupt::BusFault>() { while(1); } +template<> void interrupt<Interrupt::UsageFault>() { while(1); } +template<> void interrupt<Interrupt::PendSV>() { while(1); } diff --git a/interrupt/interrupt.cpp b/interrupt/interrupt.cpp new file mode 100644 index 0000000..cebfed7 --- /dev/null +++ b/interrupt/interrupt.cpp @@ -0,0 +1,187 @@ +#include "interrupt.h" + +namespace Interrupt { + MFP mf_vectors[16 + NUM_IRQs]; +}; + +void entry(); + +void member_function_interrupt_gate() { + uint32_t interrupt_num; + asm ("mrs %0, ipsr" : "=r" (interrupt_num)); + + Interrupt::mf_vectors[interrupt_num].func_p(Interrupt::mf_vectors[interrupt_num].instance_p); +} + +extern "C" void unused_interrupt() { + member_function_interrupt_gate(); + //while(1); +} + +template<> void interrupt<Interrupt::NMI>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::HardFault>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::MemManage>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::BusFault>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::UsageFault>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::SVCall>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::PendSV>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::SysTick>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::WWDG>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::PVD>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TAMPER>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::RTC>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::FLASH>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::RCC>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::EXTI0>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::EXTI1>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::EXTI2>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::EXTI3>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::EXTI4>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA1_Channel1>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA1_Channel2>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA1_Channel3>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA1_Channel4>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA1_Channel5>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA1_Channel6>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA1_Channel7>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::ADC1_2>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::USB_HP_CAN_TX>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::USB_LP_CAN_RX0>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::CAN_RX1>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::CAN_SCE>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::EXTI9_5>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM1_BRK>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM1_UP>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM1_TRG_COM>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM1_CC>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM2>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM3>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM4>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::I2C1_EV>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::I2C1_ER>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::I2C2_EV>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::I2C2_ER>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::SPI1>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::SPI2>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::USART1>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::USART2>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::USART3>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::EXTI15_10>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::RTCAlarm>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::USBWakeup>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM8_BRK>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM8_UP>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM8_TRG_COM>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM8_CC>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::ADC3>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::FSMC>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::SDIO>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM5>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::SPI3>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::UART4>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::UART5>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM6>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::TIM7>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA2_Channel1>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA2_Channel2>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA2_Channel3>() __attribute__ ((weak, alias ("unused_interrupt"))); +template<> void interrupt<Interrupt::DMA2_Channel4_5>() __attribute__ ((weak, alias ("unused_interrupt"))); + +typedef void (*vector_t)(); + +vector_t vectors[] __attribute__((section(".vectors"))) = { + (vector_t)0x20004ffc, + entry, + interrupt<Interrupt::NMI>, + interrupt<Interrupt::HardFault>, + interrupt<Interrupt::MemManage>, + interrupt<Interrupt::BusFault>, + interrupt<Interrupt::UsageFault>, + 0, + 0, + 0, + 0, + interrupt<Interrupt::SVCall>, + 0, + 0, + interrupt<Interrupt::PendSV>, + interrupt<Interrupt::SysTick>, + interrupt<Interrupt::WWDG>, + interrupt<Interrupt::PVD>, + interrupt<Interrupt::TAMPER>, + interrupt<Interrupt::RTC>, + interrupt<Interrupt::FLASH>, + interrupt<Interrupt::RCC>, + interrupt<Interrupt::EXTI0>, + interrupt<Interrupt::EXTI1>, + interrupt<Interrupt::EXTI2>, + interrupt<Interrupt::EXTI3>, + interrupt<Interrupt::EXTI4>, + interrupt<Interrupt::DMA1_Channel1>, + interrupt<Interrupt::DMA1_Channel2>, + interrupt<Interrupt::DMA1_Channel3>, + interrupt<Interrupt::DMA1_Channel4>, + interrupt<Interrupt::DMA1_Channel5>, + interrupt<Interrupt::DMA1_Channel6>, + interrupt<Interrupt::DMA1_Channel7>, + interrupt<Interrupt::ADC1_2>, + interrupt<Interrupt::USB_HP_CAN_TX>, + interrupt<Interrupt::USB_LP_CAN_RX0>, + interrupt<Interrupt::CAN_RX1>, + interrupt<Interrupt::CAN_SCE>, + interrupt<Interrupt::EXTI9_5>, + interrupt<Interrupt::TIM1_BRK>, + interrupt<Interrupt::TIM1_UP>, + interrupt<Interrupt::TIM1_TRG_COM>, + interrupt<Interrupt::TIM1_CC>, + interrupt<Interrupt::TIM2>, + interrupt<Interrupt::TIM3>, + interrupt<Interrupt::TIM4>, + interrupt<Interrupt::I2C1_EV>, + interrupt<Interrupt::I2C1_ER>, + interrupt<Interrupt::I2C2_EV>, + interrupt<Interrupt::I2C2_ER>, + interrupt<Interrupt::SPI1>, + interrupt<Interrupt::SPI2>, + interrupt<Interrupt::USART1>, + interrupt<Interrupt::USART2>, + interrupt<Interrupt::USART3>, + interrupt<Interrupt::EXTI15_10>, + interrupt<Interrupt::RTCAlarm>, + interrupt<Interrupt::USBWakeup>, + interrupt<Interrupt::TIM8_BRK>, + interrupt<Interrupt::TIM8_UP>, + interrupt<Interrupt::TIM8_TRG_COM>, + interrupt<Interrupt::TIM8_CC>, + interrupt<Interrupt::ADC3>, + interrupt<Interrupt::FSMC>, + interrupt<Interrupt::SDIO>, + interrupt<Interrupt::TIM5>, + interrupt<Interrupt::SPI3>, + interrupt<Interrupt::UART4>, + interrupt<Interrupt::UART5>, + interrupt<Interrupt::TIM6>, + interrupt<Interrupt::TIM7>, + interrupt<Interrupt::DMA2_Channel1>, + interrupt<Interrupt::DMA2_Channel2>, + interrupt<Interrupt::DMA2_Channel3>, + interrupt<Interrupt::DMA2_Channel4_5>, + 0, // 60 + 0, // 61 + 0, // 62 + 0, // 63 + 0, // 64 + 0, // 65 + 0, // 66 + 0, // 67 + 0, // 68 + 0, // 69 + 0, // 70 + 0, // 71 + 0, // 72 + 0, // 73 + 0, // 74 + 0, // 75 + 0, // 76 + interrupt<(Interrupt::IRQ)77>, // 77 +}; diff --git a/interrupt/interrupt.h b/interrupt/interrupt.h new file mode 100644 index 0000000..80ccb76 --- /dev/null +++ b/interrupt/interrupt.h @@ -0,0 +1,151 @@ +#ifndef INTERRUPT_H +#define INTERRUPT_H + +#include <stdint.h> + +struct NVIC_t { + volatile uint32_t ISER[32]; + volatile uint32_t ICER[32]; + volatile uint32_t ISPR[32]; + volatile uint32_t ICPR[32]; + volatile uint32_t IABR[64]; + volatile uint8_t IPR[2816]; + volatile uint32_t STIR; +}; + +static NVIC_t& NVIC = *(NVIC_t*)0xe000e100; + +struct SCB_t { + volatile uint32_t CPUID; + volatile uint32_t ICSR; + volatile uint32_t VTOR; + volatile uint32_t AIRCR; + volatile uint32_t SCR; + volatile uint32_t CCR; + volatile uint8_t SHPR[12]; + volatile uint32_t SHCSR; + volatile uint32_t CFSR; + volatile uint32_t HFSR; + volatile uint32_t DFSR; + volatile uint32_t MMAR; + volatile uint32_t BFAR; +}; + +static SCB_t& SCB = *(SCB_t*)0xe000ed00; + +namespace Interrupt { + enum Exception { + NMI = 2, + HardFault = 3, + MemManage = 4, + BusFault = 5, + UsageFault = 6, + SVCall = 11, + PendSV = 14, + SysTick = 15 + }; + + enum IRQ { + WWDG, + PVD, + TAMPER, + RTC, + FLASH, + RCC, + EXTI0, + EXTI1, + EXTI2, + EXTI3, + EXTI4, + DMA1_Channel1, + DMA1_Channel2, + DMA1_Channel3, + DMA1_Channel4, + DMA1_Channel5, + DMA1_Channel6, + DMA1_Channel7, + ADC1_2, + USB_HP_CAN_TX, + USB_LP_CAN_RX0, + CAN_RX1, + CAN_SCE, + EXTI9_5, + TIM1_BRK, + TIM1_UP, + TIM1_TRG_COM, + TIM1_CC, + TIM2, + TIM3, + TIM4, + I2C1_EV, + I2C1_ER, + I2C2_EV, + I2C2_ER, + SPI1, + SPI2, + USART1, + USART2, + USART3, + EXTI15_10, + RTCAlarm, + USBWakeup, + TIM8_BRK, + TIM8_UP, + TIM8_TRG_COM, + TIM8_CC, + ADC3, + FSMC, + SDIO, + TIM5, + SPI3, + UART4, + UART5, + TIM6, + TIM7, + DMA2_Channel1, + DMA2_Channel2, + DMA2_Channel3, + DMA2_Channel4_5, + NUM_IRQs + }; + + inline void enable(IRQ n) { + NVIC.ISER[n >> 5] = 1 << (n & 0x1f); + } + + inline void set_priority(Exception n, uint8_t priority) { + SCB.SHPR[n - 4] = priority; + } + + inline void set_priority(IRQ n, uint8_t priority) { + NVIC.IPR[n] = priority; + } + + struct MFP { + void (*func_p)(void*); + void* instance_p; + }; + + extern MFP mf_vectors[]; + + template<class T> + inline void set_handler(IRQ n, void (T::*f)(), T* i) { + MFP& mfp = mf_vectors[16 + n]; + mfp.func_p = reinterpret_cast<void (*)(void*)>(f); + mfp.instance_p = i; + } + + template<class T> + inline void enable(IRQ n, void (T::*f)(), T* i) { + set_handler(n, f, i); + enable(n); + } +}; + +template<Interrupt::Exception> +void interrupt(); + +template<Interrupt::IRQ> +void interrupt(); + +#endif diff --git a/ld_scripts/arm_flash_ram.ld b/ld_scripts/arm_flash_ram.ld new file mode 100644 index 0000000..4839b16 --- /dev/null +++ b/ld_scripts/arm_flash_ram.ld @@ -0,0 +1,78 @@ +_ram_start = ORIGIN(ram); +_ram_size = LENGTH(ram); |