diff --git a/bricks/cityhub/pbioconfig.h b/bricks/cityhub/pbioconfig.h index 69916e7c9..fc15c4fe1 100644 --- a/bricks/cityhub/pbioconfig.h +++ b/bricks/cityhub/pbioconfig.h @@ -2,5 +2,7 @@ // Copyright (c) 2019 David Lechner #define PBIO_CONFIG_UARTDEV (1) +#define PBIO_CONFIG_UARTDEV_NUM_DEV (2) + #define PBIO_CONFIG_ENABLE_DEINIT (0) #define PBIO_CONFIG_ENABLE_SYS (1) diff --git a/bricks/debug/modules/boot.py b/bricks/debug/modules/boot.py new file mode 100644 index 000000000..cfc9f58a7 --- /dev/null +++ b/bricks/debug/modules/boot.py @@ -0,0 +1,3 @@ +from advanced import * +from debug import * +from parameters import * diff --git a/bricks/debug/mpconfigport.h b/bricks/debug/mpconfigport.h index 86f514f12..0419a82cb 100644 --- a/bricks/debug/mpconfigport.h +++ b/bricks/debug/mpconfigport.h @@ -9,11 +9,11 @@ #define PYBRICKS_HEAP_KB 64 // half of RAM // Pybricks modules -#define PYBRICKS_PY_ADVANCED (0) +#define PYBRICKS_PY_ADVANCED (1) #define PYBRICKS_PY_BATTERY (0) #define PYBRICKS_PY_DEBUG (1) #define PYBRICKS_PY_MOTOR (0) -#define PYBRICKS_PY_PARAMETERS (0) +#define PYBRICKS_PY_PARAMETERS (1) #define PYBRICKS_PY_PUPDEVICES (0) #define PYBRICKS_PY_TOOLS (0) diff --git a/bricks/debug/pbioconfig.h b/bricks/debug/pbioconfig.h index 5ee28b537..79f9a26e3 100644 --- a/bricks/debug/pbioconfig.h +++ b/bricks/debug/pbioconfig.h @@ -2,4 +2,6 @@ // Copyright (c) 2019 David Lechner #define PBIO_CONFIG_UARTDEV (1) +#define PBIO_CONFIG_UARTDEV_NUM_DEV (1) + #define PBIO_CONFIG_ENABLE_SYS (1) diff --git a/bricks/movehub/pbioconfig.h b/bricks/movehub/pbioconfig.h index 69916e7c9..fc15c4fe1 100644 --- a/bricks/movehub/pbioconfig.h +++ b/bricks/movehub/pbioconfig.h @@ -2,5 +2,7 @@ // Copyright (c) 2019 David Lechner #define PBIO_CONFIG_UARTDEV (1) +#define PBIO_CONFIG_UARTDEV_NUM_DEV (2) + #define PBIO_CONFIG_ENABLE_DEINIT (0) #define PBIO_CONFIG_ENABLE_SYS (1) diff --git a/bricks/stm32.mk b/bricks/stm32.mk index 5591363a9..c059d4b67 100644 --- a/bricks/stm32.mk +++ b/bricks/stm32.mk @@ -148,12 +148,12 @@ PBIO_SRC_C = $(addprefix ports/pybricks/lib/pbio/,\ drv/$(PBIO_PLATFORM)/bluetooth.c \ drv/$(PBIO_PLATFORM)/light.c \ drv/$(PBIO_PLATFORM)/motor.c \ - drv/$(PBIO_PLATFORM)/uart.c \ drv/adc/adc_stm32f$(CPU_FAMILY).c \ drv/battery/battery_adc.c \ drv/button/button_gpio.c \ drv/gpio/gpio_stm32f$(CPU_FAMILY).c \ drv/ioport/ioport_lpf2.c \ + drv/uart/uart_stm32f$(CPU_FAMILY).c \ platform/$(PBIO_PLATFORM)/clock.c \ platform/$(PBIO_PLATFORM)/platform.c \ platform/$(PBIO_PLATFORM)/sys.c \ diff --git a/extmod/modadvanced.c b/extmod/modadvanced.c index 603364581..d77c0f3fe 100644 --- a/extmod/modadvanced.c +++ b/extmod/modadvanced.c @@ -72,7 +72,7 @@ STATIC mp_obj_t advanced_IODevice_mode(size_t n_args, const mp_obj_t *args) { } else { // set mode - pb_assert(pb_iodevice_set_mode(self->iodev, mp_obj_get_int(args[1]))); + pb_iodevice_set_mode(self->iodev, mp_obj_get_int(args[1])); return mp_const_none; } } diff --git a/extmod/modpupdevices.c b/extmod/modpupdevices.c index 1afc8be45..36f7ece36 100644 --- a/extmod/modpupdevices.c +++ b/extmod/modpupdevices.c @@ -30,14 +30,14 @@ STATIC mp_obj_t pupdevices_ColorAndDistSensor_make_new(const mp_obj_type_t *type pb_assert(pbdrv_ioport_get_iodev(port, &self->iodev)); pb_iodevice_assert_type_id(self->iodev, PBIO_IODEV_TYPE_ID_COLOR_DIST_SENSOR); - pb_assert(pb_iodevice_set_mode(self->iodev, 8)); + pb_iodevice_set_mode(self->iodev, 8); return MP_OBJ_FROM_PTR(self); } STATIC uint8_t pupdevices_ColorAndDistSensor_combined_mode(pbio_iodev_t *iodev, uint8_t idx) { - pb_assert(pb_iodevice_set_mode(iodev, 8)); + pb_iodevice_set_mode(iodev, 8); uint8_t *data; - pb_assert(pbio_iodev_get_raw_values(iodev, &data)); + pb_assert(pbio_iodev_get_data(iodev, &data)); return data[idx]; } @@ -91,7 +91,7 @@ MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorAndDistSensor_reflection_obj, pupdevic STATIC mp_obj_t pupdevices_ColorAndDistSensor_ambient(mp_obj_t self_in) { pupdevices_ColorAndDistSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_iodevice_assert_type_id(self->iodev, PBIO_IODEV_TYPE_ID_COLOR_DIST_SENSOR); - pb_assert(pb_iodevice_set_mode(self->iodev, 4)); + pb_iodevice_set_mode(self->iodev, 4); return pb_iodevice_get_values(self->iodev); } MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorAndDistSensor_ambient_obj, pupdevices_ColorAndDistSensor_ambient); @@ -99,9 +99,9 @@ MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorAndDistSensor_ambient_obj, pupdevices_ STATIC mp_obj_t pupdevices_ColorAndDistSensor_rgb(mp_obj_t self_in) { pupdevices_ColorAndDistSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_iodevice_assert_type_id(self->iodev, PBIO_IODEV_TYPE_ID_COLOR_DIST_SENSOR); - pb_assert(pb_iodevice_set_mode(self->iodev, 6)); + pb_iodevice_set_mode(self->iodev, 6); uint8_t *data; - pb_assert(pbio_iodev_get_raw_values(self->iodev, &data)); + pb_assert(pbio_iodev_get_data(self->iodev, &data)); mp_obj_t rgb[3]; for (uint8_t col = 0; col < 3; col++) { int16_t intensity = ((*(int16_t *)(data + col * 2))*10)/44; diff --git a/extmod/pbiodevice.c b/extmod/pbiodevice.c index 55017b460..233f9cbe7 100644 --- a/extmod/pbiodevice.c +++ b/extmod/pbiodevice.c @@ -16,6 +16,25 @@ #include "pberror.h" #include "pbiodevice.h" +static void wait(pbio_error_t (*end)(pbio_iodev_t *), void (*cancel)(pbio_iodev_t *), pbio_iodev_t* iodev) { + nlr_buf_t nlr; + pbio_error_t err; + + if (nlr_push(&nlr) == 0) { + while ((err = end(iodev)) == PBIO_ERROR_AGAIN) { + MICROPY_EVENT_POLL_HOOK + } + nlr_pop(); + pb_assert(err); + } else { + cancel(iodev); + while (end(iodev) == PBIO_ERROR_AGAIN) { + MICROPY_VM_HOOK_LOOP + } + nlr_raise(nlr.ret_val); + } +} + void pb_iodevice_assert_type_id(pbio_iodev_t *iodev, pbio_iodev_type_id_t type_id) { if (!iodev->info || iodev->info->type_id != type_id) { pb_assert(PBIO_ERROR_NO_DEV); @@ -35,22 +54,19 @@ pbio_error_t pb_iodevice_get_mode(pbio_iodev_t *iodev, uint8_t *current_mode) { return PBIO_SUCCESS; } -pbio_error_t pb_iodevice_set_mode(pbio_iodev_t *iodev, uint8_t new_mode) { +void pb_iodevice_set_mode(pbio_iodev_t *iodev, uint8_t new_mode) { pbio_error_t err; // FIXME: it would be better to do this check on a per-sensor basis since // some sensors use setting the mode as a oneshot to update the sensor // value - e.g. LEGO EV3 Ultrasonic sensor in certain modes. if (iodev->mode == new_mode){ - return PBIO_SUCCESS; + return; } - err = pbio_iodev_set_mode(iodev, new_mode); - // Wait for mode change to complete unless there was an error. - while (err == PBIO_SUCCESS && iodev->mode != new_mode) { - mp_hal_delay_ms(1); - } - return err; + while ((err = pbio_iodev_set_mode_begin(iodev, new_mode)) == PBIO_ERROR_AGAIN); + pb_assert(err); + wait(pbio_iodev_set_mode_end, pbio_iodev_set_mode_cancel, iodev); } mp_obj_t pb_iodevice_get_values(pbio_iodev_t *iodev) { @@ -59,8 +75,8 @@ mp_obj_t pb_iodevice_get_values(pbio_iodev_t *iodev) { uint8_t len, i; pbio_iodev_data_type_t type; - pb_assert(pbio_iodev_get_raw_values(iodev, &data)); - pb_assert(pbio_iodev_get_bin_format(iodev, &len, &type)); + pb_assert(pbio_iodev_get_data(iodev, &data)); + pb_assert(pbio_iodev_get_data_format(iodev, iodev->mode, &len, &type)); // this shouldn't happen, but just in case... if (len == 0) { @@ -105,8 +121,9 @@ mp_obj_t pb_iodevice_set_values(pbio_iodev_t *iodev, mp_obj_t values) { mp_obj_t *items; uint8_t len, i; pbio_iodev_data_type_t type; + pbio_error_t err; - pb_assert(pbio_iodev_get_bin_format(iodev, &len, &type)); + pb_assert(pbio_iodev_get_data_format(iodev, iodev->mode, &len, &type)); // if we only have one value, it doesn't have to be a tuple/list if (len == 1 && (mp_obj_is_integer(values) @@ -144,7 +161,10 @@ mp_obj_t pb_iodevice_set_values(pbio_iodev_t *iodev, mp_obj_t values) { } } - pb_assert(pbio_iodev_set_raw_values(iodev, data)); + while ((err = pbio_iodev_set_data_begin(iodev, iodev->mode, data)) == PBIO_ERROR_AGAIN); + pb_assert(err); + wait(pbio_iodev_set_data_end, pbio_iodev_set_data_cancel, iodev); + return mp_const_none; } diff --git a/extmod/pbiodevice.h b/extmod/pbiodevice.h index f8fab2674..f6dc68004 100644 --- a/extmod/pbiodevice.h +++ b/extmod/pbiodevice.h @@ -11,6 +11,6 @@ void pb_iodevice_assert_type_id(pbio_iodev_t *iodev, pbio_iodev_type_id_t type_id); pbio_error_t pb_iodevice_get_type_id(pbio_iodev_t *iodev, pbio_iodev_type_id_t *id); pbio_error_t pb_iodevice_get_mode(pbio_iodev_t *iodev, uint8_t *current_mode); -pbio_error_t pb_iodevice_set_mode(pbio_iodev_t *iodev, uint8_t new_mode); +void pb_iodevice_set_mode(pbio_iodev_t *iodev, uint8_t new_mode); mp_obj_t pb_iodevice_get_values(pbio_iodev_t *iodev); mp_obj_t pb_iodevice_set_values(pbio_iodev_t *iodev, mp_obj_t values); diff --git a/lib/pbio/drv/city_hub/uart.c b/lib/pbio/drv/city_hub/uart.c deleted file mode 100644 index 9c471d7cf..000000000 --- a/lib/pbio/drv/city_hub/uart.c +++ /dev/null @@ -1,234 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2018 David Lechner - -#include -#include -#include - -#include "pbdrv/config.h" -#include "pbio/error.h" -#include "pbio/event.h" -#include "pbio/port.h" -#include "pbio/uartdev.h" -#include "pbsys/sys.h" -#include "sys/process.h" -#include "../../src/processes.h" - -#include "stm32f030xc.h" - -#define TX_BUF_SIZE 32 // must be power of 2! -#define RX_BUF_SIZE 64 // must be power of 2! - -static uint8_t usart4_tx_buf[TX_BUF_SIZE]; -static volatile uint8_t usart4_tx_buf_head; -static uint8_t usart4_tx_buf_tail; - -static uint8_t usart3_tx_buf[TX_BUF_SIZE]; -static volatile uint8_t usart3_tx_buf_head; -static uint8_t usart3_tx_buf_tail; - -static uint8_t usart4_rx_buf[RX_BUF_SIZE]; -static volatile uint8_t usart4_rx_buf_head; -static uint8_t usart4_rx_buf_tail; - -static uint8_t usart3_rx_buf[RX_BUF_SIZE]; -static volatile uint8_t usart3_rx_buf_head; -static uint8_t usart3_rx_buf_tail; - -PROCESS(pbdrv_uart_process, "UART"); - -static pbio_error_t _pbdrv_uart_get_char(pbio_port_t port, uint8_t *c, bool peek) { - switch (port) { - case PBIO_PORT_B: - if (usart4_rx_buf_head == usart4_rx_buf_tail) { - return PBIO_ERROR_AGAIN; - } - - *c = usart4_rx_buf[usart4_rx_buf_tail]; - if (!peek) { - usart4_rx_buf_tail = (usart4_rx_buf_tail + 1) & (RX_BUF_SIZE - 1); - } - break; - case PBIO_PORT_A: - if (usart3_rx_buf_head == usart3_rx_buf_tail) { - return PBIO_ERROR_AGAIN; - } - - *c = usart3_rx_buf[usart3_rx_buf_tail]; - if (!peek) { - usart3_rx_buf_tail = (usart3_rx_buf_tail + 1) & (RX_BUF_SIZE - 1); - } - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -pbio_error_t pbdrv_uart_peek_char(pbio_port_t port, uint8_t *c) { - return _pbdrv_uart_get_char(port, c, true); -} - -pbio_error_t pbdrv_uart_get_char(pbio_port_t port, uint8_t *c) { - return _pbdrv_uart_get_char(port, c, false); -} - -pbio_error_t pbdrv_uart_put_char(pbio_port_t port, uint8_t c) { - uint8_t new_head; - - switch (port) { - case PBIO_PORT_B: - if (usart4_tx_buf_head == usart4_tx_buf_tail) { - // if ring buffer is empty and Tx is not busy, just send the byte - USART4->TDR = c; - } - else { - // otherwise queue it in the ring buffer - new_head = (usart4_tx_buf_head + 1) & (TX_BUF_SIZE - 1); - if (new_head == usart4_tx_buf_tail) { - // buffer is full - return PBIO_ERROR_AGAIN; - } - usart4_tx_buf[usart4_tx_buf_head] = c; - usart4_tx_buf_head = new_head; - } - break; - case PBIO_PORT_A: - if (usart3_tx_buf_head == usart3_tx_buf_tail) { - // if ring buffer is empty and Tx is not busy, just send the byte - USART3->TDR = c; - } - else { - // otherwise queue it in the ring buffer - new_head = (usart3_tx_buf_head + 1) & (TX_BUF_SIZE - 1); - if (new_head == usart3_tx_buf_tail) { - // buffer is full - return PBIO_ERROR_AGAIN; - } - usart3_tx_buf[usart3_tx_buf_head] = c; - usart3_tx_buf_head = new_head; - } - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -pbio_error_t pbdrv_uart_set_baud_rate(pbio_port_t port, uint32_t baud) { - switch (port) { - case PBIO_PORT_B: - USART4->BRR = PBDRV_CONFIG_SYS_CLOCK_RATE / baud; - break; - case PBIO_PORT_A: - USART3->BRR = PBDRV_CONFIG_SYS_CLOCK_RATE / baud; - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -static void uart_init() { - // enable power domains - RCC->APB1ENR |= RCC_APB1ENR_USART4EN | RCC_APB1ENR_USART3EN; - - // note: pin mux is handled in ioport.c - - // enable the UARTs - USART4->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE | USART_CR1_RXNEIE | USART_CR1_TCIE; - USART3->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE | USART_CR1_RXNEIE | USART_CR1_TCIE; - - pbdrv_uart_set_baud_rate(PBIO_PORT_B, 2400); - pbdrv_uart_set_baud_rate(PBIO_PORT_A, 2400); - - // DMA is not possible on USART3/4 on STM32F070x6, so using interrupt - NVIC_EnableIRQ(USART3_6_IRQn); - NVIC_SetPriority(USART3_6_IRQn, 128); -} - -// overrides weak function in setup_*.m -void USART3_6_IRQHandler(void) { - uint8_t c, new_head; - - // port C - while (USART4->ISR & USART_ISR_RXNE) { - c = USART4->RDR; - - new_head = (usart4_rx_buf_head + 1) & (RX_BUF_SIZE - 1); - if (new_head == usart4_rx_buf_tail) { - // buffer overrun - // REVISIT: ignoring for now - will lose characters - continue; - } - usart4_rx_buf[usart4_rx_buf_head] = c; - usart4_rx_buf_head = new_head; - } - - if (USART4->ISR & USART_ISR_TC) { - if (usart4_tx_buf_tail == usart4_tx_buf_head) { - // buffer is empty, set transmition complete bit - USART4->ICR |= USART_ICR_TCCF; - } - else { - USART4->TDR = usart4_tx_buf[usart4_tx_buf_tail]; - usart4_tx_buf_tail = (usart4_tx_buf_tail + 1) & (TX_BUF_SIZE - 1); - } - } - - // port D - while (USART3->ISR & USART_ISR_RXNE) { - c = USART3->RDR; - new_head = (usart3_rx_buf_head + 1) & (RX_BUF_SIZE - 1); - if (new_head == usart3_rx_buf_tail) { - // buffer overrun - // REVISIT: ignoring for now - will lose characters - continue; - } - usart3_rx_buf[usart3_rx_buf_head] = c; - usart3_rx_buf_head = new_head; - } - - if (USART3->ISR & USART_ISR_TC) { - if (usart3_tx_buf_tail == usart3_tx_buf_head) { - // buffer is empty, set transmition complete bit - USART3->ICR |= USART_ICR_TCCF; - } - else { - USART3->TDR = usart3_tx_buf[usart3_tx_buf_tail]; - usart3_tx_buf_tail = (usart3_tx_buf_tail + 1) & (TX_BUF_SIZE - 1); - } - } - - process_poll(&pbdrv_uart_process); -} - -static void handle_poll() { - uint8_t c; - - while (pbdrv_uart_get_char(PBIO_PORT_B, &c) == PBIO_SUCCESS) { - pbio_event_uart_rx_data_t rx = { .port = PBIO_PORT_B, .byte = c }; - process_post_synch(&pbio_uartdev_process, PBIO_EVENT_UART_RX, &rx); - } - while (pbdrv_uart_get_char(PBIO_PORT_A, &c) == PBIO_SUCCESS) { - pbio_event_uart_rx_data_t rx = { .port = PBIO_PORT_A, .byte = c }; - process_post_synch(&pbio_uartdev_process, PBIO_EVENT_UART_RX, &rx); - } -} - -PROCESS_THREAD(pbdrv_uart_process, ev, data) { - PROCESS_POLLHANDLER(handle_poll()); - - PROCESS_BEGIN(); - - uart_init(); - - while (true) { - PROCESS_WAIT_EVENT(); - } - - PROCESS_END(); -} diff --git a/lib/pbio/drv/debug/uart.c b/lib/pbio/drv/debug/uart.c deleted file mode 100644 index 436302798..000000000 --- a/lib/pbio/drv/debug/uart.c +++ /dev/null @@ -1,232 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2018-2019 David Lechner - -#include -#include -#include - -#include "pbdrv/config.h" -#include "pbio/error.h" -#include "pbio/event.h" -#include "pbio/port.h" -#include "pbio/uartdev.h" -#include "pbio/util.h" -#include "pbsys/sys.h" -#include "sys/process.h" -#include "../../src/processes.h" - -#define USE_HAL_DRIVER -#include "stm32f4xx.h" - -#define TX_BUF_SIZE 32 // must be power of 2! -#define RX_BUF_SIZE 64 // must be power of 2! - -typedef struct { - UART_HandleTypeDef handle; - const uint8_t irq; - uint8_t tx_buf[TX_BUF_SIZE]; - uint8_t rx_buf[RX_BUF_SIZE]; - uint8_t tx_buf_head; - volatile uint8_t tx_buf_tail; - uint8_t tx_byte; - volatile uint8_t rx_buf_head; - uint8_t rx_buf_tail; - uint8_t rx_byte; -} pbdrv_uart_t; - -static pbdrv_uart_t pbdrv_uart[PBDRV_CONFIG_NUM_IO_PORT] = { - [0] = { - .handle.Instance = USART2, - .handle.Init.BaudRate = 2400, - .handle.Init.WordLength = UART_WORDLENGTH_8B, - .handle.Init.StopBits = UART_STOPBITS_1, - .handle.Init.Parity = UART_PARITY_NONE, - .handle.Init.Mode = UART_MODE_TX_RX, - .handle.Init.HwFlowCtl = UART_HWCONTROL_NONE, - .handle.Init.OverSampling = UART_OVERSAMPLING_16, - .irq = USART2_IRQn, - }, -}; - -PROCESS(pbdrv_uart_process, "UART"); - -// overrides weak function in stm32f4xx_hal_uart.c -void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { - pbdrv_uart_t *uart = PBIO_CONTAINER_OF(huart, pbdrv_uart_t, handle); - uint8_t new_head; - - new_head = (uart->rx_buf_head + 1) & (RX_BUF_SIZE - 1); - if (new_head == uart->rx_buf_tail) { - // buffer overrun - // REVISIT: ignoring for now - will lose characters - HAL_UART_Receive_IT(&uart->handle, &uart->rx_byte, 1); - return; - } - uart->rx_buf[uart->rx_buf_head] = uart->rx_byte; - uart->rx_buf_head = new_head; - HAL_UART_Receive_IT(&uart->handle, &uart->rx_byte, 1); - - process_poll(&pbdrv_uart_process); -} - -static pbio_error_t _pbdrv_uart_get_char(pbio_port_t port, uint8_t *c, bool peek) { - pbdrv_uart_t *uart; - - switch (port) { - case PBIO_PORT_1: - uart = &pbdrv_uart[port - PBDRV_CONFIG_FIRST_IO_PORT]; - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - if (uart->rx_buf_head == uart->rx_buf_tail) { - return PBIO_ERROR_AGAIN; - } - - *c = uart->rx_buf[uart->rx_buf_tail]; - if (!peek) { - uart->rx_buf_tail = (uart->rx_buf_tail + 1) & (RX_BUF_SIZE - 1); - } - - return PBIO_SUCCESS; -} - -pbio_error_t pbdrv_uart_peek_char(pbio_port_t port, uint8_t *c) { - return _pbdrv_uart_get_char(port, c, true); -} - -pbio_error_t pbdrv_uart_get_char(pbio_port_t port, uint8_t *c) { - return _pbdrv_uart_get_char(port, c, false); -} - -// overrides weak function in stm32f4xx_hal_uart.c -void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) { - pbdrv_uart_t *uart = PBIO_CONTAINER_OF(huart, pbdrv_uart_t, handle); - - if (uart->tx_buf_tail != uart->tx_buf_head) { - // there is still more data to send - uart->tx_byte = uart->tx_buf[uart->tx_buf_tail]; - uart->tx_buf_tail = (uart->tx_buf_tail + 1) & (TX_BUF_SIZE - 1); - HAL_UART_Transmit_IT(&uart->handle, &uart->tx_byte, 1); - } -} - -pbio_error_t pbdrv_uart_put_char(pbio_port_t port, uint8_t c) { - pbdrv_uart_t *uart; - uint8_t new_head; - - switch (port) { - case PBIO_PORT_1: - uart = &pbdrv_uart[port - PBDRV_CONFIG_FIRST_IO_PORT]; - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - if (uart->tx_buf_head == uart->tx_buf_tail) { - uart->tx_byte = c; - HAL_UART_Transmit_IT(&uart->handle, &uart->tx_byte, 1); - } - else { - // otherwise queue it in the ring buffer - new_head = (uart->tx_buf_head + 1) & (TX_BUF_SIZE - 1); - if (new_head == uart->tx_buf_tail) { - // buffer is full - return PBIO_ERROR_AGAIN; - } - uart->tx_buf[uart->tx_buf_head] = c; - uart->tx_buf_head = new_head; - } - - return PBIO_SUCCESS; -} - -pbio_error_t pbdrv_uart_set_baud_rate(pbio_port_t port, uint32_t baud) { - pbdrv_uart_t *uart; - - switch (port) { - case PBIO_PORT_1: - uart = &pbdrv_uart[port - PBDRV_CONFIG_FIRST_IO_PORT]; - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - uart->handle.Init.BaudRate = baud; - HAL_UART_Init(&uart->handle); - HAL_UART_Receive_IT(&uart->handle, &uart->rx_byte, 1); - - return PBIO_SUCCESS; -} - -// overrides weak function in stm32f4xx_hal_uart.c -void HAL_UART_MspInit(UART_HandleTypeDef *huart) { - GPIO_InitTypeDef gpio_init; - pbdrv_uart_t *uart = PBIO_CONTAINER_OF(huart, pbdrv_uart_t, handle); - - // clocks are enabled in sys.c - - gpio_init.Pin = GPIO_PIN_5; - gpio_init.Mode = GPIO_MODE_AF_PP; - gpio_init.Pull = GPIO_PULLUP; - gpio_init.Alternate = 7; - HAL_GPIO_Init(GPIOD, &gpio_init); - - gpio_init.Pin = GPIO_PIN_6; - gpio_init.Mode = GPIO_MODE_AF_PP; - gpio_init.Pull = GPIO_PULLUP; - gpio_init.Alternate = 7; - HAL_GPIO_Init(GPIOD, &gpio_init); - - HAL_NVIC_SetPriority(uart->irq, 1, 0); - HAL_NVIC_EnableIRQ(uart->irq); -} - -// overrides weak function in stm32f4xx_hal_uart.c -void HAL_UART_MspDeInit(UART_HandleTypeDef *huart) { - pbdrv_uart_t *uart = PBIO_CONTAINER_OF(huart, pbdrv_uart_t, handle); - - HAL_NVIC_DisableIRQ(uart->irq); -} - -// overrides weak function in setup.m -void USART2_IRQHandler(void) { - HAL_UART_IRQHandler(&pbdrv_uart[0].handle); -} - -static void handle_poll() { - uint8_t i, c; - - for (i = PBDRV_CONFIG_FIRST_IO_PORT; i <= PBDRV_CONFIG_LAST_IO_PORT; i++) { - while (pbdrv_uart_get_char(i, &c) == PBIO_SUCCESS) { - pbio_event_uart_rx_data_t rx = { .port = i, .byte = c }; - process_post_synch(&pbio_uartdev_process, PBIO_EVENT_UART_RX, &rx); - } - } -} - -static void handle_exit() { - for (int i = 0; i < PBDRV_CONFIG_NUM_IO_PORT; i++) { - HAL_UART_DeInit(&pbdrv_uart[i].handle); - } - -} - -PROCESS_THREAD(pbdrv_uart_process, ev, data) { - PROCESS_POLLHANDLER(handle_poll()); - PROCESS_EXITHANDLER(handle_exit()); - - PROCESS_BEGIN(); - - for (int i = 0; i < PBDRV_CONFIG_NUM_IO_PORT; i++) { - HAL_UART_Init(&pbdrv_uart[i].handle); - HAL_UART_Receive_IT(&pbdrv_uart[i].handle, &pbdrv_uart[i].rx_byte, 1); - } - - while (true) { - PROCESS_WAIT_EVENT(); - } - - PROCESS_END(); -} diff --git a/lib/pbio/drv/ioport/ioport_lpf2.c b/lib/pbio/drv/ioport/ioport_lpf2.c index 96690e9b5..3711af950 100644 --- a/lib/pbio/drv/ioport/ioport_lpf2.c +++ b/lib/pbio/drv/ioport/ioport_lpf2.c @@ -13,6 +13,7 @@ #include #include #include +#include #include "ioport_lpf2.h" #include "sys/etimer.h" @@ -35,6 +36,7 @@ typedef struct _dcm_data_t { } dcm_data_t; typedef struct { + pbio_iodev_t *iodev; const pbdrv_ioport_lpf2_platform_port_t *pins; dcm_data_t dcm; struct pt pt; @@ -60,13 +62,6 @@ static const pbio_iodev_type_id_t ioport_type_id_lookup[3][3] = { }, }; -static struct { - pbio_iodev_info_t info; - pbio_iodev_mode_t modes[PBIO_IODEV_MAX_NUM_MODES]; -} ioport_info[PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS]; - -static pbio_iodev_t iodevs[PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS]; - PROCESS(pbdrv_ioport_lpf2_process, "I/O port"); static ioport_dev_t ioport_devs[PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS] = { @@ -115,9 +110,6 @@ static void init_one(uint8_t ioport) { PT_INIT(&ioport_devs[ioport].pt); - iodevs[ioport].port = PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT + ioport; - iodevs[ioport].info = &ioport_info[ioport].info; - pbdrv_gpio_input(&pins->id1); pbdrv_gpio_input(&pins->id2); pbdrv_gpio_input(&pins->uart_buf); @@ -131,7 +123,10 @@ pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) { return PBIO_ERROR_INVALID_PORT; } - *iodev = &iodevs[port - PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT]; + *iodev = ioport_devs[port - PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT].iodev; + if (*iodev == NULL) { + return PBIO_ERROR_NO_DEV; + } return PBIO_SUCCESS; } @@ -357,6 +352,7 @@ PROCESS_THREAD(pbdrv_ioport_lpf2_process, ev, data) { ioport->prev_type_id = ioport->connected_type_id; if (ioport->connected_type_id == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { ioport_enable_uart(ioport); + pbio_uartdev_get(i, &ioport->iodev); } } } diff --git a/lib/pbio/drv/move_hub/uart.c b/lib/pbio/drv/move_hub/uart.c deleted file mode 100644 index eec3dedee..000000000 --- a/lib/pbio/drv/move_hub/uart.c +++ /dev/null @@ -1,234 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2018 David Lechner - -#include -#include -#include - -#include "pbdrv/config.h" -#include "pbio/error.h" -#include "pbio/event.h" -#include "pbio/port.h" -#include "pbio/uartdev.h" -#include "pbsys/sys.h" -#include "sys/process.h" -#include "../../src/processes.h" - -#include "stm32f070xb.h" - -#define TX_BUF_SIZE 32 // must be power of 2! -#define RX_BUF_SIZE 64 // must be power of 2! - -static uint8_t usart4_tx_buf[TX_BUF_SIZE]; -static volatile uint8_t usart4_tx_buf_head; -static uint8_t usart4_tx_buf_tail; - -static uint8_t usart3_tx_buf[TX_BUF_SIZE]; -static volatile uint8_t usart3_tx_buf_head; -static uint8_t usart3_tx_buf_tail; - -static uint8_t usart4_rx_buf[RX_BUF_SIZE]; -static volatile uint8_t usart4_rx_buf_head; -static uint8_t usart4_rx_buf_tail; - -static uint8_t usart3_rx_buf[RX_BUF_SIZE]; -static volatile uint8_t usart3_rx_buf_head; -static uint8_t usart3_rx_buf_tail; - -PROCESS(pbdrv_uart_process, "UART"); - -static pbio_error_t _pbdrv_uart_get_char(pbio_port_t port, uint8_t *c, bool peek) { - switch (port) { - case PBIO_PORT_C: - if (usart4_rx_buf_head == usart4_rx_buf_tail) { - return PBIO_ERROR_AGAIN; - } - - *c = usart4_rx_buf[usart4_rx_buf_tail]; - if (!peek) { - usart4_rx_buf_tail = (usart4_rx_buf_tail + 1) & (RX_BUF_SIZE - 1); - } - break; - case PBIO_PORT_D: - if (usart3_rx_buf_head == usart3_rx_buf_tail) { - return PBIO_ERROR_AGAIN; - } - - *c = usart3_rx_buf[usart3_rx_buf_tail]; - if (!peek) { - usart3_rx_buf_tail = (usart3_rx_buf_tail + 1) & (RX_BUF_SIZE - 1); - } - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -pbio_error_t pbdrv_uart_peek_char(pbio_port_t port, uint8_t *c) { - return _pbdrv_uart_get_char(port, c, true); -} - -pbio_error_t pbdrv_uart_get_char(pbio_port_t port, uint8_t *c) { - return _pbdrv_uart_get_char(port, c, false); -} - -pbio_error_t pbdrv_uart_put_char(pbio_port_t port, uint8_t c) { - uint8_t new_head; - - switch (port) { - case PBIO_PORT_C: - if (usart4_tx_buf_head == usart4_tx_buf_tail) { - // if ring buffer is empty and Tx is not busy, just send the byte - USART4->TDR = c; - } - else { - // otherwise queue it in the ring buffer - new_head = (usart4_tx_buf_head + 1) & (TX_BUF_SIZE - 1); - if (new_head == usart4_tx_buf_tail) { - // buffer is full - return PBIO_ERROR_AGAIN; - } - usart4_tx_buf[usart4_tx_buf_head] = c; - usart4_tx_buf_head = new_head; - } - break; - case PBIO_PORT_D: - if (usart3_tx_buf_head == usart3_tx_buf_tail) { - // if ring buffer is empty and Tx is not busy, just send the byte - USART3->TDR = c; - } - else { - // otherwise queue it in the ring buffer - new_head = (usart3_tx_buf_head + 1) & (TX_BUF_SIZE - 1); - if (new_head == usart3_tx_buf_tail) { - // buffer is full - return PBIO_ERROR_AGAIN; - } - usart3_tx_buf[usart3_tx_buf_head] = c; - usart3_tx_buf_head = new_head; - } - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -pbio_error_t pbdrv_uart_set_baud_rate(pbio_port_t port, uint32_t baud) { - switch (port) { - case PBIO_PORT_C: - USART4->BRR = PBDRV_CONFIG_SYS_CLOCK_RATE / baud; - break; - case PBIO_PORT_D: - USART3->BRR = PBDRV_CONFIG_SYS_CLOCK_RATE / baud; - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -static void uart_init() { - // enable power domains - RCC->APB1ENR |= RCC_APB1ENR_USART4EN | RCC_APB1ENR_USART3EN; - - // note: pin mux is handled in ioport.c - - // enable the UARTs - USART4->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE | USART_CR1_RXNEIE | USART_CR1_TCIE; - USART3->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE | USART_CR1_RXNEIE | USART_CR1_TCIE; - - pbdrv_uart_set_baud_rate(PBIO_PORT_C, 2400); - pbdrv_uart_set_baud_rate(PBIO_PORT_D, 2400); - - // DMA is not possible on USART3/4 on STM32F070x6, so using interrupt - NVIC_EnableIRQ(USART3_6_IRQn); - NVIC_SetPriority(USART3_6_IRQn, 128); -} - -// overrides weak function in setup_*.m -void USART3_6_IRQHandler(void) { - uint8_t c, new_head; - - // port C - while (USART4->ISR & USART_ISR_RXNE) { - c = USART4->RDR; - - new_head = (usart4_rx_buf_head + 1) & (RX_BUF_SIZE - 1); - if (new_head == usart4_rx_buf_tail) { - // buffer overrun - // REVISIT: ignoring for now - will lose characters - continue; - } - usart4_rx_buf[usart4_rx_buf_head] = c; - usart4_rx_buf_head = new_head; - } - - if (USART4->ISR & USART_ISR_TC) { - if (usart4_tx_buf_tail == usart4_tx_buf_head) { - // buffer is empty, set transmition complete bit - USART4->ICR |= USART_ICR_TCCF; - } - else { - USART4->TDR = usart4_tx_buf[usart4_tx_buf_tail]; - usart4_tx_buf_tail = (usart4_tx_buf_tail + 1) & (TX_BUF_SIZE - 1); - } - } - - // port D - while (USART3->ISR & USART_ISR_RXNE) { - c = USART3->RDR; - new_head = (usart3_rx_buf_head + 1) & (RX_BUF_SIZE - 1); - if (new_head == usart3_rx_buf_tail) { - // buffer overrun - // REVISIT: ignoring for now - will lose characters - continue; - } - usart3_rx_buf[usart3_rx_buf_head] = c; - usart3_rx_buf_head = new_head; - } - - if (USART3->ISR & USART_ISR_TC) { - if (usart3_tx_buf_tail == usart3_tx_buf_head) { - // buffer is empty, set transmition complete bit - USART3->ICR |= USART_ICR_TCCF; - } - else { - USART3->TDR = usart3_tx_buf[usart3_tx_buf_tail]; - usart3_tx_buf_tail = (usart3_tx_buf_tail + 1) & (TX_BUF_SIZE - 1); - } - } - - process_poll(&pbdrv_uart_process); -} - -static void handle_poll() { - uint8_t c; - - while (pbdrv_uart_get_char(PBIO_PORT_C, &c) == PBIO_SUCCESS) { - pbio_event_uart_rx_data_t rx = { .port = PBIO_PORT_C, .byte = c }; - process_post_synch(&pbio_uartdev_process, PBIO_EVENT_UART_RX, &rx); - } - while (pbdrv_uart_get_char(PBIO_PORT_D, &c) == PBIO_SUCCESS) { - pbio_event_uart_rx_data_t rx = { .port = PBIO_PORT_D, .byte = c }; - process_post_synch(&pbio_uartdev_process, PBIO_EVENT_UART_RX, &rx); - } -} - -PROCESS_THREAD(pbdrv_uart_process, ev, data) { - PROCESS_POLLHANDLER(handle_poll()); - - PROCESS_BEGIN(); - - uart_init(); - - while (true) { - PROCESS_WAIT_EVENT(); - } - - PROCESS_END(); -} diff --git a/lib/pbio/drv/uart/uart_stm32f0.c b/lib/pbio/drv/uart/uart_stm32f0.c new file mode 100644 index 000000000..51b90fe54 --- /dev/null +++ b/lib/pbio/drv/uart/uart_stm32f0.c @@ -0,0 +1,272 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2019 David Lechner + +// This driver is for UARTs on STM32F0 MCUs. It provides async read and write +// functions for sending and receive data and allows changing the baud rate. +// There are no hardware buffers on the UARTs, so we implement a ring buffer +// to queue received data until it is read. No extra buffering is needed for +// transmitting. + +#include "pbdrv/config.h" + +#if PBDRV_CONFIG_UART_STM32F0 + +#include +#include +#include + +#include +#include + +#include "sys/etimer.h" +#include "sys/process.h" +#include "../../src/processes.h" + +#include "stm32f0xx.h" +#include "uart_stm32f0.h" + +#define UART_RING_BUF_SIZE 32 // must be a power of 2! + +typedef struct { + pbdrv_uart_dev_t uart_dev; + USART_TypeDef *USART; + uint8_t rx_ring_buf[UART_RING_BUF_SIZE]; + volatile uint8_t rx_ring_buf_head; + uint8_t rx_ring_buf_tail; + uint8_t *rx_buf; + uint8_t rx_buf_size; + uint8_t rx_buf_index; + uint8_t *tx_buf; + uint8_t tx_buf_size; + uint8_t tx_buf_index; + struct etimer rx_timer; + struct etimer tx_timer; + volatile pbio_error_t rx_result; + volatile pbio_error_t tx_result; + uint8_t irq; + bool initalized; +} pbdrv_uart_t; + +static pbdrv_uart_t pbdrv_uart[PBDRV_CONFIG_UART_STM32F0_NUM_UART]; + +PROCESS(pbdrv_uart_process, "UART"); + +pbio_error_t pbdrv_uart_get(uint8_t id, pbdrv_uart_dev_t **uart_dev) { + if (id >= PBDRV_CONFIG_UART_STM32F0_NUM_UART) { + return PBIO_ERROR_INVALID_ARG; + } + + if (!pbdrv_uart[id].initalized) { + return PBIO_ERROR_AGAIN; + } + + *uart_dev = &pbdrv_uart[id].uart_dev; + + return PBIO_SUCCESS; +} + +pbio_error_t pbdrv_uart_read_begin(pbdrv_uart_dev_t *uart_dev, uint8_t *msg, uint8_t length, uint32_t timeout) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + + if (!msg || !length) { + return PBIO_ERROR_INVALID_ARG; + } + + if (uart->rx_buf) { + return PBIO_ERROR_AGAIN; + } + + uart->rx_buf = msg; + uart->rx_buf_size = length; + uart->rx_buf_index = 0; + uart->rx_result = PBIO_ERROR_AGAIN; + + etimer_set(&uart->rx_timer, clock_from_msec(timeout)); + + process_poll(&pbdrv_uart_process); + + return PBIO_SUCCESS; +} + +pbio_error_t pbdrv_uart_read_end(pbdrv_uart_dev_t *uart_dev) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + pbio_error_t err = uart->rx_result; // read once since interrupt can modify it + + if (uart->rx_buf == NULL) { + // begin was not called first + return PBIO_ERROR_INVALID_OP; + } + + if (err != PBIO_ERROR_AGAIN) { + etimer_stop(&uart->rx_timer); + uart->rx_buf = NULL; + } + else if (etimer_expired(&uart->rx_timer)) { + err = PBIO_ERROR_TIMEDOUT; + uart->rx_buf = NULL; + } + + return err; +} + +void pbdrv_uart_read_cancel(pbdrv_uart_dev_t *uart_dev) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + + uart->rx_result = PBIO_ERROR_CANCELED; +} + +pbio_error_t pbdrv_uart_write_begin(pbdrv_uart_dev_t *uart_dev, uint8_t *msg, uint8_t length, uint32_t timeout) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + + if (!msg || !length) { + return PBIO_ERROR_INVALID_ARG; + } + + if (uart->tx_buf) { + return PBIO_ERROR_AGAIN; + } + + uart->tx_buf = msg; + uart->tx_buf_size = length; + uart->tx_buf_index = 0; + uart->tx_result = PBIO_ERROR_AGAIN; + + etimer_set(&uart->tx_timer, clock_from_msec(timeout)); + + uart->USART->CR1 |= USART_CR1_TXEIE; + + return PBIO_SUCCESS; +} + +pbio_error_t pbdrv_uart_write_end(pbdrv_uart_dev_t *uart_dev) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + pbio_error_t err = uart->tx_result; // read once since interrupt can modify it + + if (uart->tx_buf == NULL) { + // begin was not called first + return PBIO_ERROR_INVALID_OP; + } + + if (err != PBIO_ERROR_AGAIN) { + etimer_stop(&uart->tx_timer); + uart->tx_buf = NULL; + } + else if (etimer_expired(&uart->tx_timer)) { + uart->USART->CR1 &= ~(USART_CR1_TXEIE | USART_CR1_TCIE); + err = PBIO_ERROR_TIMEDOUT; + uart->tx_buf = NULL; + } + + return err; +} + +void pbdrv_uart_write_cancel(pbdrv_uart_dev_t *uart_dev) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + + uart->USART->CR1 &= ~(USART_CR1_TXEIE | USART_CR1_TCIE); + uart->tx_result = PBIO_ERROR_CANCELED; +} + +pbio_error_t pbdrv_uart_set_baud_rate(pbdrv_uart_dev_t *uart_dev, uint32_t baud) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + + if (uart->tx_buf || uart->rx_buf) { + return PBIO_ERROR_AGAIN; + } + + uart->USART->BRR = PBDRV_CONFIG_SYS_CLOCK_RATE / baud; + + return PBIO_SUCCESS; +} + +void pbdrv_uart_stm32f0_handle_irq(uint8_t id) { + pbdrv_uart_t *uart = &pbdrv_uart[id]; + + // receive next byte + if (uart->USART->ISR & USART_ISR_RXNE) { + // REVISIT: Do we need to have an overrun error when the ring buffer gets full? + uart->rx_ring_buf[uart->rx_ring_buf_head] = uart->USART->RDR; + uart->rx_ring_buf_head = (uart->rx_ring_buf_head + 1) & (UART_RING_BUF_SIZE - 1); + process_poll(&pbdrv_uart_process); + } + + // transmit next byte + if (uart->USART->CR1 & USART_CR1_TXEIE && uart->USART->ISR & USART_ISR_TXE) { + uart->USART->TDR = uart->tx_buf[uart->tx_buf_index++]; + if (uart->tx_buf_index == uart->tx_buf_size) { + // all bytes have been sent, so enable transmit complete interrupt + uart->USART->CR1 &= ~USART_CR1_TXEIE; + uart->USART->CR1 |= USART_CR1_TCIE; + } + } + + // transmission complete + if (uart->USART->CR1 & USART_CR1_TCIE && uart->USART->ISR & USART_ISR_TC) { + uart->USART->CR1 &= ~USART_CR1_TCIE; + uart->tx_result = PBIO_SUCCESS; + process_poll(&pbdrv_uart_process); + } +} + +static void handle_poll() { + for (int i = 0; i < PBDRV_CONFIG_UART_STM32F0_NUM_UART; i++) { + pbdrv_uart_t *uart = &pbdrv_uart[i]; + + // if receive is pending and we have not received all bytes yet... + if (uart->rx_buf && uart->rx_result == PBIO_ERROR_AGAIN && uart->rx_buf_index < uart->rx_buf_size) { + // copy all available bytes to rx_buf + while (uart->rx_ring_buf_head != uart->rx_ring_buf_tail) { + uart->rx_buf[uart->rx_buf_index++] = uart->rx_ring_buf[uart->rx_ring_buf_tail]; + uart->rx_ring_buf_tail = (uart->rx_ring_buf_tail + 1) & (UART_RING_BUF_SIZE - 1); + // when rx_buf is full, send out a notification + if (uart->rx_buf_index == uart->rx_buf_size) { + uart->rx_result = PBIO_SUCCESS; + process_post(PROCESS_BROADCAST, PROCESS_EVENT_COM, NULL); + break; + } + } + } + + if (uart->tx_buf && uart->tx_buf_index == uart->tx_buf_size) { + // TODO: this should only be sent once per write_begin + process_post(PROCESS_BROADCAST, PROCESS_EVENT_COM, NULL); + } + } +} + +static void handle_exit() { + for (int i = 0; i < PBDRV_CONFIG_UART_STM32F0_NUM_UART; i++) { + pbdrv_uart_t *uart = &pbdrv_uart[i]; + NVIC_DisableIRQ(uart->irq); + } +} + +PROCESS_THREAD(pbdrv_uart_process, ev, data) { + PROCESS_POLLHANDLER(handle_poll()); + PROCESS_EXITHANDLER(handle_exit()); + + PROCESS_BEGIN(); + + for (int i = 0; i < PBDRV_CONFIG_UART_STM32F0_NUM_UART; i++) { + const pbdrv_uart_stm32f0_platform_data_t *pdata = &pbdrv_uart_stm32f0_platform_data[i]; + pbdrv_uart_t *uart = &pbdrv_uart[i]; + + uart->USART = pdata->uart, + uart->irq = pdata->irq, + + uart->USART->CR3 |= USART_CR3_OVRDIS; + uart->USART->CR1 |= USART_CR1_RXNEIE | USART_CR1_TE | USART_CR1_RE | USART_CR1_UE; + NVIC_SetPriority(uart->irq, 0); + NVIC_EnableIRQ(uart->irq); + + uart->initalized = true; + } + + while (true) { + PROCESS_WAIT_EVENT(); + } + + PROCESS_END(); +} + +#endif // PBDRV_CONFIG_UART_STM32F0 diff --git a/lib/pbio/drv/uart/uart_stm32f0.h b/lib/pbio/drv/uart/uart_stm32f0.h new file mode 100644 index 000000000..d2ca21c6a --- /dev/null +++ b/lib/pbio/drv/uart/uart_stm32f0.h @@ -0,0 +1,20 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2019 David Lechner + +#ifndef _UART_STM32F0_H_ +#define _UART_STM32F0_H_ + +#include + +#include "stm32f0xx.h" + +typedef struct { + USART_TypeDef *uart; + uint8_t irq; +} pbdrv_uart_stm32f0_platform_data_t; + +extern const pbdrv_uart_stm32f0_platform_data_t pbdrv_uart_stm32f0_platform_data[PBDRV_CONFIG_UART_STM32F0_NUM_UART]; + +void pbdrv_uart_stm32f0_handle_irq(uint8_t id); + +#endif // _UART_STM32F4_H_ diff --git a/lib/pbio/drv/uart/uart_stm32f4.c b/lib/pbio/drv/uart/uart_stm32f4.c new file mode 100644 index 000000000..0f7ee0528 --- /dev/null +++ b/lib/pbio/drv/uart/uart_stm32f4.c @@ -0,0 +1,241 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2019 David Lechner + +#include "pbdrv/config.h" + +#if PBDRV_CONFIG_UART_STM32F4 + +#include +#include +#include + +#include +#include + +#include "sys/etimer.h" +#include "sys/process.h" +#include "../../src/processes.h" + +#define USE_HAL_DRIVER +#include "stm32f4xx.h" +#include "uart_stm32f4.h" + +typedef struct { + pbdrv_uart_dev_t uart_dev; + UART_HandleTypeDef huart; + struct etimer rx_timer; + struct etimer tx_timer; + volatile pbio_error_t rx_result; + volatile pbio_error_t tx_result; + uint8_t irq; +} pbdrv_uart_t; + +static pbdrv_uart_t pbdrv_uart[PBDRV_CONFIG_UART_STM32F4_NUM_UART]; + +PROCESS(pbdrv_uart_process, "UART"); + +pbio_error_t pbdrv_uart_get(uint8_t id, pbdrv_uart_dev_t **uart_dev) { + if (id >= PBDRV_CONFIG_UART_STM32F4_NUM_UART) { + return PBIO_ERROR_INVALID_ARG; + } + + if (HAL_UART_GetState(&pbdrv_uart[id].huart) == HAL_UART_STATE_RESET) { + return PBIO_ERROR_AGAIN; + } + + *uart_dev = &pbdrv_uart[id].uart_dev; + + return PBIO_SUCCESS; +} + +pbio_error_t pbdrv_uart_read_begin(pbdrv_uart_dev_t *uart_dev, uint8_t *msg, uint8_t length, uint32_t timeout) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + HAL_StatusTypeDef ret; + + if (uart->huart.RxState != HAL_UART_STATE_READY) { + return PBIO_ERROR_AGAIN; + } + + // can't receive if overrun flag is set + __HAL_UART_CLEAR_OREFLAG(&uart->huart); + + ret = HAL_UART_Receive_IT(&uart->huart, msg, length); + if (ret == HAL_ERROR) { + return PBIO_ERROR_INVALID_ARG; + } + if (ret == HAL_BUSY) { + return PBIO_ERROR_AGAIN; + } + + etimer_set(&uart->rx_timer, clock_from_msec(timeout)); + uart->rx_result = PBIO_ERROR_AGAIN; + + return PBIO_SUCCESS; +} + +pbio_error_t pbdrv_uart_read_end(pbdrv_uart_dev_t *uart_dev) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + pbio_error_t err = uart->rx_result; // read once since interrupt can modify it + + if (err != PBIO_ERROR_AGAIN) { + etimer_stop(&uart->rx_timer); + } + else if (etimer_expired(&uart->rx_timer)) { + // NB: This function can be blocking when using DMA - if we switch to + // DMA, the timout will need to be reworked. + HAL_UART_AbortReceive(&uart->huart); + err = PBIO_ERROR_TIMEDOUT; + } + + return err; +} + +void pbdrv_uart_read_cancel(pbdrv_uart_dev_t *uart_dev) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + + HAL_UART_AbortReceive_IT(&uart->huart); +} + +pbio_error_t pbdrv_uart_write_begin(pbdrv_uart_dev_t *uart_dev, uint8_t *msg, uint8_t length, uint32_t timeout) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + HAL_StatusTypeDef ret; + + ret = HAL_UART_Transmit_IT(&uart->huart, msg, length); + if (ret == HAL_ERROR) { + return PBIO_ERROR_INVALID_ARG; + } + if (ret == HAL_BUSY) { + return PBIO_ERROR_AGAIN; + } + + etimer_set(&uart->tx_timer, clock_from_msec(timeout)); + uart->tx_result = PBIO_ERROR_AGAIN; + + return PBIO_SUCCESS; +} + +pbio_error_t pbdrv_uart_write_end(pbdrv_uart_dev_t *uart_dev) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + pbio_error_t err = uart->tx_result; // read once since interrupt can modify it + + if (err != PBIO_ERROR_AGAIN) { + etimer_stop(&uart->tx_timer); + } + else if (etimer_expired(&uart->tx_timer)) { + // NB: This function can be blocking when using DMA - if we switch to + // DMA, the timout will need to be reworked. + HAL_UART_AbortTransmit(&uart->huart); + err = PBIO_ERROR_TIMEDOUT; + } + + return err; +} + +void pbdrv_uart_write_cancel(pbdrv_uart_dev_t *uart_dev) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + + HAL_UART_AbortTransmit_IT(&uart->huart); +} + +pbio_error_t pbdrv_uart_set_baud_rate(pbdrv_uart_dev_t *uart_dev, uint32_t baud) { + pbdrv_uart_t *uart = __containerof(uart_dev, pbdrv_uart_t, uart_dev); + + if (HAL_UART_GetState(&uart->huart) != HAL_UART_STATE_READY) { + return PBIO_ERROR_AGAIN; + } + + uart->huart.Init.BaudRate = baud; + // REVISIT: This is a potentially blocking function + HAL_UART_Init(&uart->huart); + + return PBIO_SUCCESS; +} + +// overrides weak function in stm32f4xx_hal_uart.c +void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { + pbdrv_uart_t *uart = __containerof(huart, pbdrv_uart_t, huart); + + if (uart->huart.ErrorCode & HAL_UART_ERROR_ORE) { + uart->rx_result = PBIO_ERROR_IO; + } + process_poll(&pbdrv_uart_process); +} + +// overrides weak function in stm32f4xx_hal_uart.c +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { + pbdrv_uart_t *uart = __containerof(huart, pbdrv_uart_t, huart); + + uart->rx_result = PBIO_SUCCESS; + process_poll(&pbdrv_uart_process); +} + +// overrides weak function in stm32f4xx_hal_uart.c +void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) { + pbdrv_uart_t *uart = __containerof(huart, pbdrv_uart_t, huart); + + uart->rx_result = PBIO_ERROR_CANCELED; +} + +// overrides weak function in stm32f4xx_hal_uart.c +void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) { + pbdrv_uart_t *uart = __containerof(huart, pbdrv_uart_t, huart); + + uart->tx_result = PBIO_SUCCESS; + process_poll(&pbdrv_uart_process); +} + +// overrides weak function in stm32f4xx_hal_uart.c +void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) { + pbdrv_uart_t *uart = __containerof(huart, pbdrv_uart_t, huart); + + uart->tx_result = PBIO_ERROR_CANCELED; +} + +void pbdrv_uart_stm32f4_handle_irq(uint8_t id) { + HAL_UART_IRQHandler(&pbdrv_uart[id].huart); +} + +static void handle_poll() { + process_post(PROCESS_BROADCAST, PROCESS_EVENT_COM, NULL); +} + +static void handle_exit() { + for (int i = 0; i < PBDRV_CONFIG_UART_STM32F4_NUM_UART; i++) { + pbdrv_uart_t *uart = &pbdrv_uart[i]; + HAL_NVIC_DisableIRQ(uart->irq); + HAL_UART_DeInit(&uart->huart); + } +} + +PROCESS_THREAD(pbdrv_uart_process, ev, data) { + PROCESS_POLLHANDLER(handle_poll()); + PROCESS_EXITHANDLER(handle_exit()); + + PROCESS_BEGIN(); + + for (int i = 0; i < PBDRV_CONFIG_UART_STM32F4_NUM_UART; i++) { + const pbdrv_uart_stm32f4_platform_data_t *pdata = &pbdrv_uart_stm32f4_platform_data[i]; + pbdrv_uart_t *uart = &pbdrv_uart[i]; + + uart->huart.Instance = pdata->uart, + uart->huart.Init.BaudRate = 2400, + uart->huart.Init.WordLength = UART_WORDLENGTH_8B, + uart->huart.Init.StopBits = UART_STOPBITS_1, + uart->huart.Init.Parity = UART_PARITY_NONE, + uart->huart.Init.Mode = UART_MODE_TX_RX, + uart->huart.Init.HwFlowCtl = UART_HWCONTROL_NONE, + uart->huart.Init.OverSampling = UART_OVERSAMPLING_16, + uart->irq = pdata->irq, + HAL_UART_Init(&pbdrv_uart[i].huart); + HAL_NVIC_SetPriority(uart->irq, 1, 0); + HAL_NVIC_EnableIRQ(uart->irq); + } + + while (true) { + PROCESS_WAIT_EVENT(); + } + + PROCESS_END(); +} + +#endif // PBDRV_CONFIG_UART_STM32F4 diff --git a/lib/pbio/drv/uart/uart_stm32f4.h b/lib/pbio/drv/uart/uart_stm32f4.h new file mode 100644 index 000000000..af5cd25fc --- /dev/null +++ b/lib/pbio/drv/uart/uart_stm32f4.h @@ -0,0 +1,20 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2019 David Lechner + +#ifndef _UART_STM32F4_H_ +#define _UART_STM32F4_H_ + +#include + +#include "stm32f4xx.h" + +typedef struct { + USART_TypeDef *uart; + uint8_t irq; +} pbdrv_uart_stm32f4_platform_data_t; + +extern const pbdrv_uart_stm32f4_platform_data_t pbdrv_uart_stm32f4_platform_data[PBDRV_CONFIG_UART_STM32F4_NUM_UART]; + +void pbdrv_uart_stm32f4_handle_irq(uint8_t id); + +#endif // _UART_STM32F4_H_ diff --git a/lib/pbio/include/pbdrv/uart.h b/lib/pbio/include/pbdrv/uart.h index 800b71a29..97a4c10be 100644 --- a/lib/pbio/include/pbdrv/uart.h +++ b/lib/pbio/include/pbdrv/uart.h @@ -15,58 +15,41 @@ #include #include -#if PBDRV_CONFIG_UART +typedef struct { -/** - * Peeks at the next character in the UART receive buffer without removing it - * from the buffer. - * @param [in] port The I/O port - * @param [out] c The next character - * @return ::PBIO_SUCCESS if a character was available, - * ::PBIO_ERROR_INVALID_PORT if the *port* does not have a - * UART associated with it, or ::PBIO_ERROR_AGAIN if no - * character was available to be read at this time. - */ -pbio_error_t pbdrv_uart_peek_char(pbio_port_t port, uint8_t *c); +} pbdrv_uart_dev_t; -/** - * Reads one character from the UART receive buffer. - * @param [in] port The I/O port - * @param [out] c The character read - * @return ::PBIO_SUCCESS if a character was available, - * ::PBIO_ERROR_INVALID_PORT if the *port* does not have a - * UART associated with it, or ::PBIO_ERROR_AGAIN if no - * character was available to be read at this time. - */ -pbio_error_t pbdrv_uart_get_char(pbio_port_t port, uint8_t *c); +#if PBDRV_CONFIG_UART -/** - * Writes one character to the UART transmit buffer. - * @param [in] port The I/O port - * @param [in] c The character write - * @return ::PBIO_SUCCESS if the *c* was written - * ::PBIO_ERROR_INVALID_PORT if the *port* does not have a - * UART associated with it, or ::PBIO_ERROR_AGAIN if the - * character could not be written at this time. - */ -pbio_error_t pbdrv_uart_put_char(pbio_port_t port, uint8_t c); +pbio_error_t pbdrv_uart_get(uint8_t id, pbdrv_uart_dev_t **uart_dev); /** * Sets the baud rate. - * @param [in] port The I/O port + * @param [in] uart The UART device * @param [in] baud The baud rate * @return ::PBIO_SUCCESS if the baud rate was set or * ::PBIO_ERROR_INVALID_PORT if the *port* does not have a * UART associated with it. */ -pbio_error_t pbdrv_uart_set_baud_rate(pbio_port_t port, uint32_t baud); +pbio_error_t pbdrv_uart_set_baud_rate(pbdrv_uart_dev_t *uart, uint32_t baud); + +pbio_error_t pbdrv_uart_read_begin(pbdrv_uart_dev_t *uart, uint8_t *msg, uint8_t length, uint32_t timeout); +pbio_error_t pbdrv_uart_read_end(pbdrv_uart_dev_t *uart); +void pbdrv_uart_read_cancel(pbdrv_uart_dev_t *uart); +pbio_error_t pbdrv_uart_write_begin(pbdrv_uart_dev_t *uart, uint8_t *msg, uint8_t length, uint32_t timeout); +pbio_error_t pbdrv_uart_write_end(pbdrv_uart_dev_t *uart); +void pbdrv_uart_write_cancel(pbdrv_uart_dev_t *uart); #else // PBDRV_CONFIG_UART -static inline pbio_error_t pbdrv_uart_peek_char(pbio_port_t port, uint8_t *c) { return PBIO_ERROR_NOT_SUPPORTED; } -static inline pbio_error_t pbdrv_uart_get_char(pbio_port_t port, uint8_t *c) { return PBIO_ERROR_NOT_SUPPORTED; } -static inline pbio_error_t pbdrv_uart_put_char(pbio_port_t port, uint8_t c) { return PBIO_ERROR_NOT_SUPPORTED; } -static inline pbio_error_t pbdrv_uart_set_baud_rate(pbio_port_t port, uint32_t baud) { return PBIO_ERROR_NOT_SUPPORTED; } +static inline pbio_error_t pbdrv_uart_get(uint8_t id, pbdrv_uart_dev_t **uart_dev) { return PBIO_ERROR_NOT_SUPPORTED; } +static inline pbio_error_t pbdrv_uart_set_baud_rate(pbdrv_uart_dev_t *uart, uint32_t baud) { return PBIO_ERROR_NOT_SUPPORTED; } +static inline pbio_error_t pbdrv_uart_read_begin(pbdrv_uart_dev_t *uart, uint8_t *msg, uint8_t length) { return PBIO_ERROR_NOT_SUPPORTED; } +static inline pbio_error_t pbdrv_uart_read_end(pbdrv_uart_dev_t *uart) { return PBIO_ERROR_NOT_SUPPORTED; } +static inline void pbdrv_uart_read_cancel(pbdrv_uart_dev_t *uart) { } +static inline pbio_error_t pbdrv_uart_write_begin(pbdrv_uart_dev_t *uart, uint8_t *msg, uint8_t length) { return PBIO_ERROR_NOT_SUPPORTED; } +static inline pbio_error_t pbdrv_uart_write_end(pbdrv_uart_dev_t *uart) { return PBIO_ERROR_NOT_SUPPORTED; } +static inline void pbdrv_uart_write_cancel(pbdrv_uart_dev_t *uart) { } #endif // PBDRV_CONFIG_UART diff --git a/lib/pbio/include/pbio/iodev.h b/lib/pbio/include/pbio/iodev.h index 6ba437485..613eb3020 100644 --- a/lib/pbio/include/pbio/iodev.h +++ b/lib/pbio/include/pbio/iodev.h @@ -119,6 +119,22 @@ typedef enum { PBIO_IODEV_FLAG_IS_MOTOR = 0x01, } pbio_iodev_flag_t; +/** + * Mapping flags that describe the input and output values of an I/O device. + */ +typedef enum { + /** The value is a discrete value, e.g. a color index. */ + LPF2_MAPPING_FLAG_DISCRETE = 1 << 2, + /** The value is a relative value, e.g. a motor position. */ + LPF2_MAPPING_FLAG_RELATIVE = 1 << 3, + /** The value is an absolute value, e.g. a distance measurement. */ + LPF2_MAPPING_FLAG_ABSOLUTE = 1 << 4, + /** Supports functional mapping 2.0+. */ + LPF2_MAPPING_FLAG_2_0 = 1 << 6, + /** Supports NULL value. */ + LPF2_MAPPING_FLAG_NULL = 1 << 7, +} pbio_iodev_mapping_flag_t; + /** * Information about one mode of an I/O device. */ @@ -175,6 +191,14 @@ typedef struct { * The units of measurement. */ char uom[PBIO_IODEV_UOM_SIZE + 1]; + /** + * Input value mapping flags. + */ + pbio_iodev_mapping_flag_t input_flags; + /** + * Output value mapping flags. + */ + pbio_iodev_mapping_flag_t output_flags; } pbio_iodev_mode_t; /** @@ -209,49 +233,29 @@ typedef struct _pbio_iodev_t pbio_iodev_t; /** @cond INTERNAL */ /** - * Function prototype for implementation of device mode setting function. - * @param [in] iodev Pointer to an I/O device - * @param [in] mode The new mode - */ -typedef pbio_error_t (*pbio_iodev_set_mode_func_t)(pbio_iodev_t *iodev, uint8_t mode); - -/** - * Function prototype for implementation of device set data function. - * @param [in] iodev Pointer to an I/O device - * @param [in] data The data values - * - * The size of *data* in bytes must be *len* times the size of *type*. - */ -typedef pbio_error_t (*pbio_iodev_set_data_func_t)(pbio_iodev_t *iodev, uint8_t *data); - -/** - * Function prototype for implementation of device write function. - * @param [in] iodev Pointer to an I/O device - * @param [in] data The data - * @param [in] size The size of *data* in bytes + * Device-specific communication functions. */ -typedef pbio_error_t (*pbio_iodev_write_func_t)(pbio_iodev_t *iodev, const uint8_t *data, uint8_t size); +typedef struct { + pbio_error_t (*set_mode_begin)(pbio_iodev_t *iodev, uint8_t mode); + pbio_error_t (*set_mode_end)(pbio_iodev_t *iodev); + void (*set_mode_cancel)(pbio_iodev_t *iodev); + pbio_error_t (*set_data_begin)(pbio_iodev_t *iodev, const uint8_t *data); + pbio_error_t (*set_data_end)(pbio_iodev_t *iodev); + void (*set_data_cancel)(pbio_iodev_t *iodev); + pbio_error_t (*write_begin)(pbio_iodev_t *iodev, const uint8_t *data, uint8_t size); + pbio_error_t (*write_end)(pbio_iodev_t *iodev); + void (*write_cancel)(pbio_iodev_t *iodev); +} pbio_iodev_ops_t; struct _pbio_iodev_t { /** * Pointer to the mode info for this device. */ - pbio_iodev_info_t *info; - /** - * Optional callback to write data values the device. - * This should not be called directly. Use ::pbio_iodev_set_raw_values() instead. - */ - pbio_iodev_set_data_func_t set_data; - /** - * Optional callback to write to the device. - * This should not be called directly. Use ::pbio_iodev_write() instead. - */ - pbio_iodev_write_func_t write; + const pbio_iodev_info_t *info; /** - * Optional callback to set the mode the device. - * This should not be called directly. Use ::pbio_iodev_set_mode() instead. + * Pointer to the device-specific communication functions. */ - pbio_iodev_set_mode_func_t set_mode; + const pbio_iodev_ops_t *ops; /** * The port the device is attached to. */ @@ -276,10 +280,16 @@ struct _pbio_iodev_t { /** @endcond */ size_t pbio_iodev_size_of(pbio_iodev_data_type_t type); -pbio_error_t pbio_iodev_get_bin_format(pbio_iodev_t *iodev, uint8_t *len, pbio_iodev_data_type_t *type); -pbio_error_t pbio_iodev_get_raw_values(pbio_iodev_t *iodev, uint8_t **data); -pbio_error_t pbio_iodev_set_raw_values(pbio_iodev_t *iodev, uint8_t *data) ; -pbio_error_t pbio_iodev_write(pbio_iodev_t *iodev, uint8_t *data, uint8_t size) ; -pbio_error_t pbio_iodev_set_mode(pbio_iodev_t *iodev, uint8_t mode); +pbio_error_t pbio_iodev_get_data_format(pbio_iodev_t *iodev, uint8_t mode, uint8_t *len, pbio_iodev_data_type_t *type); +pbio_error_t pbio_iodev_get_data(pbio_iodev_t *iodev, uint8_t **data); +pbio_error_t pbio_iodev_set_mode_begin(pbio_iodev_t *iodev, uint8_t mode); +pbio_error_t pbio_iodev_set_mode_end(pbio_iodev_t *iodev); +void pbio_iodev_set_mode_cancel(pbio_iodev_t *iodev); +pbio_error_t pbio_iodev_set_data_begin(pbio_iodev_t *iodev, uint8_t mode, const uint8_t *data); +pbio_error_t pbio_iodev_set_data_end(pbio_iodev_t *iodev); +void pbio_iodev_set_data_cancel(pbio_iodev_t *iodev); +pbio_error_t pbio_iodev_write_begin(pbio_iodev_t *iodev, const uint8_t *data, uint8_t size); +pbio_error_t pbio_iodev_write_end(pbio_iodev_t *iodev); +void pbio_iodev_write_cancel(pbio_iodev_t *iodev); #endif // _PBIO_IODEV_H_ diff --git a/lib/pbio/include/pbio/uartdev.h b/lib/pbio/include/pbio/uartdev.h index 4d3b31909..baa9b7d3f 100644 --- a/lib/pbio/include/pbio/uartdev.h +++ b/lib/pbio/include/pbio/uartdev.h @@ -4,10 +4,30 @@ #ifndef _PBIO_UARTDEV_H_ #define _PBIO_UARTDEV_H_ +#include + #include +#include +#include #if PBIO_CONFIG_UARTDEV +pbio_error_t pbio_uartdev_get(uint8_t id, pbio_iodev_t **iodev); + +#if !PBIO_CONFIG_UARTDEV_NUM_DEV +#error Must define PBIO_CONFIG_UARTDEV_NUM_DEV +#endif + +typedef struct { + uint8_t uart_id; +} pbio_uartdev_platform_data_t; + +extern const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV]; + +#else // PBIO_CONFIG_UARTDEV + +static inline pbio_error_t pbio_uartdev_get(uint8_t id, pbio_iodev_t **iodev) { return PBIO_ERROR_NOT_SUPPORTED; } + #endif // PBIO_CONFIG_UARTDEV #endif // _PBIO_UARTDEV_H_ diff --git a/lib/pbio/platform/city_hub/pbdrvconfig.h b/lib/pbio/platform/city_hub/pbdrvconfig.h index 9df0094a2..f5f976723 100644 --- a/lib/pbio/platform/city_hub/pbdrvconfig.h +++ b/lib/pbio/platform/city_hub/pbdrvconfig.h @@ -23,6 +23,10 @@ #define PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT PBIO_PORT_A #define PBDRV_CONFIG_IOPORT_LPF2_LAST_PORT PBIO_PORT_B +#define PBDRV_CONFIG_UART (1) +#define PBDRV_CONFIG_UART_STM32F0 (1) +#define PBDRV_CONFIG_UART_STM32F0_NUM_UART (2) + #define PBDRV_CONFIG_HAS_PORT_A (1) #define PBDRV_CONFIG_HAS_PORT_B (1) diff --git a/lib/pbio/platform/city_hub/platform.c b/lib/pbio/platform/city_hub/platform.c index 55298c9ec..e437d86a7 100644 --- a/lib/pbio/platform/city_hub/platform.c +++ b/lib/pbio/platform/city_hub/platform.c @@ -3,9 +3,11 @@ #include #include +#include #include "../../drv/button/button_gpio.h" #include "../../drv/ioport/ioport_lpf2.h" +#include "../../drv/uart/uart_stm32f0.h" #include "stm32f030xc.h" @@ -37,3 +39,36 @@ const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_1 = { .uart_rx = { .bank = GPIOC, .pin = 11 }, .alt = 0, }; + +// UART + +enum { + UART_ID_0, + UART_ID_1, +}; + +const pbdrv_uart_stm32f0_platform_data_t pbdrv_uart_stm32f0_platform_data[PBDRV_CONFIG_UART_STM32F0_NUM_UART] = { + [UART_ID_0] = { + .uart = USART3, + .irq = USART3_6_IRQn, + }, + [UART_ID_1] = { + .uart = USART4, + .irq = USART3_6_IRQn, + }, +}; + +// overrides weak function in setup_*.m +void USART3_6_IRQHandler(void) { + pbdrv_uart_stm32f0_handle_irq(UART_ID_0); + pbdrv_uart_stm32f0_handle_irq(UART_ID_1); +} + +const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { + [0] = { + .uart_id = UART_ID_0, + }, + [1] = { + .uart_id = UART_ID_1, + }, +}; diff --git a/lib/pbio/platform/city_hub/sys.c b/lib/pbio/platform/city_hub/sys.c index 08870a2a7..54a44faba 100644 --- a/lib/pbio/platform/city_hub/sys.c +++ b/lib/pbio/platform/city_hub/sys.c @@ -298,6 +298,7 @@ void SystemInit(void) { RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOCEN | RCC_AHBENR_GPIODEN | RCC_AHBENR_GPIOFEN; + RCC->APB1ENR |= RCC_APB1ENR_USART3EN | RCC_APB1ENR_USART4EN; RCC->APB2ENR |= RCC_APB2ENR_SYSCFGCOMPEN; diff --git a/lib/pbio/platform/debug/pbdrvconfig.h b/lib/pbio/platform/debug/pbdrvconfig.h index 935b93bd1..8be532928 100644 --- a/lib/pbio/platform/debug/pbdrvconfig.h +++ b/lib/pbio/platform/debug/pbdrvconfig.h @@ -21,11 +21,14 @@ #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F4 (1) +#define PBDRV_CONFIG_UART (1) +#define PBDRV_CONFIG_UART_STM32F4 (1) +#define PBDRV_CONFIG_UART_STM32F4_NUM_UART (1) + #define PBDRV_CONFIG_BLUETOOTH (0) #define PBDRV_CONFIG_IOPORT (1) #define PBDRV_CONFIG_LIGHT (1) #define PBDRV_CONFIG_MOTOR (0) -#define PBDRV_CONFIG_UART (1) #define PBDRV_CONFIG_HAS_PORT_1 (1) diff --git a/lib/pbio/platform/debug/platform.c b/lib/pbio/platform/debug/platform.c index 5a43e7e84..0a0d496f2 100644 --- a/lib/pbio/platform/debug/platform.c +++ b/lib/pbio/platform/debug/platform.c @@ -7,10 +7,14 @@ #include #include #include +#include + +#define USE_HAL_DRIVER +#include "stm32f4xx.h" #include "../../drv/button/button_gpio.h" +#include "../../drv/uart/uart_stm32f4.h" -#include "stm32f446xx.h" const pbdrv_button_gpio_platform_t pbdrv_button_gpio_platform[PBDRV_CONFIG_BUTTON_GPIO_NUM_BUTTON] = { [0] = { @@ -19,26 +23,56 @@ const pbdrv_button_gpio_platform_t pbdrv_button_gpio_platform[PBDRV_CONFIG_BUTTO } }; -static struct { - pbio_iodev_info_t info; - pbio_iodev_mode_t modes[PBIO_IODEV_MAX_NUM_MODES]; -} uart_iodev_info; +// UART Config +// +// Currently using pins labeled USART on the Nucleo board as a UART sensor port. +// UART sensors can be wired to the port with 22Ω series resistors on Tx and Rx. + +enum { + UART_ID_0, +}; -pbio_iodev_t uart_iodev = { - .info = &uart_iodev_info.info, - .port = PBIO_PORT_1, +const pbdrv_uart_stm32f4_platform_data_t pbdrv_uart_stm32f4_platform_data[PBDRV_CONFIG_UART_STM32F4_NUM_UART] = { + [UART_ID_0] = { + .uart = USART2, + .irq = USART2_IRQn, + }, }; +const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { + [0] = { + .uart_id = UART_ID_0, + }, +}; + +// overrides weak function in stm32f4xx_hal_uart.c +void HAL_UART_MspInit(UART_HandleTypeDef *huart) { + GPIO_InitTypeDef gpio_init; + + // clocks are enabled in sys.c + + gpio_init.Mode = GPIO_MODE_AF_PP; + gpio_init.Pull = GPIO_PULLUP; + gpio_init.Alternate = 7; + + gpio_init.Pin = GPIO_PIN_5; + HAL_GPIO_Init(GPIOD, &gpio_init); + + gpio_init.Pin = GPIO_PIN_6; + HAL_GPIO_Init(GPIOD, &gpio_init); +} + +// overrides weak function in setup.m +void USART2_IRQHandler(void) { + pbdrv_uart_stm32f4_handle_irq(UART_ID_0); +} + // HACK: we don't have a generic ioport interface yet so defining this function // in platform.c pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) { - switch (port) { - case PBIO_PORT_1: - *iodev = &uart_iodev; - break; - default: + if (port != PBIO_PORT_1) { return PBIO_ERROR_INVALID_PORT; } - return PBIO_SUCCESS; + return pbio_uartdev_get(0, iodev); } diff --git a/lib/pbio/platform/move_hub/pbdrvconfig.h b/lib/pbio/platform/move_hub/pbdrvconfig.h index 4e0568ae9..59c199c1e 100644 --- a/lib/pbio/platform/move_hub/pbdrvconfig.h +++ b/lib/pbio/platform/move_hub/pbdrvconfig.h @@ -23,6 +23,10 @@ #define PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT PBIO_PORT_C #define PBDRV_CONFIG_IOPORT_LPF2_LAST_PORT PBIO_PORT_D +#define PBDRV_CONFIG_UART (1) +#define PBDRV_CONFIG_UART_STM32F0 (1) +#define PBDRV_CONFIG_UART_STM32F0_NUM_UART (2) + #define PBDRV_CONFIG_HAS_PORT_A (1) #define PBDRV_CONFIG_HAS_PORT_B (1) #define PBDRV_CONFIG_HAS_PORT_C (1) diff --git a/lib/pbio/platform/move_hub/platform.c b/lib/pbio/platform/move_hub/platform.c index 8dd4ca608..07d489f1e 100644 --- a/lib/pbio/platform/move_hub/platform.c +++ b/lib/pbio/platform/move_hub/platform.c @@ -3,9 +3,11 @@ #include #include +#include #include "../../drv/button/button_gpio.h" #include "../../drv/ioport/ioport_lpf2.h" +#include "../../drv/uart/uart_stm32f0.h" #include "stm32f070xb.h" @@ -37,3 +39,38 @@ const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_1 = { .uart_rx = { .bank = GPIOC, .pin = 5 }, .alt = 1, }; + +// UART + +enum { + UART_ID_0, + UART_ID_1, +}; + +const pbdrv_uart_stm32f0_platform_data_t pbdrv_uart_stm32f0_platform_data[PBDRV_CONFIG_UART_STM32F0_NUM_UART] = { + [UART_ID_0] = { + .uart = USART4, + .irq = USART3_4_IRQn, + }, + [UART_ID_1] = { + .uart = USART3, + .irq = USART3_4_IRQn, + }, +}; + +// overrides weak function in setup_*.m +void USART3_4_IRQHandler(void) { + pbdrv_uart_stm32f0_handle_irq(UART_ID_0); + pbdrv_uart_stm32f0_handle_irq(UART_ID_1); +} + +#if PBIO_CONFIG_UARTDEV +const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { + [0] = { + .uart_id = UART_ID_0, + }, + [1] = { + .uart_id = UART_ID_1, + }, +}; +#endif // PBIO_CONFIG_UARTDEV diff --git a/lib/pbio/platform/move_hub/sys.c b/lib/pbio/platform/move_hub/sys.c index d1e045626..6cbf794da 100644 --- a/lib/pbio/platform/move_hub/sys.c +++ b/lib/pbio/platform/move_hub/sys.c @@ -289,6 +289,7 @@ void SystemInit(void) { RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOCEN | RCC_AHBENR_GPIODEN | RCC_AHBENR_GPIOFEN; + RCC->APB1ENR |= RCC_APB1ENR_USART3EN | RCC_APB1ENR_USART4EN; RCC->APB2ENR |= RCC_APB2ENR_SYSCFGCOMPEN; diff --git a/lib/pbio/src/iodev.c b/lib/pbio/src/iodev.c index 9f12f1006..f25eaed17 100644 --- a/lib/pbio/src/iodev.c +++ b/lib/pbio/src/iodev.c @@ -29,15 +29,23 @@ size_t pbio_iodev_size_of(pbio_iodev_data_type_t type) { /** * Gets the binary format used by the current mode of an I/O device. * @param [in] iodev The I/O device + * @param [in] mode The mode * @param [out] len The number of values in the raw data array * @param [out] type The data type of the raw data values * @return ::PBIO_SUCCESS on success - * ::PBIO_ERROR_INVALID_PORT if the port is not valid + * ::PBIO_ERROR_INVALID_ARG if the mode is not valid * ::PBIO_ERROR_NO_DEV if the port does not have a device attached */ -pbio_error_t pbio_iodev_get_bin_format(pbio_iodev_t *iodev, uint8_t *len, pbio_iodev_data_type_t *type) { - *len = iodev->info->mode_info[iodev->mode].num_values; - *type = iodev->info->mode_info[iodev->mode].data_type; +pbio_error_t pbio_iodev_get_data_format(pbio_iodev_t *iodev, uint8_t mode, uint8_t *len, pbio_iodev_data_type_t *type) { + if (iodev->info->type_id == PBIO_IODEV_TYPE_ID_NONE) { + return PBIO_ERROR_NO_DEV; + } + if (mode >= iodev->info->num_modes) { + return PBIO_ERROR_INVALID_ARG; + } + + *len = iodev->info->mode_info[mode].num_values; + *type = iodev->info->mode_info[mode].data_type; return PBIO_SUCCESS; } @@ -47,37 +55,95 @@ pbio_error_t pbio_iodev_get_bin_format(pbio_iodev_t *iodev, uint8_t *len, pbio_i * @param [in] iodev The I/O device * @param [out] data Pointer to hold array of data values * @return ::PBIO_SUCCESS on success - * ::PBIO_ERROR_INVALID_PORT if the port is not valid * ::PBIO_ERROR_NO_DEV if the port does not have a device attached * - * The binary format and size of *data* is determined by ::pbio_iodev_get_bin_format(). + * The binary format and size of *data* is determined by ::pbio_iodev_get_data_format(). */ -pbio_error_t pbio_iodev_get_raw_values(pbio_iodev_t *iodev, uint8_t **data) { +pbio_error_t pbio_iodev_get_data(pbio_iodev_t *iodev, uint8_t **data) { + if (iodev->info->type_id == PBIO_IODEV_TYPE_ID_NONE) { + return PBIO_ERROR_NO_DEV; + } + *data = iodev->bin_data; return PBIO_SUCCESS; } +/** + * Sets the mode of an I/O device. + * @param [in] iodev The I/O device + * @param [in] mode The new mode + * @return ::PBIO_SUCCESS on success + * ::PBIO_ERROR_INVALID_ARG if the mode is not valid + * ::PBIO_ERROR_NOT_SUPPORTED if the device does not support setting the mode + */ +pbio_error_t pbio_iodev_set_mode_begin(pbio_iodev_t *iodev, uint8_t mode) { + if (!iodev->ops->set_mode_begin) { + return PBIO_ERROR_NOT_SUPPORTED; + } + + if (mode >= iodev->info->num_modes) { + return PBIO_ERROR_INVALID_ARG; + } + + return iodev->ops->set_mode_begin(iodev, mode); +} + +pbio_error_t pbio_iodev_set_mode_end(pbio_iodev_t *iodev) { + if (!iodev->ops->set_mode_end) { + return PBIO_ERROR_NOT_SUPPORTED; + } + + return iodev->ops->set_mode_end(iodev); +} + +void pbio_iodev_set_mode_cancel(pbio_iodev_t *iodev) { + if (!iodev->ops->set_mode_cancel) { + return; + } + + iodev->ops->set_mode_cancel(iodev); +} + /** * Sets the raw data of an I/O device. * @param [in] iodev The I/O device + * @param [in] mode The mode * @param [in] data Array of data values - * @param [in] len The length of the *data* array - * @param [in] type The data type of the *data* values * @return ::PBIO_SUCCESS on success - * ::PBIO_ERROR_INVALID_PORT if the port is not valid * ::PBIO_ERROR_NO_DEV if the port does not have a device attached * ::PBIO_ERROR_AGAIN if the device is busy with something else * ::PBIO_ERROR_NOT_SUPPORTED if the device does not support setting values + * ::PBIO_ERROR_INVALID_OP if the current mode does not match the requested mode * - * The binary format and size of *data* is determined by ::pbio_iodev_get_bin_format(). + * The binary format and size of *data* is determined by ::pbio_iodev_get_data_format(). */ -pbio_error_t pbio_iodev_set_raw_values(pbio_iodev_t *iodev, uint8_t *data) { - if (!iodev->set_data) { +pbio_error_t pbio_iodev_set_data_begin(pbio_iodev_t *iodev, uint8_t mode, const uint8_t *data) { + if (!iodev->ops->set_data_begin) { + return PBIO_ERROR_NOT_SUPPORTED; + } + + if (iodev->mode != mode) { + return PBIO_ERROR_INVALID_OP; + } + + return iodev->ops->set_data_begin(iodev, data); +} + +pbio_error_t pbio_iodev_set_data_end(pbio_iodev_t *iodev) { + if (!iodev->ops->set_data_end) { return PBIO_ERROR_NOT_SUPPORTED; } - return iodev->set_data(iodev, data); + return iodev->ops->set_data_end(iodev); +} + +void pbio_iodev_set_data_cancel(pbio_iodev_t *iodev) { + if (!iodev->ops->set_data_cancel) { + return; + } + + iodev->ops->set_data_cancel(iodev); } /** @@ -86,37 +152,31 @@ pbio_error_t pbio_iodev_set_raw_values(pbio_iodev_t *iodev, uint8_t *data) { * @param [in] data Pointer to raw data to write * @param [in] size Size of the *data* in bytes * @return ::PBIO_SUCCESS on success - * ::PBIO_ERROR_INVALID_PORT if the port is not valid * ::PBIO_ERROR_INVALID_ARG if *size* is too large * ::PBIO_ERROR_NO_DEV if the port does not have a device attached * ::PBIO_ERROR_AGAIN if the device is busy with something else * ::PBIO_ERROR_NOT_SUPPORTED if the device does not support writing */ -pbio_error_t pbio_iodev_write(pbio_iodev_t *iodev, uint8_t *data, uint8_t size) { - if (!iodev->write) { +pbio_error_t pbio_iodev_write_begin(pbio_iodev_t *iodev, const uint8_t *data, uint8_t size) { + if (!iodev->ops->write_begin) { return PBIO_ERROR_NOT_SUPPORTED; } - return iodev->write(iodev, data, size); + return iodev->ops->write_begin(iodev, data, size); } -/** - * Sets the mode of an I/O device. - * @param [in] iodev The I/O device - * @param [in] mode The new mode - * @return ::PBIO_SUCCESS on success - * ::PBIO_ERROR_INVALID_PORT if the port is not valid - * ::PBIO_ERROR_INVALID_ARG if the mode is not valid - * ::PBIO_ERROR_NOT_SUPPORTED if the device does not support setting the mode - */ -pbio_error_t pbio_iodev_set_mode(pbio_iodev_t *iodev, uint8_t mode) { - if (!iodev->set_mode) { +pbio_error_t pbio_iodev_write_end(pbio_iodev_t *iodev) { + if (!iodev->ops->write_end) { return PBIO_ERROR_NOT_SUPPORTED; } - if (mode >= iodev->info->num_modes) { - return PBIO_ERROR_INVALID_ARG; + return iodev->ops->write_end(iodev); +} + +void pbio_iodev_write_cancel(pbio_iodev_t *iodev) { + if (!iodev->ops->write_cancel) { + return; } - return iodev->set_mode(iodev, mode); + iodev->ops->write_cancel(iodev); } diff --git a/lib/pbio/src/uartdev.c b/lib/pbio/src/uartdev.c index d86d76031..e2d8d18bf 100644 --- a/lib/pbio/src/uartdev.c +++ b/lib/pbio/src/uartdev.c @@ -5,7 +5,7 @@ * Based on: * LEGO MINDSTORMS EV3 UART Sensor tty line discipline * - * Copyright (C) 2014-2016,2018 David Lechner + * Copyright (C) 2014-2016,2018-2019 David Lechner * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as @@ -63,6 +63,7 @@ // by using EV3_UART_INFO_MODE_PLUS_8 #define EV3_UART_DATA_KEEP_ALIVE_TIMEOUT 100 /* msec */ +#define EV3_UART_IO_TIMEOUT 250 /* msec */ enum ev3_uart_msg_type { EV3_UART_MSG_TYPE_SYS = 0x00, @@ -108,7 +109,7 @@ enum ev3_uart_info { EV3_UART_INFO_UNK2 = 0x06, // Powered Up only EV3_UART_INFO_MOTOR_BIAS = 0x07, // Powered Up only EV3_UART_INFO_CAPABILITY = 0x08, // Powered Up only - EV3_UART_INFO_MODE_PLUS_8 = 0x20, // Powered Up only - not used? + EV3_UART_INFO_MODE_PLUS_8 = 0x20, // Powered Up only EV3_UART_INFO_FORMAT = 0x80, }; @@ -180,9 +181,16 @@ typedef enum { } pbio_uartdev_status_t; /** - * struct ev3_uart_data - Discipline data for EV3 UART Sensor communication - * @iodev: Pointer to the I/O device - * @timer: Timer for sending keepalive messages. + * struct ev3_uart_port_data - Data for EV3/LPF2 UART Sensor communication + * @uart: Pointer to the UART device to use for communications + * @pt: Protothread for main communication protocol + * @pt_data: Protothread for receiving sensor data + * @timer: Timer for sending keepalive messages and other delays. + * @info: The I/O device information struct for the connected device + * @modes: The mode info array for @info + * @iodev: The I/O device state information struct + * @status: The current device connection state + * @type_id: The type ID received * @requested_mode: Mode that was requested by user. Used to restore previous * mode in case of a reconnect. * @new_mode: The mode requested by set_mode. Also used to keep track of mode @@ -190,35 +198,47 @@ typedef enum { * @new_baud_rate: New baud rate that will be set with ev3_uart_change_bitrate * @info_flags: Flags indicating what information has already been read * from the data. - * @msg: partial data->msg from previous receive callback - * @partial_msg_size: the size of the partial data->msg + * @tx_msg: Buffer to hold messages transmitted to the device + * @rx_msg: Buffer to hold messages received from the device + * @rx_msg_size: Size of the current message being received * @ext_mode: Extra mode adder for Powered Up devices (for modes > EV3_UART_MODE_MAX) * @write_cmd_size: The size parameter received from a WRITE command * @last_err: data->msg to be printed in case of an error. + * @err_count: Total number of errors that have occurred * @num_data_err: Number of bad reads when receiving DATA data->msgs. * @data_rec: Flag that indicates that good DATA data->msg has been received * since last watchdog timeout. + * @tx_busy: mutex that protects tx_msg */ -typedef struct ev3_uart_port_data { - pbio_iodev_t *iodev; +typedef struct { + pbdrv_uart_dev_t *uart; + struct pt pt; + struct pt data_pt; struct etimer timer; + pbio_iodev_info_t info; + pbio_iodev_mode_t modes[PBIO_IODEV_MAX_NUM_MODES]; + pbio_iodev_t iodev; pbio_uartdev_status_t status; + pbio_iodev_type_id_t type_id; uint8_t requested_mode; uint8_t new_mode; uint32_t new_baud_rate; uint32_t info_flags; - uint8_t msg[EV3_UART_MAX_MESSAGE_SIZE]; - uint8_t partial_msg_size; + uint8_t tx_msg[EV3_UART_MAX_MESSAGE_SIZE]; + uint8_t rx_msg[EV3_UART_MAX_MESSAGE_SIZE]; + uint8_t rx_msg_size; uint8_t ext_mode; uint8_t write_cmd_size; DBG_ERR(const char *last_err); + uint32_t err_count; uint32_t num_data_err; - unsigned data_rec:1; + bool data_rec; + bool tx_busy; } uartdev_port_data_t; PROCESS(pbio_uartdev_process, "UART device"); -static uartdev_port_data_t dev_data[PBDRV_CONFIG_NUM_IO_PORT]; +static uartdev_port_data_t dev_data[PBIO_CONFIG_UARTDEV_NUM_DEV]; static const pbio_iodev_mode_t ev3_uart_default_mode_info = { .raw_max = 1023, @@ -227,11 +247,40 @@ static const pbio_iodev_mode_t ev3_uart_default_mode_info = { .digits = 4, }; -static inline uint8_t port_to_index(pbio_port_t port) { -#if PBDRV_CONFIG_NUM_IO_PORT - return port - PBDRV_CONFIG_FIRST_IO_PORT; +#define PBIO_PT_WAIT_READY(pt, expr) PT_WAIT_UNTIL((pt), (expr) != PBIO_ERROR_AGAIN) + +pbio_error_t pbio_uartdev_get(uint8_t id, pbio_iodev_t **iodev) { + if (id >= PBIO_CONFIG_UARTDEV_NUM_DEV) { + return PBIO_ERROR_NO_DEV; + } + + *iodev = &dev_data[id].iodev; + return PBIO_SUCCESS; +} + +static inline uint32_t uint32_le(uint8_t *bytes) { +#if __BYTE_ORDER == __LITTLE_ENDIAN + uint32_t result; + + // avoiding unaligned access + memcpy(&result, bytes, 4); + + return result; #else - return 0; +#error Big endian not implemented +#endif +} + +static inline float float_le(uint8_t *bytes) { +#if __BYTE_ORDER == __LITTLE_ENDIAN + float result; + + // avoiding unaligned access + memcpy(&result, bytes, 4); + + return result; +#else +#error Big endian not implemented #endif } @@ -257,105 +306,15 @@ static uint8_t ev3_uart_get_msg_size(uint8_t header) { return size; } -static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { - uartdev_port_data_t *data; +static void pbio_uartdev_parse_msg(uartdev_port_data_t *data) { uint32_t speed; - uint8_t msg_size, msg_type, cmd, cmd2, mode, i; - - data = &dev_data[port_to_index(port)]; - - if (data->status == PBIO_UARTDEV_STATUS_ERR) { - data->partial_msg_size = 0; - data->ext_mode = 0; - data->status = PBIO_UARTDEV_STATUS_SYNCING; - } - - /* - * To get in sync with the data stream from the sensor, we look - * for a valid TYPE command. - */ - if (data->status == PBIO_UARTDEV_STATUS_SYNCING) { - // look for first byte that is TYPE command - if (data->partial_msg_size == 0) { - if (next_byte == (EV3_UART_MSG_TYPE_CMD | EV3_UART_CMD_TYPE)) { - data->msg[0] = next_byte; - data->partial_msg_size = 1; - } - return; - } - - // look for second byte that is valid device type ID - if (data->partial_msg_size == 1) { - if (next_byte <= EV3_UART_TYPE_MIN || next_byte <= EV3_UART_TYPE_MAX) { - data->msg[1] = next_byte; - data->partial_msg_size = 2; - } - else { - data->partial_msg_size = 0; - } - return; - } - - // look for third byte that is valid checksum - if (data->partial_msg_size == 2) { - uint8_t checksum = 0xff ^ data->msg[0] ^ data->msg[1]; - - if (next_byte != checksum) { - data->partial_msg_size = 0; - return; - } - } - - data->iodev->info->num_modes = 1; - data->iodev->info->num_view_modes = 1; - - for (i = 0; i < PBIO_IODEV_MAX_NUM_MODES; i++) { - data->iodev->info->mode_info[i] = ev3_uart_default_mode_info; - } - - data->iodev->info->type_id = data->msg[1]; - data->partial_msg_size = 0; - data->info_flags = EV3_UART_INFO_FLAG_CMD_TYPE; - data->data_rec = 0; - data->num_data_err = 0; - data->status = PBIO_UARTDEV_STATUS_INFO; - - return; - } - - if (data->partial_msg_size) { - // collect next_byte until we have a full data->msg - msg_size = ev3_uart_get_msg_size(data->msg[0]); - data->msg[data->partial_msg_size++] = next_byte; - if (data->partial_msg_size < msg_size) { - return; - } - } else if (next_byte == 0xFF) { - // Sometimes we get 0xFF after switching baud rates, so just ignore it. - return; - } else { - // first byte of the data->msg contains the data->msg size - msg_size = ev3_uart_get_msg_size(next_byte); - if (msg_size > EV3_UART_MAX_MESSAGE_SIZE) { - DBG_ERR(data->last_err = "Bad data->msg size"); - goto err; - } - data->msg[0] = next_byte; - data->partial_msg_size = 1; - if (msg_size > 1) { - return; - } - } - - // at this point, we have a full data->msg that can be parsed - - // reset msg size for next message - data->partial_msg_size = 0; + uint8_t msg_type, cmd, msg_size, mode, cmd2; - msg_type = data->msg[0] & EV3_UART_MSG_TYPE_MASK; - cmd = data->msg[0] & EV3_UART_MSG_CMD_MASK; + msg_type = data->rx_msg[0] & EV3_UART_MSG_TYPE_MASK; + cmd = data->rx_msg[0] & EV3_UART_MSG_CMD_MASK; + msg_size = ev3_uart_get_msg_size(data->rx_msg[0]); mode = cmd; - cmd2 = data->msg[1]; + cmd2 = data->rx_msg[1]; // The original EV3 spec only allowed for up to 8 modes (3-bit number). // The Powered UP spec extents this by adding an extra flag to INFO commands. @@ -371,24 +330,22 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { if (msg_size > 1) { uint8_t checksum = 0xFF; - for (i = 0; i < msg_size - 1; i++) { - checksum ^= data->msg[i]; + for (int i = 0; i < msg_size - 1; i++) { + checksum ^= data->rx_msg[i]; } - /* - * The LEGO EV3 color sensor sends bad checksums - * for RGB-RAW data (mode 4). The check here could be - * improved if someone can find a pattern. - */ - if (checksum != data->msg[msg_size - 1] - && data->iodev->info->type_id != PBIO_IODEV_TYPE_ID_EV3_COLOR_SENSOR - && data->msg[0] != 0xDC) - { + if (checksum != data->rx_msg[msg_size - 1]) { DBG_ERR(data->last_err = "Bad checksum"); // if INFO messages are done and we are now receiving data, it is // OK to occasionally have a bad checksum if (data->status == PBIO_UARTDEV_STATUS_DATA) { - data->num_data_err++; - return; + + // The LEGO EV3 color sensor sends bad checksums + // for RGB-RAW data (mode 4). The check here could be + // improved if someone can find a pattern. + if (data->type_id != PBIO_IODEV_TYPE_ID_EV3_COLOR_SENSOR + && data->rx_msg[0] != (EV3_UART_MSG_TYPE_DATA | EV3_UART_MSG_SIZE_8 | 4)) { + return; + } } else { goto err; } @@ -405,7 +362,7 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { } break; case EV3_UART_SYS_ACK: - if (!data->iodev->info->num_modes) { + if (!data->info.num_modes) { DBG_ERR(data->last_err = "Received ACK before all mode INFO"); goto err; } @@ -415,13 +372,7 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { } data->status = PBIO_UARTDEV_STATUS_ACK; - data->iodev->mode = data->new_mode; - - // reply with ACK - while (pbdrv_uart_put_char(port, EV3_UART_SYS_ACK) == PBIO_ERROR_AGAIN); - - // schedule baud rate change - etimer_set(&data->timer, clock_from_msec(10)); + data->iodev.mode = data->new_mode; return; } @@ -437,22 +388,22 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { DBG_ERR(data->last_err = "Number of modes is out of range"); goto err; } - data->iodev->info->num_modes = cmd2 + 1; + data->info.num_modes = cmd2 + 1; if (msg_size > 5) { // Powered Up devices can have an extended mode message that // includes modes > EV3_UART_MODE_MAX - data->iodev->info->num_modes = data->msg[3] + 1; - data->iodev->info->num_view_modes = data->msg[4] + 1; + data->info.num_modes = data->rx_msg[3] + 1; + data->info.num_view_modes = data->rx_msg[4] + 1; } else if (msg_size > 3) { - data->iodev->info->num_view_modes = data->msg[2] + 1; + data->info.num_view_modes = data->rx_msg[2] + 1; } else { - data->iodev->info->num_view_modes = data->iodev->info->num_modes; + data->info.num_view_modes = data->info.num_modes; } - debug_pr("num_modes: %d\n", data->iodev->info->num_modes); - debug_pr("num_view_modes: %d\n", data->iodev->info->num_view_modes); + debug_pr("num_modes: %d\n", data->info.num_modes); + debug_pr("num_view_modes: %d\n", data->info.num_view_modes); break; case EV3_UART_CMD_SPEED: @@ -460,7 +411,7 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { DBG_ERR(data->last_err = "Received duplicate speed INFO"); goto err; } - speed = *(int*)(data->msg + 1); + speed = uint32_le(data->rx_msg + 1); if (speed < EV3_UART_SPEED_MIN || speed > EV3_UART_SPEED_MAX) { DBG_ERR(data->last_err = "Speed is out of range"); goto err; @@ -473,9 +424,9 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { case EV3_UART_CMD_WRITE: if (cmd2 & 0x20) { data->write_cmd_size = cmd2 & 0x3; - if (data->iodev->info->type_id == PBIO_IODEV_TYPE_ID_INTERACTIVE_MOTOR) { + if (data->type_id == PBIO_IODEV_TYPE_ID_INTERACTIVE_MOTOR) { // TODO: msg[3] and msg[4] probably give us useful information - data->iodev->flags |= PBIO_IODEV_FLAG_IS_MOTOR; + data->iodev.flags |= PBIO_IODEV_FLAG_IS_MOTOR; // FIXME: clear this flag when device disconnects } else { @@ -486,7 +437,7 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { case EV3_UART_CMD_EXT_MODE: // Powered up devices can have modes > EV3_UART_MODE_MAX. This // command precedes other commands to add the extra 8 to the mode - data->ext_mode = data->msg[1]; + data->ext_mode = data->rx_msg[1]; break; case EV3_UART_CMD_VERSION: if (test_and_set_bit(EV3_UART_INFO_BIT_CMD_VERSION, &data->info_flags)) { @@ -495,7 +446,7 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { } // TODO: this might be useful someday - debug_pr("version: %08lx\n", *(uint32_t *)(data->msg + 1)); + debug_pr("version: %08lx\n", uint32_le(data->rx_msg + 1)); break; default: @@ -507,7 +458,7 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { switch (cmd2) { case EV3_UART_INFO_NAME: data->info_flags &= ~EV3_UART_INFO_FLAG_ALL_INFO; - if (data->msg[2] < 'A' || data->msg[2] > 'z') { + if (data->rx_msg[2] < 'A' || data->rx_msg[2] > 'z') { DBG_ERR(data->last_err = "Invalid name INFO"); goto err; } @@ -518,18 +469,18 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { * ensure a null terminator for the string * functions. */ - data->msg[msg_size - 1] = 0; - if (strlen((char *)data->msg + 2) > PBIO_IODEV_MODE_NAME_SIZE) { + data->rx_msg[msg_size - 1] = 0; + if (strlen((char *)data->rx_msg + 2) > PBIO_IODEV_MODE_NAME_SIZE) { DBG_ERR(data->last_err = "Name is too long"); goto err; } - snprintf(data->iodev->info->mode_info[mode].name, + snprintf(data->info.mode_info[mode].name, PBIO_IODEV_MODE_NAME_SIZE + 1, "%s", - data->msg + 2); + data->rx_msg + 2); data->new_mode = mode; data->info_flags |= EV3_UART_INFO_FLAG_INFO_NAME; - debug_pr("name: %s\n", data->iodev->info->mode_info[mode].name); + debug_pr("name: %s\n", data->info.mode_info[mode].name); break; case EV3_UART_INFO_RAW: @@ -541,11 +492,11 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { DBG_ERR(data->last_err = "Received duplicate raw scaling INFO"); goto err; } - data->iodev->info->mode_info[mode].raw_min = *(float *)(data->msg + 2); - data->iodev->info->mode_info[mode].raw_max = *(float *)(data->msg + 6); + data->info.mode_info[mode].raw_min = float_le(data->rx_msg + 2); + data->info.mode_info[mode].raw_max = float_le(data->rx_msg + 6); - debug_pr("raw: %f %f\n", (double)data->iodev->info->mode_info[mode].raw_min, - (double)data->iodev->info->mode_info[mode].raw_max); + debug_pr("raw: %f %f\n", (double)data->info.mode_info[mode].raw_min, + (double)data->info.mode_info[mode].raw_max); break; case EV3_UART_INFO_PCT: @@ -557,11 +508,11 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { DBG_ERR(data->last_err = "Received duplicate percent scaling INFO"); goto err; } - data->iodev->info->mode_info[mode].pct_min = *(float *)(data->msg + 2); - data->iodev->info->mode_info[mode].pct_max = *(float *)(data->msg + 6); + data->info.mode_info[mode].pct_min = uint32_le(data->rx_msg + 2); + data->info.mode_info[mode].pct_max = uint32_le(data->rx_msg + 6); - debug_pr("pct: %f %f\n", (double)data->iodev->info->mode_info[mode].pct_min, - (double)data->iodev->info->mode_info[mode].pct_max); + debug_pr("pct: %f %f\n", (double)data->info.mode_info[mode].pct_min, + (double)data->info.mode_info[mode].pct_max); break; case EV3_UART_INFO_SI: @@ -575,11 +526,11 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { DBG_ERR(data->last_err = "Received duplicate SI scaling INFO"); goto err; } - data->iodev->info->mode_info[mode].si_min = *(float *)(data->msg + 2); - data->iodev->info->mode_info[mode].si_max = *(float *)(data->msg + 6); + data->info.mode_info[mode].si_min = float_le(data->rx_msg + 2); + data->info.mode_info[mode].si_max = float_le(data->rx_msg + 6); - debug_pr("si: %f %f\n", (double)data->iodev->info->mode_info[mode].si_min, - (double)data->iodev->info->mode_info[mode].si_max); + debug_pr("si: %f %f\n", (double)data->info.mode_info[mode].si_min, + (double)data->info.mode_info[mode].si_max); break; case EV3_UART_INFO_UNITS: @@ -594,11 +545,11 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { // Units may not have null terminator and we are done with the // checksum at this point so we are writing 0 over the checksum to // ensure a null terminator for the string functions. - data->msg[msg_size - 1] = 0; - snprintf(data->iodev->info->mode_info[mode].uom, PBIO_IODEV_UOM_SIZE + 1, - "%s", data->msg + 2); + data->rx_msg[msg_size - 1] = 0; + snprintf(data->info.mode_info[mode].uom, PBIO_IODEV_UOM_SIZE + 1, + "%s", data->rx_msg + 2); - debug_pr("uom: %s\n", data->iodev->info->mode_info[mode].uom); + debug_pr("uom: %s\n", data->info.mode_info[mode].uom); break; case EV3_UART_INFO_MAPPING: @@ -610,10 +561,11 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { DBG_ERR(data->last_err = "Received duplicate mapping INFO"); goto err; } - // TODO: we should probably store this info if we want to support - // arbitrary/unknown sensors - debug_pr("mapping: %02x %02x\n", data->msg[2], data->msg[3]); + data->modes[mode].input_flags = data->rx_msg[2]; + data->modes[mode].output_flags = data->rx_msg[3]; + + debug_pr("mapping: %02x %02x\n", data->rx_msg[2], data->rx_msg[3]); break; case EV3_UART_INFO_UNK2: @@ -627,7 +579,7 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { } // TODO: what does this info tell us? - debug_pr("UNK2: %02x %02x\n", data->msg[2], data->msg[3]); + debug_pr("UNK2: %02x %02x\n", data->rx_msg[2], data->rx_msg[3]); break; case EV3_UART_INFO_MOTOR_BIAS: @@ -641,7 +593,7 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { } // TODO: do we need to store this info? - debug_pr("motor bias: %02x\n", data->msg[2]); + debug_pr("motor bias: %02x\n", data->rx_msg[2]); break; case EV3_UART_INFO_CAPABILITY: @@ -656,8 +608,8 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { // TODO: do we need to store this info? debug_pr("capability: %02x %02x %02x %02x %02x %02x\n", - data->msg[2], data->msg[3], data->msg[4], - data->msg[5], data->msg[6], data->msg[7]); + data->rx_msg[2], data->rx_msg[3], data->rx_msg[4], + data->rx_msg[5], data->rx_msg[6], data->rx_msg[7]); break; case EV3_UART_INFO_FORMAT: @@ -669,8 +621,8 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { DBG_ERR(data->last_err = "Received duplicate format INFO"); goto err; } - data->iodev->info->mode_info[mode].num_values = data->msg[2]; - if (!data->iodev->info->mode_info[mode].num_values) { + data->info.mode_info[mode].num_values = data->rx_msg[2]; + if (!data->info.mode_info[mode].num_values) { DBG_ERR(data->last_err = "Invalid number of data sets"); goto err; } @@ -682,33 +634,33 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { DBG_ERR(data->last_err = "Did not receive all required INFO"); goto err; } - switch (data->msg[3]) { + switch (data->rx_msg[3]) { case EV3_UART_DATA_8: - data->iodev->info->mode_info[mode].data_type = PBIO_IODEV_DATA_TYPE_INT8; + data->info.mode_info[mode].data_type = PBIO_IODEV_DATA_TYPE_INT8; break; case EV3_UART_DATA_16: - data->iodev->info->mode_info[mode].data_type = PBIO_IODEV_DATA_TYPE_INT16; + data->info.mode_info[mode].data_type = PBIO_IODEV_DATA_TYPE_INT16; break; case EV3_UART_DATA_32: - data->iodev->info->mode_info[mode].data_type = PBIO_IODEV_DATA_TYPE_INT32; + data->info.mode_info[mode].data_type = PBIO_IODEV_DATA_TYPE_INT32; break; case EV3_UART_DATA_FLOAT: - data->iodev->info->mode_info[mode].data_type = PBIO_IODEV_DATA_TYPE_FLOAT; + data->info.mode_info[mode].data_type = PBIO_IODEV_DATA_TYPE_FLOAT; break; default: DBG_ERR(data->last_err = "Invalid data type"); goto err; } - data->iodev->info->mode_info[mode].digits = data->msg[4]; - data->iodev->info->mode_info[mode].decimals = data->msg[5]; + data->info.mode_info[mode].digits = data->rx_msg[4]; + data->info.mode_info[mode].decimals = data->rx_msg[5]; if (data->new_mode) { data->new_mode--; } - debug_pr("num_values: %d\n", data->iodev->info->mode_info[mode].num_values); - debug_pr("data_type: %d\n", data->iodev->info->mode_info[mode].data_type); - debug_pr("digits: %d\n", data->iodev->info->mode_info[mode].digits); - debug_pr("decimals: %d\n", data->iodev->info->mode_info[mode].decimals); + debug_pr("num_values: %d\n", data->info.mode_info[mode].num_values); + debug_pr("data_type: %d\n", data->info.mode_info[mode].data_type); + debug_pr("digits: %d\n", data->info.mode_info[mode].digits); + debug_pr("decimals: %d\n", data->info.mode_info[mode].decimals); break; } @@ -719,24 +671,24 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { goto err; } - if (data->iodev->info->type_id == PBIO_IODEV_TYPE_ID_INTERACTIVE_MOTOR && data->write_cmd_size > 0) { - memcpy(data->iodev->bin_data, data->msg + 1, 6); + if (data->type_id == PBIO_IODEV_TYPE_ID_INTERACTIVE_MOTOR && data->write_cmd_size > 0) { + memcpy(data->iodev.bin_data, data->rx_msg + 1, 6); } else { - if (mode >= data->iodev->info->num_modes) { + if (mode >= data->info.num_modes) { DBG_ERR(data->last_err = "Invalid mode received"); goto err; } - if (mode != data->iodev->mode) { + if (mode != data->iodev.mode) { if (mode == data->new_mode) { - data->iodev->mode = mode; + data->iodev.mode = mode; // TODO: notify that mode has changed } else { DBG_ERR(data->last_err = "Unexpected mode"); goto err; } } - memcpy(data->iodev->bin_data, data->msg + 1, msg_size - 2); + memcpy(data->iodev.bin_data, data->rx_msg + 1, msg_size - 2); } data->data_rec = 1; @@ -749,27 +701,320 @@ static void pbio_uartdev_put(pbio_port_t port, uint8_t next_byte) { return; err: + // FIXME: Setting status to ERR here does not allow recovering from bad + // message when receiving data. Maybe return error instead? + data->status = PBIO_UARTDEV_STATUS_ERR; +} + +static PT_THREAD(pbio_uartdev_update(uartdev_port_data_t *data)) { + pbio_error_t err; + uint8_t checksum; + + PT_BEGIN(&data->pt); + + // reset state for new device + data->info.type_id = PBIO_IODEV_TYPE_ID_NONE; + data->ext_mode = 0; + data->status = PBIO_UARTDEV_STATUS_SYNCING; + PBIO_PT_WAIT_READY(&data->pt, pbdrv_uart_set_baud_rate(data->uart, EV3_UART_SPEED_MIN)); + + // To get in sync with the data stream from the sensor, we look for a valid TYPE command. + for (;;) { + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_read_begin(data->uart, data->rx_msg, 1, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx error during sync"); + goto err; + } + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_read_end(data->uart)); + if (err == PBIO_ERROR_TIMEDOUT) { + continue; + } + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx error during sync"); + goto err; + } + + if (data->rx_msg[0] == (EV3_UART_MSG_TYPE_CMD | EV3_UART_CMD_TYPE)) { + break; + } + } + + // then read the rest of the message + + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_read_begin(data->uart, data->rx_msg + 1, 2, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx error while reading type"); + goto err; + } + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_read_end(data->uart)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx error while reading type"); + goto err; + } + + if (data->rx_msg[0] < EV3_UART_TYPE_MIN || data->rx_msg[0] > EV3_UART_TYPE_MAX) { + DBG_ERR(data->last_err = "Bad device type id"); + goto err; + } + + checksum = 0xff ^ data->rx_msg[0] ^ data->rx_msg[1]; + if (data->rx_msg[2] != checksum) { + DBG_ERR(data->last_err = "Bad checksum for type id"); + goto err; + } + + // if all was good, we are ready to start receiving the mode info + + data->info.num_modes = 1; + data->info.num_view_modes = 1; + + for (int i = 0; i < PBIO_IODEV_MAX_NUM_MODES; i++) { + data->info.mode_info[i] = ev3_uart_default_mode_info; + } + + data->type_id = data->rx_msg[1]; + data->info_flags = EV3_UART_INFO_FLAG_CMD_TYPE; + data->data_rec = 0; + data->num_data_err = 0; + data->status = PBIO_UARTDEV_STATUS_INFO; + + while (data->status == PBIO_UARTDEV_STATUS_INFO) { + // read the message header + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_read_begin(data->uart, data->rx_msg, 1, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx begin error during info header"); + goto err; + } + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_read_end(data->uart)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx end error during info header"); + goto err; + } + + data->rx_msg_size = ev3_uart_get_msg_size(data->rx_msg[0]); + if (data->rx_msg_size > EV3_UART_MAX_MESSAGE_SIZE) { + DBG_ERR(data->last_err = "Bad message size during info"); + goto err; + } + + // read the rest of the message + if (data->rx_msg_size > 1) { + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_read_begin(data->uart, data->rx_msg + 1, data->rx_msg_size - 1, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx begin error during info"); + goto err; + } + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_read_end(data->uart)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx end error during info"); + goto err; + } + } + + // at this point, we have a full data->msg that can be parsed + pbio_uartdev_parse_msg(data); + } + + // at this point we should have read all of the mode info + if (data->status != PBIO_UARTDEV_STATUS_ACK) { + // data->last_err should be set by pbio_uartdev_parse_msg() + goto err; + } + + // reply with ACK + PT_WAIT_WHILE(&data->pt, data->tx_busy); + data->tx_busy = true; + data->tx_msg[0] = EV3_UART_SYS_ACK; + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_begin(data->uart, data->tx_msg, 1, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + data->tx_busy = false; + DBG_ERR(data->last_err = "UART Tx begin error during ack"); + goto err; + } + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart)); + if (err != PBIO_SUCCESS) { + data->tx_busy = false; + DBG_ERR(data->last_err = "UART Tx end error during ack"); + goto err; + } + data->tx_busy = false; + + // schedule baud rate change + etimer_set(&data->timer, clock_from_msec(10)); + PT_WAIT_UNTIL(&data->pt, etimer_expired(&data->timer)); + + // change the baud rate + PBIO_PT_WAIT_READY(&data->pt, pbdrv_uart_set_baud_rate(data->uart, data->new_baud_rate)); + + // setting type_id in info struct lets external modules know a device is connected + data->info.type_id = data->type_id; + data->status = PBIO_UARTDEV_STATUS_DATA; + // reset data rx thread + PT_INIT(&data->data_pt); + + if (data->type_id == PBIO_IODEV_TYPE_ID_INTERACTIVE_MOTOR) { + // send magic sequence to tell motor to send position and speed data + PT_WAIT_WHILE(&data->pt, data->tx_busy); + data->tx_busy = true; + + data->tx_msg[0] = 0x22; + data->tx_msg[1] = 0x00; + data->tx_msg[2] = 0x10; + data->tx_msg[3] = 0x20; + + PBIO_PT_WAIT_READY(&data->pt, + err = pbdrv_uart_write_begin(data->uart, data->tx_msg, 4, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + data->tx_busy = false; + DBG_ERR(data->last_err = "UART Tx begin error during motor"); + goto err; + } + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart)); + if (err != PBIO_SUCCESS) { + data->tx_busy = false; + DBG_ERR(data->last_err = "UART Tx end error during motor"); + goto err; + } + data->tx_busy = false; + } + + while (data->status == PBIO_UARTDEV_STATUS_DATA) { + // setup keepalive timer + etimer_reset_with_new_interval(&data->timer, clock_from_msec(EV3_UART_DATA_KEEP_ALIVE_TIMEOUT)); + PT_WAIT_UNTIL(&data->pt, etimer_expired(&data->timer)); + + // make sure we are receiving data + if (!data->data_rec) { + data->num_data_err++; + DBG_ERR(data->last_err = "No data since last keepalive"); + if (data->num_data_err > 6) { + data->status = PBIO_UARTDEV_STATUS_ERR; + } + } + data->data_rec = 0; + + // send keepalive + PT_WAIT_WHILE(&data->pt, data->tx_busy); + data->tx_busy = true; + data->tx_msg[0] = EV3_UART_SYS_NACK; + PBIO_PT_WAIT_READY(&data->pt, + err = pbdrv_uart_write_begin(data->uart, data->tx_msg, 1, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + data->tx_busy = false; + DBG_ERR(data->last_err = "UART Tx begin error during keepalive"); + goto err; + } + PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart)); + if (err != PBIO_SUCCESS) { + data->tx_busy = false; + DBG_ERR(data->last_err = "UART Tx end error during keepalive"); + goto err; + } + data->tx_busy = false; + } + +err: + // reset and start over data->status = PBIO_UARTDEV_STATUS_ERR; - data->new_baud_rate = EV3_UART_SPEED_MIN; etimer_stop(&data->timer); - pbdrv_uart_set_baud_rate(port, EV3_UART_SPEED_MIN); debug_pr("%s\n", data->last_err); + data->err_count++; + + process_post(PROCESS_BROADCAST, PROCESS_EVENT_SERVICE_REMOVED, NULL); + + PT_END(&data->pt); +} + +// REVISIT: This is not the greatest. We can easily get a buffer overrun and +// loose data. For now, the retry after bad message size helps get back into +// sync with the data stream. +static PT_THREAD(pbio_uartdev_receive_data(uartdev_port_data_t *data)) { + pbio_error_t err; + + PT_BEGIN(&data->data_pt); + +retry: + PBIO_PT_WAIT_READY(&data->data_pt, + err = pbdrv_uart_read_begin(data->uart, data->rx_msg, 1, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx data header begin error"); + goto err; + } + PBIO_PT_WAIT_READY(&data->data_pt, err = pbdrv_uart_read_end(data->uart)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx data header end error"); + goto err; + } + + data->rx_msg_size = ev3_uart_get_msg_size(data->rx_msg[0]); + if (data->rx_msg_size < 3 || data->rx_msg_size > EV3_UART_MAX_MESSAGE_SIZE) { + DBG_ERR(data->last_err = "Bad data message size"); + goto retry; + } + // TODO: also allow write cmd for motors + if ((data->rx_msg[0] & EV3_UART_MSG_TYPE_MASK) != EV3_UART_MSG_TYPE_DATA) { + DBG_ERR(data->last_err = "Bad msg type"); + goto retry; + } + + PBIO_PT_WAIT_READY(&data->data_pt, + err = pbdrv_uart_read_begin(data->uart, data->rx_msg + 1, data->rx_msg_size - 1, EV3_UART_IO_TIMEOUT)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx data begin error"); + goto err; + } + PBIO_PT_WAIT_READY(&data->data_pt, err = pbdrv_uart_read_end(data->uart)); + if (err != PBIO_SUCCESS) { + DBG_ERR(data->last_err = "UART Rx data end error"); + goto err; + } + + // at this point, we have a full data->msg that can be parsed + pbio_uartdev_parse_msg(data); + + PT_END(&data->data_pt); + +err: + PT_EXIT(&data->pt); } -#if PBDRV_CONFIG_NUM_IO_PORT static uint8_t ev3_uart_set_msg_hdr(enum ev3_uart_msg_type type, enum ev3_uart_msg_size size, enum ev3_uart_cmd cmd) { return (type & EV3_UART_MSG_TYPE_MASK) | (size & EV3_UART_MSG_SIZE_MASK) | (cmd & EV3_UART_MSG_CMD_MASK); } -static pbio_error_t _ev3_uart_write(pbio_port_t port, enum ev3_uart_msg_type msg_type, enum ev3_uart_cmd cmd, const uint8_t *data, uint8_t len) { - uint8_t msg[EV3_UART_MAX_MESSAGE_SIZE - 2]; +static pbio_error_t ev3_uart_begin_tx_msg(uartdev_port_data_t *port_data, enum ev3_uart_msg_type msg_type, + enum ev3_uart_cmd cmd, const uint8_t *data, uint8_t len) { uint8_t header, checksum, i; + uint8_t offset = 0; enum ev3_uart_msg_size size; + if (len == 0 || len > 32) { + return PBIO_ERROR_INVALID_ARG; + } + + if (port_data->status != PBIO_UARTDEV_STATUS_DATA) { + return PBIO_ERROR_NO_DEV; + } + + if (port_data->tx_busy) { + return PBIO_ERROR_AGAIN; + } + + port_data->tx_busy = true; + + if (msg_type == EV3_UART_MSG_TYPE_DATA) { + // Only Powered Up devices support setting data, and they expect to have an + // extra command sent to give the part of the mode > 7 + port_data->tx_msg[0] = ev3_uart_set_msg_hdr(EV3_UART_MSG_TYPE_CMD, EV3_UART_MSG_SIZE_1, EV3_UART_CMD_EXT_MODE); + port_data->tx_msg[1] = port_data->iodev.mode > EV3_UART_MODE_MAX ? 8 : 0; + port_data->tx_msg[2] = 0xff ^ port_data->tx_msg[0] ^ port_data->tx_msg[1]; + offset = 3; + } + checksum = 0xff; for (i = 0; i < len; i++) { - msg[i] = data[i]; + port_data->tx_msg[offset + i + 1] = data[i]; checksum ^= data[i]; } @@ -799,136 +1044,126 @@ static pbio_error_t _ev3_uart_write(pbio_port_t port, enum ev3_uart_msg_type msg // pad with zeros for (; i < len; i++) { - msg[i] = 0; + port_data->tx_msg[offset + i + 1] = 0; } header = ev3_uart_set_msg_hdr(msg_type, size, cmd); checksum ^= header; - while (pbdrv_uart_put_char(port, header) == PBIO_ERROR_AGAIN); - for (i = 0; i < len; i++) { - while (pbdrv_uart_put_char(port, msg[i]) == PBIO_ERROR_AGAIN); - } - while (pbdrv_uart_put_char(port, checksum) == PBIO_ERROR_AGAIN); + port_data->tx_msg[offset] = header; + port_data->tx_msg[offset + i + 1] = checksum; - return PBIO_SUCCESS; + return pbdrv_uart_write_begin(port_data->uart, port_data->tx_msg, offset + i + 2, EV3_UART_IO_TIMEOUT); } -static pbio_error_t ev3_uart_set_data(pbio_iodev_t *iodev, uint8_t *data) { - uartdev_port_data_t *port_data = &dev_data[port_to_index(iodev->port)]; - uint8_t header, ext_mode, checksum, size; +static pbio_error_t ev3_uart_set_mode_begin(pbio_iodev_t *iodev, uint8_t mode) { + uartdev_port_data_t *port_data = __containerof(iodev, uartdev_port_data_t, iodev); if (port_data->status != PBIO_UARTDEV_STATUS_DATA) { return PBIO_ERROR_AGAIN; } - // TODO: not all modes support setting data. Need to find a way to check - // this and return PBIO_ERROR_INVALID_OP. + port_data->new_mode = mode; - // Only Powered Up devices support setting data, and they expect to have an - // extra command sent to give the part of the mode > 7 - header = ev3_uart_set_msg_hdr(EV3_UART_MSG_TYPE_CMD, EV3_UART_MSG_SIZE_1, EV3_UART_CMD_EXT_MODE); - ext_mode = iodev->mode > EV3_UART_MODE_MAX ? 8 : 0; - checksum = 0xff ^ header ^ ext_mode; - while (pbdrv_uart_put_char(iodev->port, header) == PBIO_ERROR_AGAIN); - while (pbdrv_uart_put_char(iodev->port, ext_mode) == PBIO_ERROR_AGAIN); - while (pbdrv_uart_put_char(iodev->port, checksum) == PBIO_ERROR_AGAIN); + return ev3_uart_begin_tx_msg(port_data, EV3_UART_MSG_TYPE_CMD, EV3_UART_CMD_SELECT, &mode, 1); +} + +static pbio_error_t ev3_uart_set_mode_end(pbio_iodev_t *iodev) { + uartdev_port_data_t *port_data = __containerof(iodev, uartdev_port_data_t, iodev); + pbio_error_t err; - size = iodev->info->mode_info[iodev->mode].num_values * pbio_iodev_size_of(iodev->info->mode_info[iodev->mode].data_type); + err = pbdrv_uart_write_end(port_data->uart); + if (err != PBIO_ERROR_AGAIN) { + port_data->tx_busy = false; + // TODO: should wait until we receive at least one data message to + // ensure that the mode has actually changed (also ensures that we have + // a new data value in the case of single shot modes) + } - return _ev3_uart_write(iodev->port, EV3_UART_MSG_TYPE_DATA, iodev->mode, data, size); + return err; } -static pbio_error_t ev3_uart_write(pbio_iodev_t *iodev, const uint8_t *data, uint8_t len) { - uartdev_port_data_t *port_data = &dev_data[port_to_index(iodev->port)]; +static pbio_error_t ev3_uart_set_data_begin(pbio_iodev_t *iodev, const uint8_t *data) { + uartdev_port_data_t *port_data = __containerof(iodev, uartdev_port_data_t, iodev); + uint8_t size; - if (port_data->status != PBIO_UARTDEV_STATUS_DATA) { - return PBIO_ERROR_AGAIN; + // not all modes support setting data + if (!port_data->modes[iodev->mode].output_flags) { + return PBIO_ERROR_INVALID_OP; } - if (len == 0 || len >= EV3_UART_MAX_MESSAGE_SIZE - 2) { - return PBIO_ERROR_INVALID_ARG; - } + size = port_data->modes[iodev->mode].num_values * pbio_iodev_size_of(port_data->modes[iodev->mode].data_type); - return _ev3_uart_write(iodev->port, EV3_UART_MSG_TYPE_CMD, EV3_UART_CMD_WRITE, data, len); + return ev3_uart_begin_tx_msg(port_data, EV3_UART_MSG_TYPE_DATA, iodev->mode, data, size); } -static pbio_error_t ev3_uart_set_mode(pbio_iodev_t *iodev, uint8_t mode) { - uartdev_port_data_t *port_data = &dev_data[port_to_index(iodev->port)]; - uint8_t header, checksum; +static pbio_error_t ev3_uart_write_begin(pbio_iodev_t *iodev, const uint8_t *data, uint8_t size) { + uartdev_port_data_t *port_data = __containerof(iodev, uartdev_port_data_t, iodev); - if (port_data->status != PBIO_UARTDEV_STATUS_DATA) { - return PBIO_ERROR_AGAIN; + return ev3_uart_begin_tx_msg(port_data, EV3_UART_MSG_TYPE_CMD, EV3_UART_CMD_WRITE, data, size); +} + +static pbio_error_t ev3_uart_write_end(pbio_iodev_t *iodev) { + uartdev_port_data_t *port_data = __containerof(iodev, uartdev_port_data_t, iodev); + pbio_error_t err; + + err = pbdrv_uart_write_end(port_data->uart); + if (err != PBIO_ERROR_AGAIN) { + port_data->tx_busy = false; } - port_data->new_mode = mode; - header = ev3_uart_set_msg_hdr(EV3_UART_MSG_TYPE_CMD, EV3_UART_MSG_SIZE_1, EV3_UART_CMD_SELECT); - checksum = 0xff ^ header ^ mode; + return err; +} - while (pbdrv_uart_put_char(iodev->port, header) == PBIO_ERROR_AGAIN); - while (pbdrv_uart_put_char(iodev->port, mode) == PBIO_ERROR_AGAIN); - while (pbdrv_uart_put_char(iodev->port, checksum) == PBIO_ERROR_AGAIN); +static void ev3_uart_write_cancel(pbio_iodev_t *iodev) { + uartdev_port_data_t *port_data = __containerof(iodev, uartdev_port_data_t, iodev); - return PBIO_SUCCESS; + pbdrv_uart_write_cancel(port_data->uart); +} + +static const pbio_iodev_ops_t pbio_uartdev_ops = { + .set_mode_begin = ev3_uart_set_mode_begin, + .set_mode_end = ev3_uart_set_mode_end, + .set_mode_cancel = ev3_uart_write_cancel, + .set_data_begin = ev3_uart_set_data_begin, + .set_data_end = ev3_uart_write_end, + .set_data_cancel = ev3_uart_write_cancel, + .write_begin = ev3_uart_write_begin, + .write_end = ev3_uart_write_end, + .write_cancel = ev3_uart_write_cancel, +}; + +static PT_THREAD(pbio_uartdev_init(struct pt *pt, uint8_t id)) { + const pbio_uartdev_platform_data_t *pdata = &pbio_uartdev_platform_data[id]; + uartdev_port_data_t *port_data = &dev_data[id]; + + PT_BEGIN(pt); + + PT_WAIT_UNTIL(pt, pbdrv_uart_get(pdata->uart_id, &port_data->uart) == PBIO_SUCCESS); + port_data->iodev.info = &port_data->info; + port_data->iodev.ops = &pbio_uartdev_ops; + + PT_END(pt); } -#endif // PBDRV_CONFIG_NUM_IO_PORT PROCESS_THREAD(pbio_uartdev_process, ev, data) { + static struct pt pt; + static int i; PROCESS_BEGIN(); -#if PBDRV_CONFIG_NUM_IO_PORT - for (int i = 0; i < PBDRV_CONFIG_NUM_IO_PORT; i++) { - pbdrv_ioport_get_iodev(PBDRV_CONFIG_FIRST_IO_PORT + i, &dev_data[i].iodev); + for (i = 0; i < PBIO_CONFIG_UARTDEV_NUM_DEV; i++) { + PROCESS_PT_SPAWN(&pt, pbio_uartdev_init(&pt, i)); } -#endif while (true) { - PROCESS_WAIT_EVENT(); - if (ev == PBIO_EVENT_UART_RX) { - pbio_event_uart_rx_data_t *rx = data; - pbio_uartdev_put(rx->port, rx->byte); - } - else if (ev == PROCESS_EVENT_TIMER) { -#if PBDRV_CONFIG_NUM_IO_PORT - for (int i = 0; i < PBDRV_CONFIG_NUM_IO_PORT; i++) { - // keepalive timer - if (etimer_expired(&dev_data[i].timer)) { - if (dev_data[i].status == PBIO_UARTDEV_STATUS_ACK) { - // change the baud rate - etimer_reset_with_new_interval(&dev_data[i].timer, clock_from_msec(EV3_UART_DATA_KEEP_ALIVE_TIMEOUT)); - pbdrv_uart_set_baud_rate(dev_data[i].iodev->port, dev_data[i].new_baud_rate); - dev_data[i].status = PBIO_UARTDEV_STATUS_DATA; - dev_data[i].iodev->set_data = ev3_uart_set_data; - dev_data[i].iodev->write = ev3_uart_write; - dev_data[i].iodev->set_mode = ev3_uart_set_mode; - - if (dev_data[i].iodev->info->type_id == PBIO_IODEV_TYPE_ID_INTERACTIVE_MOTOR) { - const uint8_t msg[] = { 0x22, 0x00, 0x10, 0x20 }; - - // send magic sequence to tell motor to send position and speed data - while (ev3_uart_write(dev_data[i].iodev, msg, 4) == PBIO_ERROR_AGAIN); - } - } - else if (dev_data[i].num_data_err > 6) { - etimer_stop(&dev_data[i].timer); - dev_data[i].status = PBIO_UARTDEV_STATUS_ERR; - pbdrv_uart_set_baud_rate(dev_data[i].iodev->port, EV3_UART_SPEED_MIN); - } - else { - etimer_reset(&dev_data[i].timer); - if (!dev_data[i].data_rec) { - dev_data[i].num_data_err++; - DBG_ERR(dev_data[i].last_err = "No data since last keepalive"); - } - // send keepalive - while (pbdrv_uart_put_char(dev_data[i].iodev->port, EV3_UART_SYS_NACK) == PBIO_ERROR_AGAIN); - dev_data[i].data_rec = 0; - } - } + for (i = 0; i < PBIO_CONFIG_UARTDEV_NUM_DEV; i++) { + pbio_uartdev_update(&dev_data[i]); + if (dev_data[i].status == PBIO_UARTDEV_STATUS_DATA) { + pbio_uartdev_receive_data(&dev_data[i]); } -#endif // PBDRV_CONFIG_NUM_IO_PORT } + PROCESS_WAIT_EVENT(); } PROCESS_END();