From ca15a0bf818b2e2919140d1d16348d6cb02cd30a Mon Sep 17 00:00:00 2001 From: Desuuuu Date: Fri, 10 Sep 2021 01:08:20 +0200 Subject: [PATCH] serial: Make serial runtime-configurable Rework `generic/serial_irq` to handle multiple serial connections. Buffers are now allocated dynamically during setup. Rework board-specific serial implementations to handle multiple serial connections and runtime configuration. Move host-communication related code to separate files. `sendf()` now returns a value indicating whether it succeeds or not. Host-communication serial interrupts are given priority over other serial interrupts. For STM32: merge STM32F0 serial implementation with generic STM32 serial implementation and use a separate, static implementation on STM32F031 to limit footprint. --- src/atsam/Makefile | 3 +- src/atsam/gpio.h | 11 ++ src/atsam/serial.c | 332 ++++++++++++++++++++++++++++++----- src/atsam/serial_host.c | 42 +++++ src/atsamd/Makefile | 3 +- src/atsamd/gpio.h | 10 ++ src/atsamd/internal.h | 4 + src/atsamd/sercom.c | 79 +++++++-- src/atsamd/serial.c | 309 +++++++++++++++++++++++++++----- src/atsamd/serial_host.c | 41 +++++ src/avr/Makefile | 3 +- src/avr/gpio.h | 11 ++ src/avr/serial.c | 232 +++++++++++++++++------- src/avr/serial_host.c | 62 +++++++ src/command.c | 8 +- src/command.h | 2 +- src/generic/canbus.c | 5 +- src/generic/gpio.h | 10 ++ src/generic/misc.h | 2 +- src/generic/serial_irq.c | 258 +++++++++++++++++++++------ src/generic/serial_irq.h | 20 ++- src/generic/usb_cdc.c | 5 +- src/linux/console.c | 8 +- src/lpc176x/Makefile | 3 +- src/lpc176x/gpio.h | 11 ++ src/lpc176x/internal.h | 2 + src/lpc176x/serial.c | 185 ++++++++++++------- src/lpc176x/serial_host.c | 39 ++++ src/pru/main.c | 6 +- src/pru/pru0.c | 3 +- src/rp2040/Makefile | 3 +- src/rp2040/gpio.h | 11 ++ src/rp2040/serial.c | 127 +++++++++----- src/rp2040/serial_host.c | 35 ++++ src/simulator/Makefile | 3 +- src/simulator/serial.c | 36 ++-- src/simulator/serial_host.c | 38 ++++ src/stm32/Makefile | 5 +- src/stm32/gpio.h | 15 ++ src/stm32/serial.c | 197 ++++++++++++++------- src/stm32/serial_host.c | 56 ++++++ src/stm32/stm32f031_serial.c | 80 +++++++++ src/stm32/stm32f0_serial.c | 91 ---------- 43 files changed, 1894 insertions(+), 512 deletions(-) create mode 100644 src/atsam/serial_host.c create mode 100644 src/atsamd/serial_host.c create mode 100644 src/avr/serial_host.c create mode 100644 src/lpc176x/serial_host.c create mode 100644 src/rp2040/serial_host.c create mode 100644 src/simulator/serial_host.c create mode 100644 src/stm32/serial_host.c create mode 100644 src/stm32/stm32f031_serial.c delete mode 100644 src/stm32/stm32f0_serial.c diff --git a/src/atsam/Makefile b/src/atsam/Makefile index 359cfc3d..9c087e75 100644 --- a/src/atsam/Makefile +++ b/src/atsam/Makefile @@ -25,11 +25,12 @@ $(OUT)klipper.elf: $(OUT)src/generic/armcm_link.ld # Add source files src-y += atsam/main.c atsam/gpio.c atsam/i2c.c atsam/spi.c atsam/hard_pwm.c src-y += generic/armcm_boot.c generic/armcm_irq.c generic/armcm_timer.c +src-y += atsam/serial.c generic/serial_irq.c src-y += generic/crc16_ccitt.c usb-src-$(CONFIG_MACH_SAM3X) := atsam/sam3_usb.c usb-src-$(CONFIG_MACH_SAM4) := atsam/sam4_usb.c src-$(CONFIG_USBSERIAL) += $(usb-src-y) atsam/chipid.c generic/usb_cdc.c -src-$(CONFIG_SERIAL) += atsam/serial.c generic/serial_irq.c +src-$(CONFIG_SERIAL) += atsam/serial_host.c src-$(CONFIG_MACH_SAM3X) += atsam/adc.c src-$(CONFIG_MACH_SAM4S) += atsam/adc.c src-$(CONFIG_MACH_SAM4E) += atsam/sam4e_afec.c atsam/sam4_cache.c diff --git a/src/atsam/gpio.h b/src/atsam/gpio.h index e9fc4c3e..86cc9bec 100644 --- a/src/atsam/gpio.h +++ b/src/atsam/gpio.h @@ -54,4 +54,15 @@ void i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write); void i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg , uint8_t read_len, uint8_t *read); +#define GPIO_UART_MAX_ID 5 + +struct uart_config { + void *dev; + uint8_t is_usart; +}; + +struct uart_config uart_setup(uint8_t bus, uint32_t baud, uint8_t *id + , uint32_t priority); +void uart_enable_tx_irq(struct uart_config config); + #endif // gpio.h diff --git a/src/atsam/serial.c b/src/atsam/serial.c index cf69acb0..81acda97 100644 --- a/src/atsam/serial.c +++ b/src/atsam/serial.c @@ -4,75 +4,317 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. -#include "autoconf.h" // CONFIG_SERIAL_BAUD -#include "board/armcm_boot.h" // armcm_enable_irq -#include "board/serial_irq.h" // serial_rx_data -#include "command.h" // DECL_CONSTANT_STR +#include // uint32_t +#include "autoconf.h" // CONFIG_MACH_* +#include "board/armcm_boot.h" // DECL_ARMCM_IRQ +#include "board/serial_irq.h" // serial_rx_byte +#include "command.h" // DECL_ENUMERATION +#include "gpio.h" // uart_setup #include "internal.h" // gpio_peripheral -#include "sched.h" // DECL_INIT +#include "sched.h" // sched_shutdown + +struct bus_info { + uint8_t id, is_usart; + void *dev; + uint32_t dev_id; + IRQn_Type irqn; + uint8_t rx_pin, tx_pin, function; +}; -// Serial port pins #if CONFIG_MACH_SAM3X -#define UARTx_IRQn UART_IRQn -static Uart * const Port = UART; -static const uint32_t Pmc_id = ID_UART; -static const uint32_t rx_pin = GPIO('A', 8), tx_pin = GPIO('A', 9); -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA8,PA9"); -#elif CONFIG_MACH_SAM4S -#define UARTx_IRQn UART1_IRQn -static Uart * const Port = UART1; -static const uint32_t Pmc_id = ID_UART1; -static const uint32_t rx_pin = GPIO('B', 2), tx_pin = GPIO('B', 3); -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PB2,PB3"); -#elif CONFIG_MACH_SAM4E -#define UARTx_IRQn UART0_IRQn -static Uart * const Port = UART0; -static const uint32_t Pmc_id = ID_UART0; -static const uint32_t rx_pin = GPIO('A', 9), tx_pin = GPIO('A', 10); -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA9,PA10"); + +DECL_ENUMERATION("uart_bus", "uart", 0); +DECL_ENUMERATION_RANGE("uart_bus", "usart0", 1, 3); +DECL_CONSTANT_STR("BUS_PINS_uart", "[_],PA8,PA9"); +DECL_CONSTANT_STR("BUS_PINS_usart0", "[_],PA10,PA11"); +DECL_CONSTANT_STR("BUS_PINS_usart1", "[_],PA12,PA13"); +DECL_CONSTANT_STR("BUS_PINS_usart2", "[_],PB21,PB20"); + +#ifdef USART3 + DECL_ENUMERATION("uart_bus", "usart3", 4); + DECL_CONSTANT_STR("BUS_PINS_usart3", "[_],PD5,PD4"); #endif +static const struct bus_info bus_data[] = { + { 0, 0, UART, ID_UART, UART_IRQn, GPIO('A', 8), GPIO('A', 9), 'A' }, + { 1, 1, USART0, ID_USART0, USART0_IRQn, GPIO('A', 10), GPIO('A', 11), 'A' }, + { 2, 1, USART1, ID_USART1, USART1_IRQn, GPIO('A', 12), GPIO('A', 13), 'A' }, + { 3, 1, USART2, ID_USART2, USART2_IRQn, GPIO('B', 21), GPIO('B', 20), 'A' }, +#ifdef USART3 + { 4, 1, USART3, ID_USART3, USART3_IRQn, GPIO('D', 5), GPIO('D', 4), 'B' }, +#endif +}; + void -UARTx_Handler(void) +UART_IRQHandler(void) { - uint32_t status = Port->UART_SR; + uint32_t status = UART->UART_SR; if (status & UART_SR_RXRDY) - serial_rx_byte(Port->UART_RHR); + serial_rx_byte(0, UART->UART_RHR); if (status & UART_SR_TXRDY) { uint8_t data; - int ret = serial_get_tx_byte(&data); + int ret = serial_get_tx_byte(0, &data); if (ret) - Port->UART_IDR = UART_IDR_TXRDY; + UART->UART_IDR = UART_IDR_TXRDY; else - Port->UART_THR = data; + UART->UART_THR = data; } } +DECL_ARMCM_IRQ(UART_IRQHandler, UART_IRQn); void -serial_enable_tx_irq(void) +USART0_IRQHandler(void) { - Port->UART_IER = UART_IDR_TXRDY; + uint32_t status = USART0->US_CSR; + if (status & US_CSR_RXRDY) + serial_rx_byte(1, USART0->US_RHR); + if (status & US_CSR_TXRDY) { + uint8_t data; + int ret = serial_get_tx_byte(1, &data); + if (ret) + USART0->US_IDR = US_IDR_TXRDY; + else + USART0->US_THR = data; + } } +DECL_ARMCM_IRQ(USART0_IRQHandler, USART0_IRQn); void -serial_init(void) +USART1_IRQHandler(void) { - gpio_peripheral(rx_pin, 'A', 1); - gpio_peripheral(tx_pin, 'A', 0); + uint32_t status = USART1->US_CSR; + if (status & US_CSR_RXRDY) + serial_rx_byte(2, USART1->US_RHR); + if (status & US_CSR_TXRDY) { + uint8_t data; + int ret = serial_get_tx_byte(2, &data); + if (ret) + USART1->US_IDR = US_IDR_TXRDY; + else + USART1->US_THR = data; + } +} +DECL_ARMCM_IRQ(USART1_IRQHandler, USART1_IRQn); + +void +USART2_IRQHandler(void) +{ + uint32_t status = USART2->US_CSR; + if (status & US_CSR_RXRDY) + serial_rx_byte(3, USART2->US_RHR); + if (status & US_CSR_TXRDY) { + uint8_t data; + int ret = serial_get_tx_byte(3, &data); + if (ret) + USART2->US_IDR = US_IDR_TXRDY; + else + USART2->US_THR = data; + } +} +DECL_ARMCM_IRQ(USART2_IRQHandler, USART2_IRQn); + +#ifdef USART3 + +void +USART3_IRQHandler(void) +{ + uint32_t status = USART3->US_CSR; + if (status & US_CSR_RXRDY) + serial_rx_byte(4, USART3->US_RHR); + if (status & US_CSR_TXRDY) { + uint8_t data; + int ret = serial_get_tx_byte(4, &data); + if (ret) + USART3->US_IDR = US_IDR_TXRDY; + else + USART3->US_THR = data; + } +} +DECL_ARMCM_IRQ(USART3_IRQHandler, USART3_IRQn); + +#endif // USART3 + +#else + +DECL_ENUMERATION_RANGE("uart_bus", "uart0", 0, 2); +DECL_ENUMERATION_RANGE("uart_bus", "usart0", 2, 2); +DECL_CONSTANT_STR("BUS_PINS_uart0", "[_],PA9,PA10"); +#if CONFIG_MACH_SAM4S +DECL_CONSTANT_STR("BUS_PINS_uart1", "[_],PB2,PB3"); +DECL_CONSTANT_STR("BUS_PINS_usart0", "[_],PA5,PA6"); +#else +DECL_CONSTANT_STR("BUS_PINS_uart1", "[_],PA5,PA6"); +DECL_CONSTANT_STR("BUS_PINS_usart0", "[_],PB0,PB1"); +#endif +DECL_CONSTANT_STR("BUS_PINS_usart1", "[_],PA21,PA22"); + +static const struct bus_info bus_data[] = { + { 0, 0, UART0, ID_UART0, UART0_IRQn, GPIO('A', 9), GPIO('A', 10), 'A' }, +#if CONFIG_MACH_SAM4S + { 1, 0, UART1, ID_UART1, UART1_IRQn, GPIO('B', 2), GPIO('B', 3), 'A' }, + { 2, 1, USART0, ID_USART0, USART0_IRQn, GPIO('A', 5), GPIO('A', 6), 'A' }, +#else + { 1, 0, UART1, ID_UART1, UART1_IRQn, GPIO('A', 5), GPIO('A', 6), 'C' }, + { 2, 1, USART0, ID_USART0, USART0_IRQn, GPIO('B', 0), GPIO('B', 1), 'C' }, +#endif + { 3, 1, USART1, ID_USART1, USART1_IRQn, GPIO('A', 21), GPIO('A', 22), 'A' }, +}; + +void +UART0_IRQHandler(void) +{ + uint32_t status = UART0->UART_SR; + if (status & UART_SR_RXRDY) + serial_rx_byte(0, UART0->UART_RHR); + if (status & UART_SR_TXRDY) { + uint8_t data; + int ret = serial_get_tx_byte(0, &data); + if (ret) + UART0->UART_IDR = UART_IDR_TXRDY; + else + UART0->UART_THR = data; + } +} +DECL_ARMCM_IRQ(UART0_IRQHandler, UART0_IRQn); + +void +UART1_IRQHandler(void) +{ + uint32_t status = UART1->UART_SR; + if (status & UART_SR_RXRDY) + serial_rx_byte(1, UART1->UART_RHR); + if (status & UART_SR_TXRDY) { + uint8_t data; + int ret = serial_get_tx_byte(1, &data); + if (ret) + UART1->UART_IDR = UART_IDR_TXRDY; + else + UART1->UART_THR = data; + } +} +DECL_ARMCM_IRQ(UART1_IRQHandler, UART1_IRQn); + +void +USART0_IRQHandler(void) +{ + uint32_t status = USART0->US_CSR; + if (status & US_CSR_RXRDY) + serial_rx_byte(2, USART0->US_RHR); + if (status & US_CSR_TXRDY) { + uint8_t data; + int ret = serial_get_tx_byte(2, &data); + if (ret) + USART0->US_IDR = US_IDR_TXRDY; + else + USART0->US_THR = data; + } +} +DECL_ARMCM_IRQ(USART0_IRQHandler, USART0_IRQn); + +void +USART1_IRQHandler(void) +{ + uint32_t status = USART1->US_CSR; + if (status & US_CSR_RXRDY) + serial_rx_byte(3, USART1->US_RHR); + if (status & US_CSR_TXRDY) { + uint8_t data; + int ret = serial_get_tx_byte(3, &data); + if (ret) + USART1->US_IDR = US_IDR_TXRDY; + else + USART1->US_THR = data; + } +} +DECL_ARMCM_IRQ(USART1_IRQHandler, USART1_IRQn); + +#endif + +struct uart_config +setup_dev_uart(const struct bus_info *bi, uint32_t baud, uint8_t *id + , uint32_t priority) +{ + Uart *uart = bi->dev; + + gpio_peripheral(bi->rx_pin, bi->function, 1); + gpio_peripheral(bi->tx_pin, bi->function, 0); // Reset uart - enable_pclock(Pmc_id); - Port->UART_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS; - Port->UART_CR = (UART_CR_RSTRX | UART_CR_RSTTX + enable_pclock(bi->dev_id); + uart->UART_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS; + uart->UART_CR = (UART_CR_RSTRX | UART_CR_RSTTX | UART_CR_RXDIS | UART_CR_TXDIS); - Port->UART_IDR = 0xFFFFFFFF; + uart->UART_IDR = 0xFFFFFFFF; // Enable uart - Port->UART_MR = (US_MR_CHRL_8_BIT | US_MR_NBSTOP_1_BIT | UART_MR_PAR_NO - | UART_MR_CHMODE_NORMAL); - Port->UART_BRGR = SystemCoreClock / (16 * CONFIG_SERIAL_BAUD); - Port->UART_IER = UART_IER_RXRDY; - armcm_enable_irq(UARTx_Handler, UARTx_IRQn, 0); - Port->UART_CR = UART_CR_RXEN | UART_CR_TXEN; + uart->UART_MR = (UART_MR_PAR_NO | UART_MR_CHMODE_NORMAL); + uint32_t div = DIV_ROUND_CLOSEST(SystemCoreClock, 16 * baud); + uart->UART_BRGR = UART_BRGR_CD(div); + + uart->UART_IER = UART_IER_RXRDY; + NVIC_SetPriority(bi->irqn, priority); + NVIC_EnableIRQ(bi->irqn); + uart->UART_CR = UART_CR_RXEN | UART_CR_TXEN; + + *id = bi->id; + + return (struct uart_config){ .dev=uart, .is_usart=0 }; +} + +struct uart_config +setup_dev_usart(const struct bus_info *bi, uint32_t baud, uint8_t *id + , uint32_t priority) +{ + Usart *usart = bi->dev; + + gpio_peripheral(bi->rx_pin, bi->function, 1); + gpio_peripheral(bi->tx_pin, bi->function, 0); + + // Reset usart + enable_pclock(bi->dev_id); + usart->US_PTCR = US_PTCR_RXTDIS | US_PTCR_TXTDIS; + usart->US_CR = (US_CR_RSTRX | US_CR_RSTTX + | US_CR_RXDIS | US_CR_TXDIS); + usart->US_IDR = 0xFFFFFFFF; + + // Enable usart + usart->US_MR = (US_MR_CHRL_8_BIT | US_MR_NBSTOP_1_BIT | US_MR_PAR_NO + | US_MR_USART_MODE_NORMAL | US_MR_CHMODE_NORMAL + | US_MR_USCLKS_MCK); + uint32_t div = DIV_ROUND_CLOSEST(SystemCoreClock, 16 * baud); + usart->US_BRGR = US_BRGR_CD(div); + + usart->US_IER = US_IER_RXRDY; + NVIC_SetPriority(bi->irqn, priority); + NVIC_EnableIRQ(bi->irqn); + usart->US_CR = US_CR_RXEN | US_CR_TXEN; + + *id = bi->id; + + return (struct uart_config){ .dev=usart, .is_usart=1 }; +} + +struct uart_config +uart_setup(uint8_t bus, uint32_t baud, uint8_t *id, uint32_t priority) +{ + if (bus >= ARRAY_SIZE(bus_data)) + shutdown("Invalid UART config"); + + if (bus_data[bus].is_usart) { + return setup_dev_usart(&bus_data[bus], baud, id, priority); + } + + return setup_dev_uart(&bus_data[bus], baud, id, priority); +} + +void +uart_enable_tx_irq(struct uart_config config) +{ + if (config.is_usart) { + Usart *usart = config.dev; + usart->US_IER = US_IER_TXRDY; + } else { + Uart *uart = config.dev; + uart->UART_IER = UART_IER_TXRDY; + } } -DECL_INIT(serial_init); diff --git a/src/atsam/serial_host.c b/src/atsam/serial_host.c new file mode 100644 index 00000000..af137abe --- /dev/null +++ b/src/atsam/serial_host.c @@ -0,0 +1,42 @@ +// Host serial communication initialization +// +// Copyright (C) 2021 Desuuuu +// +// 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_prepare +#include "sched.h" // DECL_INIT +#include "command.h" // DECL_CONSTANT + +#define RX_BUFFER_SIZE 192 + +DECL_CONSTANT("SERIAL_BAUD", CONFIG_SERIAL_BAUD); +DECL_CONSTANT("RECEIVE_WINDOW", RX_BUFFER_SIZE); + +#if CONFIG_MACH_SAM3X + #define CONSOLE_UART_BUS 0 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[uart],PA8,PA9"); +#elif CONFIG_MACH_SAM4S + #define CONSOLE_UART_BUS 1 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[uart1],PB2,PB3"); +#elif CONFIG_MACH_SAM4E + #define CONSOLE_UART_BUS 0 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[uart0],PA9,PA10"); +#endif + +static uint8_t serial_host_id; + +void +serial_init(void) +{ + serial_host_id = serial_prepare(CONSOLE_UART_BUS, CONFIG_SERIAL_BAUD, + RX_BUFFER_SIZE, 96, 0, NULL); +} +DECL_INIT(serial_init); + +uint_fast8_t +console_sendf(const struct command_encoder *ce, va_list args) +{ + return serial_send_command(serial_host_id, ce, args); +} diff --git a/src/atsamd/Makefile b/src/atsamd/Makefile index 3fe2a4e9..3ac6a652 100644 --- a/src/atsamd/Makefile +++ b/src/atsamd/Makefile @@ -21,8 +21,9 @@ $(OUT)klipper.elf: $(OUT)src/generic/armcm_link.ld # Add source files src-y += atsamd/main.c atsamd/gpio.c generic/crc16_ccitt.c src-y += generic/armcm_boot.c generic/armcm_irq.c generic/armcm_reset.c +src-y += atsamd/serial.c generic/serial_irq.c src-$(CONFIG_USBSERIAL) += atsamd/usbserial.c atsamd/chipid.c generic/usb_cdc.c -src-$(CONFIG_SERIAL) += atsamd/serial.c generic/serial_irq.c +src-$(CONFIG_SERIAL) += atsamd/serial_host.c src-$(CONFIG_HAVE_GPIO_ADC) += atsamd/adc.c src-$(CONFIG_HAVE_GPIO_I2C) += atsamd/i2c.c src-$(CONFIG_HAVE_GPIO_SPI) += atsamd/spi.c diff --git a/src/atsamd/gpio.h b/src/atsamd/gpio.h index b8cb3e86..6391d195 100644 --- a/src/atsamd/gpio.h +++ b/src/atsamd/gpio.h @@ -55,4 +55,14 @@ void i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write); void i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg , uint8_t read_len, uint8_t *read); +#define GPIO_UART_MAX_ID 8 + +struct uart_config { + void *usart; +}; + +struct uart_config uart_setup(uint8_t bus, uint32_t baud, uint8_t *id + , uint32_t priority); +void uart_enable_tx_irq(struct uart_config config); + #endif // gpio.h diff --git a/src/atsamd/internal.h b/src/atsamd/internal.h index f31be95c..da2e9ed4 100644 --- a/src/atsamd/internal.h +++ b/src/atsamd/internal.h @@ -22,9 +22,13 @@ void enable_pclock(uint32_t pclk_id, uint32_t pm_id); uint32_t get_pclock_frequency(uint32_t pclk_id); void gpio_peripheral(uint32_t gpio, char ptype, int32_t pull_up); +enum { SERCOM_TX_PIN, SERCOM_RX_PIN, SERCOM_CLK_PIN }; + Sercom * sercom_enable_pclock(uint32_t sercom_id); uint32_t sercom_get_pclock_frequency(uint32_t sercom_id); +void sercom_set_pin(uint8_t sercom_id, uint8_t pin_type, uint8_t pin); uint32_t sercom_spi_pins(uint32_t sercom_id); void sercom_i2c_pins(uint32_t sercom_id); +uint32_t sercom_usart_pins(uint32_t sercom_id); #endif // internal.h diff --git a/src/atsamd/sercom.c b/src/atsamd/sercom.c index 2d02ca44..d49b5b1c 100644 --- a/src/atsamd/sercom.c +++ b/src/atsamd/sercom.c @@ -290,10 +290,9 @@ sercom_lookup_pad(uint32_t sercom_id, uint8_t pin) * Runtime configuration ****************************************************************/ -enum { TX_PIN, RX_PIN, CLK_PIN }; -DECL_ENUMERATION("sercom_pin_type", "tx", TX_PIN); -DECL_ENUMERATION("sercom_pin_type", "rx", RX_PIN); -DECL_ENUMERATION("sercom_pin_type", "clk", CLK_PIN); +DECL_ENUMERATION("sercom_pin_type", "tx", SERCOM_TX_PIN); +DECL_ENUMERATION("sercom_pin_type", "rx", SERCOM_RX_PIN); +DECL_ENUMERATION("sercom_pin_type", "clk", SERCOM_CLK_PIN); // Runtime configuration struct sercom_pin { @@ -303,14 +302,19 @@ struct sercom_pin { static struct sercom_pin sercom_pins[ARRAY_SIZE(sercoms)]; void -command_set_sercom_pin(uint32_t *args) +sercom_set_pin(uint8_t sercom_id, uint8_t pin_type, uint8_t pin) { - uint8_t sercom_id = args[0], pin_type = args[1], pin = args[2]; if (sercom_id >= ARRAY_SIZE(sercom_pins) || pin_type >= ARRAY_SIZE(sercom_pins[0].pins)) shutdown("Invalid SERCOM bus"); sercom_pins[sercom_id].pins[pin_type] = pin; } + +void +command_set_sercom_pin(uint32_t *args) +{ + sercom_set_pin(args[0], args[1], args[2]); +} DECL_COMMAND(command_set_sercom_pin, "set_sercom_pin bus=%u sercom_pin_type=%u pin=%u"); @@ -345,6 +349,36 @@ sercom_lookup_spi_dopo(uint8_t tx_pad, uint8_t clk_pad) } +/**************************************************************** + * USART txpo flag mapping + ****************************************************************/ + +struct sercom_usart_map { + uint8_t tx_pad, txpo; +}; + +static const struct sercom_usart_map sercom_usart[] = { +#if CONFIG_MACH_SAMD21 + { 0, 0 }, + { 2, 1 }, +#elif CONFIG_MACH_SAMD51 + { 0, 0 }, +#endif +}; + +static uint8_t +sercom_lookup_usart_txpo(uint8_t tx_pad) +{ + const struct sercom_usart_map *um = sercom_usart; + for (; ; um++) { + if (um >= &sercom_usart[ARRAY_SIZE(sercom_usart)]) + shutdown("Invalid TX pin"); + if (um->tx_pad == tx_pad) + return um->txpo; + } +} + + /**************************************************************** * Pin setup ****************************************************************/ @@ -354,11 +388,11 @@ sercom_spi_pins(uint32_t sercom_id) { if (sercom_id >= ARRAY_SIZE(sercom_pins)) shutdown("Invalid SERCOM bus"); - uint8_t tx_pin = sercom_pins[sercom_id].pins[TX_PIN]; + uint8_t tx_pin = sercom_pins[sercom_id].pins[SERCOM_TX_PIN]; const struct sercom_pad *tx_sp = sercom_lookup_pad(sercom_id, tx_pin); - uint8_t rx_pin = sercom_pins[sercom_id].pins[RX_PIN]; + uint8_t rx_pin = sercom_pins[sercom_id].pins[SERCOM_RX_PIN]; const struct sercom_pad *rx_sp = sercom_lookup_pad(sercom_id, rx_pin); - uint8_t clk_pin = sercom_pins[sercom_id].pins[CLK_PIN]; + uint8_t clk_pin = sercom_pins[sercom_id].pins[SERCOM_CLK_PIN]; const struct sercom_pad *clk_sp = sercom_lookup_pad(sercom_id, clk_pin); uint8_t dopo = sercom_lookup_spi_dopo(tx_sp->pad, clk_sp->pad); @@ -374,9 +408,11 @@ sercom_spi_pins(uint32_t sercom_id) void sercom_i2c_pins(uint32_t sercom_id) { - uint8_t tx_pin = sercom_pins[sercom_id].pins[TX_PIN]; + if (sercom_id >= ARRAY_SIZE(sercom_pins)) + shutdown("Invalid SERCOM bus"); + uint8_t tx_pin = sercom_pins[sercom_id].pins[SERCOM_TX_PIN]; const struct sercom_pad *tx_sp = sercom_lookup_pad(sercom_id, tx_pin); - uint8_t clk_pin = sercom_pins[sercom_id].pins[CLK_PIN]; + uint8_t clk_pin = sercom_pins[sercom_id].pins[SERCOM_CLK_PIN]; const struct sercom_pad *clk_sp = sercom_lookup_pad(sercom_id, clk_pin); if (tx_sp->pad != 0 || clk_sp->pad != 1) @@ -385,3 +421,24 @@ sercom_i2c_pins(uint32_t sercom_id) gpio_peripheral(tx_pin, tx_sp->ptype, 0); gpio_peripheral(clk_pin, clk_sp->ptype, 0); } + +uint32_t +sercom_usart_pins(uint32_t sercom_id) +{ + if (sercom_id >= ARRAY_SIZE(sercom_pins)) + shutdown("Invalid SERCOM bus"); + uint8_t rx_pin = sercom_pins[sercom_id].pins[SERCOM_RX_PIN]; + const struct sercom_pad *rx_sp = sercom_lookup_pad(sercom_id, rx_pin); + uint8_t tx_pin = sercom_pins[sercom_id].pins[SERCOM_TX_PIN]; + const struct sercom_pad *tx_sp = sercom_lookup_pad(sercom_id, tx_pin); + + uint8_t txpo = sercom_lookup_usart_txpo(tx_sp->pad); + if (rx_sp->pad == tx_sp->pad) + shutdown("Sercom RX pad collides with TX pad"); + + gpio_peripheral(rx_pin, rx_sp->ptype, 0); + gpio_peripheral(tx_pin, tx_sp->ptype, 0); + + return (SERCOM_USART_CTRLA_RXPO(rx_sp->pad) + | SERCOM_USART_CTRLA_TXPO(txpo)); +} diff --git a/src/atsamd/serial.c b/src/atsamd/serial.c index 5b45931a..a8cde87f 100644 --- a/src/atsamd/serial.c +++ b/src/atsamd/serial.c @@ -4,68 +4,297 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. -#include "board/armcm_boot.h" // armcm_enable_irq -#include "board/serial_irq.h" // serial_rx_data -#include "command.h" // DECL_CONSTANT_STR +#include // uint32_t +#include "autoconf.h" // CONFIG_MACH_* +#include "board/armcm_boot.h" // DECL_ARMCM_IRQ +#include "board/serial_irq.h" // serial_rx_byte +#include "command.h" // DECL_ENUMERATION +#include "gpio.h" // uart_setup #include "internal.h" // enable_pclock -#include "sched.h" // DECL_INIT +#include "sched.h" // sched_shutdown + +#if CONFIG_MACH_SAMD21 + #define SAMD_IRQn(sercom) SERCOM ## sercom ## _IRQn +#elif CONFIG_MACH_SAMD51 + #define SAMD_x_IRQn(sercom, x) SERCOM ## sercom ## _ ## x ## _IRQn + #define SAMD_IRQn(sercom) SAMD_x_IRQn(sercom, 0), SAMD_x_IRQn(sercom, 1), \ + SAMD_x_IRQn(sercom, 2), SAMD_x_IRQn(sercom, 3) +#endif + +struct bus_info { + uint8_t id; + Sercom *sercom; +#if CONFIG_MACH_SAMD21 + IRQn_Type irqn; +#elif CONFIG_MACH_SAMD51 + IRQn_Type irqn0, irqn1, irqn2, irqn3; +#endif +}; + +static const struct bus_info bus_data[] = { + { 0, SERCOM0, SAMD_IRQn(0) }, + { 1, SERCOM1, SAMD_IRQn(1) }, + { 2, SERCOM2, SAMD_IRQn(2) }, + { 3, SERCOM3, SAMD_IRQn(3) }, +#ifdef SERCOM4 + { 4, SERCOM4, SAMD_IRQn(4) }, + { 5, SERCOM5, SAMD_IRQn(5) }, + #ifdef SERCOM6 + { 6, SERCOM6, SAMD_IRQn(6) }, + { 7, SERCOM7, SAMD_IRQn(7) }, + #endif +#endif +}; void -serial_enable_tx_irq(void) -{ - SERCOM0->USART.INTENSET.reg = SERCOM_USART_INTENSET_DRE; -} - -void -SERCOM0_Handler(void) +SERCOM0_IRQHandler(void) { uint32_t status = SERCOM0->USART.INTFLAG.reg; if (status & SERCOM_USART_INTFLAG_RXC) - serial_rx_byte(SERCOM0->USART.DATA.reg); + serial_rx_byte(0, SERCOM0->USART.DATA.reg); if (status & SERCOM_USART_INTFLAG_DRE) { uint8_t data; - int ret = serial_get_tx_byte(&data); + int ret = serial_get_tx_byte(0, &data); if (ret) SERCOM0->USART.INTENCLR.reg = SERCOM_USART_INTENSET_DRE; else SERCOM0->USART.DATA.reg = data; } } - -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA11,PA10"); +#if CONFIG_MACH_SAMD21 +DECL_ARMCM_IRQ(SERCOM0_IRQHandler, SERCOM0_IRQn); +#elif CONFIG_MACH_SAMD51 +DECL_ARMCM_IRQ(SERCOM0_IRQHandler, SERCOM0_0_IRQn); +DECL_ARMCM_IRQ(SERCOM0_IRQHandler, SERCOM0_1_IRQn); +DECL_ARMCM_IRQ(SERCOM0_IRQHandler, SERCOM0_2_IRQn); +DECL_ARMCM_IRQ(SERCOM0_IRQHandler, SERCOM0_3_IRQn); +#endif void -serial_init(void) +SERCOM1_IRQHandler(void) { - // Enable serial clock - enable_pclock(SERCOM0_GCLK_ID_CORE, ID_SERCOM0); - // Enable pins - gpio_peripheral(GPIO('A', 11), 'C', 0); - gpio_peripheral(GPIO('A', 10), 'C', 0); - // Configure serial - SercomUsart *su = &SERCOM0->USART; - su->CTRLA.reg = 0; + uint32_t status = SERCOM1->USART.INTFLAG.reg; + if (status & SERCOM_USART_INTFLAG_RXC) + serial_rx_byte(1, SERCOM1->USART.DATA.reg); + if (status & SERCOM_USART_INTFLAG_DRE) { + uint8_t data; + int ret = serial_get_tx_byte(1, &data); + if (ret) + SERCOM1->USART.INTENCLR.reg = SERCOM_USART_INTENSET_DRE; + else + SERCOM1->USART.DATA.reg = data; + } +} +#if CONFIG_MACH_SAMD21 +DECL_ARMCM_IRQ(SERCOM1_IRQHandler, SERCOM1_IRQn); +#elif CONFIG_MACH_SAMD51 +DECL_ARMCM_IRQ(SERCOM1_IRQHandler, SERCOM1_0_IRQn); +DECL_ARMCM_IRQ(SERCOM1_IRQHandler, SERCOM1_1_IRQn); +DECL_ARMCM_IRQ(SERCOM1_IRQHandler, SERCOM1_2_IRQn); +DECL_ARMCM_IRQ(SERCOM1_IRQHandler, SERCOM1_3_IRQn); +#endif + +void +SERCOM2_IRQHandler(void) +{ + uint32_t status = SERCOM2->USART.INTFLAG.reg; + if (status & SERCOM_USART_INTFLAG_RXC) + serial_rx_byte(2, SERCOM2->USART.DATA.reg); + if (status & SERCOM_USART_INTFLAG_DRE) { + uint8_t data; + int ret = serial_get_tx_byte(2, &data); + if (ret) + SERCOM2->USART.INTENCLR.reg = SERCOM_USART_INTENSET_DRE; + else + SERCOM2->USART.DATA.reg = data; + } +} +#if CONFIG_MACH_SAMD21 +DECL_ARMCM_IRQ(SERCOM2_IRQHandler, SERCOM2_IRQn); +#elif CONFIG_MACH_SAMD51 +DECL_ARMCM_IRQ(SERCOM2_IRQHandler, SERCOM2_0_IRQn); +DECL_ARMCM_IRQ(SERCOM2_IRQHandler, SERCOM2_1_IRQn); +DECL_ARMCM_IRQ(SERCOM2_IRQHandler, SERCOM2_2_IRQn); +DECL_ARMCM_IRQ(SERCOM2_IRQHandler, SERCOM2_3_IRQn); +#endif + +void +SERCOM3_IRQHandler(void) +{ + uint32_t status = SERCOM3->USART.INTFLAG.reg; + if (status & SERCOM_USART_INTFLAG_RXC) + serial_rx_byte(3, SERCOM3->USART.DATA.reg); + if (status & SERCOM_USART_INTFLAG_DRE) { + uint8_t data; + int ret = serial_get_tx_byte(3, &data); + if (ret) + SERCOM3->USART.INTENCLR.reg = SERCOM_USART_INTENSET_DRE; + else + SERCOM3->USART.DATA.reg = data; + } +} +#if CONFIG_MACH_SAMD21 +DECL_ARMCM_IRQ(SERCOM3_IRQHandler, SERCOM3_IRQn); +#elif CONFIG_MACH_SAMD51 +DECL_ARMCM_IRQ(SERCOM3_IRQHandler, SERCOM3_0_IRQn); +DECL_ARMCM_IRQ(SERCOM3_IRQHandler, SERCOM3_1_IRQn); +DECL_ARMCM_IRQ(SERCOM3_IRQHandler, SERCOM3_2_IRQn); +DECL_ARMCM_IRQ(SERCOM3_IRQHandler, SERCOM3_3_IRQn); +#endif + +#ifdef SERCOM4 + +void +SERCOM4_IRQHandler(void) +{ + uint32_t status = SERCOM4->USART.INTFLAG.reg; + if (status & SERCOM_USART_INTFLAG_RXC) + serial_rx_byte(4, SERCOM4->USART.DATA.reg); + if (status & SERCOM_USART_INTFLAG_DRE) { + uint8_t data; + int ret = serial_get_tx_byte(4, &data); + if (ret) + SERCOM4->USART.INTENCLR.reg = SERCOM_USART_INTENSET_DRE; + else + SERCOM4->USART.DATA.reg = data; + } +} +#if CONFIG_MACH_SAMD21 +DECL_ARMCM_IRQ(SERCOM4_IRQHandler, SERCOM4_IRQn); +#elif CONFIG_MACH_SAMD51 +DECL_ARMCM_IRQ(SERCOM4_IRQHandler, SERCOM4_0_IRQn); +DECL_ARMCM_IRQ(SERCOM4_IRQHandler, SERCOM4_1_IRQn); +DECL_ARMCM_IRQ(SERCOM4_IRQHandler, SERCOM4_2_IRQn); +DECL_ARMCM_IRQ(SERCOM4_IRQHandler, SERCOM4_3_IRQn); +#endif + +void +SERCOM5_IRQHandler(void) +{ + uint32_t status = SERCOM5->USART.INTFLAG.reg; + if (status & SERCOM_USART_INTFLAG_RXC) + serial_rx_byte(5, SERCOM5->USART.DATA.reg); + if (status & SERCOM_USART_INTFLAG_DRE) { + uint8_t data; + int ret = serial_get_tx_byte(5, &data); + if (ret) + SERCOM5->USART.INTENCLR.reg = SERCOM_USART_INTENSET_DRE; + else + SERCOM5->USART.DATA.reg = data; + } +} +#if CONFIG_MACH_SAMD21 +DECL_ARMCM_IRQ(SERCOM5_IRQHandler, SERCOM5_IRQn); +#elif CONFIG_MACH_SAMD51 +DECL_ARMCM_IRQ(SERCOM5_IRQHandler, SERCOM5_0_IRQn); +DECL_ARMCM_IRQ(SERCOM5_IRQHandler, SERCOM5_1_IRQn); +DECL_ARMCM_IRQ(SERCOM5_IRQHandler, SERCOM5_2_IRQn); +DECL_ARMCM_IRQ(SERCOM5_IRQHandler, SERCOM5_3_IRQn); +#endif + +#ifdef SERCOM6 + +void +SERCOM6_IRQHandler(void) +{ + uint32_t status = SERCOM6->USART.INTFLAG.reg; + if (status & SERCOM_USART_INTFLAG_RXC) + serial_rx_byte(6, SERCOM6->USART.DATA.reg); + if (status & SERCOM_USART_INTFLAG_DRE) { + uint8_t data; + int ret = serial_get_tx_byte(6, &data); + if (ret) + SERCOM6->USART.INTENCLR.reg = SERCOM_USART_INTENSET_DRE; + else + SERCOM6->USART.DATA.reg = data; + } +} +DECL_ARMCM_IRQ(SERCOM6_IRQHandler, SERCOM6_0_IRQn); +DECL_ARMCM_IRQ(SERCOM6_IRQHandler, SERCOM6_1_IRQn); +DECL_ARMCM_IRQ(SERCOM6_IRQHandler, SERCOM6_2_IRQn); +DECL_ARMCM_IRQ(SERCOM6_IRQHandler, SERCOM6_3_IRQn); + +void +SERCOM7_IRQHandler(void) +{ + uint32_t status = SERCOM7->USART.INTFLAG.reg; + if (status & SERCOM_USART_INTFLAG_RXC) + serial_rx_byte(7, SERCOM7->USART.DATA.reg); + if (status & SERCOM_USART_INTFLAG_DRE) { + uint8_t data; + int ret = serial_get_tx_byte(7, &data); + if (ret) + SERCOM7->USART.INTENCLR.reg = SERCOM_USART_INTENSET_DRE; + else + SERCOM7->USART.DATA.reg = data; + } +} +DECL_ARMCM_IRQ(SERCOM7_IRQHandler, SERCOM7_0_IRQn); +DECL_ARMCM_IRQ(SERCOM7_IRQHandler, SERCOM7_1_IRQn); +DECL_ARMCM_IRQ(SERCOM7_IRQHandler, SERCOM7_2_IRQn); +DECL_ARMCM_IRQ(SERCOM7_IRQHandler, SERCOM7_3_IRQn); + +#endif // SERCOM6 + +#endif // SERCOM4 + +const struct bus_info * +lookup_bus_info(Sercom *sercom) +{ + const struct bus_info *bi = bus_data; + for (; ; bi++) { + if (bi >= &bus_data[ARRAY_SIZE(bus_data)]) + shutdown("Invalid UART config"); + if (bi->sercom == sercom) + return bi; + } +} + +struct uart_config +uart_setup(uint8_t bus, uint32_t baud, uint8_t *id, uint32_t priority) +{ + Sercom *sercom = sercom_enable_pclock(bus); + const struct bus_info *bi = lookup_bus_info(sercom); + + SercomUsart* usart = &sercom->USART; + uint32_t pinout = sercom_usart_pins(bus); + + // Configure USART + usart->CTRLA.reg = 0; uint32_t areg = (SERCOM_USART_CTRLA_MODE(1) | SERCOM_USART_CTRLA_DORD | SERCOM_USART_CTRLA_SAMPR(1) - | SERCOM_USART_CTRLA_RXPO(3) - | SERCOM_USART_CTRLA_TXPO(1)); - su->CTRLA.reg = areg; - su->CTRLB.reg = SERCOM_USART_CTRLB_RXEN | SERCOM_USART_CTRLB_TXEN; - uint32_t freq = get_pclock_frequency(SERCOM0_GCLK_ID_CORE); - uint32_t baud8 = freq / (2 * CONFIG_SERIAL_BAUD); - su->BAUD.reg = (SERCOM_USART_BAUD_FRAC_BAUD(baud8 / 8) - | SERCOM_USART_BAUD_FRAC_FP(baud8 % 8)); + | pinout); + usart->CTRLA.reg = areg; + usart->CTRLB.reg = SERCOM_USART_CTRLB_RXEN | SERCOM_USART_CTRLB_TXEN; + uint32_t baud8 = sercom_get_pclock_frequency(bus) / (2 * baud); + usart->BAUD.reg = (SERCOM_USART_BAUD_FRAC_BAUD(baud8 / 8) + | SERCOM_USART_BAUD_FRAC_FP(baud8 % 8)); // enable irqs - su->INTENSET.reg = SERCOM_USART_INTENSET_RXC; - su->CTRLA.reg = areg | SERCOM_USART_CTRLA_ENABLE; + usart->INTENSET.reg = SERCOM_USART_INTENSET_RXC; + usart->CTRLA.reg = areg | SERCOM_USART_CTRLA_ENABLE; #if CONFIG_MACH_SAMD21 - armcm_enable_irq(SERCOM0_Handler, SERCOM0_IRQn, 0); + NVIC_SetPriority(bi->irqn, priority); + NVIC_EnableIRQ(bi->irqn); #elif CONFIG_MACH_SAMD51 - armcm_enable_irq(SERCOM0_Handler, SERCOM0_0_IRQn, 0); - armcm_enable_irq(SERCOM0_Handler, SERCOM0_1_IRQn, 0); - armcm_enable_irq(SERCOM0_Handler, SERCOM0_2_IRQn, 0); - armcm_enable_irq(SERCOM0_Handler, SERCOM0_3_IRQn, 0); + NVIC_SetPriority(bi->irqn0, priority); + NVIC_EnableIRQ(bi->irqn0); + NVIC_SetPriority(bi->irqn1, priority); + NVIC_EnableIRQ(bi->irqn1); + NVIC_SetPriority(bi->irqn2, priority); + NVIC_EnableIRQ(bi->irqn2); + NVIC_SetPriority(bi->irqn3, priority); + NVIC_EnableIRQ(bi->irqn3); #endif + + *id = bi->id; + + return (struct uart_config){ .usart=usart }; +} + +void +uart_enable_tx_irq(struct uart_config config) +{ + SercomUsart* usart = config.usart; + usart->INTENSET.reg = SERCOM_USART_INTENSET_DRE; } -DECL_INIT(serial_init); diff --git a/src/atsamd/serial_host.c b/src/atsamd/serial_host.c new file mode 100644 index 00000000..b7dee363 --- /dev/null +++ b/src/atsamd/serial_host.c @@ -0,0 +1,41 @@ +// Host serial communication initialization +// +// Copyright (C) 2021 Desuuuu +// +// 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_prepare +#include "internal.h" // sercom_set_pin +#include "sched.h" // DECL_INIT +#include "command.h" // DECL_CONSTANT + +#define RX_BUFFER_SIZE 192 + +DECL_CONSTANT("SERIAL_BAUD", CONFIG_SERIAL_BAUD); +DECL_CONSTANT("RECEIVE_WINDOW", RX_BUFFER_SIZE); + +#define CONSOLE_UART_BUS 0 + +#define GPIO_Rx GPIO('A', 11) +#define GPIO_Tx GPIO('A', 10) + +DECL_CONSTANT_STR("RESERVE_PINS_serial", "[sercom0],PA11,PA10"); + +static uint8_t serial_host_id; + +void +serial_init(void) +{ + sercom_set_pin(CONSOLE_UART_BUS, SERCOM_RX_PIN, GPIO_Rx); + sercom_set_pin(CONSOLE_UART_BUS, SERCOM_TX_PIN, GPIO_Tx); + serial_host_id = serial_prepare(CONSOLE_UART_BUS, CONFIG_SERIAL_BAUD, + RX_BUFFER_SIZE, 96, 0, NULL); +} +DECL_INIT(serial_init); + +uint_fast8_t +console_sendf(const struct command_encoder *ce, va_list args) +{ + return serial_send_command(serial_host_id, ce, args); +} diff --git a/src/avr/Makefile b/src/avr/Makefile index c9315b99..1c5019ce 100644 --- a/src/avr/Makefile +++ b/src/avr/Makefile @@ -9,6 +9,7 @@ CFLAGS += -mmcu=$(CONFIG_MCU) # Add avr source files src-y += avr/main.c avr/timer.c +src-y += avr/serial.c generic/serial_irq.c src-$(CONFIG_HAVE_GPIO) += avr/gpio.c src-$(CONFIG_HAVE_GPIO_ADC) += avr/adc.c src-$(CONFIG_HAVE_GPIO_SPI) += avr/spi.c @@ -16,7 +17,7 @@ src-$(CONFIG_HAVE_GPIO_I2C) += avr/i2c.c src-$(CONFIG_HAVE_GPIO_HARD_PWM) += avr/hard_pwm.c src-$(CONFIG_AVR_WATCHDOG) += avr/watchdog.c src-$(CONFIG_USBSERIAL) += avr/usbserial.c generic/usb_cdc.c -src-$(CONFIG_SERIAL) += avr/serial.c generic/serial_irq.c +src-$(CONFIG_SERIAL) += avr/serial_host.c # Suppress broken "misspelled signal handler" warnings on gcc 4.8.1 CFLAGS_klipper.elf := $(CFLAGS_klipper.elf) $(if $(filter 4.8.1, $(shell $(CC) -dumpversion)), -w) diff --git a/src/avr/gpio.h b/src/avr/gpio.h index 9d98ee70..3c9f8f67 100644 --- a/src/avr/gpio.h +++ b/src/avr/gpio.h @@ -54,4 +54,15 @@ void i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write); void i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg , uint8_t read_len, uint8_t *read); +#define GPIO_UART_MAX_ID 4 + +struct uart_config { + volatile void *ucsr_b; + uint8_t udrie; +}; + +struct uart_config uart_setup(uint8_t bus, uint32_t baud, uint8_t *id + , uint32_t priority); +void uart_enable_tx_irq(struct uart_config config); + #endif // gpio.h diff --git a/src/avr/serial.c b/src/avr/serial.c index e0b35bef..a7cc5748 100644 --- a/src/avr/serial.c +++ b/src/avr/serial.c @@ -4,85 +4,189 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. +#include // uint32_t #include // USART_RX_vect -#include "autoconf.h" // CONFIG_SERIAL_BAUD +#include "autoconf.h" // CONFIG_SERIAL_BAUD_U2X #include "board/serial_irq.h" // serial_rx_byte -#include "command.h" // DECL_CONSTANT_STR -#include "sched.h" // DECL_INIT +#include "command.h" // DECL_ENUMERATION +#include "gpio.h" // uart_setup +#include "sched.h" // sched_shutdown -// Reserve serial pins -#if CONFIG_SERIAL_PORT == 0 - #if CONFIG_MACH_atmega1280 || CONFIG_MACH_atmega2560 -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PE0,PE1"); - #else -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PD0,PD1"); - #endif -#elif CONFIG_SERIAL_PORT == 1 -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PD2,PD3"); -#elif CONFIG_SERIAL_PORT == 2 -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PH0,PH1"); -#else -DECL_CONSTANT_STR("RESERVE_PINS_serial", "PJ0,PJ1"); -#endif +struct bus_info { + uint8_t id; + volatile uint8_t *ucsr_a, *ucsr_b, *ucsr_c; + volatile uint16_t *ubrr; + uint8_t u2x, ucsz_0, ucsz_1, rxen, txen, rxcie, udrie; +}; -// Helper macros for defining serial port aliases -#define AVR_SERIAL_REG1(prefix, id, suffix) prefix ## id ## suffix -#define AVR_SERIAL_REG(prefix, id, suffix) AVR_SERIAL_REG1(prefix, id, suffix) +#if CONFIG_MACH_atmega168 || CONFIG_MACH_atmega328 || CONFIG_MACH_atmega328p -// Serial port register aliases -#define UCSRxA AVR_SERIAL_REG(UCSR, CONFIG_SERIAL_PORT, A) -#define UCSRxB AVR_SERIAL_REG(UCSR, CONFIG_SERIAL_PORT, B) -#define UCSRxC AVR_SERIAL_REG(UCSR, CONFIG_SERIAL_PORT, C) -#define UBRRx AVR_SERIAL_REG(UBRR, CONFIG_SERIAL_PORT,) -#define UDRx AVR_SERIAL_REG(UDR, CONFIG_SERIAL_PORT,) -#define UCSZx1 AVR_SERIAL_REG(UCSZ, CONFIG_SERIAL_PORT, 1) -#define UCSZx0 AVR_SERIAL_REG(UCSZ, CONFIG_SERIAL_PORT, 0) -#define U2Xx AVR_SERIAL_REG(U2X, CONFIG_SERIAL_PORT,) -#define RXENx AVR_SERIAL_REG(RXEN, CONFIG_SERIAL_PORT,) -#define TXENx AVR_SERIAL_REG(TXEN, CONFIG_SERIAL_PORT,) -#define RXCIEx AVR_SERIAL_REG(RXCIE, CONFIG_SERIAL_PORT,) -#define UDRIEx AVR_SERIAL_REG(UDRIE, CONFIG_SERIAL_PORT,) +DECL_ENUMERATION("uart_bus", "usart0", 0); +DECL_CONSTANT_STR("BUS_PINS_usart0", "[_],PD0,PD1"); -#if defined(USART_RX_vect) -// The atmega168 / atmega328 doesn't have an ID in the irq names -#define USARTx_RX_vect USART_RX_vect -#define USARTx_UDRE_vect USART_UDRE_vect -#else -#define USARTx_RX_vect AVR_SERIAL_REG(USART, CONFIG_SERIAL_PORT, _RX_vect) -#define USARTx_UDRE_vect AVR_SERIAL_REG(USART, CONFIG_SERIAL_PORT, _UDRE_vect) -#endif +static const struct bus_info bus_data[] = { + { 0, &UCSR0A, &UCSR0B, &UCSR0C, &UBRR0, + U2X0, UCSZ00, UCSZ01, RXEN0, TXEN0, RXCIE0, UDRIE0 }, +}; -void -serial_init(void) +ISR(USART_RX_vect) { - UCSRxA = CONFIG_SERIAL_BAUD_U2X ? (1<= ARRAY_SIZE(bus_data)) + shutdown("Invalid UART config"); + const struct bus_info *bi = &bus_data[bus]; + + *(bi->ucsr_a) = CONFIG_SERIAL_BAUD_U2X ? (1 << bi->u2x) : 0; + uint32_t cm = CONFIG_SERIAL_BAUD_U2X ? 8 : 16; + *(bi->ubrr) = DIV_ROUND_CLOSEST(CONFIG_CLOCK_FREQ, cm * baud) - 1UL; + *(bi->ucsr_c) = (1 << bi->ucsz_1) | (1 << bi->ucsz_0); + *(bi->ucsr_b) = ((1 << bi->rxen) | (1 << bi->txen) + | (1 << bi->rxcie) | (1 << bi->udrie)); + + *id = bi->id; + + return (struct uart_config){ .ucsr_b=bi->ucsr_b, .udrie=bi->udrie }; +} + +void +uart_enable_tx_irq(struct uart_config config) +{ + volatile uint8_t *ucsr_b = config.ucsr_b; + *ucsr_b |= 1 << config.udrie; } diff --git a/src/avr/serial_host.c b/src/avr/serial_host.c new file mode 100644 index 00000000..4a53bf53 --- /dev/null +++ b/src/avr/serial_host.c @@ -0,0 +1,62 @@ +// Host serial communication initialization +// +// Copyright (C) 2021 Desuuuu +// +// 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_prepare +#include "sched.h" // DECL_INIT +#include "command.h" // DECL_CONSTANT + +#define RX_BUFFER_SIZE 192 + +DECL_CONSTANT("SERIAL_BAUD", CONFIG_SERIAL_BAUD); +DECL_CONSTANT("RECEIVE_WINDOW", RX_BUFFER_SIZE); + +#if CONFIG_MACH_atmega168 || CONFIG_MACH_atmega328 || CONFIG_MACH_atmega328p + #define CONSOLE_UART_BUS 0 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart0],PD0,PD1"); +#elif CONFIG_MACH_at90usb1286 || CONFIG_MACH_at90usb646 \ + || CONFIG_MACH_atmega32u4 + #define CONSOLE_UART_BUS 0 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart1],PD2,PD3"); +#elif CONFIG_MACH_atmega1280 || CONFIG_MACH_atmega2560 + #if CONFIG_SERIAL_PORT == 0 + #define CONSOLE_UART_BUS 0 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart0],PE0,PE1"); + #elif CONFIG_SERIAL_PORT == 1 + #define CONSOLE_UART_BUS 1 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart1],PD2,PD3"); + #elif CONFIG_SERIAL_PORT == 2 + #define CONSOLE_UART_BUS 2 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart2],PH0,PH1"); + #else + #define CONSOLE_UART_BUS 3 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart3],PJ0,PJ1"); + #endif +#else + #if CONFIG_SERIAL_PORT == 0 + #define CONSOLE_UART_BUS 0 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart0],PD0,PD1"); + #else + #define CONSOLE_UART_BUS 1 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart1],PD2,PD3"); + #endif +#endif + +static uint8_t serial_host_id; + +void +serial_init(void) +{ + serial_host_id = serial_prepare(CONSOLE_UART_BUS, CONFIG_SERIAL_BAUD, + RX_BUFFER_SIZE, 96, 0, NULL); +} +DECL_INIT(serial_init); + +uint_fast8_t +console_sendf(const struct command_encoder *ce, va_list args) +{ + return serial_send_command(serial_host_id, ce, args); +} diff --git a/src/command.c b/src/command.c index 015cdb8e..b4451135 100644 --- a/src/command.c +++ b/src/command.c @@ -196,21 +196,23 @@ command_encode_and_frame(uint8_t *buf, const struct command_encoder *ce static uint8_t in_sendf; // Encode and transmit a "response" message -void +uint_fast8_t command_sendf(const struct command_encoder *ce, ...) { if (readb(&in_sendf)) // This sendf call was made from an irq handler while the main // code was already in sendf - just drop this sendf request. - return; + return 0; writeb(&in_sendf, 1); va_list args; va_start(args, ce); - console_sendf(ce, args); + uint_fast8_t res = console_sendf(ce, args); va_end(args); writeb(&in_sendf, 0); + + return res; } void diff --git a/src/command.h b/src/command.h index 67e595bc..ccf19f53 100644 --- a/src/command.h +++ b/src/command.h @@ -79,7 +79,7 @@ uint_fast8_t command_encodef(uint8_t *buf, const struct command_encoder *ce void command_add_frame(uint8_t *buf, uint_fast8_t msglen); uint_fast8_t command_encode_and_frame( uint8_t *buf, const struct command_encoder *ce, va_list args); -void command_sendf(const struct command_encoder *ce, ...); +uint_fast8_t command_sendf(const struct command_encoder *ce, ...); int_fast8_t command_find_block(uint8_t *buf, uint_fast8_t buf_len , uint_fast8_t *pop_count); void command_dispatch(uint8_t *buf, uint_fast8_t msglen); diff --git a/src/generic/canbus.c b/src/generic/canbus.c index 6c7f2ccc..4d012065 100644 --- a/src/generic/canbus.c +++ b/src/generic/canbus.c @@ -55,7 +55,7 @@ canbus_tx_task(void) DECL_TASK(canbus_tx_task); // Encode and transmit a "response" message -void +uint_fast8_t console_sendf(const struct command_encoder *ce, va_list args) { // Verify space for message @@ -66,7 +66,7 @@ console_sendf(const struct command_encoder *ce, va_list args) if (tmax + max_size > sizeof(transmit_buf)) { if (tmax + max_size - tpos > sizeof(transmit_buf)) // Not enough space for message - return; + return max_size > sizeof(transmit_buf); // Move buffer tmax -= tpos; memmove(&transmit_buf[0], &transmit_buf[tpos], tmax); @@ -80,6 +80,7 @@ console_sendf(const struct command_encoder *ce, va_list args) // Start message transmit transmit_max = tmax + msglen; canbus_notify_tx(); + return 1; } diff --git a/src/generic/gpio.h b/src/generic/gpio.h index cb104d00..5d3ff90e 100644 --- a/src/generic/gpio.h +++ b/src/generic/gpio.h @@ -41,4 +41,14 @@ void spi_prepare(struct spi_config config); void spi_transfer(struct spi_config config, uint8_t receive_data , uint8_t len, uint8_t *data); +#define GPIO_UART_MAX_ID 1 + +struct uart_config { + uint8_t cfg; +}; + +struct uart_config uart_setup(uint8_t bus, uint32_t baud, uint8_t *id + , uint32_t priority); +void uart_enable_tx_irq(struct uart_config config); + #endif // gpio.h diff --git a/src/generic/misc.h b/src/generic/misc.h index a24e8b0a..dc4e0a91 100644 --- a/src/generic/misc.h +++ b/src/generic/misc.h @@ -5,7 +5,7 @@ #include // uint8_t struct command_encoder; -void console_sendf(const struct command_encoder *ce, va_list args); +uint_fast8_t console_sendf(const struct command_encoder *ce, va_list args); void *console_receive_buffer(void); uint32_t timer_from_us(uint32_t us); diff --git a/src/generic/serial_irq.c b/src/generic/serial_irq.c index c2b23864..908ce25c 100644 --- a/src/generic/serial_irq.c +++ b/src/generic/serial_irq.c @@ -5,118 +5,270 @@ // This file may be distributed under the terms of the GNU GPLv3 license. #include // memmove -#include "autoconf.h" // CONFIG_SERIAL_BAUD +#include "autoconf.h" // CONFIG_HAVE_GPIO_UART #include "board/io.h" // readb #include "board/irq.h" // irq_save -#include "board/misc.h" // console_sendf +#include "board/misc.h" // timer_from_us #include "board/pgm.h" // READP -#include "command.h" // DECL_CONSTANT -#include "sched.h" // sched_wake_tasks -#include "serial_irq.h" // serial_enable_tx_irq +#include "board/gpio.h" // uart_config +#include "basecmd.h" // alloc_chunk +#include "command.h" // shutdown +#include "sched.h" // sched_wake_task +#include "serial_irq.h" // serial_receive_cb -#define RX_BUFFER_SIZE 192 +struct serial_data { + struct uart_config uart_config; + struct task_wake wake; +#if CONFIG_HAVE_GPIO_UART + struct timer timer; + serial_receive_cb receive_cb; + uint32_t rx_interval; + uint8_t ovfl; +#endif + uint8_t rx_buf_len, tx_buf_len; + uint8_t rpos, tpos, tmax; + uint8_t buf[]; +}; -static uint8_t receive_buf[RX_BUFFER_SIZE], receive_pos; -static uint8_t transmit_buf[96], transmit_pos, transmit_max; - -DECL_CONSTANT("SERIAL_BAUD", CONFIG_SERIAL_BAUD); -DECL_CONSTANT("RECEIVE_WINDOW", RX_BUFFER_SIZE); +static struct serial_data *serial_data[GPIO_UART_MAX_ID]; // Rx interrupt - store read data void -serial_rx_byte(uint_fast8_t data) +serial_rx_byte(uint8_t id, uint_fast8_t data) { - if (data == MESSAGE_SYNC) - sched_wake_tasks(); - if (receive_pos >= sizeof(receive_buf)) - // Serial overflow - ignore it as crc error will force retransmit + if (id >= ARRAY_SIZE(serial_data) || !serial_data[id]) return; - receive_buf[receive_pos++] = data; + struct serial_data *sd = serial_data[id]; + +#if CONFIG_HAVE_GPIO_UART + if (sd->receive_cb) { + uint8_t left = sd->rx_buf_len - sd->rpos; + if (left < 8) + sched_wake_task(&sd->wake); + if (left == 0) { + ++sd->ovfl; + return; + } + } else { +#else + { +#endif + if (data == MESSAGE_SYNC) + sched_wake_task(&sd->wake); + if (sd->rpos >= sd->rx_buf_len) + // Serial overflow - ignore it as crc error will force retransmit + return; + } + sd->buf[sd->rpos++] = data; } // Tx interrupt - get next byte to transmit int -serial_get_tx_byte(uint8_t *pdata) +serial_get_tx_byte(uint8_t id, uint8_t *pdata) { - if (transmit_pos >= transmit_max) + if (id >= ARRAY_SIZE(serial_data) || !serial_data[id]) return -1; - *pdata = transmit_buf[transmit_pos++]; + struct serial_data *sd = serial_data[id]; + + if (sd->tpos >= sd->tmax) + return -1; + *pdata = sd->buf[sd->rx_buf_len + sd->tpos++]; return 0; } -// Remove from the receive buffer the given number of bytes +#if CONFIG_HAVE_GPIO_UART +static uint_fast8_t +serial_rx_event(struct timer *timer) +{ + struct serial_data *sd = container_of(timer, struct serial_data, timer); + sched_wake_task(&sd->wake); + sd->timer.waketime += sd->rx_interval; + return SF_RESCHEDULE; +} +#endif + static void -console_pop_input(uint_fast8_t len) +serial_pop_input(struct serial_data *sd, uint_fast8_t len) { uint_fast8_t copied = 0; for (;;) { - uint_fast8_t rpos = readb(&receive_pos); + uint_fast8_t rpos = readb(&sd->rpos); uint_fast8_t needcopy = rpos - len; if (needcopy) { - memmove(&receive_buf[copied], &receive_buf[copied + len] - , needcopy - copied); + memmove(sd->buf + copied, sd->buf + copied + len, + needcopy - copied); copied = needcopy; - sched_wake_tasks(); + sched_wake_task(&sd->wake); } irqstatus_t flag = irq_save(); - if (rpos != readb(&receive_pos)) { + if (rpos != readb(&sd->rpos)) { // Raced with irq handler - retry irq_restore(flag); continue; } - receive_pos = needcopy; + sd->rpos = needcopy; irq_restore(flag); break; } } -// Process any incoming commands -void -console_task(void) +#if CONFIG_HAVE_GPIO_UART +static void +serial_process_rx(struct serial_data *sd, uint8_t id) { - uint_fast8_t rpos = readb(&receive_pos), pop_count; - int_fast8_t ret = command_find_block(receive_buf, rpos, &pop_count); + uint_fast8_t rpos = readb(&sd->rpos), ovfl = readb(&sd->ovfl), pop_count; + int_fast8_t ret; + if (rpos) { + ret = (sd->receive_cb)(id, sd->buf, rpos, &pop_count); + if (!ret) + sched_wake_task(&sd->wake); + else if (pop_count) + serial_pop_input(sd, pop_count); + } + if (ovfl) { + ret = (sd->receive_cb)(id, NULL, 0, &pop_count); + if (!ret) + sched_wake_task(&sd->wake); + else + writeb(&sd->ovfl, 0); + } +} +#endif + +static void +console_process_rx(struct serial_data *sd) +{ + uint_fast8_t rpos = readb(&sd->rpos), pop_count; + int_fast8_t ret = command_find_block(sd->buf, rpos, &pop_count); if (ret > 0) - command_dispatch(receive_buf, pop_count); + command_dispatch(sd->buf, pop_count); if (ret) { - console_pop_input(pop_count); + serial_pop_input(sd, pop_count); if (ret > 0) command_send_ack(); } } -DECL_TASK(console_task); -// Encode and transmit a "response" message +// Process incoming data void -console_sendf(const struct command_encoder *ce, va_list args) +serial_task(void) { + for (uint_fast8_t id = 0; id < ARRAY_SIZE(serial_data); ++id) { + struct serial_data *sd = serial_data[id]; + if (sd && sched_check_wake(&sd->wake)) { +#if CONFIG_HAVE_GPIO_UART + if (sd->receive_cb) + serial_process_rx(sd, id); + else +#endif + console_process_rx(sd); + } + } +} +DECL_TASK(serial_task); + +uint8_t +serial_prepare(uint8_t bus, uint32_t baud, uint8_t rx_buf, uint8_t tx_buf + , uint16_t rx_interval, serial_receive_cb receive_cb) +{ + struct serial_data *sd = alloc_chunk(sizeof(*sd) + rx_buf + tx_buf); + + uint8_t id; + sd->uart_config = uart_setup(bus, baud, &id, receive_cb ? 1 : 0); + if (id >= ARRAY_SIZE(serial_data) || serial_data[id]) + shutdown("Invalid UART config"); + + sd->rx_buf_len = rx_buf; + sd->tx_buf_len = tx_buf; + +#if CONFIG_HAVE_GPIO_UART + sd->receive_cb = receive_cb; + if (receive_cb && rx_interval > 0) { + sd->rx_interval = timer_from_us(rx_interval * 1000L); + sd->timer.func = serial_rx_event; + sd->timer.waketime = timer_read_time() + sd->rx_interval; + sched_add_timer(&sd->timer); + } +#endif + + serial_data[id] = sd; + + return id; +} + +uint_fast8_t +serial_send(uint8_t id, uint8_t *data, uint8_t len) +{ + if (id >= ARRAY_SIZE(serial_data) || !serial_data[id]) + return 1; + struct serial_data *sd = serial_data[id]; + // Verify space for message - uint_fast8_t tpos = readb(&transmit_pos), tmax = readb(&transmit_max); + uint_fast8_t tpos = readb(&sd->tpos), tmax = readb(&sd->tmax); if (tpos >= tmax) { tpos = tmax = 0; - writeb(&transmit_max, 0); - writeb(&transmit_pos, 0); + writeb(&sd->tmax, 0); + writeb(&sd->tpos, 0); + } + if (tmax + len > sd->tx_buf_len) { + if (tmax + len - tpos > sd->tx_buf_len) + // Not enough space for message + return len > sd->tx_buf_len; + // Disable TX irq and move buffer + writeb(&sd->tmax, 0); + tpos = readb(&sd->tpos); + tmax -= tpos; + memmove(sd->buf + sd->rx_buf_len, sd->buf + sd->rx_buf_len + tpos, + tmax); + writeb(&sd->tpos, 0); + writeb(&sd->tmax, tmax); + uart_enable_tx_irq(sd->uart_config); + } + + memcpy(sd->buf + sd->rx_buf_len + tmax, data, len); + + // Start message transmit + writeb(&sd->tmax, tmax + len); + uart_enable_tx_irq(sd->uart_config); + return 1; +} + +uint_fast8_t +serial_send_command(uint8_t id, const struct command_encoder *ce, va_list args) +{ + if (id >= ARRAY_SIZE(serial_data) || !serial_data[id]) + return 1; + struct serial_data *sd = serial_data[id]; + + // Verify space for message + uint_fast8_t tpos = readb(&sd->tpos), tmax = readb(&sd->tmax); + if (tpos >= tmax) { + tpos = tmax = 0; + writeb(&sd->tmax, 0); + writeb(&sd->tpos, 0); } uint_fast8_t max_size = READP(ce->max_size); - if (tmax + max_size > sizeof(transmit_buf)) { - if (tmax + max_size - tpos > sizeof(transmit_buf)) + if (tmax + max_size > sd->tx_buf_len) { + if (tmax + max_size - tpos > sd->tx_buf_len) // Not enough space for message - return; + return max_size > sd->tx_buf_len; // Disable TX irq and move buffer - writeb(&transmit_max, 0); - tpos = readb(&transmit_pos); + writeb(&sd->tmax, 0); + tpos = readb(&sd->tpos); tmax -= tpos; - memmove(&transmit_buf[0], &transmit_buf[tpos], tmax); - writeb(&transmit_pos, 0); - writeb(&transmit_max, tmax); - serial_enable_tx_irq(); + memmove(sd->buf + sd->rx_buf_len, sd->buf + sd->rx_buf_len + tpos, + tmax); + writeb(&sd->tpos, 0); + writeb(&sd->tmax, tmax); + uart_enable_tx_irq(sd->uart_config); } // Generate message - uint8_t *buf = &transmit_buf[tmax]; + uint8_t *buf = sd->buf + sd->rx_buf_len + tmax; uint_fast8_t msglen = command_encode_and_frame(buf, ce, args); // Start message transmit - writeb(&transmit_max, tmax + msglen); - serial_enable_tx_irq(); + writeb(&sd->tmax, tmax + msglen); + uart_enable_tx_irq(sd->uart_config); + return 1; } diff --git a/src/generic/serial_irq.h b/src/generic/serial_irq.h index 9aa96ab1..28f3438f 100644 --- a/src/generic/serial_irq.h +++ b/src/generic/serial_irq.h @@ -1,13 +1,23 @@ #ifndef __GENERIC_SERIAL_IRQ_H #define __GENERIC_SERIAL_IRQ_H +#include // va_list #include // uint32_t +#include "command.h" // command_encoder -// callback provided by board specific code -void serial_enable_tx_irq(void); +typedef int_fast8_t (*serial_receive_cb)(uint8_t id, uint8_t* data + , uint_fast8_t len + , uint_fast8_t *pop_count); -// serial_irq.c -void serial_rx_byte(uint_fast8_t data); -int serial_get_tx_byte(uint8_t *pdata); +uint8_t serial_prepare(uint8_t bus, uint32_t baud + , uint8_t rx_buf, uint8_t tx_buf + , uint16_t rx_interval + , serial_receive_cb receive_cb); +uint_fast8_t serial_send(uint8_t id, uint8_t *data, uint8_t len); +uint_fast8_t serial_send_command(uint8_t id, const struct command_encoder *ce + , va_list args); + +void serial_rx_byte(uint8_t id, uint_fast8_t data); +int serial_get_tx_byte(uint8_t id, uint8_t *pdata); #endif // serial_irq.h diff --git a/src/generic/usb_cdc.c b/src/generic/usb_cdc.c index 577a8635..38b23f0e 100644 --- a/src/generic/usb_cdc.c +++ b/src/generic/usb_cdc.c @@ -61,14 +61,14 @@ usb_bulk_in_task(void) DECL_TASK(usb_bulk_in_task); // Encode and transmit a "response" message -void +uint_fast8_t console_sendf(const struct command_encoder *ce, va_list args) { // Verify space for message uint_fast8_t tpos = transmit_pos, max_size = READP(ce->max_size); if (tpos + max_size > sizeof(transmit_buf)) // Not enough space for message - return; + return max_size > sizeof(transmit_buf); // Generate message uint8_t *buf = &transmit_buf[tpos]; @@ -77,6 +77,7 @@ console_sendf(const struct command_encoder *ce, va_list args) // Start message transmit transmit_pos = tpos + msglen; usb_notify_bulk_in(); + return 1; } diff --git a/src/linux/console.c b/src/linux/console.c index 55688322..60416391 100644 --- a/src/linux/console.c +++ b/src/linux/console.c @@ -175,7 +175,7 @@ console_task(void) DECL_TASK(console_task); // Encode and transmit a "response" message -void +uint_fast8_t console_sendf(const struct command_encoder *ce, va_list args) { // Generate message @@ -184,8 +184,12 @@ console_sendf(const struct command_encoder *ce, va_list args) // Transmit message int ret = write(main_pfd[MP_TTY_IDX].fd, buf, msglen); - if (ret < 0) + if (ret < 0) { report_errno("write", ret); + return 0; + } + + return 1; } // Sleep until the specified time (waking early for console input if needed) diff --git a/src/lpc176x/Makefile b/src/lpc176x/Makefile index 7ed80b26..0b87a6ad 100644 --- a/src/lpc176x/Makefile +++ b/src/lpc176x/Makefile @@ -16,12 +16,13 @@ src-y += lpc176x/main.c lpc176x/gpio.c src-y += generic/armcm_boot.c generic/armcm_irq.c generic/armcm_timer.c src-y += generic/armcm_reset.c generic/crc16_ccitt.c src-y += ../lib/lpc176x/device/system_LPC17xx.c +src-y += lpc176x/serial.c generic/serial_irq.c src-$(CONFIG_HAVE_GPIO_ADC) += lpc176x/adc.c src-$(CONFIG_HAVE_GPIO_I2C) += lpc176x/i2c.c src-$(CONFIG_HAVE_GPIO_SPI) += lpc176x/spi.c src-$(CONFIG_USBSERIAL) += lpc176x/usbserial.c lpc176x/chipid.c src-$(CONFIG_USBSERIAL) += generic/usb_cdc.c -src-$(CONFIG_SERIAL) += lpc176x/serial.c generic/serial_irq.c +src-$(CONFIG_SERIAL) += lpc176x/serial_host.c src-$(CONFIG_HAVE_GPIO_HARD_PWM) += lpc176x/hard_pwm.c # Build the additional bin output file diff --git a/src/lpc176x/gpio.h b/src/lpc176x/gpio.h index e03afbb4..86a76a11 100644 --- a/src/lpc176x/gpio.h +++ b/src/lpc176x/gpio.h @@ -55,4 +55,15 @@ void i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write); void i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg , uint8_t read_len, uint8_t *read); +#define GPIO_UART_MAX_ID 4 + +struct uart_config { + void *uart; + uint8_t id; +}; + +struct uart_config uart_setup(uint8_t bus, uint32_t baud, uint8_t *id + , uint32_t priority); +void uart_enable_tx_irq(struct uart_config config); + #endif // gpio.h diff --git a/src/lpc176x/internal.h b/src/lpc176x/internal.h index 3adb7e19..06ee592d 100644 --- a/src/lpc176x/internal.h +++ b/src/lpc176x/internal.h @@ -10,12 +10,14 @@ #define PCLK_TIMER0 1 #define PCLK_UART0 3 +#define PCLK_UART1 4 #define PCLK_PWM1 6 #define PCLK_I2C0 7 #define PCLK_SSP1 10 #define PCLK_ADC 12 #define PCLK_I2C1 19 #define PCLK_SSP0 21 +#define PCLK_UART2 24 #define PCLK_UART3 25 #define PCLK_I2C2 26 #define PCLK_USB 31 diff --git a/src/lpc176x/serial.c b/src/lpc176x/serial.c index 87a79270..c0d6a20b 100644 --- a/src/lpc176x/serial.c +++ b/src/lpc176x/serial.c @@ -4,95 +4,158 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. -#include "board/armcm_boot.h" // armcm_enable_irq -#include "autoconf.h" // CONFIG_SERIAL_BAUD +#include // uint32_t +#include "board/armcm_boot.h" // DECL_ARMCM_IRQ #include "board/irq.h" // irq_save -#include "board/serial_irq.h" // serial_rx_data -#include "command.h" // DECL_CONSTANT_STR +#include "board/serial_irq.h" // serial_rx_byte +#include "command.h" // DECL_ENUMERATION +#include "gpio.h" // uart_setup #include "internal.h" // gpio_peripheral -#include "sched.h" // DECL_INIT +#include "sched.h" // sched_shutdown -#if CONFIG_LPC_SERIAL_UART0_P03_P02 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "P0.3,P0.2"); - #define GPIO_Rx GPIO(0, 3) - #define GPIO_Tx GPIO(0, 2) - #define GPIO_FUNCTION_UARTx 1 - #define LPC_UARTx LPC_UART0 - #define UARTx_IRQn UART0_IRQn - #define PCLK_UARTx PCLK_UART0 -#elif CONFIG_LPC_SERIAL_UART3_P429_P428 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "P4.29,P4.28"); - #define GPIO_Rx GPIO(4, 29) - #define GPIO_Tx GPIO(4, 28) - #define GPIO_FUNCTION_UARTx 3 - #define LPC_UARTx LPC_UART3 - #define UARTx_IRQn UART3_IRQn - #define PCLK_UARTx PCLK_UART3 -#endif +struct bus_info { + uint8_t id; + LPC_UART_TypeDef *uart; + uint32_t pclk; + IRQn_Type irqn; + uint8_t rx_pin, tx_pin, function; +}; + +DECL_ENUMERATION_RANGE("uart_bus", "uart0", 0, 4); +DECL_ENUMERATION("uart_bus", "uart1a", 4); +DECL_ENUMERATION("uart_bus", "uart2a", 5); +DECL_ENUMERATION("uart_bus", "uart3a", 6); +DECL_ENUMERATION("uart_bus", "uart3b", 7); +DECL_CONSTANT_STR("BUS_PINS_uart0", "[_],P0.3,P0.2"); +DECL_CONSTANT_STR("BUS_PINS_uart1", "[_],P0.16,P0.15"); +DECL_CONSTANT_STR("BUS_PINS_uart2", "[_],P0.11,P0.10"); +DECL_CONSTANT_STR("BUS_PINS_uart3", "[_],P4.29,P4.28"); +DECL_CONSTANT_STR("BUS_PINS_uart1a", "[uart1],P2.1,P2.0"); +DECL_CONSTANT_STR("BUS_PINS_uart2a", "[uart2],P2.9,P2.8"); +DECL_CONSTANT_STR("BUS_PINS_uart3a", "[uart3],P0.1,P0.0"); +DECL_CONSTANT_STR("BUS_PINS_uart3b", "[uart3],P0.26,P0.25"); + +static const struct bus_info bus_data[] = { + { 0, (LPC_UART_TypeDef*)LPC_UART0, PCLK_UART0, UART0_IRQn, + GPIO(0, 3), GPIO(0, 2), 1 }, + { 1, (LPC_UART_TypeDef*)LPC_UART1, PCLK_UART1, UART1_IRQn, + GPIO(0, 16), GPIO(0, 15), 1 }, + { 2, LPC_UART2, PCLK_UART2, UART2_IRQn, GPIO(0, 11), GPIO(0, 10), 1 }, + { 3, LPC_UART3, PCLK_UART3, UART3_IRQn, GPIO(4, 29), GPIO(4, 28), 3 }, + { 1, (LPC_UART_TypeDef*)LPC_UART1, PCLK_UART1, UART1_IRQn, + GPIO(2, 1), GPIO(2, 0), 2 }, + { 2, LPC_UART2, PCLK_UART2, UART2_IRQn, GPIO(2, 9), GPIO(2, 8), 2 }, + { 3, LPC_UART3, PCLK_UART3, UART3_IRQn, GPIO(0, 1), GPIO(0, 0), 2 }, + { 3, LPC_UART3, PCLK_UART3, UART3_IRQn, GPIO(0, 26), GPIO(0, 25), 3 }, +}; // Write tx bytes to the serial port static void -kick_tx(void) +kick_tx(uint8_t id, LPC_UART_TypeDef *uart) { for (;;) { - if (!(LPC_UARTx->LSR & (1<<5))) { + if (!(uart->LSR & (1<<5))) { // Output fifo full - enable tx irq - LPC_UARTx->IER = 0x03; + uart->IER = 0x03; break; } uint8_t data; - int ret = serial_get_tx_byte(&data); + int ret = serial_get_tx_byte(id, &data); if (ret) { // No more data to send - disable tx irq - LPC_UARTx->IER = 0x01; + uart->IER = 0x01; break; } - LPC_UARTx->THR = data; + uart->THR = data; } } void -UARTx_IRQHandler(void) +UART0_IRQHandler(void) { - uint32_t iir = LPC_UARTx->IIR, status = iir & 0x0f; + uint32_t iir = LPC_UART0->IIR, status = iir & 0x0f; if (status == 0x04) - serial_rx_byte(LPC_UARTx->RBR); + serial_rx_byte(0, LPC_UART0->RBR); else if (status == 0x02) - kick_tx(); + kick_tx(0, (LPC_UART_TypeDef*)LPC_UART0); +} +DECL_ARMCM_IRQ(UART0_IRQHandler, UART0_IRQn); + +void +UART1_IRQHandler(void) +{ + uint32_t iir = LPC_UART1->IIR, status = iir & 0x0f; + if (status == 0x04) + serial_rx_byte(1, LPC_UART1->RBR); + else if (status == 0x02) + kick_tx(1, (LPC_UART_TypeDef*)LPC_UART1); +} +DECL_ARMCM_IRQ(UART1_IRQHandler, UART1_IRQn); + +void +UART2_IRQHandler(void) +{ + uint32_t iir = LPC_UART2->IIR, status = iir & 0x0f; + if (status == 0x04) + serial_rx_byte(2, LPC_UART2->RBR); + else if (status == 0x02) + kick_tx(2, LPC_UART2); +} +DECL_ARMCM_IRQ(UART2_IRQHandler, UART2_IRQn); + +void +UART3_IRQHandler(void) +{ + uint32_t iir = LPC_UART3->IIR, status = iir & 0x0f; + if (status == 0x04) + serial_rx_byte(3, LPC_UART3->RBR); + else if (status == 0x02) + kick_tx(3, LPC_UART3); +} +DECL_ARMCM_IRQ(UART3_IRQHandler, UART3_IRQn); + +struct uart_config +uart_setup(uint8_t bus, uint32_t baud, uint8_t *id, uint32_t priority) +{ + if (bus >= ARRAY_SIZE(bus_data)) + shutdown("Invalid UART config"); + const struct bus_info *bi = &bus_data[bus]; + LPC_UART_TypeDef *uart = bi->uart; + + // Setup baud + uart->LCR = (1<<7); // set DLAB bit + enable_pclock(bi->pclk); + uint32_t pclk = get_pclock_frequency(bi->pclk); + uint32_t div = pclk / (baud * 16); + uart->DLL = div & 0xff; + uart->DLM = (div >> 8) & 0xff; + uart->FDR = 0x10; + uart->LCR = 3; // 8N1 ; clear DLAB bit + + // Enable fifo + uart->FCR = 0x01; + + // Setup pins + gpio_peripheral(bi->rx_pin, bi->function, 0); + gpio_peripheral(bi->tx_pin, bi->function, 0); + + // Enable receive irq + NVIC_SetPriority(bi->irqn, priority); + NVIC_EnableIRQ(bi->irqn); + uart->IER = 0x01; + + *id = bi->id; + + return (struct uart_config){ .uart=uart, .id=bi->id }; } void -serial_enable_tx_irq(void) +uart_enable_tx_irq(struct uart_config config) { - if (LPC_UARTx->LSR & (1<<5)) { + LPC_UART_TypeDef *uart = config.uart; + if (uart->LSR & (1<<5)) { irqstatus_t flag = irq_save(); - kick_tx(); + kick_tx(config.id, uart); irq_restore(flag); } } - -void -serial_init(void) -{ - // Setup baud - LPC_UARTx->LCR = (1<<7); // set DLAB bit - enable_pclock(PCLK_UARTx); - uint32_t pclk = get_pclock_frequency(PCLK_UARTx); - uint32_t div = pclk / (CONFIG_SERIAL_BAUD * 16); - LPC_UARTx->DLL = div & 0xff; - LPC_UARTx->DLM = (div >> 8) & 0xff; - LPC_UARTx->FDR = 0x10; - LPC_UARTx->LCR = 3; // 8N1 ; clear DLAB bit - - // Enable fifo - LPC_UARTx->FCR = 0x01; - - // Setup pins - gpio_peripheral(GPIO_Rx, GPIO_FUNCTION_UARTx, 0); - gpio_peripheral(GPIO_Tx, GPIO_FUNCTION_UARTx, 0); - - // Enable receive irq - armcm_enable_irq(UARTx_IRQHandler, UARTx_IRQn, 0); - LPC_UARTx->IER = 0x01; -} -DECL_INIT(serial_init); diff --git a/src/lpc176x/serial_host.c b/src/lpc176x/serial_host.c new file mode 100644 index 00000000..08cd78d5 --- /dev/null +++ b/src/lpc176x/serial_host.c @@ -0,0 +1,39 @@ +// Host serial communication initialization +// +// Copyright (C) 2021 Desuuuu +// +// 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_prepare +#include "sched.h" // DECL_INIT +#include "command.h" // DECL_CONSTANT + +#define RX_BUFFER_SIZE 192 + +DECL_CONSTANT("SERIAL_BAUD", CONFIG_SERIAL_BAUD); +DECL_CONSTANT("RECEIVE_WINDOW", RX_BUFFER_SIZE); + +#if CONFIG_LPC_SERIAL_UART0_P03_P02 + #define CONSOLE_UART_BUS 0 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[uart0],P0.3,P0.2"); +#elif CONFIG_LPC_SERIAL_UART3_P429_P428 + #define CONSOLE_UART_BUS 3 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[uart3],P4.29,P4.28"); +#endif + +static uint8_t serial_host_id; + +void +serial_init(void) +{ + serial_host_id = serial_prepare(CONSOLE_UART_BUS, CONFIG_SERIAL_BAUD, + RX_BUFFER_SIZE, 96, 0, NULL); +} +DECL_INIT(serial_init); + +uint_fast8_t +console_sendf(const struct command_encoder *ce, va_list args) +{ + return serial_send_command(serial_host_id, ce, args); +} diff --git a/src/pru/main.c b/src/pru/main.c index 37354d90..27f765d9 100644 --- a/src/pru/main.c +++ b/src/pru/main.c @@ -138,14 +138,14 @@ console_task(void) DECL_TASK(console_task); // Encode and transmit a "response" message -void +uint_fast8_t console_sendf(const struct command_encoder *ce, va_list args) { // Verify space for message uint32_t send_push_pos = SHARED_MEM->send_push_pos; struct shared_response_buffer *s = &SHARED_MEM->send_data[send_push_pos]; if (readl(&s->count)) - return; + return 0; // Generate message uint32_t msglen = command_encodef(s->data, ce, args); @@ -155,6 +155,8 @@ console_sendf(const struct command_encoder *ce, va_list args) write_r31(R31_WRITE_IRQ_SELECT | (KICK_PRU0_EVENT - R31_WRITE_IRQ_OFFSET)); SHARED_MEM->send_push_pos = ( (send_push_pos + 1) % ARRAY_SIZE(SHARED_MEM->send_data)); + + return 1; } void diff --git a/src/pru/pru0.c b/src/pru/pru0.c index 266b9b1f..58d2b14c 100644 --- a/src/pru/pru0.c +++ b/src/pru/pru0.c @@ -210,11 +210,12 @@ sched_shutdown(uint_fast8_t reason) } // Generate messages - only used for ack/nak messages -void +uint_fast8_t console_sendf(const struct command_encoder *ce, va_list args) { uint8_t buf[MESSAGE_MIN]; build_message(buf, sizeof(buf)); + return 1; } diff --git a/src/rp2040/Makefile b/src/rp2040/Makefile index b40df32d..4fbefefa 100644 --- a/src/rp2040/Makefile +++ b/src/rp2040/Makefile @@ -16,9 +16,10 @@ $(OUT)klipper.elf: $(OUT)stage2.o $(OUT)src/rp2040/rp2040_link.ld src-y += rp2040/main.c rp2040/gpio.c rp2040/adc.c generic/crc16_ccitt.c src-y += generic/armcm_boot.c generic/armcm_irq.c generic/armcm_reset.c src-y += generic/timer_irq.c rp2040/timer.c rp2040/bootrom.c +src-y += rp2040/serial.c generic/serial_irq.c src-$(CONFIG_USBSERIAL) += rp2040/usbserial.c generic/usb_cdc.c src-$(CONFIG_USBSERIAL) += rp2040/chipid.c -src-$(CONFIG_SERIAL) += rp2040/serial.c generic/serial_irq.c +src-$(CONFIG_SERIAL) += rp2040/serial_host.c src-$(CONFIG_HAVE_GPIO_HARD_PWM) += rp2040/hard_pwm.c src-$(CONFIG_HAVE_GPIO_SPI) += rp2040/spi.c diff --git a/src/rp2040/gpio.h b/src/rp2040/gpio.h index 90680c6e..6a3c0a3d 100644 --- a/src/rp2040/gpio.h +++ b/src/rp2040/gpio.h @@ -44,4 +44,15 @@ void spi_prepare(struct spi_config config); void spi_transfer(struct spi_config config, uint8_t receive_data , uint8_t len, uint8_t *data); +#define GPIO_UART_MAX_ID 2 + +struct uart_config { + void *uart; + uint8_t id; +}; + +struct uart_config uart_setup(uint8_t bus, uint32_t baud, uint8_t *id + , uint32_t priority); +void uart_enable_tx_irq(struct uart_config config); + #endif // gpio.h diff --git a/src/rp2040/serial.c b/src/rp2040/serial.c index abfdcb8b..1ed35cbf 100644 --- a/src/rp2040/serial.c +++ b/src/rp2040/serial.c @@ -5,88 +5,135 @@ // This file may be distributed under the terms of the GNU GPLv3 license. #include // uint32_t -#include "autoconf.h" // CONFIG_SERIAL -#include "board/armcm_boot.h" // armcm_enable_irq +#include "board/armcm_boot.h" // DECL_ARMCM_IRQ #include "board/irq.h" // irq_save -#include "board/serial_irq.h" // serial_rx_data +#include "board/serial_irq.h" // serial_rx_byte #include "hardware/structs/resets.h" // RESETS_RESET_UART0_BITS #include "hardware/structs/uart.h" // UART0_BASE -#include "internal.h" // UART0_IRQn -#include "sched.h" // DECL_INIT +#include "command.h" // DECL_ENUMERATION +#include "gpio.h" // uart_setup +#include "internal.h" // gpio_peripheral +#include "sched.h" // sched_shutdown -#define UARTx uart0_hw -#define UARTx_IRQn UART0_IRQ_IRQn -#define GPIO_Rx 1 -#define GPIO_Tx 0 +struct bus_info { + uint8_t id; + uart_hw_t *uart; + uint32_t reset_bit; + IRQn_Type irqn; + uint8_t rx_pin, tx_pin; +}; + +DECL_ENUMERATION_RANGE("uart_bus", "uart0", 0, 2); +DECL_ENUMERATION("uart_bus", "uart0a", 2); +DECL_ENUMERATION("uart_bus", "uart0b", 3); +DECL_ENUMERATION("uart_bus", "uart1a", 4); +DECL_CONSTANT_STR("BUS_PINS_uart0", "[_],gpio1,gpio0"); +DECL_CONSTANT_STR("BUS_PINS_uart1", "[_],gpio5,gpio4"); +DECL_CONSTANT_STR("BUS_PINS_uart0a", "[uart0],gpio13,gpio12"); +DECL_CONSTANT_STR("BUS_PINS_uart0b", "[uart0],gpio17,gpio16"); +DECL_CONSTANT_STR("BUS_PINS_uart1a", "[uart1],gpio9,gpio8"); + +static const struct bus_info bus_data[] = { + { 0, uart0_hw, RESETS_RESET_UART0_BITS, UART0_IRQ_IRQn, 1, 0 }, + { 1, uart1_hw, RESETS_RESET_UART1_BITS, UART1_IRQ_IRQn, 5, 4 }, + { 0, uart0_hw, RESETS_RESET_UART0_BITS, UART0_IRQ_IRQn, 13, 12 }, + { 0, uart0_hw, RESETS_RESET_UART0_BITS, UART0_IRQ_IRQn, 17, 16 }, + { 1, uart1_hw, RESETS_RESET_UART1_BITS, UART1_IRQ_IRQn, 9, 8 }, +}; // Write tx bytes to the serial port static void -kick_tx(void) +kick_tx(uint8_t id, uart_hw_t *uart) { for (;;) { - if (UARTx->fr & UART_UARTFR_TXFF_BITS) { + if (uart->fr & UART_UARTFR_TXFF_BITS) { // Output fifo full - enable tx irq - UARTx->imsc = (UART_UARTIMSC_RXIM_BITS | UART_UARTIMSC_RTIM_BITS + uart->imsc = (UART_UARTIMSC_RXIM_BITS | UART_UARTIMSC_RTIM_BITS | UART_UARTIMSC_TXIM_BITS); break; } uint8_t data; - int ret = serial_get_tx_byte(&data); + int ret = serial_get_tx_byte(id, &data); if (ret) { // No more data to send - disable tx irq - UARTx->imsc = UART_UARTIMSC_RXIM_BITS | UART_UARTIMSC_RTIM_BITS; + uart->imsc = UART_UARTIMSC_RXIM_BITS | UART_UARTIMSC_RTIM_BITS; break; } - UARTx->dr = data; + uart->dr = data; } } void -UARTx_IRQHandler(void) +UART0_IRQHandler(void) { - uint32_t mis = UARTx->mis; + uint32_t mis = uart0_hw->mis; if (mis & (UART_UARTMIS_RXMIS_BITS | UART_UARTMIS_RTMIS_BITS)) { do { - serial_rx_byte(UARTx->dr); - } while (!(UARTx->fr & UART_UARTFR_RXFE_BITS)); + serial_rx_byte(0, uart0_hw->dr); + } while (!(uart0_hw->fr & UART_UARTFR_RXFE_BITS)); } else if (mis & UART_UARTMIS_TXMIS_BITS) { - kick_tx(); + kick_tx(0, uart0_hw); } } +DECL_ARMCM_IRQ(UART0_IRQHandler, UART0_IRQ_IRQn); void -serial_enable_tx_irq(void) +UART1_IRQHandler(void) { - if (!(UARTx->fr & UART_UARTFR_TXFF_BITS)) { - irqstatus_t flag = irq_save(); - kick_tx(); - irq_restore(flag); + uint32_t mis = uart1_hw->mis; + if (mis & (UART_UARTMIS_RXMIS_BITS | UART_UARTMIS_RTMIS_BITS)) { + do { + serial_rx_byte(1, uart1_hw->dr); + } while (!(uart1_hw->fr & UART_UARTFR_RXFE_BITS)); + } else if (mis & UART_UARTMIS_TXMIS_BITS) { + kick_tx(1, uart1_hw); } } +DECL_ARMCM_IRQ(UART1_IRQHandler, UART1_IRQ_IRQn); -void -serial_init(void) +struct uart_config +uart_setup(uint8_t bus, uint32_t baud, uint8_t *id, uint32_t priority) { - enable_pclock(RESETS_RESET_UART0_BITS); + if (bus >= ARRAY_SIZE(bus_data)) + shutdown("Invalid UART config"); + const struct bus_info *bi = &bus_data[bus]; + uart_hw_t *uart = bi->uart; + + enable_pclock(bi->reset_bit); // Setup baud - uint32_t pclk = get_pclock_frequency(RESETS_RESET_UART0_BITS); - uint32_t div = DIV_ROUND_CLOSEST(pclk * 4, CONFIG_SERIAL_BAUD); - UARTx->ibrd = div >> 6; - UARTx->fbrd = div & 0x3f; + uint32_t pclk = get_pclock_frequency(bi->reset_bit); + uint32_t div = DIV_ROUND_CLOSEST(pclk * 4, baud); + uart->ibrd = div >> 6; + uart->fbrd = div & 0x3f; // Enable fifo, set 8N1 - UARTx->lcr_h = UART_UARTLCR_H_FEN_BITS | UART_UARTLCR_H_WLEN_BITS; - UARTx->ifls = 0; - UARTx->cr = (UART_UARTCR_RXE_BITS | UART_UARTCR_TXE_BITS + uart->lcr_h = UART_UARTLCR_H_FEN_BITS | UART_UARTLCR_H_WLEN_BITS; + uart->ifls = 0; + uart->cr = (UART_UARTCR_RXE_BITS | UART_UARTCR_TXE_BITS | UART_UARTCR_UARTEN_BITS); // Setup pins - gpio_peripheral(GPIO_Rx, 2, 1); - gpio_peripheral(GPIO_Tx, 2, 0); + gpio_peripheral(bi->rx_pin, 2, 1); + gpio_peripheral(bi->tx_pin, 2, 0); // Enable receive irq - armcm_enable_irq(UARTx_IRQHandler, UARTx_IRQn, 0); - UARTx->imsc = UART_UARTIMSC_RXIM_BITS | UART_UARTIMSC_RTIM_BITS; + NVIC_SetPriority(bi->irqn, priority); + NVIC_EnableIRQ(bi->irqn); + uart->imsc = UART_UARTIMSC_RXIM_BITS | UART_UARTIMSC_RTIM_BITS; + + *id = bi->id; + + return (struct uart_config){ .uart=uart, .id=bi->id }; +} + +void +uart_enable_tx_irq(struct uart_config config) +{ + uart_hw_t *uart = config.uart; + if (!(uart->fr & UART_UARTFR_TXFF_BITS)) { + irqstatus_t flag = irq_save(); + kick_tx(config.id, uart); + irq_restore(flag); + } } -DECL_INIT(serial_init); diff --git a/src/rp2040/serial_host.c b/src/rp2040/serial_host.c new file mode 100644 index 00000000..dd7894db --- /dev/null +++ b/src/rp2040/serial_host.c @@ -0,0 +1,35 @@ +// Host serial communication initialization +// +// Copyright (C) 2021 Desuuuu +// +// 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_prepare +#include "sched.h" // DECL_INIT +#include "command.h" // DECL_CONSTANT + +#define RX_BUFFER_SIZE 192 + +DECL_CONSTANT("SERIAL_BAUD", CONFIG_SERIAL_BAUD); +DECL_CONSTANT("RECEIVE_WINDOW", RX_BUFFER_SIZE); + +#define CONSOLE_UART_BUS 0 + +DECL_CONSTANT_STR("RESERVE_PINS_serial", "[uart0],gpio1,gpio0"); + +static uint8_t serial_host_id; + +void +serial_init(void) +{ + serial_host_id = serial_prepare(CONSOLE_UART_BUS, CONFIG_SERIAL_BAUD, + RX_BUFFER_SIZE, 96, 0, NULL); +} +DECL_INIT(serial_init); + +uint_fast8_t +console_sendf(const struct command_encoder *ce, va_list args) +{ + return serial_send_command(serial_host_id, ce, args); +} diff --git a/src/simulator/Makefile b/src/simulator/Makefile index 6447a38b..5878df8f 100644 --- a/src/simulator/Makefile +++ b/src/simulator/Makefile @@ -2,6 +2,7 @@ dirs-y += src/simulator src/generic -src-y += simulator/main.c simulator/gpio.c simulator/timer.c simulator/serial.c +src-y += simulator/main.c simulator/gpio.c simulator/timer.c \ + simulator/serial.c simulator/serial_host.c src-y += generic/crc16_ccitt.c generic/alloc.c src-y += generic/timer_irq.c generic/serial_irq.c diff --git a/src/simulator/serial.c b/src/simulator/serial.c index c8111acf..ba64dfde 100644 --- a/src/simulator/serial.c +++ b/src/simulator/serial.c @@ -7,30 +7,15 @@ #include // fcntl #include // STDIN_FILENO #include "board/serial_irq.h" // serial_get_tx_byte -#include "sched.h" // DECL_INIT - -void -serial_init(void) -{ - // Make stdin/stdout non-blocking - fcntl(STDIN_FILENO, F_SETFL, fcntl(STDIN_FILENO, F_GETFL, 0) | O_NONBLOCK); - fcntl(STDOUT_FILENO, F_SETFL - , fcntl(STDOUT_FILENO, F_GETFL, 0) | O_NONBLOCK); -} -DECL_INIT(serial_init); - -void * -console_receive_buffer(void) -{ - return NULL; -} +#include "board/gpio.h" // uart_setup +#include "sched.h" // sched_shutdown static void do_uart(void) { for (;;) { uint8_t data; - int ret = serial_get_tx_byte(&data); + int ret = serial_get_tx_byte(0, &data); if (ret) break; else @@ -41,8 +26,21 @@ do_uart(void) } } +struct uart_config +uart_setup(uint8_t bus, uint32_t baud, uint8_t *id, uint32_t priority) +{ + // Make stdin/stdout non-blocking + fcntl(STDIN_FILENO, F_SETFL, fcntl(STDIN_FILENO, F_GETFL, 0) | O_NONBLOCK); + fcntl(STDOUT_FILENO, F_SETFL + , fcntl(STDOUT_FILENO, F_GETFL, 0) | O_NONBLOCK); + + *id = 0; + + return (struct uart_config){ }; +} + void -serial_enable_tx_irq(void) +uart_enable_tx_irq(struct uart_config config) { // Normally this would enable the hardware irq, but we just call // do_uart() directly in this demo code. diff --git a/src/simulator/serial_host.c b/src/simulator/serial_host.c new file mode 100644 index 00000000..aa9d8f3d --- /dev/null +++ b/src/simulator/serial_host.c @@ -0,0 +1,38 @@ +// Host serial communication initialization +// +// Copyright (C) 2021 Desuuuu +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // NULL +#include "autoconf.h" // CONFIG_SERIAL_BAUD +#include "board/serial_irq.h" // serial_prepare +#include "sched.h" // DECL_INIT +#include "command.h" // DECL_CONSTANT + +#define RX_BUFFER_SIZE 192 + +DECL_CONSTANT("SERIAL_BAUD", CONFIG_SERIAL_BAUD); +DECL_CONSTANT("RECEIVE_WINDOW", RX_BUFFER_SIZE); + +static uint8_t serial_host_id; + +void +serial_init(void) +{ + serial_host_id = serial_prepare(0, CONFIG_SERIAL_BAUD, + RX_BUFFER_SIZE, 96, 0, NULL); +} +DECL_INIT(serial_init); + +uint_fast8_t +console_sendf(const struct command_encoder *ce, va_list args) +{ + return serial_send_command(serial_host_id, ce, args); +} + +void * +console_receive_buffer(void) +{ + return NULL; +} diff --git a/src/stm32/Makefile b/src/stm32/Makefile index 31fefb6c..c1248b9d 100644 --- a/src/stm32/Makefile +++ b/src/stm32/Makefile @@ -44,8 +44,9 @@ usb-src-$(CONFIG_HAVE_STM32_USBFS) := stm32/usbfs.c usb-src-$(CONFIG_HAVE_STM32_USBOTG) := stm32/usbotg.c src-$(CONFIG_USBSERIAL) += $(usb-src-y) stm32/chipid.c generic/usb_cdc.c serial-src-y := stm32/serial.c -serial-src-$(CONFIG_MACH_STM32F0) := stm32/stm32f0_serial.c -src-$(CONFIG_SERIAL) += $(serial-src-y) generic/serial_irq.c +serial-src-$(CONFIG_MACH_STM32F031) := stm32/stm32f031_serial.c +src-y += $(serial-src-y) generic/serial_irq.c +src-$(CONFIG_SERIAL) += stm32/serial_host.c src-$(CONFIG_CANSERIAL) += stm32/can.c ../lib/fast-hash/fasthash.c src-$(CONFIG_CANSERIAL) += generic/canbus.c src-$(CONFIG_HAVE_GPIO_HARD_PWM) += stm32/hard_pwm.c diff --git a/src/stm32/gpio.h b/src/stm32/gpio.h index 281e3a8d..037b7143 100644 --- a/src/stm32/gpio.h +++ b/src/stm32/gpio.h @@ -2,6 +2,7 @@ #define __STM32_GPIO_H #include // uint32_t +#include "autoconf.h" // CONFIG_MACH_STM32F031 struct gpio_out { void *regs; @@ -55,4 +56,18 @@ void i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write); void i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg , uint8_t read_len, uint8_t *read); +#if CONFIG_MACH_STM32F031 + #define GPIO_UART_MAX_ID 1 +#else + #define GPIO_UART_MAX_ID 4 +#endif + +struct uart_config { + void *usart; +}; + +struct uart_config uart_setup(uint8_t bus, uint32_t baud, uint8_t *id + , uint32_t priority); +void uart_enable_tx_irq(struct uart_config config); + #endif // gpio.h diff --git a/src/stm32/serial.c b/src/stm32/serial.c index 5806eaab..c984c20d 100644 --- a/src/stm32/serial.c +++ b/src/stm32/serial.c @@ -4,90 +4,165 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. -#include "autoconf.h" // CONFIG_SERIAL_BAUD -#include "board/armcm_boot.h" // armcm_enable_irq +#include // uint32_t +#include "autoconf.h" // CONFIG_* +#include "board/armcm_boot.h" // DECL_ARMCM_IRQ #include "board/serial_irq.h" // serial_rx_byte -#include "command.h" // DECL_CONSTANT_STR -#include "internal.h" // enable_pclock -#include "sched.h" // DECL_INIT +#include "command.h" // DECL_ENUMERATION +#include "gpio.h" // uart_setup +#include "internal.h" // gpio_peripheral +#include "sched.h" // sched_shutdown -// Select the configured serial port -#if CONFIG_STM32_SERIAL_USART1 - 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 -#elif CONFIG_STM32_SERIAL_USART1_ALT_PB7_PB6 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "PB7,PB6"); - #define GPIO_Rx GPIO('B', 7) - #define GPIO_Tx GPIO('B', 6) - #define USARTx USART1 - #define USARTx_IRQn USART1_IRQn -#elif CONFIG_STM32_SERIAL_USART2 - 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 -#elif CONFIG_STM32_SERIAL_USART2_ALT_PD6_PD5 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "PD6,PD5"); - #define GPIO_Rx GPIO('D', 6) - #define GPIO_Tx GPIO('D', 5) - #define USARTx USART2 - #define USARTx_IRQn USART2_IRQn -#elif CONFIG_STM32_SERIAL_USART3 - 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 -#elif CONFIG_STM32_SERIAL_USART3_ALT_PD9_PD8 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "PD9,PD8"); - #define GPIO_Rx GPIO('D', 9) - #define GPIO_Tx GPIO('D', 8) - #define USARTx USART3 - #define USARTx_IRQn USART3_IRQn +struct bus_info { + uint8_t id; + USART_TypeDef *usart; + IRQn_Type irqn; + uint8_t rx_pin, tx_pin, function; +}; + +#if CONFIG_MACH_STM32F0 + #define USART_FUNCTION(fn) GPIO_FUNCTION(fn) + + #define USART_SR(usart) (usart)->ISR + #define USART_RDR(usart) (usart)->RDR + #define USART_TDR(usart) (usart)->TDR + + #define USART_SR_RXNE USART_ISR_RXNE + #define USART_SR_ORE USART_ISR_ORE + #define USART_SR_TXE USART_ISR_TXE + + #define USART_BRR_DIV_Mantissa_Pos USART_BRR_DIV_MANTISSA_Pos + #define USART_BRR_DIV_Fraction_Pos USART_BRR_DIV_FRACTION_Pos +#else + #define USART_FUNCTION(fn) GPIO_FUNCTION(7) + + #define USART_SR(usart) (usart)->SR + #define USART_RDR(usart) (usart)->DR + #define USART_TDR(usart) (usart)->DR #endif +DECL_ENUMERATION_RANGE("uart_bus", "usart1", 0, 2); +DECL_ENUMERATION("uart_bus", "usart1a", 2); +DECL_ENUMERATION("uart_bus", "usart2a", 3); +DECL_CONSTANT_STR("BUS_PINS_usart1", "[_],PA10,PA9"); +DECL_CONSTANT_STR("BUS_PINS_usart2", "[_],PA3,PA2"); +DECL_CONSTANT_STR("BUS_PINS_usart1a", "[usart1],PB7,PB6"); + +#if CONFIG_MACH_STM32F0 + DECL_CONSTANT_STR("BUS_PINS_usart2a", "[usart2],PA15,PA14"); +#else + DECL_CONSTANT_STR("BUS_PINS_usart2a", "[usart2],PD6,PD5"); + + #ifdef USART3 + DECL_ENUMERATION("uart_bus", "usart3", 4); + DECL_ENUMERATION("uart_bus", "usart3a", 5); + DECL_CONSTANT_STR("BUS_PINS_usart3", "[_],PB11,PB10"); + DECL_CONSTANT_STR("BUS_PINS_usart3a", "[usart3],PD9,PD8"); + #endif +#endif + +static const struct bus_info bus_data[] = { + { 1, USART1, USART1_IRQn, GPIO('A', 10), GPIO('A', 9), USART_FUNCTION(1) }, + { 2, USART2, USART2_IRQn, GPIO('A', 3), GPIO('A', 2), USART_FUNCTION(1) }, + { 1, USART1, USART1_IRQn, GPIO('B', 7), GPIO('B', 6), USART_FUNCTION(0) }, +#if CONFIG_MACH_STM32F0 + { 2, USART2, USART2_IRQn, GPIO('A', 15), GPIO('A', 14), USART_FUNCTION(1) }, +#else + { 2, USART2, USART2_IRQn, GPIO('D', 6), GPIO('D', 5), USART_FUNCTION(1) }, + #ifdef USART3 + { 3, USART3, USART3_IRQn, GPIO('B', 11), GPIO('B', 10), USART_FUNCTION(7) }, + { 3, USART3, USART3_IRQn, GPIO('D', 9), GPIO('D', 8), USART_FUNCTION(7) }, + #endif +#endif +}; + #define CR1_FLAGS (USART_CR1_UE | USART_CR1_RE | USART_CR1_TE \ | USART_CR1_RXNEIE) void -USARTx_IRQHandler(void) +USART1_IRQHandler(void) { - uint32_t sr = USARTx->SR; + uint32_t sr = USART_SR(USART1); if (sr & (USART_SR_RXNE | USART_SR_ORE)) - serial_rx_byte(USARTx->DR); - if (sr & USART_SR_TXE && USARTx->CR1 & USART_CR1_TXEIE) { + serial_rx_byte(1, USART_RDR(USART1)); + if (sr & USART_SR_TXE && USART1->CR1 & USART_CR1_TXEIE) { uint8_t data; - int ret = serial_get_tx_byte(&data); + int ret = serial_get_tx_byte(1, &data); if (ret) - USARTx->CR1 = CR1_FLAGS; + USART1->CR1 = CR1_FLAGS; else - USARTx->DR = data; + USART_TDR(USART1) = data; } } +DECL_ARMCM_IRQ(USART1_IRQHandler, USART1_IRQn); void -serial_enable_tx_irq(void) +USART2_IRQHandler(void) { - USARTx->CR1 = CR1_FLAGS | USART_CR1_TXEIE; + uint32_t sr = USART_SR(USART2); + if (sr & (USART_SR_RXNE | USART_SR_ORE)) + serial_rx_byte(2, USART_RDR(USART2)); + if (sr & USART_SR_TXE && USART2->CR1 & USART_CR1_TXEIE) { + uint8_t data; + int ret = serial_get_tx_byte(2, &data); + if (ret) + USART2->CR1 = CR1_FLAGS; + else + USART_TDR(USART2) = data; + } } +DECL_ARMCM_IRQ(USART2_IRQHandler, USART2_IRQn); + +#if !CONFIG_MACH_STM32F0 && defined(USART3) void -serial_init(void) +USART3_IRQHandler(void) { - enable_pclock((uint32_t)USARTx); + uint32_t sr = USART_SR(USART3); + if (sr & (USART_SR_RXNE | USART_SR_ORE)) + serial_rx_byte(3, USART_RDR(USART3)); + if (sr & USART_SR_TXE && USART3->CR1 & USART_CR1_TXEIE) { + uint8_t data; + int ret = serial_get_tx_byte(3, &data); + if (ret) + USART3->CR1 = CR1_FLAGS; + else + USART_TDR(USART3) = data; + } +} +DECL_ARMCM_IRQ(USART3_IRQHandler, USART3_IRQn); - 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) +#endif // USART3 + +struct uart_config +uart_setup(uint8_t bus, uint32_t baud, uint8_t *id, uint32_t priority) +{ + if (bus >= ARRAY_SIZE(bus_data)) + shutdown("Invalid UART config"); + const struct bus_info *bi = &bus_data[bus]; + USART_TypeDef *usart = bi->usart; + + enable_pclock((uint32_t)usart); + + uint32_t pclk = get_pclock_frequency((uint32_t)usart); + uint32_t div = DIV_ROUND_CLOSEST(pclk, baud); + usart->BRR = (((div / 16) << USART_BRR_DIV_Mantissa_Pos) | ((div % 16) << USART_BRR_DIV_Fraction_Pos)); - USARTx->CR1 = CR1_FLAGS; - armcm_enable_irq(USARTx_IRQHandler, USARTx_IRQn, 0); + usart->CR1 = CR1_FLAGS; + NVIC_SetPriority(bi->irqn, priority); + NVIC_EnableIRQ(bi->irqn); - gpio_peripheral(GPIO_Rx, GPIO_FUNCTION(7), 1); - gpio_peripheral(GPIO_Tx, GPIO_FUNCTION(7), 0); + gpio_peripheral(bi->rx_pin, bi->function, 1); + gpio_peripheral(bi->tx_pin, bi->function, 0); + + *id = bi->id; + + return (struct uart_config){ .usart=usart }; +} + +void +uart_enable_tx_irq(struct uart_config config) +{ + USART_TypeDef *usart = config.usart; + usart->CR1 = CR1_FLAGS | USART_CR1_TXEIE; } -DECL_INIT(serial_init); diff --git a/src/stm32/serial_host.c b/src/stm32/serial_host.c new file mode 100644 index 00000000..7a9ee7da --- /dev/null +++ b/src/stm32/serial_host.c @@ -0,0 +1,56 @@ +// Host serial communication initialization +// +// Copyright (C) 2021 Desuuuu +// +// 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_prepare +#include "sched.h" // DECL_INIT +#include "command.h" // DECL_CONSTANT + +#define RX_BUFFER_SIZE 192 + +DECL_CONSTANT("SERIAL_BAUD", CONFIG_SERIAL_BAUD); +DECL_CONSTANT("RECEIVE_WINDOW", RX_BUFFER_SIZE); + +#if CONFIG_MACH_STM32F031 + #define CONSOLE_UART_BUS 0 +#elif CONFIG_STM32_SERIAL_USART1 + #define CONSOLE_UART_BUS 0 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart1],PA10,PA9"); +#elif CONFIG_STM32_SERIAL_USART2 + #define CONSOLE_UART_BUS 1 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart2],PA3,PA2"); +#elif CONFIG_STM32_SERIAL_USART1_ALT_PB7_PB6 + #define CONSOLE_UART_BUS 2 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart1],PB7,PB6"); +#elif CONFIG_STM32_SERIAL_USART2_ALT_PA15_PA14 + #define CONSOLE_UART_BUS 3 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart2],PA15,PA14"); +#elif CONFIG_STM32_SERIAL_USART2_ALT_PD6_PD5 + #define CONSOLE_UART_BUS 3 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart2],PD6,PD5"); +#elif CONFIG_STM32_SERIAL_USART3 + #define CONSOLE_UART_BUS 4 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart3],PB11,PB10"); +#elif CONFIG_STM32_SERIAL_USART3_ALT_PD9_PD8 + #define CONSOLE_UART_BUS 5 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "[usart3],PD9,PD8"); +#endif + +static uint8_t serial_host_id; + +void +serial_init(void) +{ + serial_host_id = serial_prepare(CONSOLE_UART_BUS, CONFIG_SERIAL_BAUD, + RX_BUFFER_SIZE, 96, 0, NULL); +} +DECL_INIT(serial_init); + +uint_fast8_t +console_sendf(const struct command_encoder *ce, va_list args) +{ + return serial_send_command(serial_host_id, ce, args); +} diff --git a/src/stm32/stm32f031_serial.c b/src/stm32/stm32f031_serial.c new file mode 100644 index 00000000..0c8218f6 --- /dev/null +++ b/src/stm32/stm32f031_serial.c @@ -0,0 +1,80 @@ +// STM32F031 serial +// +// Copyright (C) 2019 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // uint32_t +#include "autoconf.h" // CONFIG_* +#include "board/armcm_boot.h" // armcm_enable_irq +#include "board/serial_irq.h" // serial_rx_byte +#include "command.h" // DECL_CONSTANT_STR +#include "gpio.h" // uart_setup +#include "internal.h" // gpio_peripheral + +#if CONFIG_STM32_SERIAL_USART1 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA10,PA9"); + #define GPIO_Rx GPIO('A', 10) + #define GPIO_Tx GPIO('A', 9) + #define USART1_FUNCTION GPIO_FUNCTION(1) +#elif CONFIG_STM32_SERIAL_USART2 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA3,PA2"); + #define GPIO_Rx GPIO('A', 3) + #define GPIO_Tx GPIO('A', 2) + #define USART1_FUNCTION GPIO_FUNCTION(1) +#elif CONFIG_STM32_SERIAL_USART1_ALT_PB7_PB6 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "PB7,PB6"); + #define GPIO_Rx GPIO('B', 7) + #define GPIO_Tx GPIO('B', 6) + #define USART1_FUNCTION GPIO_FUNCTION(0) +#elif CONFIG_STM32_SERIAL_USART2_ALT_PA15_PA14 + DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA15,PA14"); + #define GPIO_Rx GPIO('A', 15) + #define GPIO_Tx GPIO('A', 14) + #define USART1_FUNCTION GPIO_FUNCTION(1) +#endif + +#define CR1_FLAGS (USART_CR1_UE | USART_CR1_RE | USART_CR1_TE \ + | USART_CR1_RXNEIE) + +void +USART1_IRQHandler(void) +{ + uint32_t sr = USART1->ISR; + if (sr & (USART_ISR_RXNE | USART_ISR_ORE)) + serial_rx_byte(1, USART1->RDR); + if (sr & USART_ISR_TXE && USART1->CR1 & USART_CR1_TXEIE) { + uint8_t data; + int ret = serial_get_tx_byte(1, &data); + if (ret) + USART1->CR1 = CR1_FLAGS; + else + USART1->TDR = data; + } +} + +struct uart_config +uart_setup(uint8_t bus, uint32_t baud, uint8_t *id, uint32_t priority) +{ + enable_pclock((uint32_t)USART1); + + uint32_t pclk = get_pclock_frequency((uint32_t)USART1); + uint32_t div = DIV_ROUND_CLOSEST(pclk, baud); + USART1->BRR = (((div / 16) << USART_BRR_DIV_MANTISSA_Pos) + | ((div % 16) << USART_BRR_DIV_FRACTION_Pos)); + USART1->CR1 = CR1_FLAGS; + armcm_enable_irq(USART1_IRQHandler, USART1_IRQn, priority); + + gpio_peripheral(GPIO_Rx, USART1_FUNCTION, 1); + gpio_peripheral(GPIO_Tx, USART1_FUNCTION, 0); + + *id = 0; + + return (struct uart_config){ }; +} + +void +uart_enable_tx_irq(struct uart_config config) +{ + USART1->CR1 = CR1_FLAGS | USART_CR1_TXEIE; +} diff --git a/src/stm32/stm32f0_serial.c b/src/stm32/stm32f0_serial.c deleted file mode 100644 index 367c2991..00000000 --- a/src/stm32/stm32f0_serial.c +++ /dev/null @@ -1,91 +0,0 @@ -// STM32F0 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/armcm_boot.h" // armcm_enable_irq -#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_STM32_SERIAL_USART1 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA10,PA9"); - #define GPIO_Rx GPIO('A', 10) - #define GPIO_Tx GPIO('A', 9) - #define USARTx_FUNCTION GPIO_FUNCTION(1) - #define USARTx USART1 - #define USARTx_IRQn USART1_IRQn -#elif CONFIG_STM32_SERIAL_USART1_ALT_PB7_PB6 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "PB7,PB6"); - #define GPIO_Rx GPIO('B', 7) - #define GPIO_Tx GPIO('B', 6) - #define USARTx_FUNCTION GPIO_FUNCTION(0) - #define USARTx USART1 - #define USARTx_IRQn USART1_IRQn -#elif CONFIG_STM32_SERIAL_USART2 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA3,PA2"); - #define GPIO_Rx GPIO('A', 3) - #define GPIO_Tx GPIO('A', 2) - #define USARTx_FUNCTION GPIO_FUNCTION(1) - #define USARTx USART2 - #define USARTx_IRQn USART2_IRQn -#elif CONFIG_STM32_SERIAL_USART2_ALT_PA15_PA14 - DECL_CONSTANT_STR("RESERVE_PINS_serial", "PA15,PA14"); - #define GPIO_Rx GPIO('A', 15) - #define GPIO_Tx GPIO('A', 14) - #define USARTx_FUNCTION GPIO_FUNCTION(1) - #define USARTx USART2 - #define USARTx_IRQn USART2_IRQn -#endif - -#if CONFIG_MACH_STM32F031 -// The stm32f031 has same pins for USART2, but everything is routed to USART1 -#define USART2 USART1 -#define USART2_IRQn USART1_IRQn -#endif - -#define CR1_FLAGS (USART_CR1_UE | USART_CR1_RE | USART_CR1_TE \ - | USART_CR1_RXNEIE) - -void -USARTx_IRQHandler(void) -{ - uint32_t sr = USARTx->ISR; - if (sr & (USART_ISR_RXNE | USART_ISR_ORE)) - serial_rx_byte(USARTx->RDR); - if (sr & USART_ISR_TXE && USARTx->CR1 & USART_CR1_TXEIE) { - uint8_t data; - int ret = serial_get_tx_byte(&data); - if (ret) - USARTx->CR1 = CR1_FLAGS; - else - USARTx->TDR = data; - } -} - -void -serial_enable_tx_irq(void) -{ - USARTx->CR1 = CR1_FLAGS | USART_CR1_TXEIE; -} - -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; - armcm_enable_irq(USARTx_IRQHandler, USARTx_IRQn, 0); - - gpio_peripheral(GPIO_Rx, USARTx_FUNCTION, 1); - gpio_peripheral(GPIO_Tx, USARTx_FUNCTION, 0); -} -DECL_INIT(serial_init);