From 8b9cc62359057a686929cc713ffe2931e2203946 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sun, 28 Jul 2019 23:15:54 -0400 Subject: stm32: Rename stm32f4/ directory to stm32/ Now that the code in stm32f4/ can handle both stm32f1 and stm32f4 chips, rename the directory to just "stm32". Signed-off-by: Kevin O'Connor --- src/stm32/Kconfig | 123 ++++++++++++++++++++ src/stm32/Makefile | 63 +++++++++++ src/stm32/adc.c | 107 ++++++++++++++++++ src/stm32/clock.c | 158 ++++++++++++++++++++++++++ src/stm32/gpio.c | 130 +++++++++++++++++++++ src/stm32/gpio.h | 41 +++++++ src/stm32/internal.h | 30 +++++ src/stm32/main.c | 47 ++++++++ src/stm32/serial.c | 78 +++++++++++++ src/stm32/spi.c | 59 ++++++++++ src/stm32/stm32.lds.S | 63 +++++++++++ src/stm32/stm32f1.c | 125 ++++++++++++++++++++ src/stm32/usbfs.c | 307 ++++++++++++++++++++++++++++++++++++++++++++++++++ src/stm32/watchdog.c | 25 ++++ 14 files changed, 1356 insertions(+) create mode 100644 src/stm32/Kconfig create mode 100644 src/stm32/Makefile create mode 100644 src/stm32/adc.c create mode 100644 src/stm32/clock.c create mode 100644 src/stm32/gpio.c create mode 100644 src/stm32/gpio.h create mode 100644 src/stm32/internal.h create mode 100644 src/stm32/main.c create mode 100644 src/stm32/serial.c create mode 100644 src/stm32/spi.c create mode 100644 src/stm32/stm32.lds.S create mode 100644 src/stm32/stm32f1.c create mode 100644 src/stm32/usbfs.c create mode 100644 src/stm32/watchdog.c (limited to 'src/stm32') diff --git a/src/stm32/Kconfig b/src/stm32/Kconfig new file mode 100644 index 00000000..707215ec --- /dev/null +++ b/src/stm32/Kconfig @@ -0,0 +1,123 @@ +# Kconfig settings for STM32 processors + +if MACH_STM32 + +config STM32_SELECT + bool + default y + select HAVE_GPIO + select HAVE_GPIO_ADC + select HAVE_GPIO_SPI + select HAVE_GPIO_BITBANGING + +config BOARD_DIRECTORY + string + default "stm32" + +choice + prompt "Processor model" + config MACH_STM32F103 + bool "STM32F103" + select MACH_STM32F1 + select HAVE_STM32_USBFS + config MACH_STM32F405 + bool "STM32F405" + select MACH_STM32F4 + config MACH_STM32F407 + bool "STM32F407" + select MACH_STM32F4 + config MACH_STM32F446 + bool "STM32F446" + select MACH_STM32F4 +endchoice + +config MACH_STM32F1 + bool +config MACH_STM32F4 + bool +config HAVE_STM32_USBFS + bool + +config MCU + string + default "stm32f103xb" if MACH_STM32F103 + default "stm32f405xx" if MACH_STM32F405 + default "stm32f407xx" if MACH_STM32F407 + default "stm32f446xx" if MACH_STM32F446 + +config CLOCK_FREQ + int + default 72000000 if MACH_STM32F103 + default 168000000 if MACH_STM32F405 || MACH_STM32F407 + default 180000000 if MACH_STM32F446 + +config FLASH_SIZE + hex + default 0x10000 if MACH_STM32F103 + default 0x80000 if MACH_STM32F405 || MACH_STM32F407 + default 0x80000 if MACH_STM32F446 + +config RAM_SIZE + hex + default 0x5000 if MACH_STM32F103 + default 0x30000 if MACH_STM32F405 || MACH_STM32F407 + default 0x20000 if MACH_STM32F446 + +config STACK_SIZE + int + default 512 + +choice + prompt "Bootloader offset" if MACH_STM32F103 + config STM32_FLASH_START_2000 + bool "8KiB bootloader (stm32duino)" + config STM32_FLASH_START_7000 + bool "28KiB bootloader" + config STM32_FLASH_START_0000 + bool "No bootloader" +endchoice +config FLASH_START + hex + default 0x2000 if STM32_FLASH_START_2000 + default 0x7000 if STM32_FLASH_START_7000 + default 0x0000 + +choice + prompt "Clock Reference" if LOW_LEVEL_OPTIONS + config STM32_CLOCK_REF_8M + bool "8Mhz crystal" + config STM32_CLOCK_REF_INTERNAL + bool "Internal clock" +endchoice +config CLOCK_REF_8M + bool + default n if STM32_CLOCK_REF_INTERNAL + default y + +config USBSERIAL + bool "Use USB for communication (instead of serial)" + depends on HAVE_STM32_USBFS + default y +config SERIAL + depends on !USBSERIAL + bool + default y +choice + depends on SERIAL + prompt "Serial Port" if LOW_LEVEL_OPTIONS + help + Select the serial device to use. + config STM32_SERIAL_USART1 + bool "USART1" + config STM32_SERIAL_USART2 + bool "USART2" + config STM32_SERIAL_USART3 + bool "USART3" +endchoice +config SERIAL_PORT + int + default 3 if STM32_SERIAL_USART3 + default 2 if STM32_SERIAL_USART2 + default 1 + +endif diff --git a/src/stm32/Makefile b/src/stm32/Makefile new file mode 100644 index 00000000..24c60c58 --- /dev/null +++ b/src/stm32/Makefile @@ -0,0 +1,63 @@ +# Additional STM32 build rules + +# Setup the toolchain +CROSS_PREFIX=arm-none-eabi- + +dirs-y += src/stm32 src/generic +dirs-$(CONFIG_MACH_STM32F1) += lib/stm32f1 lib/stm32f1/gcc +dirs-$(CONFIG_MACH_STM32F4) += lib/stm32f4 lib/stm32f4/gcc + +MCU := $(shell echo $(CONFIG_MCU)) +MCU_UPPER := $(shell echo $(CONFIG_MCU) | tr a-z A-Z | tr X x) + +CFLAGS-$(CONFIG_MACH_STM32F1) += -mcpu=cortex-m3 -Ilib/stm32f1/include +CFLAGS-$(CONFIG_MACH_STM32F4) += -mcpu=cortex-m4 -Ilib/stm32f4/include +CFLAGS-$(CONFIG_MACH_STM32F4) += -mfpu=fpv4-sp-d16 -mfloat-abi=hard + +CFLAGS += $(CFLAGS-y) -D$(MCU_UPPER) -mthumb -Ilib/cmsis-core +CFLAGS_klipper.elf += -T $(OUT)stm32.ld --specs=nano.specs --specs=nosys.specs + +# Add source files +src-y += stm32/main.c stm32/watchdog.c stm32/gpio.c +src-y += generic/crc16_ccitt.c generic/armcm_irq.c generic/armcm_timer.c +src-$(CONFIG_MACH_STM32F1) += ../lib/stm32f1/system_stm32f1xx.c +src-$(CONFIG_MACH_STM32F1) += stm32/stm32f1.c +src-$(CONFIG_MACH_STM32F4) += ../lib/stm32f4/system_stm32f4xx.c +src-$(CONFIG_MACH_STM32F4) += stm32/clock.c +src-$(CONFIG_HAVE_GPIO_ADC) += stm32/adc.c +src-$(CONFIG_HAVE_GPIO_SPI) += stm32/spi.c +src-$(CONFIG_USBSERIAL) += stm32/usbfs.c generic/usb_cdc.c +src-$(CONFIG_SERIAL) += stm32/serial.c generic/serial_irq.c + +# Add assembler build rules +$(OUT)%.o: %.s $(OUT)autoconf.h $(OUT)board-link + @echo " Assembling $@" + $(Q)$(AS) $< -o $@ + +asmsrc-$(CONFIG_MACH_STM32F1) := ../lib/stm32f1/gcc/startup_$(MCU).s +asmsrc-$(CONFIG_MACH_STM32F4) := ../lib/stm32f4/gcc/startup_$(MCU).s +OBJS_klipper.elf += $(patsubst %.s, $(OUT)src/%.o,$(asmsrc-y)) + +# Build the linker script +$(OUT)stm32.ld: src/stm32/stm32.lds.S $(OUT)board-link + @echo " Preprocessing $@" + $(Q)$(CPP) -I$(OUT) -P -MD -MT $@ $< -o $@ +$(OUT)klipper.elf: $(OUT)stm32.ld + +# Binary output file rules +target-y += $(OUT)klipper.bin + +$(OUT)klipper.bin: $(OUT)klipper.elf + @echo " Creating hex file $@" + $(Q)$(OBJCOPY) -O binary $< $@ + +FLASH_TYPE-$(CONFIG_MACH_STM32F1) := stm32f1 +FLASH_TYPE-$(CONFIG_MACH_STM32F4) := stm32f4 + +flash: $(OUT)klipper.bin + @echo " Flashing $< to $(FLASH_DEVICE)" + $(Q)$(PYTHON) ./scripts/flash_usb.py -t $(FLASH_TYPE-y) -d "$(FLASH_DEVICE)" $(if $(NOSUDO),--no-sudo) $(OUT)klipper.bin + +serialflash: $(OUT)klipper.bin + @echo " Flashing $< to $(FLASH_DEVICE) via stm32flash" + $(Q)stm32flash -w $< -v -g 0 $(FLASH_DEVICE) diff --git a/src/stm32/adc.c b/src/stm32/adc.c new file mode 100644 index 00000000..b24b9e06 --- /dev/null +++ b/src/stm32/adc.c @@ -0,0 +1,107 @@ +// ADC functions on STM32 +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "board/irq.h" // irq_save +#include "board/misc.h" // timer_from_us +#include "command.h" // shutdown +#include "compiler.h" // ARRAY_SIZE +#include "generic/armcm_timer.h" // udelay +#include "gpio.h" // gpio_adc_setup +#include "internal.h" // GPIO +#include "sched.h" // sched_shutdown + +DECL_CONSTANT("ADC_MAX", 4095); + +static const uint8_t adc_pins[] = { + GPIO('A', 0), GPIO('A', 1), GPIO('A', 2), GPIO('A', 3), + GPIO('A', 4), GPIO('A', 5), GPIO('A', 6), GPIO('A', 7), + GPIO('B', 0), GPIO('B', 1), GPIO('C', 0), GPIO('C', 1), + GPIO('C', 2), GPIO('C', 3), GPIO('C', 4), GPIO('C', 5) +}; + +#if CONFIG_MACH_STM32F1 +#define CR2_FLAGS (ADC_CR2_ADON | (7 << ADC_CR2_EXTSEL_Pos) | ADC_CR2_EXTTRIG) +#else +#define CR2_FLAGS ADC_CR2_ADON +#endif + +struct gpio_adc +gpio_adc_setup(uint32_t pin) +{ + // Find pin in adc_pins table + int chan; + for (chan=0; ; chan++) { + if (chan >= ARRAY_SIZE(adc_pins)) + shutdown("Not a valid ADC pin"); + if (adc_pins[chan] == pin) + break; + } + + // Enable the ADC + if (!is_enabled_pclock(ADC1_BASE)) { + enable_pclock(ADC1_BASE); + uint32_t aticks = 3; // 2.5-3.2us (depending on stm32 chip) + ADC1->SMPR1 = (aticks | (aticks << 3) | (aticks << 6) | (aticks << 9) + | (aticks << 12) | (aticks << 15) | (aticks << 18) + | (aticks << 21) | (aticks << 24)); + ADC1->SMPR2 = (aticks | (aticks << 3) | (aticks << 6) | (aticks << 9) + | (aticks << 12) | (aticks << 15) | (aticks << 18) + | (aticks << 21) | (aticks << 24) | (aticks << 27)); + ADC1->CR2 = CR2_FLAGS; + +#if CONFIG_MACH_STM32F1 + // Perform calibration + udelay(timer_from_us(1)); + ADC1->CR2 = ADC_CR2_CAL | CR2_FLAGS; + while (ADC1->CR2 & ADC_CR2_CAL) + ; +#endif + } + + gpio_peripheral(pin, GPIO_ANALOG, 0); + + return (struct gpio_adc){ .chan = chan }; +} + +// Try to sample a value. Returns zero if sample ready, otherwise +// returns the number of clock ticks the caller should wait before +// retrying this function. +uint32_t +gpio_adc_sample(struct gpio_adc g) +{ + uint32_t sr = ADC1->SR; + if (sr & ADC_SR_STRT) { + if (!(sr & ADC_SR_EOC) || ADC1->SQR3 != g.chan) + // Conversion still in progress or busy on another channel + goto need_delay; + // Conversion ready + return 0; + } + // Start sample + ADC1->SQR3 = g.chan; + ADC1->CR2 = ADC_CR2_SWSTART | CR2_FLAGS; + +need_delay: + return timer_from_us(10); +} + +// Read a value; use only after gpio_adc_sample() returns zero +uint16_t +gpio_adc_read(struct gpio_adc g) +{ + ADC1->SR = ~ADC_SR_STRT; + return ADC1->DR; +} + +// Cancel a sample that may have been started with gpio_adc_sample() +void +gpio_adc_cancel_sample(struct gpio_adc g) +{ + irqstatus_t flag = irq_save(); + if (ADC1->SR & ADC_SR_STRT && ADC1->SQR3 == g.chan) + gpio_adc_read(g); + irq_restore(flag); +} diff --git a/src/stm32/clock.c b/src/stm32/clock.c new file mode 100644 index 00000000..d0a2f249 --- /dev/null +++ b/src/stm32/clock.c @@ -0,0 +1,158 @@ +// Code to setup clocks and gpio on stm32f4 +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "autoconf.h" // CONFIG_CLOCK_REF_8M +#include "command.h" // DECL_CONSTANT_STR +#include "internal.h" // enable_pclock + +#define FREQ_PERIPH (CONFIG_CLOCK_FREQ / 4) + +// Enable a peripheral clock +void +enable_pclock(uint32_t periph_base) +{ + if (periph_base < APB2PERIPH_BASE) { + uint32_t pos = (periph_base - APB1PERIPH_BASE) / 0x400; + RCC->APB1ENR |= (1<APB1ENR; + } else if (periph_base < AHB1PERIPH_BASE) { + uint32_t pos = (periph_base - APB2PERIPH_BASE) / 0x400; + RCC->APB2ENR |= (1<APB2ENR; + } else if (periph_base < AHB2PERIPH_BASE) { + uint32_t pos = (periph_base - AHB1PERIPH_BASE) / 0x400; + RCC->AHB1ENR |= (1<AHB1ENR; + } +} + +// Check if a peripheral clock has been enabled +int +is_enabled_pclock(uint32_t periph_base) +{ + if (periph_base < APB2PERIPH_BASE) { + uint32_t pos = (periph_base - APB1PERIPH_BASE) / 0x400; + return RCC->APB1ENR & (1<APB2ENR & (1<AHB1ENR & (1<AHB1ENR |= (1<> 4; + uint32_t pup = pullup ? (pullup > 0 ? 1 : 2) : 0; + uint32_t pos = gpio % 16, af_reg = pos / 8; + uint32_t af_shift = (pos % 8) * 4, af_msk = 0x0f << af_shift; + uint32_t m_shift = pos * 2, m_msk = 0x03 << m_shift; + + regs->AFR[af_reg] = (regs->AFR[af_reg] & ~af_msk) | (func << af_shift); + regs->MODER = (regs->MODER & ~m_msk) | (mode_bits << m_shift); + regs->PUPDR = (regs->PUPDR & ~m_msk) | (pup << m_shift); + regs->OSPEEDR = (regs->OSPEEDR & ~m_msk) | (0x02 << m_shift); +} + +#if CONFIG_CLOCK_REF_8M +DECL_CONSTANT_STR("RESERVE_PINS_crystal", "PH0,PH1"); +#endif + +// Clock configuration +static void +enable_clock_stm32f40x(void) +{ +#if CONFIG_MACH_STM32F405 || CONFIG_MACH_STM32F407 + if (CONFIG_CLOCK_REF_8M) { + // Configure 168Mhz PLL from external 8Mhz crystal (HSE) + RCC->CR |= RCC_CR_HSEON; + RCC->PLLCFGR = ( + RCC_PLLCFGR_PLLSRC_HSE | (4 << RCC_PLLCFGR_PLLM_Pos) + | (168 << RCC_PLLCFGR_PLLN_Pos) | (0 << RCC_PLLCFGR_PLLP_Pos) + | (7 << RCC_PLLCFGR_PLLQ_Pos)); + } else { + // Configure 168Mhz PLL from internal 16Mhz oscillator (HSI) + RCC->PLLCFGR = ( + RCC_PLLCFGR_PLLSRC_HSI | (8 << RCC_PLLCFGR_PLLM_Pos) + | (168 << RCC_PLLCFGR_PLLN_Pos) | (0 << RCC_PLLCFGR_PLLP_Pos) + | (7 << RCC_PLLCFGR_PLLQ_Pos)); + } + RCC->CR |= RCC_CR_PLLON; +#endif +} + +static void +enable_clock_stm32f446(void) +{ +#if CONFIG_MACH_STM32F446 + if (CONFIG_CLOCK_REF_8M) { + // Configure 180Mhz PLL from external 8Mhz crystal (HSE) + RCC->CR |= RCC_CR_HSEON; + RCC->PLLCFGR = ( + RCC_PLLCFGR_PLLSRC_HSE | (4 << RCC_PLLCFGR_PLLM_Pos) + | (180 << RCC_PLLCFGR_PLLN_Pos) | (0 << RCC_PLLCFGR_PLLP_Pos) + | (7 << RCC_PLLCFGR_PLLQ_Pos) | (6 << RCC_PLLCFGR_PLLR_Pos)); + } else { + // Configure 180Mhz PLL from internal 16Mhz oscillator (HSI) + RCC->PLLCFGR = ( + RCC_PLLCFGR_PLLSRC_HSI | (8 << RCC_PLLCFGR_PLLM_Pos) + | (180 << RCC_PLLCFGR_PLLN_Pos) | (0 << RCC_PLLCFGR_PLLP_Pos) + | (7 << RCC_PLLCFGR_PLLQ_Pos) | (6 << RCC_PLLCFGR_PLLR_Pos)); + } + RCC->CR |= RCC_CR_PLLON; + + // Enable "over drive" + enable_pclock(PWR_BASE); + PWR->CR = (3 << PWR_CR_VOS_Pos) | PWR_CR_ODEN; + while (!(PWR->CSR & PWR_CSR_ODRDY)) + ; + PWR->CR = (3 << PWR_CR_VOS_Pos) | PWR_CR_ODEN | PWR_CR_ODSWEN; + while (!(PWR->CSR & PWR_CSR_ODSWRDY)) + ; +#endif +} + +// Main clock setup called at chip startup +void +clock_setup(void) +{ + if (CONFIG_MACH_STM32F405 || CONFIG_MACH_STM32F407) + enable_clock_stm32f40x(); + else + enable_clock_stm32f446(); + + // Set flash latency + FLASH->ACR = (FLASH_ACR_LATENCY_5WS | FLASH_ACR_ICEN | FLASH_ACR_DCEN + | FLASH_ACR_PRFTEN); + + // Wait for PLL lock + while (!(RCC->CR & RCC_CR_PLLRDY)) + ; + + // Switch system clock to PLL + RCC->CFGR = RCC_CFGR_PPRE1_DIV4 | RCC_CFGR_PPRE2_DIV4 | RCC_CFGR_SW_PLL; + while ((RCC->CFGR & RCC_CFGR_SWS_Msk) != RCC_CFGR_SWS_PLL) + ; +} diff --git a/src/stm32/gpio.c b/src/stm32/gpio.c new file mode 100644 index 00000000..9fbbdaec --- /dev/null +++ b/src/stm32/gpio.c @@ -0,0 +1,130 @@ +// GPIO functions on stm32f4 +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // ffs +#include "board/irq.h" // irq_save +#include "command.h" // DECL_ENUMERATION_RANGE +#include "gpio.h" // gpio_out_setup +#include "internal.h" // gpio_peripheral +#include "sched.h" // sched_shutdown + +DECL_ENUMERATION_RANGE("pin", "PA0", GPIO('A', 0), 32); +DECL_ENUMERATION_RANGE("pin", "PB0", GPIO('B', 0), 32); +DECL_ENUMERATION_RANGE("pin", "PC0", GPIO('C', 0), 32); +DECL_ENUMERATION_RANGE("pin", "PD0", GPIO('D', 0), 32); +DECL_ENUMERATION_RANGE("pin", "PE0", GPIO('E', 0), 32); +#ifdef GPIOH +DECL_ENUMERATION_RANGE("pin", "PF0", GPIO('F', 0), 32); +DECL_ENUMERATION_RANGE("pin", "PG0", GPIO('G', 0), 32); +DECL_ENUMERATION_RANGE("pin", "PH0", GPIO('H', 0), 32); +#endif +#ifdef GPIOI +DECL_ENUMERATION_RANGE("pin", "PI0", GPIO('I', 0), 32); +#endif + +GPIO_TypeDef * const digital_regs[] = { + GPIOA, GPIOB, GPIOC, GPIOD, GPIOE, +#ifdef GPIOH + GPIOF, GPIOG, GPIOH, +#endif +#ifdef GPIOI + GPIOI, +#endif +}; + +// Convert a register and bit location back to an integer pin identifier +static int +regs_to_pin(GPIO_TypeDef *regs, uint32_t bit) +{ + int i; + for (i=0; i= ARRAY_SIZE(digital_regs)) + goto fail; + GPIO_TypeDef *regs = digital_regs[GPIO2PORT(pin)]; + struct gpio_out g = { .regs=regs, .bit=GPIO2BIT(pin) }; + gpio_out_reset(g, val); + return g; +fail: + shutdown("Not an output pin"); +} + +void +gpio_out_reset(struct gpio_out g, uint32_t val) +{ + GPIO_TypeDef *regs = g.regs; + int pin = regs_to_pin(regs, g.bit); + irqstatus_t flag = irq_save(); + if (val) + regs->BSRR = g.bit; + else + regs->BSRR = g.bit << 16; + gpio_peripheral(pin, GPIO_OUTPUT, 0); + irq_restore(flag); +} + +void +gpio_out_toggle_noirq(struct gpio_out g) +{ + GPIO_TypeDef *regs = g.regs; + regs->ODR ^= g.bit; +} + +void +gpio_out_toggle(struct gpio_out g) +{ + irqstatus_t flag = irq_save(); + gpio_out_toggle_noirq(g); + irq_restore(flag); +} + +void +gpio_out_write(struct gpio_out g, uint32_t val) +{ + GPIO_TypeDef *regs = g.regs; + if (val) + regs->BSRR = g.bit; + else + regs->BSRR = g.bit << 16; +} + + +struct gpio_in +gpio_in_setup(uint32_t pin, int32_t pull_up) +{ + if (GPIO2PORT(pin) >= ARRAY_SIZE(digital_regs)) + goto fail; + GPIO_TypeDef *regs = digital_regs[GPIO2PORT(pin)]; + struct gpio_in g = { .regs=regs, .bit=GPIO2BIT(pin) }; + gpio_in_reset(g, pull_up); + return g; +fail: + shutdown("Not a valid input pin"); +} + +void +gpio_in_reset(struct gpio_in g, int32_t pull_up) +{ + GPIO_TypeDef *regs = g.regs; + int pin = regs_to_pin(regs, g.bit); + irqstatus_t flag = irq_save(); + gpio_peripheral(pin, GPIO_INPUT, pull_up); + irq_restore(flag); +} + +uint8_t +gpio_in_read(struct gpio_in g) +{ + GPIO_TypeDef *regs = g.regs; + return !!(regs->IDR & g.bit); +} diff --git a/src/stm32/gpio.h b/src/stm32/gpio.h new file mode 100644 index 00000000..c7602504 --- /dev/null +++ b/src/stm32/gpio.h @@ -0,0 +1,41 @@ +#ifndef __STM32_GPIO_H +#define __STM32_GPIO_H + +#include // uint32_t + +struct gpio_out { + void *regs; + uint32_t bit; +}; +struct gpio_out gpio_out_setup(uint32_t pin, uint32_t val); +void gpio_out_reset(struct gpio_out g, uint32_t val); +void gpio_out_toggle_noirq(struct gpio_out g); +void gpio_out_toggle(struct gpio_out g); +void gpio_out_write(struct gpio_out g, uint32_t val); + +struct gpio_in { + void *regs; + uint32_t bit; +}; +struct gpio_in gpio_in_setup(uint32_t pin, int32_t pull_up); +void gpio_in_reset(struct gpio_in g, int32_t pull_up); +uint8_t gpio_in_read(struct gpio_in g); + +struct gpio_adc { + uint32_t chan; +}; +struct gpio_adc gpio_adc_setup(uint32_t pin); +uint32_t gpio_adc_sample(struct gpio_adc g); +uint16_t gpio_adc_read(struct gpio_adc g); +void gpio_adc_cancel_sample(struct gpio_adc g); + +struct spi_config { + void *spidev; + uint32_t spi_cr1; +}; +struct spi_config spi_setup(uint32_t bus, uint8_t mode, uint32_t rate); +void spi_prepare(struct spi_config config); +void spi_transfer(struct spi_config config, uint8_t receive_data + , uint8_t len, uint8_t *data); + +#endif // gpio.h diff --git a/src/stm32/internal.h b/src/stm32/internal.h new file mode 100644 index 00000000..0edfefe5 --- /dev/null +++ b/src/stm32/internal.h @@ -0,0 +1,30 @@ +#ifndef __STM32_INTERNAL_H +#define __STM32_INTERNAL_H +// Local definitions for STM32 code + +#include "autoconf.h" // CONFIG_MACH_STM32F1 + +#if CONFIG_MACH_STM32F1 +#include "stm32f1xx.h" +#else +#include "stm32f4xx.h" +#endif + +extern GPIO_TypeDef * const digital_regs[]; + +#define GPIO(PORT, NUM) (((PORT)-'A') * 16 + (NUM)) +#define GPIO2PORT(PIN) ((PIN) / 16) +#define GPIO2BIT(PIN) (1<<((PIN) % 16)) + +#define GPIO_INPUT 0 +#define GPIO_OUTPUT 1 +#define GPIO_FUNCTION(fn) (2 | ((fn) << 4)) +#define GPIO_ANALOG 3 + +void enable_pclock(uint32_t periph_base); +int is_enabled_pclock(uint32_t periph_base); +uint32_t get_pclock_frequency(uint32_t periph_base); +void clock_setup(void); +void gpio_peripheral(uint32_t gpio, uint32_t mode, int pullup); + +#endif // internal.h diff --git a/src/stm32/main.c b/src/stm32/main.c new file mode 100644 index 00000000..20549799 --- /dev/null +++ b/src/stm32/main.c @@ -0,0 +1,47 @@ +// Main starting point for STM32 boards. +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "autoconf.h" // CONFIG_MCU +#include "command.h" // DECL_CONSTANT_STR +#include "internal.h" // clock_setup +#include "sched.h" // sched_main + +DECL_CONSTANT_STR("MCU", CONFIG_MCU); + +// Return the start of memory available for dynamic allocations +void * +dynmem_start(void) +{ + extern uint32_t _ebss; + return &_ebss; +} + +// Return the end of memory available for dynamic allocations +void * +dynmem_end(void) +{ + extern uint32_t _sstack; + return &_sstack; +} + +void +command_reset(uint32_t *args) +{ + NVIC_SystemReset(); +} +DECL_COMMAND_FLAGS(command_reset, HF_IN_SHUTDOWN, "reset"); + +// Main entry point +int +main(void) +{ + SCB->VTOR += CONFIG_FLASH_START; + + clock_setup(); + + sched_main(); + return 0; +} diff --git a/src/stm32/serial.c b/src/stm32/serial.c new file mode 100644 index 00000000..0142aaa6 --- /dev/null +++ b/src/stm32/serial.c @@ -0,0 +1,78 @@ +// STM32 serial +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "autoconf.h" // CONFIG_SERIAL_BAUD +#include "board/serial_irq.h" // serial_rx_byte +#include "command.h" // DECL_CONSTANT_STR +#include "internal.h" // enable_pclock +#include "sched.h" // DECL_INIT + +// Select the configured serial port +#if CONFIG_SERIAL_PORT == 1 +DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA10,PA9"); +#define GPIO_Rx GPIO('A', 10) +#define GPIO_Tx GPIO('A', 9) +#define USARTx USART1 +#define USARTx_IRQn USART1_IRQn +#define USARTx_IRQHandler USART1_IRQHandler +#elif CONFIG_SERIAL_PORT == 2 +DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA3,PA2"); +#define GPIO_Rx GPIO('A', 3) +#define GPIO_Tx GPIO('A', 2) +#define USARTx USART2 +#define USARTx_IRQn USART2_IRQn +#define USARTx_IRQHandler USART2_IRQHandler +#else +DECL_CONSTANT_STR("RESERVE_PINS_serial", "PB11,PB10"); +#define GPIO_Rx GPIO('B', 11) +#define GPIO_Tx GPIO('B', 10) +#define USARTx USART3 +#define USARTx_IRQn USART3_IRQn +#define USARTx_IRQHandler USART3_IRQHandler +#endif + +#define CR1_FLAGS (USART_CR1_UE | USART_CR1_RE | USART_CR1_TE \ + | USART_CR1_RXNEIE) + +void +serial_init(void) +{ + enable_pclock((uint32_t)USARTx); + + uint32_t pclk = get_pclock_frequency((uint32_t)USARTx); + uint32_t div = DIV_ROUND_CLOSEST(pclk, CONFIG_SERIAL_BAUD); + USARTx->BRR = (((div / 16) << USART_BRR_DIV_Mantissa_Pos) + | ((div % 16) << USART_BRR_DIV_Fraction_Pos)); + USARTx->CR1 = CR1_FLAGS; + NVIC_SetPriority(USARTx_IRQn, 0); + NVIC_EnableIRQ(USARTx_IRQn); + + gpio_peripheral(GPIO_Rx, GPIO_FUNCTION(7), 1); + gpio_peripheral(GPIO_Tx, GPIO_FUNCTION(7), 0); +} +DECL_INIT(serial_init); + +void __visible +USARTx_IRQHandler(void) +{ + uint32_t sr = USARTx->SR; + if (sr & (USART_SR_RXNE | USART_SR_ORE)) + serial_rx_byte(USARTx->DR); + if (sr & USART_SR_TXE && USARTx->CR1 & USART_CR1_TXEIE) { + uint8_t data; + int ret = serial_get_tx_byte(&data); + if (ret) + USARTx->CR1 = CR1_FLAGS; + else + USARTx->DR = data; + } +} + +void +serial_enable_tx_irq(void) +{ + USARTx->CR1 = CR1_FLAGS | USART_CR1_TXEIE; +} diff --git a/src/stm32/spi.c b/src/stm32/spi.c new file mode 100644 index 00000000..77072958 --- /dev/null +++ b/src/stm32/spi.c @@ -0,0 +1,59 @@ +// SPI functions on STM32 +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "command.h" // shutdown +#include "gpio.h" // spi_setup +#include "internal.h" // gpio_peripheral +#include "sched.h" // sched_shutdown + +DECL_ENUMERATION("spi_bus", "spi2", 0); +DECL_CONSTANT_STR("BUS_PINS_spi2", "PB14,PB15,PB13"); + +struct spi_config +spi_setup(uint32_t bus, uint8_t mode, uint32_t rate) +{ + if (bus) + shutdown("Invalid spi bus"); + + // Enable SPI + if (!is_enabled_pclock(SPI2_BASE)) { + enable_pclock(SPI2_BASE); + gpio_peripheral(GPIO('B', 14), GPIO_FUNCTION(5), 1); + gpio_peripheral(GPIO('B', 15), GPIO_FUNCTION(5), 0); + gpio_peripheral(GPIO('B', 13), GPIO_FUNCTION(5), 0); + } + + // Calculate CR1 register + uint32_t pclk = get_pclock_frequency(SPI2_BASE); + uint32_t div = 0; + while ((pclk >> (div + 1)) > rate && div < 7) + div++; + uint32_t cr1 = ((mode << SPI_CR1_CPHA_Pos) | (div << SPI_CR1_BR_Pos) + | SPI_CR1_SPE | SPI_CR1_MSTR | SPI_CR1_SSM | SPI_CR1_SSI); + + return (struct spi_config){ .spi_cr1 = cr1 }; +} + +void +spi_prepare(struct spi_config config) +{ + SPI2->CR1 = config.spi_cr1; +} + +void +spi_transfer(struct spi_config config, uint8_t receive_data, + uint8_t len, uint8_t *data) +{ + while (len--) { + SPI2->DR = *data; + while (!(SPI2->SR & SPI_SR_RXNE)) + ; + uint8_t rdata = SPI2->DR; + if (receive_data) + *data = rdata; + data++; + } +} diff --git a/src/stm32/stm32.lds.S b/src/stm32/stm32.lds.S new file mode 100644 index 00000000..17edd23a --- /dev/null +++ b/src/stm32/stm32.lds.S @@ -0,0 +1,63 @@ +/* Linker script for stm32f4 chips + * + * Copyright (C) 2019 Kevin O'Connor + * + * This file may be distributed under the terms of the GNU GPLv3 license. + */ + +#include "autoconf.h" + +OUTPUT_FORMAT("elf32-littlearm", "elf32-littlearm", "elf32-littlearm") +OUTPUT_ARCH(arm) + +MEMORY +{ + rom (rx) : ORIGIN = 0x8000000 + CONFIG_FLASH_START , LENGTH = CONFIG_FLASH_SIZE + ram (rwx) : ORIGIN = 0x20000000, LENGTH = CONFIG_RAM_SIZE +} + +SECTIONS +{ + .text : { + . = ALIGN(4); + _sfixed = .; + KEEP(*(.isr_vector)) + *(.text .text.*) + *(.rodata .rodata*) + + . = ALIGN(4); + KEEP(*(.init)) + . = ALIGN(4); + KEEP(*(.fini)) + } > rom + + . = ALIGN(4); + _sidata = .; + + .data : AT (_sidata) + { + . = ALIGN(4); + _sdata = .; + *(.ramfunc .ramfunc.*); + *(.data .data.*); + . = ALIGN(4); + _edata = .; + } > ram + + .bss (NOLOAD) : + { + . = ALIGN(4); + _sbss = .; + *(.bss .bss.*) + *(COMMON) + . = ALIGN(4); + _ebss = .; + } > ram + + _sstack = 0x20000000 + CONFIG_RAM_SIZE - CONFIG_STACK_SIZE ; + .stack _sstack (NOLOAD) : + { + . = . + CONFIG_STACK_SIZE; + _estack = .; + } > ram +} diff --git a/src/stm32/stm32f1.c b/src/stm32/stm32f1.c new file mode 100644 index 00000000..3064d4c8 --- /dev/null +++ b/src/stm32/stm32f1.c @@ -0,0 +1,125 @@ +// Code to setup clocks and gpio on stm32f1 +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "autoconf.h" // CONFIG_CLOCK_REF_8M +#include "internal.h" // enable_pclock + +#define FREQ_PERIPH (CONFIG_CLOCK_FREQ / 2) + +// Enable a peripheral clock +void +enable_pclock(uint32_t periph_base) +{ + if (periph_base < APB2PERIPH_BASE) { + uint32_t pos = (periph_base - APB1PERIPH_BASE) / 0x400; + RCC->APB1ENR |= (1<APB1ENR; + } else if (periph_base < AHBPERIPH_BASE) { + uint32_t pos = (periph_base - APB2PERIPH_BASE) / 0x400; + RCC->APB2ENR |= (1<APB2ENR; + } else { + uint32_t pos = (periph_base - AHBPERIPH_BASE) / 0x400; + RCC->AHBENR |= (1<AHBENR; + } +} + +// Check if a peripheral clock has been enabled +int +is_enabled_pclock(uint32_t periph_base) +{ + if (periph_base < APB2PERIPH_BASE) { + uint32_t pos = (periph_base - APB1PERIPH_BASE) / 0x400; + return RCC->APB1ENR & (1<APB2ENR & (1<AHBENR & (1<APB2ENR |= 1 << rcc_pos; + + // Configure GPIO + uint32_t pos = gpio % 16, shift = (pos % 8) * 4, msk = 0xf << shift, cfg; + if (mode == GPIO_INPUT) { + cfg = pullup ? 0x8 : 0x4; + } else if (mode == GPIO_OUTPUT) { + cfg = 0x1; + } else if (mode == GPIO_ANALOG) { + cfg = 0x0; + } else { + if (pullup > 0) + // Alternate function input pins use GPIO_INPUT mode on the stm32f1 + cfg = 0x8; + else + cfg = 0x9; + } + if (pos & 0x8) + regs->CRH = (regs->CRH & ~msk) | (cfg << shift); + else + regs->CRL = (regs->CRL & ~msk) | (cfg << shift); + + if (pullup > 0) + regs->BSRR = 1 << pos; + else if (pullup < 0) + regs->BSRR = 1 << (pos + 16); +} + +// Main clock setup called at chip startup +void +clock_setup(void) +{ + uint32_t cfgr; + if (CONFIG_CLOCK_REF_8M) { + // Configure 72Mhz PLL from external 8Mhz crystal (HSE) + RCC->CR |= RCC_CR_HSEON; + cfgr = ((1 << RCC_CFGR_PLLSRC_Pos) | ((9 - 2) << RCC_CFGR_PLLMULL_Pos) + | RCC_CFGR_PPRE1_DIV2 | RCC_CFGR_PPRE2_DIV2 + | RCC_CFGR_ADCPRE_DIV4); + } else { + // Configure 72Mhz PLL from internal 8Mhz oscillator (HSI) + cfgr = ((0 << RCC_CFGR_PLLSRC_Pos) | ((18 - 2) << RCC_CFGR_PLLMULL_Pos) + | RCC_CFGR_PPRE1_DIV2 | RCC_CFGR_PPRE2_DIV2 + | RCC_CFGR_ADCPRE_DIV4); + } + RCC->CFGR = cfgr; + RCC->CR |= RCC_CR_PLLON; + + // Set flash latency + FLASH->ACR = (2 << FLASH_ACR_LATENCY_Pos) | FLASH_ACR_PRFTBE; + + // Wait for PLL lock + while (!(RCC->CR & RCC_CR_PLLRDY)) + ; + + // Switch system clock to PLL + RCC->CFGR = cfgr | RCC_CFGR_SW_PLL; + while ((RCC->CFGR & RCC_CFGR_SWS_Msk) != RCC_CFGR_SWS_PLL) + ; + + // Disable JTAG to free PA15, PB3, PB4 + enable_pclock(AFIO_BASE); + AFIO->MAPR = AFIO_MAPR_SWJ_CFG_JTAGDISABLE; +} diff --git a/src/stm32/usbfs.c b/src/stm32/usbfs.c new file mode 100644 index 00000000..929ec01b --- /dev/null +++ b/src/stm32/usbfs.c @@ -0,0 +1,307 @@ +// Hardware interface to "fullspeed USB controller" on stm32f1 +// +// Copyright (C) 2018-2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // NULL +#include "autoconf.h" // CONFIG_STM32_FLASH_START_2000 +#include "board/armcm_timer.h" // udelay +#include "board/gpio.h" // gpio_out_setup +#include "board/io.h" // writeb +#include "board/irq.h" // irq_disable +#include "board/usb_cdc.h" // usb_notify_ep0 +#include "board/usb_cdc_ep.h" // USB_CDC_EP_BULK_IN +#include "command.h" // DECL_CONSTANT_STR +#include "internal.h" // GPIO +#include "sched.h" // DECL_INIT + + +/**************************************************************** + * USB transfer memory + ****************************************************************/ + +struct ep_desc { + uint32_t addr_tx, count_tx, addr_rx, count_rx; +}; + +struct ep_mem { + struct ep_desc ep0, ep_acm, ep_bulk_out, ep_bulk_in; + uint32_t ep0_tx[USB_CDC_EP0_SIZE / 2]; + uint32_t ep0_rx[USB_CDC_EP0_SIZE / 2]; + uint32_t ep_acm_tx[USB_CDC_EP_ACM_SIZE / 2]; + uint32_t ep_bulk_out_rx[USB_CDC_EP_BULK_OUT_SIZE / 2]; + uint32_t ep_bulk_in_tx[USB_CDC_EP_BULK_IN_SIZE / 2]; +}; + +#define USB_BTABLE ((struct ep_mem *)(USB_BASE + 0x400)) + +#define CALC_ADDR(p) (((void*)(p) - (void*)USB_BTABLE) / 2) +#define CALC_SIZE(s) ((s) > 32 ? (DIV_ROUND_UP((s), 32) << 10) | 0x8000 \ + : DIV_ROUND_UP((s), 2) << 10) + +// Setup the transfer descriptors in dedicated usb memory +static void +btable_configure(void) +{ + USB_BTABLE->ep0.count_tx = 0; + USB_BTABLE->ep0.addr_tx = CALC_ADDR(USB_BTABLE->ep0_tx); + USB_BTABLE->ep0.count_rx = CALC_SIZE(USB_CDC_EP0_SIZE); + USB_BTABLE->ep0.addr_rx = CALC_ADDR(USB_BTABLE->ep0_rx); + + USB_BTABLE->ep_acm.count_tx = 0; + USB_BTABLE->ep_acm.addr_tx = CALC_ADDR(USB_BTABLE->ep_acm_tx); + + USB_BTABLE->ep_bulk_out.count_rx = CALC_SIZE(USB_CDC_EP_BULK_OUT_SIZE); + USB_BTABLE->ep_bulk_out.addr_rx = CALC_ADDR(USB_BTABLE->ep_bulk_out_rx); + + USB_BTABLE->ep_bulk_in.count_tx = 0; + USB_BTABLE->ep_bulk_in.addr_tx = CALC_ADDR(USB_BTABLE->ep_bulk_in_tx); +} + +// Read a packet stored in dedicated usb memory +static void +btable_read_packet(uint8_t *dest, uint32_t *src, int count) +{ + uint_fast8_t i; + for (i=0; i<(count/2); i++) { + uint32_t d = *src++; + *dest++ = d; + *dest++ = d >> 8; + } + if (count & 1) + *dest = *src; +} + +// Write a packet to dedicated usb memory +static void +btable_write_packet(uint32_t *dest, const uint8_t *src, int count) +{ + int i; + for (i=0; i<(count/2); i++) { + uint8_t b1 = *src++, b2 = *src++; + *dest++ = b1 | (b2 << 8); + } + if (count & 1) + *dest = *src; +} + + +/**************************************************************** + * USB endpoint register + ****************************************************************/ + +#define USB_EPR ((volatile uint32_t *)USB_BASE) + +#define EP_BULK (0 << USB_EP0R_EP_TYPE_Pos) +#define EP_CONTROL (1 << USB_EP0R_EP_TYPE_Pos) +#define EP_INTERRUPT (3 << USB_EP0R_EP_TYPE_Pos) +#define RX_STALL (1 << USB_EP0R_STAT_RX_Pos) +#define RX_NAK (2 << USB_EP0R_STAT_RX_Pos) +#define RX_VALID (3 << USB_EP0R_STAT_RX_Pos) +#define TX_STALL (1 << USB_EP0R_STAT_TX_Pos) +#define TX_NAK (2 << USB_EP0R_STAT_TX_Pos) +#define TX_VALID (3 << USB_EP0R_STAT_TX_Pos) +#define EPR_RWBITS (USB_EP0R_EA | USB_EP0R_EP_KIND | USB_EP0R_EP_TYPE) +#define EPR_RWCBITS (USB_EP0R_CTR_RX | USB_EP0R_CTR_TX) + +static uint32_t +set_stat_rx_bits(uint32_t epr, uint32_t bits) +{ + return ((epr & (EPR_RWBITS | USB_EP0R_STAT_RX_Msk)) ^ bits) | EPR_RWCBITS; +} + +static uint32_t +set_stat_tx_bits(uint32_t epr, uint32_t bits) +{ + return ((epr & (EPR_RWBITS | USB_EP0R_STAT_TX_Msk)) ^ bits) | EPR_RWCBITS; +} + +static uint32_t +set_stat_rxtx_bits(uint32_t epr, uint32_t bits) +{ + uint32_t mask = EPR_RWBITS | USB_EP0R_STAT_RX_Msk | USB_EP0R_STAT_TX_Msk; + return ((epr & mask) ^ bits) | EPR_RWCBITS; +} + + +/**************************************************************** + * USB interface + ****************************************************************/ + +int_fast8_t +usb_read_bulk_out(void *data, uint_fast8_t max_len) +{ + uint32_t epr = USB_EPR[USB_CDC_EP_BULK_OUT]; + if ((epr & USB_EP0R_STAT_RX_Msk) == RX_VALID) + // No data ready + return -1; + uint32_t count = USB_BTABLE->ep_bulk_out.count_rx & 0x3ff; + if (count > max_len) + count = max_len; + btable_read_packet(data, USB_BTABLE->ep_bulk_out_rx, count); + USB_EPR[USB_CDC_EP_BULK_OUT] = set_stat_rx_bits(epr, RX_VALID); + return count; +} + +int_fast8_t +usb_send_bulk_in(void *data, uint_fast8_t len) +{ + uint32_t epr = USB_EPR[USB_CDC_EP_BULK_IN]; + if ((epr & USB_EP0R_STAT_TX_Msk) != TX_NAK) + // No buffer space available + return -1; + btable_write_packet(USB_BTABLE->ep_bulk_in_tx, data, len); + USB_BTABLE->ep_bulk_in.count_tx = len; + USB_EPR[USB_CDC_EP_BULK_IN] = set_stat_tx_bits(epr, TX_VALID); + return len; +} + +int_fast8_t +usb_read_ep0(void *data, uint_fast8_t max_len) +{ + uint32_t epr = USB_EPR[0]; + if ((epr & USB_EP0R_STAT_RX_Msk) != RX_NAK) + // No data ready + return -1; + uint32_t count = USB_BTABLE->ep0.count_rx & 0x3ff; + if (count > max_len) + count = max_len; + btable_read_packet(data, USB_BTABLE->ep0_rx, count); + USB_EPR[0] = set_stat_rxtx_bits(epr, RX_VALID | TX_NAK); + return count; +} + +int_fast8_t +usb_read_ep0_setup(void *data, uint_fast8_t max_len) +{ + return usb_read_ep0(data, max_len); +} + +int_fast8_t +usb_send_ep0(const void *data, uint_fast8_t len) +{ + uint32_t epr = USB_EPR[0]; + if ((epr & USB_EP0R_STAT_RX_Msk) != RX_VALID) + // Transfer interrupted + return -2; + if ((epr & USB_EP0R_STAT_TX_Msk) != TX_NAK) + // No buffer space available + return -1; + btable_write_packet(USB_BTABLE->ep0_tx, data, len); + USB_BTABLE->ep0.count_tx = len; + USB_EPR[0] = set_stat_tx_bits(epr, TX_VALID); + return len; +} + +void +usb_stall_ep0(void) +{ + USB_EPR[0] = set_stat_rxtx_bits(USB_EPR[0], RX_STALL | TX_STALL); +} + +static uint8_t set_address; + +void +usb_set_address(uint_fast8_t addr) +{ + writeb(&set_address, addr | USB_DADDR_EF); + usb_send_ep0(NULL, 0); +} + +void +usb_set_configure(void) +{ +} + +void +usb_request_bootloader(void) +{ + if (!CONFIG_STM32_FLASH_START_2000) + return; + // Enter "stm32duino" bootloader + irq_disable(); + RCC->APB1ENR |= RCC_APB1ENR_PWREN | RCC_APB1ENR_BKPEN; + PWR->CR |= PWR_CR_DBP; + BKP->DR10 = 0x01; + PWR->CR &=~ PWR_CR_DBP; + NVIC_SystemReset(); +} + + +/**************************************************************** + * Setup and interrupts + ****************************************************************/ + +DECL_CONSTANT_STR("RESERVE_PINS_USB", "PA11,PA12"); + +// Initialize the usb controller +void +usb_init(void) +{ + // Pull the D+ pin low briefly to signal a new connection + gpio_out_setup(GPIO('A', 12), 0); + udelay(5000); + gpio_in_setup(GPIO('A', 12), 0); + + // Setup USB packet memory + btable_configure(); + + // Enable USB clock + enable_pclock(USB_BASE); + + // Reset usb controller and enable interrupts + USB->CNTR = USB_CNTR_FRES; + USB->BTABLE = 0; + USB->DADDR = 0; + USB->CNTR = USB_CNTR_RESETM; + USB->ISTR = 0; + NVIC_SetPriority(USB_LP_CAN1_RX0_IRQn, 1); + NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn); +} +DECL_INIT(usb_init); + +// Configure interface after a USB reset event +static void +usb_reset(void) +{ + USB_EPR[0] = 0 | EP_CONTROL | RX_VALID | TX_NAK; + USB_EPR[USB_CDC_EP_ACM] = USB_CDC_EP_ACM | EP_INTERRUPT | RX_NAK | TX_NAK; + USB_EPR[USB_CDC_EP_BULK_OUT] = (USB_CDC_EP_BULK_OUT | EP_BULK + | RX_VALID | TX_NAK); + USB_EPR[USB_CDC_EP_BULK_IN] = (USB_CDC_EP_BULK_IN | EP_BULK + | RX_NAK | TX_NAK); + + USB->CNTR = USB_CNTR_CTRM | USB_CNTR_RESETM; + USB->DADDR = USB_DADDR_EF; +} + +// Main irq handler +void __visible +USB_LP_CAN1_RX0_IRQHandler(void) +{ + uint32_t istr = USB->ISTR; + if (istr & USB_ISTR_CTR) { + // Endpoint activity + uint32_t ep = istr & USB_ISTR_EP_ID; + uint32_t epr = USB_EPR[ep]; + USB_EPR[ep] = epr & EPR_RWBITS; + if (ep == 0) { + usb_notify_ep0(); + if (epr & USB_EP_CTR_TX && set_address) { + // Apply address after last "in" message transmitted + USB->DADDR = set_address; + set_address = 0; + } + } else if (ep == USB_CDC_EP_BULK_OUT) { + usb_notify_bulk_out(); + } else if (ep == USB_CDC_EP_BULK_IN) { + usb_notify_bulk_in(); + } + } + if (istr & USB_ISTR_RESET) { + // USB Reset + USB->ISTR = (uint16_t)~USB_ISTR_RESET; + usb_reset(); + } +} diff --git a/src/stm32/watchdog.c b/src/stm32/watchdog.c new file mode 100644 index 00000000..696d64d6 --- /dev/null +++ b/src/stm32/watchdog.c @@ -0,0 +1,25 @@ +// Watchdog handler on STM32 +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "internal.h" // IWDG +#include "sched.h" // DECL_TASK + +void +watchdog_reset(void) +{ + IWDG->KR = 0xAAAA; +} +DECL_TASK(watchdog_reset); + +void +watchdog_init(void) +{ + IWDG->KR = 0x5555; + IWDG->PR = 0; + IWDG->RLR = 0x0FFF; // 410-512ms timeout (depending on stm32 chip) + IWDG->KR = 0xCCCC; +} +DECL_INIT(watchdog_init); -- cgit v1.2.3-70-g09d2