From f2205506b12503572c8c41719564a334c88a66e7 Mon Sep 17 00:00:00 2001 From: David Lechner Date: Sat, 1 Jun 2019 15:40:26 -0500 Subject: [PATCH] drv/ioport: share ioport code This refactors the ioport driver code so that it can be shared by all LPF2 platforms. The device connection manager has also been refactored to use protothreads instead of a state machine. It also introduces a new GPIO driver that could also be used by other drivers. movehub fw size -40 --- bricks/stm32.mk | 4 +- lib/pbio/drv/debug/ioport.c | 422 --------------------- lib/pbio/drv/gpio/gpio_stm32f0.c | 36 ++ lib/pbio/drv/gpio/gpio_stm32f4.c | 36 ++ lib/pbio/drv/hub4/ioport.c | 444 ----------------------- lib/pbio/drv/ioport/ioport_lpf2.c | 368 +++++++++++++++++++ lib/pbio/drv/ioport/ioport_lpf2.h | 53 +++ lib/pbio/drv/move_hub/ioport.c | 444 ----------------------- lib/pbio/include/pbdrv/gpio.h | 39 ++ lib/pbio/include/pbdrv/ioport.h | 15 +- lib/pbio/platform/debug/platform.c | 31 ++ lib/pbio/platform/hub4/pbdrvconfig.h | 9 + lib/pbio/platform/hub4/platform.c | 26 ++ lib/pbio/platform/move_hub/pbdrvconfig.h | 9 + lib/pbio/platform/move_hub/platform.c | 26 ++ lib/pbio/src/ioport/ioport_lpf2.c | 388 ++++++++++++++++++++ lib/pbio/src/main.c | 9 +- lib/pbio/src/processes.h | 15 + 18 files changed, 1048 insertions(+), 1326 deletions(-) delete mode 100644 lib/pbio/drv/debug/ioport.c create mode 100644 lib/pbio/drv/gpio/gpio_stm32f0.c create mode 100644 lib/pbio/drv/gpio/gpio_stm32f4.c delete mode 100644 lib/pbio/drv/hub4/ioport.c create mode 100644 lib/pbio/drv/ioport/ioport_lpf2.c create mode 100644 lib/pbio/drv/ioport/ioport_lpf2.h delete mode 100644 lib/pbio/drv/move_hub/ioport.c create mode 100644 lib/pbio/include/pbdrv/gpio.h create mode 100644 lib/pbio/platform/debug/platform.c create mode 100644 lib/pbio/platform/hub4/platform.c create mode 100644 lib/pbio/platform/move_hub/platform.c create mode 100644 lib/pbio/src/ioport/ioport_lpf2.c create mode 100644 lib/pbio/src/processes.h diff --git a/bricks/stm32.mk b/bricks/stm32.mk index 01395778c..768d3c8eb 100644 --- a/bricks/stm32.mk +++ b/bricks/stm32.mk @@ -147,13 +147,15 @@ HAL_SRC_C = $(addprefix lib/stm32lib/STM32F$(CPU_FAMILY)xx_HAL_Driver/Src/,\ PBIO_SRC_C = $(addprefix ports/pybricks/lib/pbio/,\ drv/adc/adc_stm32f$(CPU_FAMILY).c \ drv/battery/battery_adc.c \ + drv/gpio/gpio_stm32f$(CPU_FAMILY).c \ + drv/ioport/ioport_lpf2.c \ drv/$(PBIO_PLATFORM)/bluetooth.c \ drv/$(PBIO_PLATFORM)/button.c \ drv/$(PBIO_PLATFORM)/light.c \ - drv/$(PBIO_PLATFORM)/ioport.c \ drv/$(PBIO_PLATFORM)/motor.c \ drv/$(PBIO_PLATFORM)/uart.c \ platform/$(PBIO_PLATFORM)/clock.c \ + platform/$(PBIO_PLATFORM)/platform.c \ platform/$(PBIO_PLATFORM)/sys.c \ src/motor.c \ src/error.c \ diff --git a/lib/pbio/drv/debug/ioport.c b/lib/pbio/drv/debug/ioport.c deleted file mode 100644 index db2e8348b..000000000 --- a/lib/pbio/drv/debug/ioport.c +++ /dev/null @@ -1,422 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2018-2019 David Lechner - -#include -#include - -#include "pbdrv/config.h" -#include "pbio/error.h" -#include "pbio/iodev.h" -#include "sys/etimer.h" -#include "sys/process.h" - -#include "stm32f413xx.h" - -typedef enum _ioport_t { - IOPORT_1, - NUM_IOPORT -} ioport_t; - -// Device connection manager state -typedef enum _dcm_state_t { - DCM_STATE_0, - DCM_STATE_1, - DCM_STATE_2, - DCM_STATE_3, - DCM_STATE_4, - DCM_STATE_5, - DCM_STATE_6, - DCM_STATE_7, - DCM_STATE_8, - DCM_STATE_9, - DCM_STATE_10, - DCM_STATE_11, -} dcm_state_t; - -typedef enum _dev_id1_group_t { - DEV_ID1_GROUP_GND, - DEV_ID1_GROUP_VCC, - DEV_ID1_GROUP_PULL_DOWN, - DEV_ID1_GROUP_OPEN, -} dev_id1_group_t; - -typedef struct _ioport_gpio_t { - GPIO_TypeDef *bank; - const uint8_t bit; -} ioport_gpio_t; - -// GPIOs associated with ID1 and ID2 pins -typedef struct _ioport_pins_t { - const ioport_gpio_t id1; // pin 5 in - const ioport_gpio_t id2; // pin 6 in/out - const ioport_gpio_t uart_buf; - const ioport_gpio_t uart_tx; // pin 5 out - const ioport_gpio_t uart_rx; // pin 6 in -} ioport_pins_t; - -// Device connection manager state for each port -typedef struct _dcm_data_t { - dcm_state_t dcm_state; - dev_id1_group_t dev_id1_group; - pbio_iodev_type_id_t type_id; - pbio_iodev_type_id_t prev_type_id; - uint8_t prev_gpio_value; - uint8_t dev_id_match_count; -} dcm_data_t; - -static dcm_data_t dcm_data[NUM_IOPORT]; - -static const ioport_pins_t ioport_pins[NUM_IOPORT] = { - [IOPORT_1] = { // UART2 - .id1 = { .bank = GPIOD, .bit = 3 }, - .id2 = { .bank = GPIOD, .bit = 4 }, - .uart_buf = { .bank = GPIOD, .bit = 7 }, - .uart_tx = { .bank = GPIOD, .bit = 5 }, - .uart_rx = { .bank = GPIOD, .bit = 6 }, - }, -}; - -static const pbio_iodev_type_id_t ioport_type_id_lookup[3][3] = { - [DEV_ID1_GROUP_GND] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_POWER, - [1] = PBIO_IODEV_TYPE_ID_LPF2_TURN, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT2, - }, - [DEV_ID1_GROUP_VCC] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_TRAIN, - [1] = PBIO_IODEV_TYPE_ID_LPF2_LMOTOR, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT1, - }, - [DEV_ID1_GROUP_PULL_DOWN] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_MMOTOR, - [1] = PBIO_IODEV_TYPE_ID_LPF2_XMOTOR, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT, - }, -}; - -static pbio_iodev_type_id_t connected_type_id[NUM_IOPORT]; -static pbio_iodev_type_id_t prev_type_id[NUM_IOPORT]; - -static struct { - pbio_iodev_info_t info; - pbio_iodev_mode_t modes[PBIO_IODEV_MAX_NUM_MODES]; -} ioport_info[NUM_IOPORT]; - -pbio_iodev_t iodevs[NUM_IOPORT]; - -PROCESS(pbdrv_ioport_process, "I/O port"); - -static void ioport_gpio_out_low(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (1 << (gpio->bit * 2)); - gpio->bank->BSRR = (1 << 16) << gpio->bit; -} - -static void ioport_gpio_out_high(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (1 << (gpio->bit * 2)); - gpio->bank->BSRR = 1 << gpio->bit; -} - -static uint8_t ioport_gpio_input(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (0 << (gpio->bit * 2)); - return (gpio->bank->IDR >> gpio->bit) & 1; -} - -static void ioport_gpio_alt(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (2 << (gpio->bit * 2)); -} - -static void ioport_enable_uart(ioport_t ioport) { - const ioport_pins_t *pins = &ioport_pins[ioport]; - - ioport_gpio_alt(&pins->uart_rx); - ioport_gpio_alt(&pins->uart_tx); - ioport_gpio_out_low(&pins->uart_buf); -} - -static void init_one(ioport_t ioport) { - const ioport_pins_t pins = ioport_pins[ioport]; - - iodevs[ioport].port = PBIO_PORT_1 + ioport; - iodevs[ioport].info = &ioport_info[ioport].info; - - // set up alternate function for UART pins - if (ioport == IOPORT_1) { - GPIOD->AFR[0] = (GPIOD->AFR[0] & ~GPIO_AFRL_AFSEL5_Msk) | (7 << GPIO_AFRL_AFSEL5_Pos); - GPIOD->AFR[0] = (GPIOD->AFR[0] & ~GPIO_AFRL_AFSEL6_Msk) | (7 << GPIO_AFRL_AFSEL6_Pos); - } - - ioport_gpio_input(&pins.id1); - ioport_gpio_input(&pins.id2); - ioport_gpio_input(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - ioport_gpio_input(&pins.uart_rx); -} - -pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) { - switch (port) { - case PBIO_PORT_1: - *iodev = &iodevs[IOPORT_1]; - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -// This is the device connection manager (dcm). It monitors the ID1 and ID2 pins -// on the port to see when devices are connected or disconnected. -// It is expected for there to be a 2ms delay between calls to this function. -static void poll_dcm(ioport_t ioport) { - // copying the data struct reduces the code size by a few hundred bytes, - // but we need to rember to copy it back if we change anything - dcm_data_t data = dcm_data[ioport]; - const ioport_pins_t pins = ioport_pins[ioport]; - uint8_t gpio_input; - - switch (data.dcm_state) { - case DCM_STATE_0: - data.type_id = PBIO_IODEV_TYPE_ID_NONE; - data.dev_id1_group = DEV_ID1_GROUP_OPEN; - - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - // set ID2 as input - ioport_gpio_input(&pins.id2); - - data.dcm_state = DCM_STATE_1; - break; - case DCM_STATE_1: - // save current ID2 value - data.prev_gpio_value = ioport_gpio_input(&pins.id2); - - // set ID1 low - ioport_gpio_out_low(&pins.uart_tx); - - data.dcm_state = DCM_STATE_2; - break; - case DCM_STATE_2: - // read ID2 - gpio_input = ioport_gpio_input(&pins.id2); - - // if ID2 changed from high to low - if (data.prev_gpio_value == 1 && gpio_input == 0) { - // we have touch sensor - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_TOUCH; - - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - data.dcm_state = DCM_STATE_3; - } - // if ID2 changed from low to high - else if (data.prev_gpio_value == 0 && gpio_input == 1) { - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_TPOINT; - - data.dcm_state = DCM_STATE_11; - } - else { - // read ID1 - data.prev_gpio_value = ioport_gpio_input(&pins.id1); - - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - - data.dcm_state = DCM_STATE_4; - } - break; - case DCM_STATE_3: - // ID1 is inverse of touch sensor value - // TODO: save this value to sensor data - //sensor_data = !ioport_gpio_input(&pins.id1); - - data.dcm_state = DCM_STATE_11; - break; - case DCM_STATE_4: - // read ID1 - gpio_input = ioport_gpio_input(&pins.id1); - - // if ID1 did not change and is high - if (data.prev_gpio_value == 1 && gpio_input == 1) { - // we have ID1 == VCC - data.dev_id1_group = DEV_ID1_GROUP_VCC; - - data.dcm_state = DCM_STATE_6; - } - // if ID1 did not change and is low - else if (data.prev_gpio_value == 0 && gpio_input == 0) { - // we have ID1 == GND - data.dev_id1_group = DEV_ID1_GROUP_GND; - - data.dcm_state = DCM_STATE_6; - } - else { - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - data.dcm_state = DCM_STATE_5; - } - break; - case DCM_STATE_5: - // read ID1 - if (ioport_gpio_input(&pins.id1) == 1) { - // we have ID1 == open - data.dev_id1_group = DEV_ID1_GROUP_OPEN; - } - else { - // we have ID1 == pull down - data.dev_id1_group = DEV_ID1_GROUP_PULL_DOWN; - } - - data.dcm_state = DCM_STATE_6; - break; - case DCM_STATE_6: - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - // set ID2 high - ioport_gpio_out_high(&pins.id2); - - data.dcm_state = DCM_STATE_7; - break; - case DCM_STATE_7: - // read ID1 - data.prev_gpio_value = ioport_gpio_input(&pins.id1); - - // set ID2 low - ioport_gpio_out_low(&pins.id2); - - data.dcm_state = DCM_STATE_8; - break; - case DCM_STATE_8: - // read ID1 - gpio_input = ioport_gpio_input(&pins.id1); - - // if ID1 changed from high to low - if (data.prev_gpio_value == 1 && gpio_input == 0) { - // if we have ID1 = open - if (data.dev_id1_group == DEV_ID1_GROUP_OPEN) { - // then we have this - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_3_PART; - } - - data.dcm_state = DCM_STATE_11; - } - // if ID1 changed from low to high - else if (data.prev_gpio_value == 0 && gpio_input == 1) { - // something might explode - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_EXPLOD; - - data.dcm_state = DCM_STATE_11; - } - else { - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - // set ID2 high - ioport_gpio_out_high(&pins.id2); - - data.dcm_state = DCM_STATE_9; - } - break; - case DCM_STATE_9: - // if ID2 is high - if (ioport_gpio_input(&pins.uart_rx) == 1) { - // set ID2 low - ioport_gpio_out_low(&pins.id2); - - data.dcm_state = DCM_STATE_10; - } - else { - // we know the device now - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][0]; - } - else { - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART; - } - - data.dcm_state = DCM_STATE_11; - } - break; - case DCM_STATE_10: - // if ID2 is low - if (ioport_gpio_input(&pins.uart_rx) == 0) { - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][2]; - } - } - else { - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][1]; - } - } - - data.dcm_state = DCM_STATE_11; - break; - case DCM_STATE_11: - // set ID2 as input - ioport_gpio_input(&pins.id2); - - // set ID1 low - ioport_gpio_out_low(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - if (data.type_id == data.prev_type_id) { - if (++data.dev_id_match_count >= 20) { - - if (data.type_id != connected_type_id[ioport]) { - connected_type_id[ioport] = data.type_id; - } - - // don't want to wrap around and re-trigger - data.dev_id_match_count--; - } - } - - data.prev_type_id = data.type_id; - - data.dcm_state = DCM_STATE_0; - break; - } - - // copy local variable back to global - dcm_data[ioport] = data; -} - -PROCESS_THREAD(pbdrv_ioport_process, ev, data) { - static struct etimer timer; - - PROCESS_BEGIN(); - - etimer_set(&timer, clock_from_msec(2)); - - init_one(IOPORT_1); - ioport_enable_uart(IOPORT_1); - - while (false) { - PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&timer)); - etimer_reset(&timer); - - for (int i = 0; i < NUM_IOPORT; i++) { - if (connected_type_id[i] != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - poll_dcm(i); - } - - if (connected_type_id[i] != prev_type_id[i]) { - prev_type_id[i] = connected_type_id[i]; - if (connected_type_id[i] == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - ioport_enable_uart(i); - } - } - } - } - - PROCESS_END(); -} diff --git a/lib/pbio/drv/gpio/gpio_stm32f0.c b/lib/pbio/drv/gpio/gpio_stm32f0.c new file mode 100644 index 000000000..35fb125c5 --- /dev/null +++ b/lib/pbio/drv/gpio/gpio_stm32f0.c @@ -0,0 +1,36 @@ + +#include "pbdrv/config.h" + +#if PBDRV_CONFIG_GPIO_STM32F0 + +#include + +#include "pbdrv/gpio.h" + +#include "stm32f0xx.h" + +void pbdrv_gpio_out_low(const pbdrv_gpio_t *gpio) { + GPIO_TypeDef *bank = gpio->bank; + bank->BRR = 1 << gpio->pin; + bank->MODER = (bank->MODER & ~(3 << (gpio->pin * 2))) | (1 << (gpio->pin * 2)); +} + +void pbdrv_gpio_out_high(const pbdrv_gpio_t *gpio) { + GPIO_TypeDef *bank = gpio->bank; + bank->BSRR = 1 << gpio->pin; + bank->MODER = (bank->MODER & ~(3 << (gpio->pin * 2))) | (1 << (gpio->pin * 2)); +} + +uint8_t pbdrv_gpio_input(const pbdrv_gpio_t *gpio) { + GPIO_TypeDef *bank = gpio->bank; + bank->MODER = (bank->MODER & ~(3 << (gpio->pin * 2))) | (0 << (gpio->pin * 2)); + return (bank->IDR >> gpio->pin) & 1; +} + +void pbdrv_gpio_alt(const pbdrv_gpio_t *gpio, uint8_t alt) { + GPIO_TypeDef *bank = gpio->bank; + bank->AFR[gpio->pin / 8] = (bank->AFR[gpio->pin / 8] & ~(0xf << (gpio->pin % 8 * 4))) | (alt << (gpio->pin % 8 * 4)); + bank->MODER = (bank->MODER & ~(3 << (gpio->pin * 2))) | (2 << (gpio->pin * 2)); +} + +#endif // PBDRV_CONFIG_GPIO_STM32F0 diff --git a/lib/pbio/drv/gpio/gpio_stm32f4.c b/lib/pbio/drv/gpio/gpio_stm32f4.c new file mode 100644 index 000000000..bc0b2a065 --- /dev/null +++ b/lib/pbio/drv/gpio/gpio_stm32f4.c @@ -0,0 +1,36 @@ + +#include "pbdrv/config.h" + +#if PBDRV_CONFIG_GPIO_STM32F4 + +#include + +#include "pbdrv/gpio.h" + +#include "stm32f4xx.h" + +void pbdrv_gpio_out_low(const pbdrv_gpio_t *gpio) { + GPIO_TypeDef *bank = gpio->bank; + bank->BSRR = (1 << 16) << gpio->pin; + bank->MODER = (bank->MODER & ~(3 << (gpio->pin * 2))) | (1 << (gpio->pin * 2)); +} + +void pbdrv_gpio_out_high(const pbdrv_gpio_t *gpio) { + GPIO_TypeDef *bank = gpio->bank; + bank->BSRR = 1 << gpio->pin; + bank->MODER = (bank->MODER & ~(3 << (gpio->pin * 2))) | (1 << (gpio->pin * 2)); +} + +uint8_t pbdrv_gpio_input(const pbdrv_gpio_t *gpio) { + GPIO_TypeDef *bank = gpio->bank; + bank->MODER = (bank->MODER & ~(3 << (gpio->pin * 2))) | (0 << (gpio->pin * 2)); + return (bank->IDR >> gpio->pin) & 1; +} + +void pbdrv_gpio_alt(const pbdrv_gpio_t *gpio, uint8_t alt) { + GPIO_TypeDef *bank = gpio->bank; + bank->AFR[gpio->pin / 8] = (bank->AFR[gpio->pin / 8] & ~(0xf << (gpio->pin % 8 * 4))) | (alt << (gpio->pin % 8 * 4)); + bank->MODER = (bank->MODER & ~(3 << (gpio->pin * 2))) | (2 << (gpio->pin * 2)); +} + +#endif // PBDRV_CONFIG_GPIO_STM32F4 diff --git a/lib/pbio/drv/hub4/ioport.c b/lib/pbio/drv/hub4/ioport.c deleted file mode 100644 index a2853cda4..000000000 --- a/lib/pbio/drv/hub4/ioport.c +++ /dev/null @@ -1,444 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2018 David Lechner - -#include -#include - -#include "pbdrv/config.h" -#include "pbio/error.h" -#include "pbio/iodev.h" -#include "sys/etimer.h" -#include "sys/process.h" - -#include "stm32f030xc.h" - -typedef enum _ioport_t { - IOPORT_A, - IOPORT_B, - NUM_IOPORT -} ioport_t; - -// Device connection manager state -typedef enum _dcm_state_t { - DCM_STATE_0, - DCM_STATE_1, - DCM_STATE_2, - DCM_STATE_3, - DCM_STATE_4, - DCM_STATE_5, - DCM_STATE_6, - DCM_STATE_7, - DCM_STATE_8, - DCM_STATE_9, - DCM_STATE_10, - DCM_STATE_11, -} dcm_state_t; - -typedef enum _dev_id1_group_t { - DEV_ID1_GROUP_GND, - DEV_ID1_GROUP_VCC, - DEV_ID1_GROUP_PULL_DOWN, - DEV_ID1_GROUP_OPEN, -} dev_id1_group_t; - -typedef struct _ioport_gpio_t { - GPIO_TypeDef *bank; - const uint8_t bit; -} ioport_gpio_t; - -// GPIOs associated with ID1 and ID2 pins -typedef struct _ioport_pins_t { - const ioport_gpio_t id1; // pin 5 in - const ioport_gpio_t id2; // pin 6 in/out - const ioport_gpio_t uart_buf; - const ioport_gpio_t uart_tx; // pin 5 out - const ioport_gpio_t uart_rx; // pin 6 in -} ioport_pins_t; - -// Device connection manager state for each port -typedef struct _dcm_data_t { - dcm_state_t dcm_state; - dev_id1_group_t dev_id1_group; - pbio_iodev_type_id_t type_id; - pbio_iodev_type_id_t prev_type_id; - uint8_t prev_gpio_value; - uint8_t dev_id_match_count; -} dcm_data_t; - -static dcm_data_t dcm_data[NUM_IOPORT]; - -static const ioport_pins_t ioport_pins[NUM_IOPORT] = { - [IOPORT_A] = { // USART3 - .id1 = { .bank = GPIOA, .bit = 1 }, - .id2 = { .bank = GPIOA, .bit = 3 }, - .uart_buf = { .bank = GPIOB, .bit = 5 }, - .uart_tx = { .bank = GPIOC, .bit = 4 }, - .uart_rx = { .bank = GPIOC, .bit = 5 }, - }, - [IOPORT_B] = { // USART4 - .id1 = { .bank = GPIOA, .bit = 0 }, - .id2 = { .bank = GPIOA, .bit = 2 }, - .uart_buf = { .bank = GPIOB, .bit = 4 }, - .uart_tx = { .bank = GPIOC, .bit = 10 }, - .uart_rx = { .bank = GPIOC, .bit = 11 }, - }, -}; - -static const pbio_iodev_type_id_t ioport_type_id_lookup[3][3] = { - [DEV_ID1_GROUP_GND] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_POWER, - [1] = PBIO_IODEV_TYPE_ID_LPF2_TURN, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT2, - }, - [DEV_ID1_GROUP_VCC] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_TRAIN, - [1] = PBIO_IODEV_TYPE_ID_LPF2_LMOTOR, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT1, - }, - [DEV_ID1_GROUP_PULL_DOWN] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_MMOTOR, - [1] = PBIO_IODEV_TYPE_ID_LPF2_XMOTOR, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT, - }, -}; - -static pbio_iodev_type_id_t connected_type_id[NUM_IOPORT]; -static pbio_iodev_type_id_t prev_type_id[NUM_IOPORT]; - -static struct { - pbio_iodev_info_t info; - pbio_iodev_mode_t modes[PBIO_IODEV_MAX_NUM_MODES]; -} ioport_info[NUM_IOPORT]; - -pbio_iodev_t iodevs[NUM_IOPORT]; - -PROCESS(pbdrv_ioport_process, "I/O port"); - -static void ioport_gpio_out_low(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (1 << (gpio->bit * 2)); - gpio->bank->BRR = 1 << gpio->bit; -} - -static void ioport_gpio_out_high(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (1 << (gpio->bit * 2)); - gpio->bank->BSRR = 1 << gpio->bit; -} - -static uint8_t ioport_gpio_input(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (0 << (gpio->bit * 2)); - return (gpio->bank->IDR >> gpio->bit) & 1; -} - -static void ioport_gpio_alt(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (2 << (gpio->bit * 2)); -} - -static void ioport_enable_uart(ioport_t ioport) { - const ioport_pins_t *pins = &ioport_pins[ioport]; - - ioport_gpio_alt(&pins->uart_rx); - ioport_gpio_alt(&pins->uart_tx); - ioport_gpio_out_low(&pins->uart_buf); -} - -static void init_one(ioport_t ioport) { - const ioport_pins_t pins = ioport_pins[ioport]; - - iodevs[ioport].port = PBIO_PORT_A + ioport; - iodevs[ioport].info = &ioport_info[ioport].info; - - // set up alternate function for UART pins - if (ioport == IOPORT_B) { - GPIOC->AFR[1] = (GPIOC->AFR[1] & ~(GPIO_AFRH_AFSEL10_Msk | GPIO_AFRH_AFSEL11_Msk)) | (0 << GPIO_AFRH_AFSEL10_Pos) | (0 << GPIO_AFRH_AFSEL11_Pos); - } - else if (ioport == IOPORT_A) { - GPIOC->AFR[0] = (GPIOC->AFR[0] & ~(GPIO_AFRL_AFSEL4_Msk | GPIO_AFRL_AFSEL5_Msk)) | (1 << GPIO_AFRL_AFSEL4_Pos) | (1 << GPIO_AFRL_AFSEL5_Pos); - } - - ioport_gpio_input(&pins.id1); - ioport_gpio_input(&pins.id2); - ioport_gpio_input(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - ioport_gpio_input(&pins.uart_rx); -} - -pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) { - switch (port) { - case PBIO_PORT_A: - *iodev = &iodevs[IOPORT_A]; - break; - case PBIO_PORT_B: - *iodev = &iodevs[IOPORT_B]; - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -// This is the device connection manager (dcm). It monitors the ID1 and ID2 pins -// on the port to see when devices are connected or disconnected. -// It is expected for there to be a 2ms delay between calls to this function. -static void poll_dcm(ioport_t ioport) { - // copying the data struct reduces the code size by a few hundred bytes, - // but we need to rember to copy it back if we change anything - dcm_data_t data = dcm_data[ioport]; - const ioport_pins_t pins = ioport_pins[ioport]; - uint8_t gpio_input; - - switch (data.dcm_state) { - case DCM_STATE_0: - data.type_id = PBIO_IODEV_TYPE_ID_NONE; - data.dev_id1_group = DEV_ID1_GROUP_OPEN; - - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - // set ID2 as input - ioport_gpio_input(&pins.id2); - - data.dcm_state = DCM_STATE_1; - break; - case DCM_STATE_1: - // save current ID2 value - data.prev_gpio_value = ioport_gpio_input(&pins.id2); - - // set ID1 low - ioport_gpio_out_low(&pins.uart_tx); - - data.dcm_state = DCM_STATE_2; - break; - case DCM_STATE_2: - // read ID2 - gpio_input = ioport_gpio_input(&pins.id2); - - // if ID2 changed from high to low - if (data.prev_gpio_value == 1 && gpio_input == 0) { - // we have touch sensor - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_TOUCH; - - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - data.dcm_state = DCM_STATE_3; - } - // if ID2 changed from low to high - else if (data.prev_gpio_value == 0 && gpio_input == 1) { - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_TPOINT; - - data.dcm_state = DCM_STATE_11; - } - else { - // read ID1 - data.prev_gpio_value = ioport_gpio_input(&pins.id1); - - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - - data.dcm_state = DCM_STATE_4; - } - break; - case DCM_STATE_3: - // ID1 is inverse of touch sensor value - // TODO: save this value to sensor data - //sensor_data = !ioport_gpio_input(&pins.id1); - - data.dcm_state = DCM_STATE_11; - break; - case DCM_STATE_4: - // read ID1 - gpio_input = ioport_gpio_input(&pins.id1); - - // if ID1 did not change and is high - if (data.prev_gpio_value == 1 && gpio_input == 1) { - // we have ID1 == VCC - data.dev_id1_group = DEV_ID1_GROUP_VCC; - - data.dcm_state = DCM_STATE_6; - } - // if ID1 did not change and is low - else if (data.prev_gpio_value == 0 && gpio_input == 0) { - // we have ID1 == GND - data.dev_id1_group = DEV_ID1_GROUP_GND; - - data.dcm_state = DCM_STATE_6; - } - else { - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - data.dcm_state = DCM_STATE_5; - } - break; - case DCM_STATE_5: - // read ID1 - if (ioport_gpio_input(&pins.id1) == 1) { - // we have ID1 == open - data.dev_id1_group = DEV_ID1_GROUP_OPEN; - } - else { - // we have ID1 == pull down - data.dev_id1_group = DEV_ID1_GROUP_PULL_DOWN; - } - - data.dcm_state = DCM_STATE_6; - break; - case DCM_STATE_6: - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - // set ID2 high - ioport_gpio_out_high(&pins.id2); - - data.dcm_state = DCM_STATE_7; - break; - case DCM_STATE_7: - // read ID1 - data.prev_gpio_value = ioport_gpio_input(&pins.id1); - - // set ID2 low - ioport_gpio_out_low(&pins.id2); - - data.dcm_state = DCM_STATE_8; - break; - case DCM_STATE_8: - // read ID1 - gpio_input = ioport_gpio_input(&pins.id1); - - // if ID1 changed from high to low - if (data.prev_gpio_value == 1 && gpio_input == 0) { - // if we have ID1 = open - if (data.dev_id1_group == DEV_ID1_GROUP_OPEN) { - // then we have this - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_3_PART; - } - - data.dcm_state = DCM_STATE_11; - } - // if ID1 changed from low to high - else if (data.prev_gpio_value == 0 && gpio_input == 1) { - // something might explode - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_EXPLOD; - - data.dcm_state = DCM_STATE_11; - } - else { - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - // set ID2 high - ioport_gpio_out_high(&pins.id2); - - data.dcm_state = DCM_STATE_9; - } - break; - case DCM_STATE_9: - // if ID2 is high - if (ioport_gpio_input(&pins.uart_rx) == 1) { - // set ID2 low - ioport_gpio_out_low(&pins.id2); - - data.dcm_state = DCM_STATE_10; - } - else { - // we know the device now - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][0]; - } - else { - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART; - } - - data.dcm_state = DCM_STATE_11; - } - break; - case DCM_STATE_10: - // if ID2 is low - if (ioport_gpio_input(&pins.uart_rx) == 0) { - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][2]; - } - } - else { - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][1]; - } - } - - data.dcm_state = DCM_STATE_11; - break; - case DCM_STATE_11: - // set ID2 as input - ioport_gpio_input(&pins.id2); - - // set ID1 low - ioport_gpio_out_low(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - if (data.type_id == data.prev_type_id) { - if (++data.dev_id_match_count >= 20) { - - if (data.type_id != connected_type_id[ioport]) { - connected_type_id[ioport] = data.type_id; - } - - // don't want to wrap around and re-trigger - data.dev_id_match_count--; - } - } - - data.prev_type_id = data.type_id; - - data.dcm_state = DCM_STATE_0; - break; - } - - // copy local variable back to global - dcm_data[ioport] = data; -} - -PROCESS_THREAD(pbdrv_ioport_process, ev, data) { - static struct etimer timer; - - PROCESS_BEGIN(); - - etimer_set(&timer, clock_from_msec(2)); - - init_one(IOPORT_A); - init_one(IOPORT_B); - - while (true) { - PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&timer)); - etimer_reset(&timer); - - if (connected_type_id[IOPORT_A] != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - poll_dcm(IOPORT_A); - } - - if (connected_type_id[IOPORT_A] != prev_type_id[IOPORT_A]) { - prev_type_id[IOPORT_A] = connected_type_id[IOPORT_A]; - if (connected_type_id[IOPORT_A] == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - ioport_enable_uart(IOPORT_A); - } - } - - if (connected_type_id[IOPORT_B] != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - poll_dcm(IOPORT_B); - } - - if (connected_type_id[IOPORT_B] != prev_type_id[IOPORT_B]) { - prev_type_id[IOPORT_B] = connected_type_id[IOPORT_B]; - if (connected_type_id[IOPORT_B] == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - ioport_enable_uart(IOPORT_B); - } - } - } - - PROCESS_END(); -} diff --git a/lib/pbio/drv/ioport/ioport_lpf2.c b/lib/pbio/drv/ioport/ioport_lpf2.c new file mode 100644 index 000000000..96690e9b5 --- /dev/null +++ b/lib/pbio/drv/ioport/ioport_lpf2.c @@ -0,0 +1,368 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2019 David Lechner + +// LEGO Power Functions 2 I/O port + +#include + +#if PBDRV_CONFIG_IOPORT_LPF2 + +#include +#include + +#include +#include +#include + +#include "ioport_lpf2.h" +#include "sys/etimer.h" +#include "sys/process.h" + +typedef enum _dev_id1_group_t { + DEV_ID1_GROUP_GND, + DEV_ID1_GROUP_VCC, + DEV_ID1_GROUP_PULL_DOWN, + DEV_ID1_GROUP_OPEN, +} dev_id1_group_t; + +// Device connection manager state for each port +typedef struct _dcm_data_t { + dev_id1_group_t dev_id1_group; + pbio_iodev_type_id_t type_id; + pbio_iodev_type_id_t prev_type_id; + uint8_t prev_gpio_value; + uint8_t dev_id_match_count; +} dcm_data_t; + +typedef struct { + const pbdrv_ioport_lpf2_platform_port_t *pins; + dcm_data_t dcm; + struct pt pt; + pbio_iodev_type_id_t connected_type_id; + pbio_iodev_type_id_t prev_type_id; +} ioport_dev_t; + +static const pbio_iodev_type_id_t ioport_type_id_lookup[3][3] = { + [DEV_ID1_GROUP_GND] = { + [0] = PBIO_IODEV_TYPE_ID_LPF2_POWER, + [1] = PBIO_IODEV_TYPE_ID_LPF2_TURN, + [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT2, + }, + [DEV_ID1_GROUP_VCC] = { + [0] = PBIO_IODEV_TYPE_ID_LPF2_TRAIN, + [1] = PBIO_IODEV_TYPE_ID_LPF2_LMOTOR, + [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT1, + }, + [DEV_ID1_GROUP_PULL_DOWN] = { + [0] = PBIO_IODEV_TYPE_ID_LPF2_MMOTOR, + [1] = PBIO_IODEV_TYPE_ID_LPF2_XMOTOR, + [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT, + }, +}; + +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] = { +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 0 + [0] = { + .pins = &pbdrv_ioport_lpf2_platform_port_0, + }, +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 1 + [1] = { + .pins = &pbdrv_ioport_lpf2_platform_port_1, + }, +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 2 + [2] = { + .pins = &pbdrv_ioport_lpf2_platform_port_2, + }, +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 3 + [3] = { + .pins = &pbdrv_ioport_lpf2_platform_port_3, + }, +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 4 + [4] = { + .pins = &pbdrv_ioport_lpf2_platform_port_4, + }, +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 5 + [5] = { + .pins = &pbdrv_ioport_lpf2_platform_port_5, + }, +#endif +}; + +static void ioport_enable_uart(ioport_dev_t *ioport) { + const pbdrv_ioport_lpf2_platform_port_t *pins = ioport->pins; + + pbdrv_gpio_alt(&pins->uart_rx, pins->alt); + pbdrv_gpio_alt(&pins->uart_tx, pins->alt); + pbdrv_gpio_out_low(&pins->uart_buf); +} + +static void init_one(uint8_t ioport) { + const pbdrv_ioport_lpf2_platform_port_t *pins = ioport_devs[ioport].pins; + + 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); + pbdrv_gpio_input(&pins->uart_tx); + pbdrv_gpio_input(&pins->uart_rx); +} + +// TODO: This should be moved to a common ioport_core.c file or removed entirely +pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) { + if (port < PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT || port > PBDRV_CONFIG_IOPORT_LPF2_LAST_PORT) { + return PBIO_ERROR_INVALID_PORT; + } + + *iodev = &iodevs[port - PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT]; + + return PBIO_SUCCESS; +} + +// This is the device connection manager (dcm). It monitors the ID1 and ID2 pins +// on the port to see when devices are connected or disconnected. +// It is expected for there to be a 2ms delay between calls to this function. +static PT_THREAD(poll_dcm(ioport_dev_t *ioport)) { + struct pt *pt = &ioport->pt; + dcm_data_t *data = &ioport->dcm; + const pbdrv_ioport_lpf2_platform_port_t pins = *ioport->pins; + uint8_t gpio_input; + + PT_BEGIN(pt); + + data->type_id = PBIO_IODEV_TYPE_ID_NONE; + data->dev_id1_group = DEV_ID1_GROUP_OPEN; + + // set ID1 high + pbdrv_gpio_out_high(&pins.uart_tx); + pbdrv_gpio_out_low(&pins.uart_buf); + + // set ID2 as input + pbdrv_gpio_input(&pins.id2); + + PT_YIELD(pt); + + // save current ID2 value + data->prev_gpio_value = pbdrv_gpio_input(&pins.id2); + + // set ID1 low + pbdrv_gpio_out_low(&pins.uart_tx); + + PT_YIELD(pt); + + // read ID2 + gpio_input = pbdrv_gpio_input(&pins.id2); + + // if ID2 changed from high to low + if (data->prev_gpio_value == 1 && gpio_input == 0) { + // we have touch sensor + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_TOUCH; + + // set ID1 as input + pbdrv_gpio_out_high(&pins.uart_buf); + pbdrv_gpio_input(&pins.uart_tx); + + PT_YIELD(pt); + + // ID1 is inverse of touch sensor value + // TODO: save this value to sensor data + //sensor_data = !pbdrv_gpio_input(&pins.id1); + } + // if ID2 changed from low to high + else if (data->prev_gpio_value == 0 && gpio_input == 1) { + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_TPOINT; + } + else { + // read ID1 + data->prev_gpio_value = pbdrv_gpio_input(&pins.id1); + + // set ID1 high + pbdrv_gpio_out_high(&pins.uart_tx); + + PT_YIELD(pt); + + // read ID1 + gpio_input = pbdrv_gpio_input(&pins.id1); + + // if ID1 did not change and is high + if (data->prev_gpio_value == 1 && gpio_input == 1) { + // we have ID1 == VCC + data->dev_id1_group = DEV_ID1_GROUP_VCC; + } + // if ID1 did not change and is low + else if (data->prev_gpio_value == 0 && gpio_input == 0) { + // we have ID1 == GND + data->dev_id1_group = DEV_ID1_GROUP_GND; + } + else { + // set ID1 as input + pbdrv_gpio_out_high(&pins.uart_buf); + pbdrv_gpio_input(&pins.uart_tx); + + PT_YIELD(pt); + + // read ID1 + if (pbdrv_gpio_input(&pins.id1) == 1) { + // we have ID1 == open + data->dev_id1_group = DEV_ID1_GROUP_OPEN; + } + else { + // we have ID1 == pull down + data->dev_id1_group = DEV_ID1_GROUP_PULL_DOWN; + } + } + + PT_YIELD(pt); + + // set ID1 as input + pbdrv_gpio_out_high(&pins.uart_buf); + pbdrv_gpio_input(&pins.uart_tx); + + // set ID2 high + pbdrv_gpio_out_high(&pins.id2); + + PT_YIELD(pt); + + // read ID1 + data->prev_gpio_value = pbdrv_gpio_input(&pins.id1); + + // set ID2 low + pbdrv_gpio_out_low(&pins.id2); + + PT_YIELD(pt); + + // read ID1 + gpio_input = pbdrv_gpio_input(&pins.id1); + + // if ID1 changed from high to low + if (data->prev_gpio_value == 1 && gpio_input == 0) { + // if we have ID1 = open + if (data->dev_id1_group == DEV_ID1_GROUP_OPEN) { + // then we have this + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_3_PART; + } + } + // if ID1 changed from low to high + else if (data->prev_gpio_value == 0 && gpio_input == 1) { + // something might explode + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_EXPLOD; + } + else { + // set ID1 high + pbdrv_gpio_out_high(&pins.uart_tx); + pbdrv_gpio_out_low(&pins.uart_buf); + + // set ID2 high + pbdrv_gpio_out_high(&pins.id2); + + PT_YIELD(pt); + + // if ID2 is high + if (pbdrv_gpio_input(&pins.uart_rx) == 1) { + // set ID2 low + pbdrv_gpio_out_low(&pins.id2); + + PT_YIELD(pt); + + // if ID2 is low + if (pbdrv_gpio_input(&pins.uart_rx) == 0) { + if (data->dev_id1_group < 3) { + data->type_id = ioport_type_id_lookup[data->dev_id1_group][2]; + } + } + else { + if (data->dev_id1_group < 3) { + data->type_id = ioport_type_id_lookup[data->dev_id1_group][1]; + } + } + } + else { + // we know the device now + if (data->dev_id1_group < 3) { + data->type_id = ioport_type_id_lookup[data->dev_id1_group][0]; + } + else { + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART; + } + } + } + } + + PT_YIELD(pt); + + // set ID2 as input + pbdrv_gpio_input(&pins.id2); + + // set ID1 low + pbdrv_gpio_out_low(&pins.uart_tx); + pbdrv_gpio_out_low(&pins.uart_buf); + + if (data->type_id == data->prev_type_id) { + if (++data->dev_id_match_count >= 20) { + + if (data->type_id != ioport->connected_type_id) { + ioport->connected_type_id = data->type_id; + } + + // don't want to wrap around and re-trigger + data->dev_id_match_count--; + } + } + + data->prev_type_id = data->type_id; + + PT_END(pt); +} + +PROCESS_THREAD(pbdrv_ioport_lpf2_process, ev, data) { + static struct etimer timer; + + PROCESS_BEGIN(); + + etimer_set(&timer, clock_from_msec(2)); + + for (int i = 0; i < PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS; i++) { + init_one(i); + } + + while (true) { + PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&timer)); + etimer_reset(&timer); + + for (int i = 0; i < PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS; i++) { + ioport_dev_t *ioport = &ioport_devs[i]; + + if (ioport->connected_type_id != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { + poll_dcm(ioport); + } + + if (ioport->connected_type_id != ioport->prev_type_id) { + ioport->prev_type_id = ioport->connected_type_id; + if (ioport->connected_type_id == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { + ioport_enable_uart(ioport); + } + } + } + } + + PROCESS_END(); +} + +#endif // PBDRV_CONFIG_IOPORT_LPF2 diff --git a/lib/pbio/drv/ioport/ioport_lpf2.h b/lib/pbio/drv/ioport/ioport_lpf2.h new file mode 100644 index 000000000..437d7448f --- /dev/null +++ b/lib/pbio/drv/ioport/ioport_lpf2.h @@ -0,0 +1,53 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2019 David Lechner + +#ifndef _IOPORT_LPF2_H_ +#define _IOPORT_LPF2_H_ + +#include + +#include +#include + +// GPIOs associated with ID1 and ID2 pins +typedef struct { + const pbdrv_gpio_t id1; // pin 5 in + const pbdrv_gpio_t id2; // pin 6 in/out + const pbdrv_gpio_t uart_buf; + const pbdrv_gpio_t uart_tx; // pin 5 out + const pbdrv_gpio_t uart_rx; // pin 6 in + const uint8_t alt; +} pbdrv_ioport_lpf2_platform_port_t; + +// platforom configuration checks +#ifndef PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS +#error Platform must define PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS +#endif +#ifndef PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT +#error Platform must define PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT +#endif +#ifndef PBDRV_CONFIG_IOPORT_LPF2_LAST_PORT +#error Platform must define PBDRV_CONFIG_IOPORT_LPF2_LAST_PORT +#endif + +// These are defined in platform/*/platform.c +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 0 +extern const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_0; +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 1 +extern const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_1; +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 2 +extern const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_2; +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 3 +extern const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_3; +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 4 +extern const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_4; +#endif +#if PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS > 5 +extern const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_5; +#endif + +#endif // _IOPORT_LPF2_H_ diff --git a/lib/pbio/drv/move_hub/ioport.c b/lib/pbio/drv/move_hub/ioport.c deleted file mode 100644 index c3a066507..000000000 --- a/lib/pbio/drv/move_hub/ioport.c +++ /dev/null @@ -1,444 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2018 David Lechner - -#include -#include - -#include "pbdrv/config.h" -#include "pbio/error.h" -#include "pbio/iodev.h" -#include "sys/etimer.h" -#include "sys/process.h" - -#include "stm32f070xb.h" - -typedef enum _ioport_t { - IOPORT_C, - IOPORT_D, - NUM_IOPORT -} ioport_t; - -// Device connection manager state -typedef enum _dcm_state_t { - DCM_STATE_0, - DCM_STATE_1, - DCM_STATE_2, - DCM_STATE_3, - DCM_STATE_4, - DCM_STATE_5, - DCM_STATE_6, - DCM_STATE_7, - DCM_STATE_8, - DCM_STATE_9, - DCM_STATE_10, - DCM_STATE_11, -} dcm_state_t; - -typedef enum _dev_id1_group_t { - DEV_ID1_GROUP_GND, - DEV_ID1_GROUP_VCC, - DEV_ID1_GROUP_PULL_DOWN, - DEV_ID1_GROUP_OPEN, -} dev_id1_group_t; - -typedef struct _ioport_gpio_t { - GPIO_TypeDef *bank; - const uint8_t bit; -} ioport_gpio_t; - -// GPIOs associated with ID1 and ID2 pins -typedef struct _ioport_pins_t { - const ioport_gpio_t id1; // pin 5 in - const ioport_gpio_t id2; // pin 6 in/out - const ioport_gpio_t uart_buf; - const ioport_gpio_t uart_tx; // pin 5 out - const ioport_gpio_t uart_rx; // pin 6 in -} ioport_pins_t; - -// Device connection manager state for each port -typedef struct _dcm_data_t { - dcm_state_t dcm_state; - dev_id1_group_t dev_id1_group; - pbio_iodev_type_id_t type_id; - pbio_iodev_type_id_t prev_type_id; - uint8_t prev_gpio_value; - uint8_t dev_id_match_count; -} dcm_data_t; - -static dcm_data_t dcm_data[NUM_IOPORT]; - -static const ioport_pins_t ioport_pins[NUM_IOPORT] = { - [IOPORT_C] = { // USART4 - .id1 = { .bank = GPIOB, .bit = 7 }, - .id2 = { .bank = GPIOC, .bit = 15 }, - .uart_buf = { .bank = GPIOB, .bit = 4 }, - .uart_tx = { .bank = GPIOC, .bit = 10 }, - .uart_rx = { .bank = GPIOC, .bit = 11 }, - }, - [IOPORT_D] = { //USART3 - .id1 = { .bank = GPIOB, .bit = 10 }, - .id2 = { .bank = GPIOA, .bit = 12 }, - .uart_buf = { .bank = GPIOB, .bit = 0 }, - .uart_tx = { .bank = GPIOC, .bit = 4 }, - .uart_rx = { .bank = GPIOC, .bit = 5 }, - }, -}; - -static const pbio_iodev_type_id_t ioport_type_id_lookup[3][3] = { - [DEV_ID1_GROUP_GND] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_POWER, - [1] = PBIO_IODEV_TYPE_ID_LPF2_TURN, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT2, - }, - [DEV_ID1_GROUP_VCC] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_TRAIN, - [1] = PBIO_IODEV_TYPE_ID_LPF2_LMOTOR, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT1, - }, - [DEV_ID1_GROUP_PULL_DOWN] = { - [0] = PBIO_IODEV_TYPE_ID_LPF2_MMOTOR, - [1] = PBIO_IODEV_TYPE_ID_LPF2_XMOTOR, - [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT, - }, -}; - -static pbio_iodev_type_id_t connected_type_id[NUM_IOPORT]; -static pbio_iodev_type_id_t prev_type_id[NUM_IOPORT]; - -static struct { - pbio_iodev_info_t info; - pbio_iodev_mode_t modes[PBIO_IODEV_MAX_NUM_MODES]; -} ioport_info[NUM_IOPORT]; - -pbio_iodev_t iodevs[NUM_IOPORT]; - -PROCESS(pbdrv_ioport_process, "I/O port"); - -static void ioport_gpio_out_low(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (1 << (gpio->bit * 2)); - gpio->bank->BRR = 1 << gpio->bit; -} - -static void ioport_gpio_out_high(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (1 << (gpio->bit * 2)); - gpio->bank->BSRR = 1 << gpio->bit; -} - -static uint8_t ioport_gpio_input(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (0 << (gpio->bit * 2)); - return (gpio->bank->IDR >> gpio->bit) & 1; -} - -static void ioport_gpio_alt(const ioport_gpio_t *gpio) { - gpio->bank->MODER = (gpio->bank->MODER & ~(3 << (gpio->bit * 2))) | (2 << (gpio->bit * 2)); -} - -static void ioport_enable_uart(ioport_t ioport) { - const ioport_pins_t *pins = &ioport_pins[ioport]; - - ioport_gpio_alt(&pins->uart_rx); - ioport_gpio_alt(&pins->uart_tx); - ioport_gpio_out_low(&pins->uart_buf); -} - -static void init_one(ioport_t ioport) { - const ioport_pins_t pins = ioport_pins[ioport]; - - iodevs[ioport].port = PBIO_PORT_C + ioport; - iodevs[ioport].info = &ioport_info[ioport].info; - - // set up alternate function for UART pins - if (ioport == IOPORT_C) { - GPIOC->AFR[1] = (GPIOC->AFR[1] & ~(GPIO_AFRH_AFSEL10_Msk | GPIO_AFRH_AFSEL11_Msk)) | (0 << GPIO_AFRH_AFSEL10_Pos) | (0 << GPIO_AFRH_AFSEL11_Pos); - } - else if (ioport == IOPORT_D) { - GPIOC->AFR[0] = (GPIOC->AFR[0] & ~(GPIO_AFRL_AFSEL4_Msk | GPIO_AFRL_AFSEL5_Msk)) | (1 << GPIO_AFRL_AFSEL4_Pos) | (1 << GPIO_AFRL_AFSEL5_Pos); - } - - ioport_gpio_input(&pins.id1); - ioport_gpio_input(&pins.id2); - ioport_gpio_input(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - ioport_gpio_input(&pins.uart_rx); -} - -pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) { - switch (port) { - case PBIO_PORT_C: - *iodev = &iodevs[IOPORT_C]; - break; - case PBIO_PORT_D: - *iodev = &iodevs[IOPORT_D]; - break; - default: - return PBIO_ERROR_INVALID_PORT; - } - - return PBIO_SUCCESS; -} - -// This is the device connection manager (dcm). It monitors the ID1 and ID2 pins -// on the port to see when devices are connected or disconnected. -// It is expected for there to be a 2ms delay between calls to this function. -static void poll_dcm(ioport_t ioport) { - // copying the data struct reduces the code size by a few hundred bytes, - // but we need to rember to copy it back if we change anything - dcm_data_t data = dcm_data[ioport]; - const ioport_pins_t pins = ioport_pins[ioport]; - uint8_t gpio_input; - - switch (data.dcm_state) { - case DCM_STATE_0: - data.type_id = PBIO_IODEV_TYPE_ID_NONE; - data.dev_id1_group = DEV_ID1_GROUP_OPEN; - - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - // set ID2 as input - ioport_gpio_input(&pins.id2); - - data.dcm_state = DCM_STATE_1; - break; - case DCM_STATE_1: - // save current ID2 value - data.prev_gpio_value = ioport_gpio_input(&pins.id2); - - // set ID1 low - ioport_gpio_out_low(&pins.uart_tx); - - data.dcm_state = DCM_STATE_2; - break; - case DCM_STATE_2: - // read ID2 - gpio_input = ioport_gpio_input(&pins.id2); - - // if ID2 changed from high to low - if (data.prev_gpio_value == 1 && gpio_input == 0) { - // we have touch sensor - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_TOUCH; - - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - data.dcm_state = DCM_STATE_3; - } - // if ID2 changed from low to high - else if (data.prev_gpio_value == 0 && gpio_input == 1) { - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_TPOINT; - - data.dcm_state = DCM_STATE_11; - } - else { - // read ID1 - data.prev_gpio_value = ioport_gpio_input(&pins.id1); - - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - - data.dcm_state = DCM_STATE_4; - } - break; - case DCM_STATE_3: - // ID1 is inverse of touch sensor value - // TODO: save this value to sensor data - //sensor_data = !ioport_gpio_input(&pins.id1); - - data.dcm_state = DCM_STATE_11; - break; - case DCM_STATE_4: - // read ID1 - gpio_input = ioport_gpio_input(&pins.id1); - - // if ID1 did not change and is high - if (data.prev_gpio_value == 1 && gpio_input == 1) { - // we have ID1 == VCC - data.dev_id1_group = DEV_ID1_GROUP_VCC; - - data.dcm_state = DCM_STATE_6; - } - // if ID1 did not change and is low - else if (data.prev_gpio_value == 0 && gpio_input == 0) { - // we have ID1 == GND - data.dev_id1_group = DEV_ID1_GROUP_GND; - - data.dcm_state = DCM_STATE_6; - } - else { - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - data.dcm_state = DCM_STATE_5; - } - break; - case DCM_STATE_5: - // read ID1 - if (ioport_gpio_input(&pins.id1) == 1) { - // we have ID1 == open - data.dev_id1_group = DEV_ID1_GROUP_OPEN; - } - else { - // we have ID1 == pull down - data.dev_id1_group = DEV_ID1_GROUP_PULL_DOWN; - } - - data.dcm_state = DCM_STATE_6; - break; - case DCM_STATE_6: - // set ID1 as input - ioport_gpio_out_high(&pins.uart_buf); - ioport_gpio_input(&pins.uart_tx); - - // set ID2 high - ioport_gpio_out_high(&pins.id2); - - data.dcm_state = DCM_STATE_7; - break; - case DCM_STATE_7: - // read ID1 - data.prev_gpio_value = ioport_gpio_input(&pins.id1); - - // set ID2 low - ioport_gpio_out_low(&pins.id2); - - data.dcm_state = DCM_STATE_8; - break; - case DCM_STATE_8: - // read ID1 - gpio_input = ioport_gpio_input(&pins.id1); - - // if ID1 changed from high to low - if (data.prev_gpio_value == 1 && gpio_input == 0) { - // if we have ID1 = open - if (data.dev_id1_group == DEV_ID1_GROUP_OPEN) { - // then we have this - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_3_PART; - } - - data.dcm_state = DCM_STATE_11; - } - // if ID1 changed from low to high - else if (data.prev_gpio_value == 0 && gpio_input == 1) { - // something might explode - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_EXPLOD; - - data.dcm_state = DCM_STATE_11; - } - else { - // set ID1 high - ioport_gpio_out_high(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - // set ID2 high - ioport_gpio_out_high(&pins.id2); - - data.dcm_state = DCM_STATE_9; - } - break; - case DCM_STATE_9: - // if ID2 is high - if (ioport_gpio_input(&pins.uart_rx) == 1) { - // set ID2 low - ioport_gpio_out_low(&pins.id2); - - data.dcm_state = DCM_STATE_10; - } - else { - // we know the device now - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][0]; - } - else { - data.type_id = PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART; - } - - data.dcm_state = DCM_STATE_11; - } - break; - case DCM_STATE_10: - // if ID2 is low - if (ioport_gpio_input(&pins.uart_rx) == 0) { - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][2]; - } - } - else { - if (data.dev_id1_group < 3) { - data.type_id = ioport_type_id_lookup[data.dev_id1_group][1]; - } - } - - data.dcm_state = DCM_STATE_11; - break; - case DCM_STATE_11: - // set ID2 as input - ioport_gpio_input(&pins.id2); - - // set ID1 low - ioport_gpio_out_low(&pins.uart_tx); - ioport_gpio_out_low(&pins.uart_buf); - - if (data.type_id == data.prev_type_id) { - if (++data.dev_id_match_count >= 20) { - - if (data.type_id != connected_type_id[ioport]) { - connected_type_id[ioport] = data.type_id; - } - - // don't want to wrap around and re-trigger - data.dev_id_match_count--; - } - } - - data.prev_type_id = data.type_id; - - data.dcm_state = DCM_STATE_0; - break; - } - - // copy local variable back to global - dcm_data[ioport] = data; -} - -PROCESS_THREAD(pbdrv_ioport_process, ev, data) { - static struct etimer timer; - - PROCESS_BEGIN(); - - etimer_set(&timer, clock_from_msec(2)); - - init_one(IOPORT_C); - init_one(IOPORT_D); - - while (true) { - PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&timer)); - etimer_reset(&timer); - - if (connected_type_id[IOPORT_C] != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - poll_dcm(IOPORT_C); - } - - if (connected_type_id[IOPORT_C] != prev_type_id[IOPORT_C]) { - prev_type_id[IOPORT_C] = connected_type_id[IOPORT_C]; - if (connected_type_id[IOPORT_C] == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - ioport_enable_uart(IOPORT_C); - } - } - - if (connected_type_id[IOPORT_D] != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - poll_dcm(IOPORT_D); - } - - if (connected_type_id[IOPORT_D] != prev_type_id[IOPORT_D]) { - prev_type_id[IOPORT_D] = connected_type_id[IOPORT_D]; - if (connected_type_id[IOPORT_D] == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { - ioport_enable_uart(IOPORT_D); - } - } - } - - PROCESS_END(); -} diff --git a/lib/pbio/include/pbdrv/gpio.h b/lib/pbio/include/pbdrv/gpio.h new file mode 100644 index 000000000..fb204db6c --- /dev/null +++ b/lib/pbio/include/pbdrv/gpio.h @@ -0,0 +1,39 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018 David Lechner + +/** + * \addtogroup GPIODriver GPIO driver + * @{ + */ + +#ifndef _PBDRV_GPIO_H_ +#define _PBDRV_GPIO_H_ + +#include + +#include + +typedef struct { + void *bank; + const uint8_t pin; +} pbdrv_gpio_t; + +#if PBDRV_CONFIG_GPIO + +void pbdrv_gpio_out_low(const pbdrv_gpio_t *gpio); +void pbdrv_gpio_out_high(const pbdrv_gpio_t *gpio); +uint8_t pbdrv_gpio_input(const pbdrv_gpio_t *gpio); +void pbdrv_gpio_alt(const pbdrv_gpio_t *gpio, uint8_t alt); + +#else // + +static inline void pbdrv_gpio_out_low(const pbdrv_gpio_t *gpio) { } +static inline void pbdrv_gpio_out_high(const pbdrv_gpio_t *gpio) { } +static inline uint8_t pbdrv_gpio_input(const pbdrv_gpio_t *gpio) { return 0; } +static inline void pbdrv_gpio_alt(const pbdrv_gpio_t *gpio, uint8_t alt) { } + +#endif // PBDRV_CONFIG_GPIO + +#endif // _PBDRV_GPIO_H_ + +/** @} */ diff --git a/lib/pbio/include/pbdrv/ioport.h b/lib/pbio/include/pbdrv/ioport.h index 3aac6f5b3..af20d6cd1 100644 --- a/lib/pbio/include/pbdrv/ioport.h +++ b/lib/pbio/include/pbdrv/ioport.h @@ -9,22 +9,15 @@ #ifndef _PBDRV_IOPORT_H_ #define _PBDRV_IOPORT_H_ -#include "pbdrv/config.h" -#include "pbio/error.h" -#include "pbio/iodev.h" -#include "pbio/port.h" -#include "sys/process.h" +#include +#include +#include +#include #if PBDRV_CONFIG_IOPORT pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev); -/** @cond INTERNAL */ - -PROCESS_NAME(pbdrv_ioport_process); - -/** @endcond */ - #else // PBDRV_CONFIG_IOPORT static inline pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) { diff --git a/lib/pbio/platform/debug/platform.c b/lib/pbio/platform/debug/platform.c new file mode 100644 index 000000000..0d1e56ba7 --- /dev/null +++ b/lib/pbio/platform/debug/platform.c @@ -0,0 +1,31 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2019 David Lechner + +#include +#include +#include +#include + +static struct { + pbio_iodev_info_t info; + pbio_iodev_mode_t modes[PBIO_IODEV_MAX_NUM_MODES]; +} uart_iodev_info; + +pbio_iodev_t uart_iodev = { + .info = &uart_iodev_info.info, + .port = PBIO_PORT_1, +}; + +// 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: + return PBIO_ERROR_INVALID_PORT; + } + + return PBIO_SUCCESS; +} diff --git a/lib/pbio/platform/hub4/pbdrvconfig.h b/lib/pbio/platform/hub4/pbdrvconfig.h index 1c3761ac6..68d1562d3 100644 --- a/lib/pbio/platform/hub4/pbdrvconfig.h +++ b/lib/pbio/platform/hub4/pbdrvconfig.h @@ -10,6 +10,15 @@ #define PBDRV_CONFIG_BATTERY_ADC_VOLTAGE_CH 11 #define PBDRV_CONFIG_BATTERY_ADC_CURRENT_CH 10 +#define PBDRV_CONFIG_GPIO (1) +#define PBDRV_CONFIG_GPIO_STM32F0 (1) + +#define PBDRV_CONFIG_IOPORT (1) +#define PBDRV_CONFIG_IOPORT_LPF2 (1) +#define PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS (2) +#define PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT PBIO_PORT_A +#define PBDRV_CONFIG_IOPORT_LPF2_LAST_PORT PBIO_PORT_B + #define PBDRV_CONFIG_HAS_PORT_A (1) #define PBDRV_CONFIG_HAS_PORT_B (1) diff --git a/lib/pbio/platform/hub4/platform.c b/lib/pbio/platform/hub4/platform.c new file mode 100644 index 000000000..3d0fb276c --- /dev/null +++ b/lib/pbio/platform/hub4/platform.c @@ -0,0 +1,26 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2019 David Lechner + +#include "../../drv/ioport/ioport_lpf2.h" + +#include "stm32f030xc.h" + +// Port A - USART3 +const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_0 = { + .id1 = { .bank = GPIOB, .pin = 1 }, + .id2 = { .bank = GPIOC, .pin = 3 }, + .uart_buf = { .bank = GPIOB, .pin = 5 }, + .uart_tx = { .bank = GPIOC, .pin = 4 }, + .uart_rx = { .bank = GPIOC, .pin = 5 }, + .alt = 1, +}; + +// Port B - USART4 +const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_1 = { + .id1 = { .bank = GPIOB, .pin = 0 }, + .id2 = { .bank = GPIOA, .pin = 2 }, + .uart_buf = { .bank = GPIOB, .pin = 4 }, + .uart_tx = { .bank = GPIOC, .pin = 10 }, + .uart_rx = { .bank = GPIOC, .pin = 11 }, + .alt = 0, +}; diff --git a/lib/pbio/platform/move_hub/pbdrvconfig.h b/lib/pbio/platform/move_hub/pbdrvconfig.h index f3012b8e5..e5d3cd16d 100644 --- a/lib/pbio/platform/move_hub/pbdrvconfig.h +++ b/lib/pbio/platform/move_hub/pbdrvconfig.h @@ -10,6 +10,15 @@ #define PBDRV_CONFIG_BATTERY_ADC_VOLTAGE_CH 11 #define PBDRV_CONFIG_BATTERY_ADC_CURRENT_CH 10 +#define PBDRV_CONFIG_GPIO (1) +#define PBDRV_CONFIG_GPIO_STM32F0 (1) + +#define PBDRV_CONFIG_IOPORT (1) +#define PBDRV_CONFIG_IOPORT_LPF2 (1) +#define PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS (2) +#define PBDRV_CONFIG_IOPORT_LPF2_FIRST_PORT PBIO_PORT_C +#define PBDRV_CONFIG_IOPORT_LPF2_LAST_PORT PBIO_PORT_D + #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 new file mode 100644 index 000000000..52cd45aae --- /dev/null +++ b/lib/pbio/platform/move_hub/platform.c @@ -0,0 +1,26 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2019 David Lechner + +#include "../../drv/ioport/ioport_lpf2.h" + +#include "stm32f070xb.h" + +// Port C - USART4 +const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_0 = { + .id1 = { .bank = GPIOB, .pin = 7 }, + .id2 = { .bank = GPIOC, .pin = 15 }, + .uart_buf = { .bank = GPIOB, .pin = 4 }, + .uart_tx = { .bank = GPIOC, .pin = 10 }, + .uart_rx = { .bank = GPIOC, .pin = 11 }, + .alt = 0, +}; + +// Port D - USART3 +const pbdrv_ioport_lpf2_platform_port_t pbdrv_ioport_lpf2_platform_port_1 = { + .id1 = { .bank = GPIOB, .pin = 10 }, + .id2 = { .bank = GPIOA, .pin = 12 }, + .uart_buf = { .bank = GPIOB, .pin = 0 }, + .uart_tx = { .bank = GPIOC, .pin = 4 }, + .uart_rx = { .bank = GPIOC, .pin = 5 }, + .alt = 1, +}; diff --git a/lib/pbio/src/ioport/ioport_lpf2.c b/lib/pbio/src/ioport/ioport_lpf2.c new file mode 100644 index 000000000..92be07317 --- /dev/null +++ b/lib/pbio/src/ioport/ioport_lpf2.c @@ -0,0 +1,388 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2019 David Lechner + +// LEGO Power Functions 2 I/O port + +#include +#include + +#include "pbdrv/config.h" +#include "pbdrv/gpio.h" +#include "pbio/error.h" +#include "pbio/iodev.h" +#include "sys/etimer.h" +#include "sys/process.h" + +typedef enum _dev_id1_group_t { + DEV_ID1_GROUP_GND, + DEV_ID1_GROUP_VCC, + DEV_ID1_GROUP_PULL_DOWN, + DEV_ID1_GROUP_OPEN, +} dev_id1_group_t; + +// GPIOs associated with ID1 and ID2 pins +typedef struct _pbdrv_ioport_pins_t { + const pbdrv_gpio_t id1; // pin 5 in + const pbdrv_gpio_t id2; // pin 6 in/out + const pbdrv_gpio_t uart_buf; + const pbdrv_gpio_t uart_tx; // pin 5 out + const pbdrv_gpio_t uart_rx; // pin 6 in + const uint8_t alt; +} pbdrv_ioport_pins_t; + +// Device connection manager state for each port +typedef struct _dcm_data_t { + dev_id1_group_t dev_id1_group; + pbio_iodev_type_id_t type_id; + pbio_iodev_type_id_t prev_type_id; + uint8_t prev_gpio_value; + uint8_t dev_id_match_count; +} dcm_data_t; + +typedef struct { + void (*init)(const pbdrv_ioport_pins_t *pins); + void (*gpio_out_low)(const pbdrv_gpio_t *gpio); + void (*gpio_out_high)(const pbdrv_gpio_t *gpio); + uint8_t (*gpio_input)(const pbdrv_gpio_t *gpio); + void (*mux_uart)(const pbdrv_gpio_t *gpio); +} pbio_ioport_lpf2_funcs_t; + +typedef struct { + const pbio_ioport_lpf2_funcs_t *funcs; + const pbdrv_ioport_pins_t *pins; + dcm_data_t dcm; + struct pt pt; + pbio_iodev_type_id_t connected_type_id; + pbio_iodev_type_id_t prev_type_id; +} ioport_dev_t; + +static const pbio_iodev_type_id_t ioport_type_id_lookup[3][3] = { + [DEV_ID1_GROUP_GND] = { + [0] = PBIO_IODEV_TYPE_ID_LPF2_POWER, + [1] = PBIO_IODEV_TYPE_ID_LPF2_TURN, + [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT2, + }, + [DEV_ID1_GROUP_VCC] = { + [0] = PBIO_IODEV_TYPE_ID_LPF2_TRAIN, + [1] = PBIO_IODEV_TYPE_ID_LPF2_LMOTOR, + [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT1, + }, + [DEV_ID1_GROUP_PULL_DOWN] = { + [0] = PBIO_IODEV_TYPE_ID_LPF2_MMOTOR, + [1] = PBIO_IODEV_TYPE_ID_LPF2_XMOTOR, + [2] = PBIO_IODEV_TYPE_ID_LPF2_LIGHT, + }, +}; + +static struct { + pbio_iodev_info_t info; + pbio_iodev_mode_t modes[PBIO_IODEV_MAX_NUM_MODES]; +} ioport_info[PBDRV_CONFIG_NUM_IO_PORT]; + +pbio_iodev_t iodevs[PBDRV_CONFIG_NUM_IO_PORT]; + +PROCESS(pbio_ioport_lpf2_process, "I/O port"); + +static void ioport_init(const pbdrv_ioport_pins_t *pins) { + pbdrv_gpio_alt(&pins->uart_tx, pins->alt); + pbdrv_gpio_alt(&pins->uart_rx, pins->alt); +} + +static const pbio_ioport_lpf2_funcs_t ioport_funcs = { + .init = ioport_init, + .gpio_out_low = ioport_gpio_out_low, + .gpio_out_high = ioport_gpio_out_high, + .gpio_input = ioport_gpio_input, + .mux_uart = ioport_gpio_uart, +}; + +static ioport_dev_t ioport_devs[PBDRV_CONFIG_NUM_IO_PORT] = { + [0] = { // Port C - USART4 + .funcs = &ioport_funcs, + .pins = &(const pbdrv_ioport_pins_t){ + .id1 = { .bank = GPIOB, .pin = 7 }, + .id2 = { .bank = GPIOC, .pin = 15 }, + .uart_buf = { .bank = GPIOB, .pin = 4 }, + .uart_tx = { .bank = GPIOC, .pin = 10 }, + .uart_rx = { .bank = GPIOC, .pin = 11 }, + .alt = 0, + }, + }, + [1] = { // Port D - USART3 + .funcs = &ioport_funcs, + .pins = &(const pbdrv_ioport_pins_t){ + .id1 = { .bank = GPIOB, .pin = 10 }, + .id2 = { .bank = GPIOA, .pin = 12 }, + .uart_buf = { .bank = GPIOB, .pin = 0 }, + .uart_tx = { .bank = GPIOC, .pin = 4 }, + .uart_rx = { .bank = GPIOC, .pin = 5 }, + .alt = 1, + }, + }, +}; + +static void ioport_enable_uart(ioport_dev_t *ioport) { + const pbdrv_ioport_pins_t *pins = ioport->pins; + const pbio_ioport_lpf2_funcs_t *funcs = ioport->funcs; + + funcs->mux_uart(&pins->uart_rx); + funcs->mux_uart(&pins->uart_tx); + funcs->gpio_out_low(&pins->uart_buf); +} + +static void init_one(uint8_t ioport) { + const pbdrv_ioport_pins_t *pins = ioport_devs[ioport].pins; + const pbio_ioport_lpf2_funcs_t funcs = *ioport_devs[ioport].funcs; + + PT_INIT(&ioport_devs[ioport].pt); + + iodevs[ioport].port = PBDRV_CONFIG_FIRST_IO_PORT + ioport; + iodevs[ioport].info = &ioport_info[ioport].info; + + funcs.init(pins); + funcs.gpio_input(&pins->id1); + funcs.gpio_input(&pins->id2); + funcs.gpio_input(&pins->uart_buf); + funcs.gpio_input(&pins->uart_tx); + funcs.gpio_input(&pins->uart_rx); +} + +pbio_error_t pbio_ioport_lpf2_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) { + if (port < PBDRV_CONFIG_FIRST_IO_PORT || port > PBDRV_CONFIG_LAST_IO_PORT) { + return PBIO_ERROR_INVALID_PORT; + } + + *iodev = &iodevs[port - PBDRV_CONFIG_FIRST_IO_PORT]; + + return PBIO_SUCCESS; +} + +// This is the device connection manager (dcm). It monitors the ID1 and ID2 pins +// on the port to see when devices are connected or disconnected. +// It is expected for there to be a 2ms delay between calls to this function. +static PT_THREAD(poll_dcm(ioport_dev_t *ioport)) { + struct pt *pt = &ioport->pt; + dcm_data_t *data = &ioport->dcm; + const pbdrv_ioport_pins_t pins = *ioport->pins; + const pbio_ioport_lpf2_funcs_t funcs = *ioport->funcs; + uint8_t gpio_input; + + PT_BEGIN(pt); + + data->type_id = PBIO_IODEV_TYPE_ID_NONE; + data->dev_id1_group = DEV_ID1_GROUP_OPEN; + + // set ID1 high + funcs.gpio_out_high(&pins.uart_tx); + funcs.gpio_out_low(&pins.uart_buf); + + // set ID2 as input + funcs.gpio_input(&pins.id2); + + PT_YIELD(pt); + + // save current ID2 value + data->prev_gpio_value = funcs.gpio_input(&pins.id2); + + // set ID1 low + funcs.gpio_out_low(&pins.uart_tx); + + PT_YIELD(pt); + + // read ID2 + gpio_input = funcs.gpio_input(&pins.id2); + + // if ID2 changed from high to low + if (data->prev_gpio_value == 1 && gpio_input == 0) { + // we have touch sensor + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_TOUCH; + + // set ID1 as input + funcs.gpio_out_high(&pins.uart_buf); + funcs.gpio_input(&pins.uart_tx); + + PT_YIELD(pt); + + // ID1 is inverse of touch sensor value + // TODO: save this value to sensor data + //sensor_data = !funcs.gpio_input(&pins.id1); + } + // if ID2 changed from low to high + else if (data->prev_gpio_value == 0 && gpio_input == 1) { + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_TPOINT; + } + else { + // read ID1 + data->prev_gpio_value = funcs.gpio_input(&pins.id1); + + // set ID1 high + funcs.gpio_out_high(&pins.uart_tx); + + PT_YIELD(pt); + + // read ID1 + gpio_input = funcs.gpio_input(&pins.id1); + + // if ID1 did not change and is high + if (data->prev_gpio_value == 1 && gpio_input == 1) { + // we have ID1 == VCC + data->dev_id1_group = DEV_ID1_GROUP_VCC; + } + // if ID1 did not change and is low + else if (data->prev_gpio_value == 0 && gpio_input == 0) { + // we have ID1 == GND + data->dev_id1_group = DEV_ID1_GROUP_GND; + } + else { + // set ID1 as input + funcs.gpio_out_high(&pins.uart_buf); + funcs.gpio_input(&pins.uart_tx); + + PT_YIELD(pt); + + // read ID1 + if (funcs.gpio_input(&pins.id1) == 1) { + // we have ID1 == open + data->dev_id1_group = DEV_ID1_GROUP_OPEN; + } + else { + // we have ID1 == pull down + data->dev_id1_group = DEV_ID1_GROUP_PULL_DOWN; + } + } + + PT_YIELD(pt); + + // set ID1 as input + funcs.gpio_out_high(&pins.uart_buf); + funcs.gpio_input(&pins.uart_tx); + + // set ID2 high + funcs.gpio_out_high(&pins.id2); + + PT_YIELD(pt); + + // read ID1 + data->prev_gpio_value = funcs.gpio_input(&pins.id1); + + // set ID2 low + funcs.gpio_out_low(&pins.id2); + + PT_YIELD(pt); + + // read ID1 + gpio_input = funcs.gpio_input(&pins.id1); + + // if ID1 changed from high to low + if (data->prev_gpio_value == 1 && gpio_input == 0) { + // if we have ID1 = open + if (data->dev_id1_group == DEV_ID1_GROUP_OPEN) { + // then we have this + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_3_PART; + } + } + // if ID1 changed from low to high + else if (data->prev_gpio_value == 0 && gpio_input == 1) { + // something might explode + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_EXPLOD; + } + else { + // set ID1 high + funcs.gpio_out_high(&pins.uart_tx); + funcs.gpio_out_low(&pins.uart_buf); + + // set ID2 high + funcs.gpio_out_high(&pins.id2); + + PT_YIELD(pt); + + // if ID2 is high + if (funcs.gpio_input(&pins.uart_rx) == 1) { + // set ID2 low + funcs.gpio_out_low(&pins.id2); + + PT_YIELD(pt); + + // if ID2 is low + if (funcs.gpio_input(&pins.uart_rx) == 0) { + if (data->dev_id1_group < 3) { + data->type_id = ioport_type_id_lookup[data->dev_id1_group][2]; + } + } + else { + if (data->dev_id1_group < 3) { + data->type_id = ioport_type_id_lookup[data->dev_id1_group][1]; + } + } + } + else { + // we know the device now + if (data->dev_id1_group < 3) { + data->type_id = ioport_type_id_lookup[data->dev_id1_group][0]; + } + else { + data->type_id = PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART; + } + } + } + } + + PT_YIELD(pt); + + // set ID2 as input + funcs.gpio_input(&pins.id2); + + // set ID1 low + funcs.gpio_out_low(&pins.uart_tx); + funcs.gpio_out_low(&pins.uart_buf); + + if (data->type_id == data->prev_type_id) { + if (++data->dev_id_match_count >= 20) { + + if (data->type_id != ioport->connected_type_id) { + ioport->connected_type_id = data->type_id; + } + + // don't want to wrap around and re-trigger + data->dev_id_match_count--; + } + } + + data->prev_type_id = data->type_id; + + PT_END(pt); +} + +PROCESS_THREAD(pbio_ioport_lpf2_process, ev, data) { + static struct etimer timer; + + PROCESS_BEGIN(); + + etimer_set(&timer, clock_from_msec(2)); + + for (int i = 0; i < PBDRV_CONFIG_NUM_IO_PORT; i++) { + init_one(i); + } + + while (true) { + PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&timer)); + etimer_reset(&timer); + + for (int i = 0; i < PBDRV_CONFIG_NUM_IO_PORT; i++) { + ioport_dev_t *ioport = &ioport_devs[i]; + + if (ioport->connected_type_id != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { + poll_dcm(ioport); + } + + if (ioport->connected_type_id != ioport->prev_type_id) { + ioport->prev_type_id = ioport->connected_type_id; + if (ioport->connected_type_id == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) { + ioport_enable_uart(ioport); + } + } + } + } + + PROCESS_END(); +} diff --git a/lib/pbio/src/main.c b/lib/pbio/src/main.c index ff4dc8357..02273e60b 100644 --- a/lib/pbio/src/main.c +++ b/lib/pbio/src/main.c @@ -12,8 +12,8 @@ #include "pbdrv/bluetooth.h" #include "pbdrv/button.h" #include "pbdrv/battery.h" +#include "pbdrv/config.h" #include "pbdrv/light.h" -#include "pbdrv/ioport.h" #include "pbdrv/motor.h" #include "pbdrv/uart.h" #include "pbsys/sys.h" @@ -25,6 +25,7 @@ #include "sys/clock.h" #include "sys/etimer.h" #include "sys/process.h" +#include "processes.h" static uint32_t prev_fast_poll_time; static uint32_t prev_slow_poll_time; @@ -40,9 +41,9 @@ AUTOSTART_PROCESSES( #if PBDRV_CONFIG_BLUETOOTH ,&pbdrv_bluetooth_hci_process ,&pbdrv_bluetooth_spi_process -#endif // PBDRV_CONFIG_BLUETOOTH -#if PBDRV_CONFIG_IOPORT - ,&pbdrv_ioport_process +#endif +#if PBDRV_CONFIG_IOPORT_LPF2 + ,&pbdrv_ioport_lpf2_process #endif #if PBDRV_CONFIG_UART ,&pbdrv_uart_process diff --git a/lib/pbio/src/processes.h b/lib/pbio/src/processes.h new file mode 100644 index 000000000..4df4b72ea --- /dev/null +++ b/lib/pbio/src/processes.h @@ -0,0 +1,15 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2019 David Lechner + +#ifndef _PBIO_PROCESSES_H_ +#define _PBIO_PROCESSES_H_ + +#include + +// All of the contiki processes + +#if PBDRV_CONFIG_IOPORT_LPF2 +PROCESS_NAME(pbdrv_ioport_lpf2_process); +#endif + +#endif // _PBIO_PROCESSES_H_