From 1decccb55f8768454dd15ee7db91ace624edb723 Mon Sep 17 00:00:00 2001 From: ben Date: Thu, 2 Jan 2025 15:06:38 +0100 Subject: [PATCH 1/3] BENB pico main rewrite and custom profiler --- .gitignore | 1 + .../grid_esp32_port/grid_esp32_port.c | 3 + grid_pico/.gitignore | 1 + grid_pico/main/CMakeLists.txt | 6 + grid_pico/main/grid_pico_platform.c | 4 +- grid_pico/main/grid_pico_platform.h | 4 +- grid_pico/main/main.c | 1033 ++++++----------- grid_pico/main/pico_pool.c | 105 ++ grid_pico/main/pico_pool.h | 88 ++ grid_pico/main/pico_swsr.c | 50 + grid_pico/main/pico_swsr.h | 31 + grid_pico/main/vmp/build.sh | 4 + grid_pico/main/vmp/proc.c | 34 + grid_pico/main/vmp/vmp_def.c | 242 ++++ grid_pico/main/vmp/vmp_def.h | 66 ++ grid_pico/main/vmp/vmp_tag.h | 19 + vmp/build.sh | 4 + vmp/recv.c | 243 ++++ vmp/vmp.c | 120 ++ vmp/vmp.h | 73 ++ 20 files changed, 1456 insertions(+), 675 deletions(-) create mode 100644 grid_pico/main/pico_pool.c create mode 100644 grid_pico/main/pico_pool.h create mode 100644 grid_pico/main/pico_swsr.c create mode 100644 grid_pico/main/pico_swsr.h create mode 100755 grid_pico/main/vmp/build.sh create mode 100644 grid_pico/main/vmp/proc.c create mode 100644 grid_pico/main/vmp/vmp_def.c create mode 100644 grid_pico/main/vmp/vmp_def.h create mode 100644 grid_pico/main/vmp/vmp_tag.h create mode 100755 vmp/build.sh create mode 100644 vmp/recv.c create mode 100644 vmp/vmp.c create mode 100644 vmp/vmp.h diff --git a/.gitignore b/.gitignore index 92bf4edb..9068d8ff 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,4 @@ lists.py out.json codeql-db iperf +vmp/build diff --git a/grid_esp/components/grid_esp32_port/grid_esp32_port.c b/grid_esp/components/grid_esp32_port/grid_esp32_port.c index 7de9c536..f2a502c1 100644 --- a/grid_esp/components/grid_esp32_port/grid_esp32_port.c +++ b/grid_esp/components/grid_esp32_port/grid_esp32_port.c @@ -209,6 +209,9 @@ static void IRAM_ATTR my_post_trans_cb(spi_slave_transaction_t* trans) { return; } + // reset timeout counter + por->partner_last_timestamp = grid_platform_rtc_get_micros(); + // struct grid_buffer* rx_buffer = uart_buffer_rx_array[por->index]; // grid_buffer_write_from_chunk(rx_buffer, message, length); diff --git a/grid_pico/.gitignore b/grid_pico/.gitignore index b9acf019..a3fa2551 100644 --- a/grid_pico/.gitignore +++ b/grid_pico/.gitignore @@ -4,3 +4,4 @@ CMakeCache.txt elf2uf2 generated pioasm +/main/vmp/build diff --git a/grid_pico/main/CMakeLists.txt b/grid_pico/main/CMakeLists.txt index 7ea19b6b..7fef8317 100644 --- a/grid_pico/main/CMakeLists.txt +++ b/grid_pico/main/CMakeLists.txt @@ -4,8 +4,14 @@ add_executable(main grid_pico_platform.c ../../grid_common/grid_msg.c ../../grid_common/grid_port.c + pico_pool.c + pico_swsr.c + vmp/vmp_def.c + ../../vmp/vmp.c ) +target_include_directories(main PRIVATE ../../vmp) + pico_generate_pio_header(main ${CMAKE_CURRENT_LIST_DIR}/uart_tx.pio) pico_generate_pio_header(main ${CMAKE_CURRENT_LIST_DIR}/uart_rx.pio) diff --git a/grid_pico/main/grid_pico_platform.c b/grid_pico/main/grid_pico_platform.c index b80271a4..c04a65bd 100644 --- a/grid_pico/main/grid_pico_platform.c +++ b/grid_pico/main/grid_pico_platform.c @@ -5,9 +5,9 @@ #include #include -uint64_t grid_platform_rtc_get_micros(void) { return time_us_64(); } +// uint64_t grid_platform_rtc_get_micros(void) { return time_us_64(); } -uint64_t grid_platform_rtc_get_elapsed_time(uint64_t told) { return time_us_64() - told; } +// uint64_t grid_platform_rtc_get_elapsed_time(uint64_t told) { return time_us_64() - told; } void* grid_platform_allocate_volatile(size_t size) { diff --git a/grid_pico/main/grid_pico_platform.h b/grid_pico/main/grid_pico_platform.h index 461280d8..ae212761 100644 --- a/grid_pico/main/grid_pico_platform.h +++ b/grid_pico/main/grid_pico_platform.h @@ -3,6 +3,6 @@ #include #include -uint64_t grid_platform_rtc_get_micros(void); -uint64_t grid_platform_rtc_get_elapsed_time(uint64_t told); +// uint64_t grid_platform_rtc_get_micros(void); +// uint64_t grid_platform_rtc_get_elapsed_time(uint64_t told); void* grid_platform_allocate_volatile(size_t size); diff --git a/grid_pico/main/main.c b/grid_pico/main/main.c index ae54ed0c..b33b0786 100644 --- a/grid_pico/main/main.c +++ b/grid_pico/main/main.c @@ -1,850 +1,568 @@ -/** - * - * On-Board LED Blinky - */ - +#include #include #include -#include "hardware/pio.h" #include "pico/multicore.h" #include "pico/stdlib.h" +#include "hardware/clocks.h" #include "hardware/irq.h" +#include "hardware/pio.h" #include "hardware/spi.h" +#include "grid_pico_spi.h" + #include "uart_rx.pio.h" #include "uart_tx.pio.h" -#include "hardware/uart.h" +const PIO GRID_TX_PIO = pio0; +const PIO GRID_RX_PIO = pio1; -#include "pico/multicore.h" +#include "grid_pico_pins.h" #include "../../grid_common/grid_msg.h" #include "../../grid_common/grid_port.h" #include "../../grid_common/grid_protocol.h" -#include - -#include "pico/time.h" - -#include "grid_pico_pins.h" -#include "grid_pico_platform.h" -#include "grid_pico_spi.h" - -#include "hardware/dma.h" +#include "vmp/vmp_def.h" +#include "vmp/vmp_tag.h" -#include "hardware/watchdog.h" +#include "pico_pool.h" +#include "pico_swsr.h" -volatile int context __attribute__((section(".uninitialized_data"))); // no initializer; it won't work! -volatile uint32_t line __attribute__((section(".uninitialized_data"))); // no initializer; it won't work! -volatile uint32_t userdata0 __attribute__((section(".uninitialized_data"))); // no initializer; it won't work! -volatile uint32_t userdata1 __attribute__((section(".uninitialized_data"))); // no initializer; it won't work! -volatile uint32_t userdata2 __attribute__((section(".uninitialized_data"))); // no initializer; it won't work! -volatile uint32_t userdata3 __attribute__((section(".uninitialized_data"))); // no initializer; it won't work! +struct pico_pool_t pool; -#define BUCKET_ARRAY_LENGTH 50 +uint32_t grid_pico_time() { return time_us_32(); } +uint32_t grid_pico_time_diff(uint32_t t1, uint32_t t2) { return t2 - t1; } -#define PRINT_FIFO_BUFFER_LENGTH 100 - -struct print_fifo { - char data[PRINT_FIFO_BUFFER_LENGTH]; - uint8_t read_index; - uint8_t write_index; +struct grid_pico_task_timer { + uint32_t last; + uint32_t period; }; -// #define PRINT_FIFO_DISABLED +bool grid_pico_task_timer_elapsed(struct grid_pico_task_timer* timer) { -int print_fifo_put(struct print_fifo* q, char c) { -#ifdef PRINT_FIFO_DISABLED - return 0; -#endif - if ((q->write_index + 1) % PRINT_FIFO_BUFFER_LENGTH == q->read_index) { - return 1; - } - - q->data[q->write_index] = c; - q->write_index = (q->write_index + 1) % PRINT_FIFO_BUFFER_LENGTH; - - return 0; -} + uint32_t now = grid_pico_time(); -int print_fifo_put_str(struct print_fifo* q, char* str) { - -#ifdef PRINT_FIFO_DISABLED - return 0; -#endif + bool ret = grid_pico_time_diff(timer->last, now) >= timer->period; - uint8_t len = strlen(str); - - for (int i = 0; i < len; i++) { - if (print_fifo_put(q, str[i])) { - return 1; - } + if (ret) { + timer->last = now; } - return 0; -} -int print_fifo_put_str_format(struct print_fifo* q, char* format, ...) { - -#ifdef PRINT_FIFO_DISABLED - return 0; -#endif - - char temp[PRINT_FIFO_BUFFER_LENGTH] = {0}; - va_list args; - va_start(args, format); - int ret = vsnprintf(temp, PRINT_FIFO_BUFFER_LENGTH, format, args); - va_end(args); - return print_fifo_put_str(q, temp); -} - -char print_fifo_get(struct print_fifo* q) { -#ifdef PRINT_FIFO_DISABLED - return 0; -#endif - - if (q->write_index == q->read_index) { - return '\0'; - } - - char ret = q->data[q->read_index]; - q->read_index = (q->read_index + 1) % PRINT_FIFO_BUFFER_LENGTH; return ret; } -struct print_fifo message_queue = {0}; - -static uint slice_num = 0; - -struct grid_msg_recent_buffer recent_messages; - -static uint8_t grid_pico_spi_txbuf[GRID_PARAMETER_SPI_TRANSACTION_length]; -static uint8_t grid_pico_spi_rxbuf[GRID_PARAMETER_SPI_TRANSACTION_length]; - -uint8_t grid_pico_uart_tx_ready_bitmap = 255; - -volatile uint8_t rolling_id_last_sent = 255; -volatile uint8_t rolling_id_last_received = 255; -volatile uint8_t rolling_id_error_count = 0; - -const PIO GRID_TX_PIO = pio0; -const PIO GRID_RX_PIO = pio1; - -enum grid_bucket_status_t { - - GRID_BUCKET_STATUS_EMPTY, - GRID_BUCKET_STATUS_RECEIVING, - GRID_BUCKET_STATUS_RECEIVE_COMPLETED, - GRID_BUCKET_STATUS_FULL_SEND_TO_SPI, - GRID_BUCKET_STATUS_FULL_SEND_TO_NORTH, - GRID_BUCKET_STATUS_FULL_SEND_TO_EAST, - GRID_BUCKET_STATUS_FULL_SEND_TO_SOUTH, - GRID_BUCKET_STATUS_FULL_SEND_TO_WEST, - GRID_BUCKET_STATUS_TRANSMITTING, - GRID_BUCKET_STATUS_DONE, - +enum { + ROLLING_MAX = GRID_PARAMETER_SPI_ROLLING_ID_maximum, }; -struct grid_bucket { - - enum grid_bucket_status_t status; - uint8_t index; - uint8_t source_port_index; - uint8_t buffer[GRID_PARAMETER_SPI_TRANSACTION_length]; - uint16_t buffer_index; +struct grid_pico_rolling { + uint8_t last_send; + uint8_t last_recv; + uint8_t errors; }; -// this is a doublebuffer -struct grid_pico_uart_port { - - uint8_t port_index; - uint8_t tx_buffer[GRID_PARAMETER_SPI_TRANSACTION_length]; - - struct grid_bucket* rx_bucket; - - struct grid_bucket* tx_active_bucket; - struct grid_bucket* tx_lastinserted_bucket; - struct grid_bucket* tx_previous_bucket; +volatile struct grid_pico_rolling rolling = (struct grid_pico_rolling){ + .last_send = UINT8_MAX, + .last_recv = UINT8_MAX, + .errors = 0, }; -uint8_t bucket_array_length = BUCKET_ARRAY_LENGTH; -struct grid_bucket bucket_array[BUCKET_ARRAY_LENGTH]; - -struct grid_pico_uart_port uart_port_array[4]; +void grid_pico_rolling_recv(volatile struct grid_pico_rolling* rolling, uint8_t recv) { -struct grid_pico_uart_port* uart_port_N = &uart_port_array[0]; -struct grid_pico_uart_port* uart_port_E = &uart_port_array[1]; -struct grid_pico_uart_port* uart_port_S = &uart_port_array[2]; -struct grid_pico_uart_port* uart_port_W = &uart_port_array[3]; + uint8_t expect = (rolling->last_recv + 1) % ROLLING_MAX; -void grid_pico_uart_uart_port_init(struct grid_pico_uart_port* uart_port, uint8_t index) { - - uart_port->port_index = index; - - for (uint16_t i = 0; i < GRID_PARAMETER_SPI_TRANSACTION_length; i++) { - uart_port->tx_buffer[i] = 0; + if (recv != expect) { + if (rolling->errors < UINT8_MAX) { + ++rolling->errors; + } } - uart_port->rx_bucket = NULL; - - uart_port->tx_active_bucket = NULL; - uart_port->tx_previous_bucket = NULL; - uart_port->tx_lastinserted_bucket = NULL; + rolling->last_recv = recv; } -void grid_bucket_clear(struct grid_bucket* bucket) { +void grid_pico_rolling_send(volatile struct grid_pico_rolling* rolling) { rolling->last_send = (rolling->last_send + 1) % ROLLING_MAX; } - bucket->status = GRID_BUCKET_STATUS_EMPTY; +struct grid_pico_uart_port { + uint8_t index; + uint8_t ready; + struct pico_bkt_t* uart_tx_bucket; + struct pico_bkt_t* uart_rx_bucket; + struct pico_swsr_t swsr; +}; - bucket->source_port_index = 255; +struct grid_pico_uart_port uart_ports[4]; - // for (uint16_t i=0; ibuffer[i] = 0; - // } +int grid_pico_uart_port_malloc(struct grid_pico_uart_port* port) { - bucket->buffer_index = 0; + if (!pico_swsr_malloc(&port->swsr, GRID_PARAMETER_SPI_TRANSACTION_length)) { + return 1; + } + + return 0; } -void grid_bucket_init(struct grid_bucket* bucket, uint8_t index) { +void grid_pico_uart_port_init(struct grid_pico_uart_port* port, uint8_t index) { - bucket->index = index; - grid_bucket_clear(bucket); + assert(index >= 0 && index < 4); + + port->index = index; + port->ready = 1; + port->uart_tx_bucket = NULL; + port->uart_rx_bucket = NULL; } -// find next bucket, so UART can safely receive data into it -struct grid_bucket* grid_bucket_find_next_match(struct grid_bucket* previous_bucket, enum grid_bucket_status_t expected_status) { +void grid_pico_uart_port_attach_rx(struct grid_pico_uart_port* port) { - // if no previous bucket was given then start from the end of the array (+1 - // will make this the beginning of the array later) - if (previous_bucket == NULL) { - previous_bucket = &bucket_array[bucket_array_length - 1]; + if (port->uart_rx_bucket) { + return; } - for (uint8_t i = 0; i < bucket_array_length; i++) { + struct pico_bkt_t* bkt = pico_pool_get_first(&pool, PICO_BKT_STATE_EMPTY); - uint8_t next_index = (previous_bucket->index + i + 1) % bucket_array_length; + if (!bkt) { + return; + } - if (bucket_array[next_index].status == expected_status) { + enum pico_bkt_state_t state = PICO_BKT_STATE_UART_RX_NORTH + port->index; + bkt = pico_pool_change(&pool, bkt, state); - // print_fifo_put_str_format(&message_queue, "Bucket 4 u : %d\r\n", next_index); - return &bucket_array[next_index]; - } + if (!bkt) { + return; } - // print_fifo_put_str(&message_queue, "No bucket 4 u :(\r\n"); - return NULL; -} + bkt->port = port->index; -struct grid_bucket* spi_tx_active_bucket = NULL; -struct grid_bucket* spi_rx_previous_bucket = NULL; + port->uart_rx_bucket = bkt; -void grid_bucket_put_character(struct grid_bucket* bucket, char next_char) { + pico_bkt_reset(port->uart_rx_bucket); +} - if (bucket == NULL) { +void grid_pico_uart_port_detach_rx(struct grid_pico_uart_port* port) { - print_fifo_put_str(&message_queue, "PUTC\n"); + if (!port->uart_rx_bucket) { return; } - if (bucket->buffer_index < GRID_PARAMETER_SPI_TRANSACTION_length) { + pico_bkt_reset(port->uart_rx_bucket); - bucket->buffer[bucket->buffer_index] = next_char; - bucket->buffer_index++; - } else { - print_fifo_put_str(&message_queue, "PUTC: no more space"); - } + enum pico_bkt_state_t state = PICO_BKT_STATE_UART_RX_FULL_NORTH + port->index; + pico_pool_change(&pool, port->uart_rx_bucket, state); + + port->uart_rx_bucket = NULL; } -char grid_bucket_get_character(struct grid_bucket* bucket) { +void grid_pico_uart_port_attach_tx(struct grid_pico_uart_port* port) { - if (bucket == NULL) { - return 0; + if (port->uart_tx_bucket) { + return; } - char c = bucket->buffer[bucket->buffer_index]; - bucket->buffer_index++; - return c; -} - -void grid_uart_port_attach_rx_bucket(struct grid_pico_uart_port* uart_port) { + enum pico_bkt_state_t state = PICO_BKT_STATE_UART_TX_NORTH + port->index; + struct pico_bkt_t* bkt = pico_pool_get_first(&pool, state); - struct grid_bucket* bucket = grid_bucket_find_next_match(uart_port->rx_bucket, GRID_BUCKET_STATUS_EMPTY); - - if (bucket == NULL) { + if (!bkt) { + port->ready = 1; return; } - uart_port->rx_bucket = bucket; + port->ready = 0; - uart_port->rx_bucket->status = GRID_BUCKET_STATUS_RECEIVING; - uart_port->rx_bucket->source_port_index = uart_port->port_index; -} + port->uart_tx_bucket = bkt; -void grid_uart_port_attach_tx_bucket(struct grid_pico_uart_port* uart_port) { + pico_bkt_reset(port->uart_tx_bucket); +} - struct grid_bucket* bucket = grid_bucket_find_next_match(uart_port->tx_previous_bucket, GRID_BUCKET_STATUS_FULL_SEND_TO_NORTH + uart_port->port_index); +void grid_pico_uart_port_detach_tx(struct grid_pico_uart_port* port) { - if (bucket == NULL) { - // no more message, ready to receive more - grid_pico_uart_tx_ready_bitmap |= (1 << uart_port->port_index); + if (!port->uart_tx_bucket) { return; } - bucket->buffer_index = 0; - bucket->status = GRID_BUCKET_STATUS_TRANSMITTING; - - uart_port->tx_active_bucket = bucket; -} + pico_bkt_reset(port->uart_tx_bucket); -void grid_uart_port_deattach_tx_bucket(struct grid_pico_uart_port* uart_port) { + enum pico_bkt_state_t state = PICO_BKT_STATE_EMPTY; + pico_pool_change(&pool, port->uart_tx_bucket, state); - grid_bucket_clear(uart_port->tx_active_bucket); - uart_port->tx_previous_bucket = uart_port->tx_active_bucket; - uart_port->tx_active_bucket = NULL; + port->uart_tx_bucket = NULL; } -void grid_pico_uart_transmit_task_inner(struct grid_pico_uart_port* uart_port) { +struct grid_pico_task_timer timer_uart_tx[4]; + +void grid_pico_task_uart_tx(struct grid_pico_uart_port* port, struct grid_pico_task_timer* timer) { - if (uart_port == NULL) { + if (!grid_pico_task_timer_elapsed(timer)) { return; } - if (uart_port->tx_active_bucket == NULL) { - grid_uart_port_attach_tx_bucket(uart_port); + if (!port->uart_tx_bucket) { + grid_pico_uart_port_attach_tx(port); return; } - // if transmission is in progress then send the next character - char c = grid_bucket_get_character(uart_port->tx_active_bucket); - uart_tx_program_putc(GRID_TX_PIO, uart_port->port_index, c); + char c = pico_bkt_next(port->uart_tx_bucket); + uart_tx_program_putc(GRID_TX_PIO, port->index, c); if (c == '\n') { - grid_uart_port_deattach_tx_bucket(uart_port); + grid_pico_uart_port_detach_tx(port); + // vmp_push(UART_TX); } } -int grid_bucket_create_uart_tx_clone(struct grid_pico_uart_port* uart_port, struct grid_bucket* source_bucket) { +void grid_uart_rx_clone_tx(struct grid_pico_uart_port* dest_port, struct pico_bkt_t* src_bkt) { - if (uart_port == NULL) { - return 1; - } + struct pico_bkt_t* bkt = pico_pool_get_first(&pool, PICO_BKT_STATE_EMPTY); - if (source_bucket == NULL) { - return 1; + if (!bkt) { + return; } - struct grid_bucket* bucket = grid_bucket_find_next_match(uart_port->tx_lastinserted_bucket, GRID_BUCKET_STATUS_EMPTY); - - if (bucket == NULL) { - return 1; - } + pico_bkt_reset(bkt); - strcpy(bucket->buffer, source_bucket->buffer); - bucket->status = GRID_BUCKET_STATUS_FULL_SEND_TO_NORTH + uart_port->port_index; + strcpy(bkt->buf, src_bkt->buf); - return 0; + enum pico_bkt_state_t state = PICO_BKT_STATE_UART_TX_NORTH + dest_port->index; + pico_pool_change(&pool, bkt, state); } -int grid_uart_rx_process_bucket(struct grid_bucket* rx_bucket) { - - if (rx_bucket == NULL) { - return 1; - } +struct grid_msg_recent_buffer recent_msgs; - context = 21, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = 0; +enum pico_bkt_state_t grid_uart_rx_process_bkt(struct grid_pico_uart_port* port, struct pico_bkt_t* bkt) { - char* message = (char*)rx_bucket->buffer; - uint16_t length = strlen(message); + char* msg = (char*)bkt->buf; + uint16_t len = strlen(msg); - if (length < 14) { - print_fifo_put_str_format(&message_queue, "ERROR: length = %d\n", length); - grid_bucket_clear(rx_bucket); - return 1; + if (len < 14) { + return PICO_BKT_STATE_EMPTY; } - if (rx_bucket->source_port_index > 3) { - // 0-3 are the only valid values - print_fifo_put_str_format(&message_queue, "ERROR: source_port_index = %d\n", rx_bucket->source_port_index); - grid_bucket_clear(rx_bucket); - return 1; + if (bkt->port > 3) { + return PICO_BKT_STATE_EMPTY; } - int status = grid_str_verify_frame(message); - - context = 22, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = 0; + int status = grid_str_verify_frame(msg); if (status != 0) { - - grid_bucket_clear(rx_bucket); - - return 1; + return PICO_BKT_STATE_EMPTY; } - context = 23, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = 0; - - struct grid_port* por = grid_transport_get_port(&grid_transport_state, rx_bucket->source_port_index); - - if (por == NULL) { + struct grid_port* por = grid_transport_get_port(&grid_transport_state, port->index); - print_fifo_put_str(&message_queue, "PORT NOT FOUND\n"); - grid_bucket_clear(rx_bucket); - return; + if (!por) { + return PICO_BKT_STATE_EMPTY; } - context = 24, line = rx_bucket->source_port_index, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; - - if (message[1] != GRID_CONST_BRC) { - - context = 241, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; - if (message[2] == GRID_CONST_BELL) { + if (msg[1] != GRID_CONST_BRC) { - context = 25, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; + if (msg[2] == GRID_CONST_BELL) { - // reset timeout counter - // por->partner_last_timestamp = grid_platform_rtc_get_micros(); - - context = 251, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; - - // if (por->partner_status == 0) { - - // context = 252, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; - // print_fifo_put_str(&message_queue, "C\n"); - - // context = 253, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; - // } - // CONNECT - context = 254, userdata0 = por, userdata1 = message[3], userdata2 = por->direction, userdata3 = length; - por->partner_fi = (message[3] - por->direction + 6) % 4; // 0, 1, 2, 3 base on relative rotation of the modules - - context = 255, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; + por->partner_fi = (msg[3] - por->direction + 6) % 4; por->partner_status = 1; - rx_bucket->status = GRID_BUCKET_STATUS_FULL_SEND_TO_SPI; - - context = 256, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; - return 1; - - } else { - - context = 242, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = length; - grid_bucket_clear(rx_bucket); - return; + return PICO_BKT_STATE_SPI_TX; } - /// this cannot be reached - return; + return PICO_BKT_STATE_EMPTY; } - context = 240, userdata0 = 6, userdata1 = 6, userdata2 = 6, userdata3 = 6; - context = 243, userdata0 = por, userdata1 = (por->dx) + 10, userdata2 = (por->dy) + 10, userdata3 = length; - grid_str_transform_brc_params(message, por->dx, por->dy, por->partner_fi); // update age, sx, sy, dx, dy, rot etc... + grid_str_transform_brc_params(msg, por->dx, por->dy, por->partner_fi); - context = 26, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = 0; + uint32_t fingerprint = grid_msg_recent_fingerprint_calculate(msg); - // check if message is alreadys in recent messages buffer - uint32_t fingerprint = grid_msg_recent_fingerprint_calculate(message); - if (grid_msg_recent_fingerprint_find(&recent_messages, fingerprint)) { - // Already heard this message - print_fifo_put_str(&message_queue, "H "); - for (uint8_t i = 0; i < 14; i++) { - if (message[i] < 32) { - print_fifo_put_str_format(&message_queue, "[%d] ", message[i]); - } else { - print_fifo_put_str_format(&message_queue, "%c ", message[i]); - } - } - print_fifo_put_str(&message_queue, "...\n"); - grid_bucket_clear(rx_bucket); - return 1; + if (grid_msg_recent_fingerprint_find(&recent_msgs, fingerprint)) { + return PICO_BKT_STATE_EMPTY; } - grid_msg_recent_fingerprint_store(&recent_messages, fingerprint); + grid_msg_recent_fingerprint_store(&recent_msgs, fingerprint); - context = 27, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = 0; + for (int i = 0; i < 4; ++i) { - for (uint8_t i = 0; i < 4; i++) { - struct grid_pico_uart_port* uart_port = &uart_port_array[i]; - if (uart_port->port_index == rx_bucket->source_port_index) { - // don't send the message to the same port it was received from - continue; - } + struct grid_pico_uart_port* target = &uart_ports[i]; - context = 28, userdata0 = 0, userdata1 = 0, userdata2 = 0, userdata3 = 0; - - grid_bucket_create_uart_tx_clone(uart_port, rx_bucket); + if (port->index != target->index) { + grid_uart_rx_clone_tx(target, bkt); + } } - // bucket content verified, close bucket and set it full to indicate that it is ready to be sent through to ESP32 via SPI - rx_bucket->status = GRID_BUCKET_STATUS_FULL_SEND_TO_SPI; - - return 0; + return PICO_BKT_STATE_SPI_TX; } -void grid_pico_uart_port_receive_character(struct grid_pico_uart_port* uart_port, uint8_t character) { - - if (uart_port->rx_bucket == NULL) { - grid_uart_port_attach_rx_bucket(uart_port); - } - - if (character == GRID_CONST_SOH && uart_port->rx_bucket->buffer_index > 0) { - // Start of Header character received in the middle of a transmission this typically happens when a partner is disconnected and reconnected - print_fifo_put_str(&message_queue, "E\n"); +struct grid_pico_task_timer timer_uart_rx_full[4]; - // clear the current active bucket, and attach to a new bucket to start - // receiving packet - grid_bucket_clear(uart_port->rx_bucket); +void grid_pico_task_uart_rx_full(struct grid_pico_uart_port* port, struct grid_pico_task_timer* timer) { - grid_uart_port_attach_rx_bucket(uart_port); + if (!grid_pico_task_timer_elapsed(timer)) { + return; } - grid_bucket_put_character(uart_port->rx_bucket, character); - - if (character == '\n') { - - // end of message, put termination zero character - grid_bucket_put_character(uart_port->rx_bucket, '\0'); - - uart_port->rx_bucket->status = GRID_BUCKET_STATUS_RECEIVE_COMPLETED; + enum pico_bkt_state_t state = PICO_BKT_STATE_UART_RX_FULL_NORTH + port->index; + struct pico_bkt_t* bkt = pico_pool_get_first(&pool, state); - if (uart_port->rx_bucket->buffer_index < 12) { // 12 is the minimum length of a ping packet - print_fifo_put_str(&message_queue, "WARNING\n"); - for (uint8_t i = 0; i < uart_port->rx_bucket->buffer_index; i++) { - if (uart_port->rx_bucket->buffer[i] < 32) { - print_fifo_put_str_format(&message_queue, "[%d] ", uart_port->rx_bucket->buffer[i]); - - } else { + if (!bkt) { + return; + } - print_fifo_put_str_format(&message_queue, "%c ", uart_port->rx_bucket->buffer[i]); - } - } + enum pico_bkt_state_t next_state = grid_uart_rx_process_bkt(port, bkt); - print_fifo_put_str(&message_queue, "\n"); - } + pico_bkt_reset(bkt); - // attack new bucket for receiving the next packet - grid_uart_port_attach_rx_bucket(uart_port); - } + pico_pool_change(&pool, bkt, next_state); } -void fifo_try_receive_characters(void) { - - uint32_t data = 0; - uint32_t status = 0; - // continue; +// - while (multicore_fifo_rvalid()) { +static uint8_t spi_tx_buf[GRID_PARAMETER_SPI_TRANSACTION_length]; +static uint8_t spi_rx_buf[GRID_PARAMETER_SPI_TRANSACTION_length]; - data = multicore_fifo_pop_blocking(); +struct pico_bkt_t* spi_tx_bucket = NULL; - // print_fifo_put_str(&message_queue, "POP"); - // direction is based on the position of the character in the uint32_t - // i==0 is north...i==3 is west - for (uint8_t i = 0; i < 4; i++) { +void grid_spi_msg_to_bkt(struct grid_pico_uart_port* port, char* msg) { - uint8_t c = (data >> (8 * i)) & 0x000000FF; + struct pico_bkt_t* bkt = pico_pool_get_first(&pool, PICO_BKT_STATE_EMPTY); - if (c == 0) { - continue; - } - - grid_pico_uart_port_receive_character(&uart_port_array[i], c); - } + if (!bkt) { + return; } -} -void core_1_main_entry() { + pico_bkt_reset(bkt); - print_fifo_put_str(&message_queue, "Core 1 init\r\n"); + size_t len = strnlen(msg, GRID_PARAMETER_SPI_TRANSACTION_length - 1); - while (1) { + memcpy(bkt->buf, msg, len); + bkt->buf[len] = '\0'; - uint32_t packed_chars = 0; + enum pico_bkt_state_t state = PICO_BKT_STATE_UART_TX_NORTH + port->index; + pico_pool_change(&pool, bkt, state); - // iterate through all the uart_ports and pack available characters - for (uint8_t i = 0; i < 4; i++) { - if (uart_rx_program_is_available(GRID_RX_PIO, i)) { + // port->ready = 0; +} - char c = uart_rx_program_getc(GRID_RX_PIO, i); - packed_chars |= (c << (8 * i)); - } - } +struct grid_pico_task_timer timer_spi_rx; - // try again if no characters were packed - if (packed_chars == 0) { - continue; - } +void grid_pico_task_spi_rx(struct grid_pico_task_timer* timer) { - uint8_t ok = multicore_fifo_push_timeout_us(packed_chars, 1); + if (!grid_pico_task_timer_elapsed(timer)) { + return; + } - if (!ok) { - print_fifo_put_str(&message_queue, "F\n"); - } + if (!grid_pico_spi_is_rx_data_available()) { + return; } -} -void core0_interrupt_handler(void) { + // vmp_push(SPI_RX); - fifo_try_receive_characters(); - multicore_fifo_clear_irq(); -} + grid_pico_spi_clear_rx_data_available_flag(); -volatile uint8_t sync1_drive = 0; -volatile uint8_t sync1_interrupt = 0; -volatile uint8_t sync2_interrupt = 0; + // Receive rolling ID + uint8_t rolling_recv = spi_rx_buf[GRID_PARAMETER_SPI_ROLLING_ID_index]; + grid_pico_rolling_recv(&rolling, rolling_recv); -void gpio_sync_pin_callback(uint gpio, uint32_t events) { + // Control LCD backlight + gpio_put(GRID_PICO_LCD_BACKLIGHT_PIN, spi_rx_buf[GRID_PARAMETER_SPI_BACKLIGHT_PWM_index]); - if (gpio == GRID_PICO_PIN_SYNC1) { + // The number of trailing zeroes in the destination flag + // indexes the destination UART port of the message + uint8_t dest_flags = spi_rx_buf[GRID_PARAMETER_SPI_SOURCE_FLAGS_index]; + uint8_t tz = __builtin_ctz(dest_flags); + struct grid_pico_uart_port* uart_port = tz < 4 ? &uart_ports[tz] : NULL; - sync1_interrupt = 1; - } else if (gpio == GRID_PICO_PIN_SYNC2) { + // If the destination UART port is valid + if (uart_port) { - sync2_interrupt = 1; + // Put the message to be sent out into a bucket + grid_spi_msg_to_bkt(uart_port, spi_rx_buf); } -} -void spi_uart_port_set_sync_state(uint8_t* buff) { + // Reset any attached tx bucket and mark it for reuse after use, + // this arbitrary bit of logic is specific to the SPI RX/TX task pair + if (spi_tx_bucket) { - buff[GRID_PARAMETER_SPI_SYNC1_STATE_index] = 0; - buff[GRID_PARAMETER_SPI_SYNC2_STATE_index] = 0; + pico_bkt_reset(spi_tx_bucket); + pico_pool_change(&pool, spi_tx_bucket, PICO_BKT_STATE_EMPTY); - if (sync1_interrupt) { - sync1_interrupt = 0; - buff[GRID_PARAMETER_SPI_SYNC1_STATE_index] = 1; - } - if (sync2_interrupt) { - sync2_interrupt = 0; - buff[GRID_PARAMETER_SPI_SYNC2_STATE_index] = 1; + spi_tx_bucket = NULL; } } -uint32_t grid_pico_get_time() { return time_us_32(); } - -uint32_t grid_pico_get_elapsed_time(uint32_t t_old) { return time_us_32() - t_old; } - -static uint32_t spi_receive_lastrealtime = 0; -static uint32_t spi_receive_interval_us = 100; +struct grid_pico_task_timer timer_spi_tx; -static uint32_t spi_transmit_lastrealtime = 0; -static uint32_t spi_transmit_interval_us = 500; +void grid_pico_task_spi_tx(struct grid_pico_task_timer* timer) { -static uint32_t uart_receive_lastrealtime = 0; -static uint32_t uart_receive_interval_us = 50; - -int spi_message_to_bucket(struct grid_pico_uart_port* uart_port, char* message) { - - if (uart_port == NULL) { - return 1; + if (!grid_pico_task_timer_elapsed(timer)) { + return; } - struct grid_bucket* bucket = grid_bucket_find_next_match(uart_port->tx_lastinserted_bucket, GRID_BUCKET_STATUS_EMPTY); + if (!grid_pico_spi_isready()) { + return; + } - if (bucket == NULL) { - return 1; + // Check that the previous transfer's rx task has reset the bucket + if (spi_tx_bucket) { + return; } - for (uint16_t i = 0; i < GRID_PARAMETER_SPI_TRANSACTION_length; i++) { - char c = grid_pico_spi_rxbuf[i]; - grid_bucket_put_character(bucket, c); + // Attempt to attach next bucket to be sent through SPI + enum pico_bkt_state_t state = PICO_BKT_STATE_SPI_TX; + spi_tx_bucket = pico_pool_get_first(&pool, state); - if (c == '\n') { - grid_bucket_put_character(bucket, '\0'); - break; - } - } + // vmp_push(SPI_TX); - bucket->status = GRID_BUCKET_STATUS_FULL_SEND_TO_NORTH + uart_port->port_index; + // Construct the bitfield containing which ports are ready to transmit + uint8_t tx_ready = 0; + for (int i = 0; i < 4; ++i) { + tx_ready |= (uart_ports[i].ready != 0) << uart_ports[i].index; + } - // set bucket as last inserted - uart_port->tx_lastinserted_bucket = bucket; + // Increment rolling ID + grid_pico_rolling_send(&rolling); - grid_pico_uart_tx_ready_bitmap &= ~(1 << uart_port->port_index); // clear ready -} + // No full bucket received from UART yet + if (!spi_tx_bucket) { -void grid_pico_uart_receive_task_inner(void) { + // Send empty packet with status flags, + // to convince the main processor of our presence + sprintf(spi_tx_buf, "DUMMY OK"); + spi_tx_buf[GRID_PARAMETER_SPI_ROLLING_ID_index] = rolling.last_send; + spi_tx_buf[GRID_PARAMETER_SPI_STATUS_FLAGS_index] = tx_ready; + spi_tx_buf[GRID_PARAMETER_SPI_SOURCE_FLAGS_index] = 0; // no origin port - if (grid_pico_get_elapsed_time(uart_receive_lastrealtime) < uart_receive_interval_us) { - return; + grid_pico_spi_transfer(spi_tx_buf, spi_rx_buf); } + // Found full bucket to be sent through SPI + else { - uart_receive_lastrealtime = grid_pico_get_time(); + // pico_bkt_reset(spi_tx_bucket); - struct grid_bucket* last_uart_rx_processed_bucket = NULL; - struct grid_bucket* bucket = grid_bucket_find_next_match(last_uart_rx_processed_bucket, GRID_BUCKET_STATUS_RECEIVE_COMPLETED); + uint8_t* buf = spi_tx_bucket->buf; + buf[GRID_PARAMETER_SPI_ROLLING_ID_index] = rolling.last_send; + buf[GRID_PARAMETER_SPI_STATUS_FLAGS_index] = tx_ready; + buf[GRID_PARAMETER_SPI_SOURCE_FLAGS_index] = 1 << spi_tx_bucket->port; - grid_uart_rx_process_bucket(bucket); + grid_pico_spi_transfer(buf, spi_rx_buf); + } } -void grid_pico_spi_receive_task_inner(void) { +void grid_pico_uart_port_rx_char(struct grid_pico_uart_port* port, char ch) { - if (grid_pico_get_elapsed_time(spi_receive_lastrealtime) < spi_receive_interval_us) { - return; + if (!port->uart_rx_bucket) { + grid_pico_uart_port_attach_rx(port); } - spi_receive_lastrealtime = grid_pico_get_time(); - if (!grid_pico_spi_is_rx_data_available()) { + if (!port->uart_rx_bucket) { return; } - grid_pico_spi_clear_rx_data_available_flag(); - - uint8_t rolling_id_now_received = grid_pico_spi_rxbuf[GRID_PARAMETER_SPI_ROLLING_ID_index]; - - if (rolling_id_now_received != (rolling_id_last_received + 1) % GRID_PARAMETER_SPI_ROLLING_ID_maximum) { - - if (rolling_id_error_count < 255) { - rolling_id_error_count++; - } - } - rolling_id_last_received = rolling_id_now_received; - - uint8_t destination_flags = grid_pico_spi_rxbuf[GRID_PARAMETER_SPI_SOURCE_FLAGS_index]; - uint8_t sync1_state = grid_pico_spi_rxbuf[GRID_PARAMETER_SPI_SYNC1_STATE_index]; - - gpio_put(GRID_PICO_LCD_BACKLIGHT_PIN, grid_pico_spi_rxbuf[GRID_PARAMETER_SPI_BACKLIGHT_PWM_index]); - - if (sync1_state) { + // Receiving start-of-header in the middle of a transmission, + // which typically happens after disconnection, then reconnection + if (ch == GRID_CONST_SOH && port->uart_rx_bucket->index > 0) { - gpio_pull_down(GRID_PICO_PIN_SYNC1); - sync1_drive = 1; - gpio_set_dir(GRID_PICO_PIN_SYNC1, GPIO_OUT); - gpio_put(GRID_PICO_PIN_SYNC1, 1); - // set_drive_mode(GRID_PICO_PIN_SYNC1, GPIO_DRIVE_MODE_OPEN_DRAIN); - } else if (sync1_drive) { - gpio_set_dir(GRID_PICO_PIN_SYNC1, GPIO_IN); - sync1_drive = 0; + pico_bkt_reset(port->uart_rx_bucket); } - // print_fifo_put_str_format(&message_queue, "%d\r\n", destination_flags); + pico_bkt_push(port->uart_rx_bucket, ch); - // iterate through all the uart_ports + if (ch == '\n') { - uint8_t index = __builtin_ctz(destination_flags); - struct grid_pico_uart_port* uart_port = index < 4 ? &uart_port_array[index] : NULL; + // End of message, requires null termination + pico_bkt_push(port->uart_rx_bucket, '\0'); - spi_message_to_bucket(uart_port, grid_pico_spi_rxbuf); + grid_pico_uart_port_detach_rx(port); - if (spi_tx_active_bucket != NULL) { - // clear the bucket after use - grid_bucket_clear(spi_tx_active_bucket); - spi_rx_previous_bucket = spi_tx_active_bucket; - spi_tx_active_bucket = NULL; + // vmp_push(UART_RX); } } -void grid_pico_spi_transmit_task_inner(void) { +struct grid_pico_task_timer timer_uart_rx_0[4]; - if (grid_pico_get_elapsed_time(spi_transmit_lastrealtime) < spi_transmit_interval_us) { - return; - } - spi_transmit_lastrealtime = grid_pico_get_time(); +void grid_pico_task_uart_rx_0(struct grid_pico_uart_port* port, struct grid_pico_task_timer* timer) { - if (!grid_pico_spi_isready()) { + if (!grid_pico_task_timer_elapsed(timer)) { return; } - if (spi_tx_active_bucket != NULL) { - // previous transfer's rx task has not run yet + if (!pico_swsr_readable(&port->swsr)) { return; } - // try to send bucket content through SPI - - spi_tx_active_bucket = grid_bucket_find_next_match(spi_rx_previous_bucket, GRID_BUCKET_STATUS_FULL_SEND_TO_SPI); + char c = pico_swsr_read(&port->swsr); + grid_pico_uart_port_rx_char(port, c); +} - if (spi_tx_active_bucket == NULL) { - // No fully bucket is received from uart yet - // send empty packet with status flags +struct grid_pico_task_timer timer_uart_rx_1[4]; - grid_pico_spi_txbuf[0] = 0; - sprintf(grid_pico_spi_txbuf, "DUMMY OK"); +void grid_pico_task_uart_rx_1(struct grid_pico_uart_port* port, struct grid_pico_task_timer* timer) { - rolling_id_last_sent = (rolling_id_last_sent + 1) % GRID_PARAMETER_SPI_ROLLING_ID_maximum; - grid_pico_spi_txbuf[GRID_PARAMETER_SPI_ROLLING_ID_index] = rolling_id_last_sent; // not received from any of the ports - grid_pico_spi_txbuf[GRID_PARAMETER_SPI_STATUS_FLAGS_index] = grid_pico_uart_tx_ready_bitmap; - grid_pico_spi_txbuf[GRID_PARAMETER_SPI_SOURCE_FLAGS_index] = 0; // not received from any of the ports + if (!grid_pico_task_timer_elapsed(timer)) { + return; + } - spi_uart_port_set_sync_state(grid_pico_spi_txbuf); - grid_pico_spi_transfer(grid_pico_spi_txbuf, grid_pico_spi_rxbuf); + if (!uart_rx_program_is_available(GRID_RX_PIO, port->index)) { + return; + } + if (!pico_swsr_writable(&port->swsr)) { return; } - // found full bucket, send it through SPI - spi_tx_active_bucket->buffer_index = 0; - // print_fifo_put_str_format(&message_queue, "SPI send: %s\r\n", spi_tx_active_bucket->buffer); + char c = uart_rx_program_getc(GRID_RX_PIO, port->index); + pico_swsr_write(&port->swsr, c); +} - rolling_id_last_sent = (rolling_id_last_sent + 1) % GRID_PARAMETER_SPI_ROLLING_ID_maximum; - spi_tx_active_bucket->buffer[GRID_PARAMETER_SPI_ROLLING_ID_index] = rolling_id_last_sent; // not received from any of the ports - spi_tx_active_bucket->buffer[GRID_PARAMETER_SPI_STATUS_FLAGS_index] = grid_pico_uart_tx_ready_bitmap; - spi_tx_active_bucket->buffer[GRID_PARAMETER_SPI_SOURCE_FLAGS_index] = (1 << (spi_tx_active_bucket->source_port_index)); +void core_1_main_entry() { - // print_fifo_put_str_format(&message_queue, "%d\n", spi_tx_active_bucket->buffer[GRID_PARAMETER_SPI_SOURCE_FLAGS_index]); + // Configure task timers + for (int i = 0; i < 4; ++i) { + timer_uart_rx_1[i] = (struct grid_pico_task_timer){ + .last = grid_pico_time(), + .period = 5, + }; + } - spi_uart_port_set_sync_state(spi_tx_active_bucket->buffer); - grid_pico_spi_transfer(spi_tx_active_bucket->buffer, grid_pico_spi_rxbuf); + while (1) { + + for (int i = 0; i < 4; ++i) { + grid_pico_task_uart_rx_1(&uart_ports[i], &timer_uart_rx_1[i]); + } + } } int main() { - set_sys_clock_khz(250000, true); // 2x overclock for RP2040. This is required because UART rx processing is not fast enough and I get lots of "F\n" in console otherwise stdio_init_all(); - uart_init(uart0, 2000000); - - if (watchdog_caused_reboot()) { - - printf("Rebooted by Watchdog!\n"); - printf("Context: %d, line: %d, userdata: %d %d %d %d\n", context, line, userdata0, userdata1, userdata2, userdata3); - - } else { - printf("Clean boot\n"); - printf("Context: %d, line: %d, userdata: %d %d %d %d\n", context, line, userdata0, userdata1, userdata2, userdata3); - } + grid_pico_spi_init(); - context = 0; - line = 0; - userdata0 = 0; - userdata1 = 0; - userdata2 = 0; - userdata3 = 0; + uart_init(uart0, 2000000); - print_fifo_put_str(&message_queue, "RP2040 START\r\n"); - print_fifo_put_str_format(&message_queue, "Build date and time: %s %s\n", __DATE__, __TIME__); + // Reset and launch second core + multicore_reset_core1(); + multicore_launch_core1(core_1_main_entry); - grid_msg_recent_fingerprint_buffer_init(&recent_messages, 32); + // Initialize fingerprint buffer + grid_msg_recent_fingerprint_buffer_init(&recent_msgs, 32); + // Initialize transport, used for some protocol mechanisms + // such as keeping track of delta offsets and rotation grid_transport_init(&grid_transport_state); - + printf("grid_transport_register_port ALL\n"); grid_transport_register_port(&grid_transport_state, grid_port_allocate_init(GRID_PORT_TYPE_USART, GRID_CONST_NORTH)); grid_transport_register_port(&grid_transport_state, grid_port_allocate_init(GRID_PORT_TYPE_USART, GRID_CONST_EAST)); grid_transport_register_port(&grid_transport_state, grid_port_allocate_init(GRID_PORT_TYPE_USART, GRID_CONST_SOUTH)); grid_transport_register_port(&grid_transport_state, grid_port_allocate_init(GRID_PORT_TYPE_USART, GRID_CONST_WEST)); - uint offset_tx = pio_add_program(GRID_TX_PIO, &uart_tx_program); - uint offset_rx = pio_add_program(GRID_RX_PIO, &uart_rx_program); - - // setup gpio sync interrupts - gpio_set_irq_enabled_with_callback(GRID_PICO_PIN_SYNC1, GPIO_IRQ_EDGE_RISE, true, &gpio_sync_pin_callback); - gpio_set_irq_enabled(GRID_PICO_PIN_SYNC2, GPIO_IRQ_EDGE_RISE, true); - - grid_pico_uart_uart_port_init(uart_port_N, 0); - grid_pico_uart_uart_port_init(uart_port_E, 1); - grid_pico_uart_uart_port_init(uart_port_S, 2); - grid_pico_uart_uart_port_init(uart_port_W, 3); - - for (uint8_t i = 0; i < bucket_array_length; i++) { - grid_bucket_init(&bucket_array[i], i); + // Initialize UART ports + for (int i = 0; i < 4; ++i) { + grid_pico_uart_port_malloc(&uart_ports[i]); } + grid_pico_uart_port_init(&uart_ports[0], 0); + grid_pico_uart_port_init(&uart_ports[1], 1); + grid_pico_uart_port_init(&uart_ports[2], 2); + grid_pico_uart_port_init(&uart_ports[3], 3); - for (uint8_t i = 0; i < 4; i++) { - - struct grid_pico_uart_port* uart_port = &uart_port_array[i]; - - grid_uart_port_attach_rx_bucket(uart_port); - } + int offset_tx = pio_add_program(GRID_TX_PIO, &uart_tx_program); + int offset_rx = pio_add_program(GRID_RX_PIO, &uart_rx_program); uart_tx_program_init(GRID_TX_PIO, 0, offset_tx, GRID_PICO_PIN_NORTH_TX, GRID_PARAMETER_UART_baudrate); uart_tx_program_init(GRID_TX_PIO, 1, offset_tx, GRID_PICO_PIN_EAST_TX, GRID_PARAMETER_UART_baudrate); @@ -856,93 +574,66 @@ int main() { uart_rx_program_init(GRID_RX_PIO, 2, offset_rx, GRID_PICO_PIN_SOUTH_RX, GRID_PARAMETER_UART_baudrate); uart_rx_program_init(GRID_RX_PIO, 3, offset_rx, GRID_PICO_PIN_WEST_RX, GRID_PARAMETER_UART_baudrate); - gpio_init(GRID_PICO_PIN_SYNC1); - gpio_init(GRID_PICO_PIN_SYNC2); - - // Setup COMMON stuff - gpio_init(GRID_PICO_PIN_LED); - gpio_set_dir(GRID_PICO_PIN_LED, GPIO_OUT); - - gpio_init(GRID_PICO_LCD_RESET_PIN); - gpio_put(GRID_PICO_LCD_RESET_PIN, 1); - gpio_set_dir(GRID_PICO_LCD_RESET_PIN, GPIO_OUT); - - gpio_init(GRID_PICO_LCD_BACKLIGHT_PIN); - gpio_set_dir(GRID_PICO_LCD_BACKLIGHT_PIN, GPIO_OUT); - gpio_put(GRID_PICO_LCD_BACKLIGHT_PIN, 0); - - grid_pico_spi_init(); - - for (uint i = 0; i < GRID_PARAMETER_SPI_TRANSACTION_length; i++) { - grid_pico_spi_txbuf[i] = 0; - } - - uint32_t loopcouter = 0; - uint8_t spi_counter = 0; - - multicore_reset_core1(); - multicore_launch_core1(core_1_main_entry); - - multicore_fifo_clear_irq(); - irq_set_exclusive_handler(SIO_IRQ_PROC0, core0_interrupt_handler); - irq_set_enabled(SIO_IRQ_PROC0, true); - - watchdog_enable(100, 1); - + // Initialize bucket pool + pico_pool_init(&pool); + + // Configure task timers + for (int i = 0; i < 4; ++i) { + timer_uart_tx[i] = (struct grid_pico_task_timer){ + .last = grid_pico_time(), + .period = 5, + }; + timer_uart_rx_full[i] = (struct grid_pico_task_timer){ + .last = grid_pico_time(), + .period = 5, + }; + timer_uart_rx_0[i] = (struct grid_pico_task_timer){ + .last = grid_pico_time(), + .period = 5, + }; + } + timer_spi_rx = (struct grid_pico_task_timer){ + .last = grid_pico_time(), + .period = 100, + }; + timer_spi_tx = (struct grid_pico_task_timer){ + .last = grid_pico_time(), + .period = 500, + }; + + // Allocate profiler & assign its interface + vmp_buf_malloc(&vmp, 100, sizeof(struct vmp_evt_t)); + struct vmp_reg_t reg = { + .evt_serialized_size = vmp_evt_serialized_size, + .evt_serialize = vmp_evt_serialize, + .fwrite = vmp_fwrite, + }; + + //bool vmp_flushed = false; while (1) { - watchdog_update(); - - context = 1; + //vmp_push(MAIN); - char c = '\0'; - while (c = print_fifo_get(&message_queue)) { - printf("%c", c); + for (int i = 0; i < 4; ++i) { + grid_pico_task_uart_tx(&uart_ports[i], &timer_uart_tx[i]); + grid_pico_task_uart_rx_0(&uart_ports[i], &timer_uart_rx_0[i]); + grid_pico_task_uart_rx_full(&uart_ports[i], &timer_uart_rx_full[i]); } + grid_pico_task_spi_rx(&timer_spi_rx); + grid_pico_task_spi_tx(&timer_spi_tx); - context = 2; - - grid_pico_uart_receive_task_inner(); - - context = 3; - - grid_pico_spi_receive_task_inner(); - - context = 4; - - grid_pico_spi_transmit_task_inner(); - - context = 5; - - if (rolling_id_error_count > 0) { - rolling_id_error_count = 0; - print_fifo_put_str(&message_queue, "$\n"); - } - - // iterate through all the uart_ports - for (uint8_t i = 0; i < 4; i++) { - grid_pico_uart_transmit_task_inner(&uart_port_array[i]); - } - - context = 6; - - // for (uint8_t i = 0; i < 4; i++) { - - // struct grid_port* por = grid_transport_get_port(&grid_transport_state, i); + /* + if (!vmp_flushed && vmp.size == vmp.capacity) { - // if (grid_port_should_uart_timeout_disconect_now(por)) { // try disconnect for uart port - // print_fifo_put_str(&message_queue, "D\n"); - // por->partner_status = 0; - // // grid_port_receiver_softreset(por, doublebuffer_rx); - // } - // } + vmp_serialize_start(®); + vmp_buf_serialize_and_write(&vmp, ®); + vmp_uid_str_serialize_and_write(VMP_UID_COUNT, VMP_ASSOC, ®); + vmp_serialize_close(®); - loopcouter++; + vmp_buf_free(&vmp); - if (loopcouter % 1000 == 0) { - gpio_put(GRID_PICO_PIN_LED, 1); - } else { - gpio_put(GRID_PICO_PIN_LED, 0); + vmp_flushed = true; } + */ } } diff --git a/grid_pico/main/pico_pool.c b/grid_pico/main/pico_pool.c new file mode 100644 index 00000000..8d67be72 --- /dev/null +++ b/grid_pico/main/pico_pool.c @@ -0,0 +1,105 @@ +#include "pico_pool.h" + +void pico_bkt_init(struct pico_bkt_t* bkt, pico_bkt_idx_t bucket, struct pico_bkt_t* prev, struct pico_bkt_t* next) { + + bkt->state = PICO_BKT_STATE_EMPTY; + bkt->prev = prev; + bkt->next = next; + bkt->index = 0; +} + +void pico_bkt_reset(struct pico_bkt_t* bkt) { bkt->index = 0; } + +uint8_t pico_bkt_next(struct pico_bkt_t* bkt) { + + uint32_t index = bkt->index; + bkt->index = (bkt->index + 1) % PICO_BKT_SIZE; + return bkt->buf[index]; +} + +void pico_bkt_push(struct pico_bkt_t* bkt, uint8_t rx) { + + bkt->buf[bkt->index] = rx; + bkt->index = (bkt->index + 1) % PICO_BKT_SIZE; +} + +void pico_pool_init(struct pico_pool_t* pool) { + + if (PICO_BKT_COUNT > PICO_BKT_LIMIT) { + return; + } + + for (int i = 0; i < PICO_BKT_STATES; ++i) { + pool->first[i] = NULL; + } + + pool->first[PICO_BKT_STATE_EMPTY] = &pool->bkts[0]; + + for (int i = 0; i < PICO_BKT_COUNT; ++i) { + uint8_t prev = (i - 1 + PICO_BKT_COUNT) % PICO_BKT_COUNT; + uint8_t next = (i + 1 + PICO_BKT_COUNT) % PICO_BKT_COUNT; + pico_bkt_init(&pool->bkts[i], i, &pool->bkts[prev], &pool->bkts[next]); + } +} + +struct pico_bkt_t* pico_pool_get_first(struct pico_pool_t* pool, enum pico_bkt_state_t state) { + + int valid = state >= 0 && state < PICO_BKT_STATES; + return valid ? pool->first[state] : NULL; +} + +struct pico_bkt_t* pico_pool_change(struct pico_pool_t* pool, struct pico_bkt_t* bkt, enum pico_bkt_state_t state) { + + int valid = state >= 0 && state < PICO_BKT_STATES; + if (!valid) { + return NULL; + } + + // The bucket must be the first in the cycle of its type + if (bkt != pool->first[bkt->state]) { + return NULL; + } + + // If the bucket is part of a cycle longer than one + if (!(bkt->next == bkt && bkt->prev == bkt)) { + + // The first bucket of the old list becomes the second + pool->first[bkt->state] = bkt->next; + + // Detach bucket from the cycle of its current type + bkt->prev->next = bkt->next; + bkt->next->prev = bkt->prev; + } + // If the bucket under change is in a list of one + else { + + // Reset first-of-type bucket for the current type + pool->first[bkt->state] = NULL; + } + + // If a cycle of the new type already exists + if (pool->first[state]) { + + struct pico_bkt_t* fst = pool->first[state]; + + // Link the bucket into an existing cycle, before the first + bkt->prev = fst->prev; + bkt->next = fst; + fst->prev->next = bkt; + fst->prev = bkt; + } + // If no cycle of the new type currently exists + else { + + // Set the bucket links to loop back onto the bucket + bkt->prev = bkt; + bkt->next = bkt; + + // Set this bucket as the first-of-type bucket for its type + pool->first[state] = bkt; + } + + bkt->state = state; + + return bkt; +} diff --git a/grid_pico/main/pico_pool.h b/grid_pico/main/pico_pool.h new file mode 100644 index 00000000..004ed540 --- /dev/null +++ b/grid_pico/main/pico_pool.h @@ -0,0 +1,88 @@ +#ifndef PICO_POOL_H +#define PICO_POOL_H + +#include +#include +#include +#include + +#include "../../grid_common/grid_protocol.h" + +enum pico_bkt_state_t { + PICO_BKT_STATE_EMPTY = 0, + PICO_BKT_STATE_SPI_TX, + PICO_BKT_STATE_UART_TX_NORTH, + PICO_BKT_STATE_UART_TX_EAST, + PICO_BKT_STATE_UART_TX_SOUTH, + PICO_BKT_STATE_UART_TX_WEST, + PICO_BKT_STATE_UART_RX_NORTH, + PICO_BKT_STATE_UART_RX_EAST, + PICO_BKT_STATE_UART_RX_SOUTH, + PICO_BKT_STATE_UART_RX_WEST, + PICO_BKT_STATE_UART_RX_FULL_NORTH, + PICO_BKT_STATE_UART_RX_FULL_EAST, + PICO_BKT_STATE_UART_RX_FULL_SOUTH, + PICO_BKT_STATE_UART_RX_FULL_WEST, + PICO_BKT_STATES, +}; + +typedef uint8_t pico_bkt_idx_t; + +enum { + PICO_BKT_SIZE = GRID_PARAMETER_SPI_TRANSACTION_length, +}; + +struct pico_bkt_t; + +struct pico_bkt_t { + + // Current state + enum pico_bkt_state_t state; + + // Previous bucket in cycle + struct pico_bkt_t* prev; + + // Next bucket in cycle + struct pico_bkt_t* next; + + // Read/write index + uint32_t index; + + // Buffer contents + uint8_t buf[PICO_BKT_SIZE]; + + // Source port index (used in an external mechanism, could be removed) + uint8_t port; +}; + +void pico_bkt_init(struct pico_bkt_t* bkt, pico_bkt_idx_t bucket, struct pico_bkt_t* prev, struct pico_bkt_t* next); +void pico_bkt_reset(struct pico_bkt_t* bkt); +uint8_t pico_bkt_next(struct pico_bkt_t* bkt); +void pico_bkt_push(struct pico_bkt_t* bkt, uint8_t rx); + +enum { + PICO_BKT_COUNT = 50, +}; +enum { + PICO_BKT_LIMIT = UINT8_MAX, +}; +enum { + PICO_BKT_IDX_NIL = PICO_BKT_LIMIT, +}; + +#define PICO_BKT_IDX_VALID(i) ((i) < PICO_BKT_IDX_NIL) + +struct pico_pool_t { + + // First bucket in a cycle for each state + struct pico_bkt_t* first[PICO_BKT_STATES]; + + // Pool of buckets + struct pico_bkt_t bkts[PICO_BKT_COUNT]; +}; + +void pico_pool_init(struct pico_pool_t* pool); +struct pico_bkt_t* pico_pool_get_first(struct pico_pool_t* pool, enum pico_bkt_state_t state); +struct pico_bkt_t* pico_pool_change(struct pico_pool_t* pool, struct pico_bkt_t* bkt, enum pico_bkt_state_t state); + +#endif /* PICO_POOL_H */ diff --git a/grid_pico/main/pico_swsr.c b/grid_pico/main/pico_swsr.c new file mode 100644 index 00000000..bbf55f10 --- /dev/null +++ b/grid_pico/main/pico_swsr.c @@ -0,0 +1,50 @@ +#include "pico_swsr.h" + +int pico_swsr_malloc(struct pico_swsr_t* swsr, int capacity) { + + if (capacity < 1) { + return 1; + } + + char* data = malloc(capacity); + + if (!data) { + return 1; + } + + swsr->capacity = capacity; + swsr->data = data; + + pico_swsr_init(swsr); + + return 0; +} + +void pico_swsr_free(struct pico_swsr_t* swsr) { + + free(swsr->data); + + swsr->data = NULL; +} + +void pico_swsr_init(struct pico_swsr_t* swsr) { + + swsr->write = 0; + swsr->read = swsr->capacity - 1; +} + +bool pico_swsr_writable(struct pico_swsr_t* swsr) { return (swsr->write + 1) % swsr->capacity != swsr->read; } + +bool pico_swsr_readable(struct pico_swsr_t* swsr) { return (swsr->read + 1) % swsr->capacity != swsr->write; } + +void pico_swsr_write(struct pico_swsr_t* swsr, char c) { + + swsr->data[swsr->write] = c; + swsr->write = (swsr->write + 1) % swsr->capacity; +} + +char pico_swsr_read(struct pico_swsr_t* swsr) { + + swsr->read = (swsr->read + 1) % swsr->capacity; + return swsr->data[swsr->read]; +} diff --git a/grid_pico/main/pico_swsr.h b/grid_pico/main/pico_swsr.h new file mode 100644 index 00000000..c743c006 --- /dev/null +++ b/grid_pico/main/pico_swsr.h @@ -0,0 +1,31 @@ +#ifndef PICO_SWSR_H +#define PICO_SWSR_H + +#include +#include + +// Single writer, single reader ring buffer +struct pico_swsr_t { + + // Allocated element count + int capacity; + + // Write index + int write; + + // Read index + int read; + + // Buffer contents + char* data; +}; + +int pico_swsr_malloc(struct pico_swsr_t* swsr, int capacity); +void pico_swsr_free(struct pico_swsr_t* swsr); +void pico_swsr_init(struct pico_swsr_t* swsr); +bool pico_swsr_writable(struct pico_swsr_t* swsr); +bool pico_swsr_readable(struct pico_swsr_t* swsr); +void pico_swsr_write(struct pico_swsr_t* swsr, char c); +char pico_swsr_read(struct pico_swsr_t* swsr); + +#endif /* PICO_SWSR_H */ diff --git a/grid_pico/main/vmp/build.sh b/grid_pico/main/vmp/build.sh new file mode 100755 index 00000000..3c2bfed7 --- /dev/null +++ b/grid_pico/main/vmp/build.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +mkdir -p build +gcc -g -o build/proc proc.c vmp_def.c ../../../vmp/vmp.c -I ./ -I ../../../vmp diff --git a/grid_pico/main/vmp/proc.c b/grid_pico/main/vmp/proc.c new file mode 100644 index 00000000..4aadec63 --- /dev/null +++ b/grid_pico/main/vmp/proc.c @@ -0,0 +1,34 @@ +#include +#include +#include + +#include "vmp_def.h" + +int main() { + struct vmp_reg_t reg = { + .evt_serialized_size = vmp_evt_serialized_size, + .evt_deserialize = vmp_evt_deserialize, + .fread = vmp_fread, + }; + + struct vmp_uid_str_t uid_str; + vmp_buf_read_and_deserialize(&vmp, ®); + vmp_uid_str_read_and_deserialize(&uid_str, ®); + + struct vmp_buf_t vmp_vis; + vmp_buf_malloc(&vmp_vis, vmp.capacity, sizeof(struct vmp_vis_t)); + vmp_evt_to_vis(&vmp, &vmp_vis); + + for (int i = 0; i < vmp_vis.capacity; ++i) { + struct vmp_vis_t* vis = VMP_VIS(vmp_buf_get(&(vmp_vis), i)); + struct vmp_evt_t* evt = &vis->evt; + printf("%02u id, %6u abs, %6u rel, %6u pre, %4u:%s,\n", evt->uid, evt->time, vis->time_abs, vis->time_rel, evt->line, uid_str.strs[evt->uid]); + } + + vmp_buf_free(&vmp_vis); + + vmp_uid_str_free(&uid_str); + vmp_buf_free(&vmp); + + return 0; +} diff --git a/grid_pico/main/vmp/vmp_def.c b/grid_pico/main/vmp/vmp_def.c new file mode 100644 index 00000000..3b733329 --- /dev/null +++ b/grid_pico/main/vmp/vmp_def.c @@ -0,0 +1,242 @@ +#include "vmp_def.h" + +struct vmp_buf_t vmp; + +#define hex_to_uint8_range(h, min, max, add) (((h) >= (min) && (h) <= (max)) * ((h) - (min) + (add))) + +uint8_t hex_to_uint8(char h[2]) { + uint8_t hi = hex_to_uint8_range(h[0], '0', '9', 0) + hex_to_uint8_range(h[0], 'a', 'f', 10); + uint8_t lo = hex_to_uint8_range(h[1], '0', '9', 0) + hex_to_uint8_range(h[1], 'a', 'f', 10); + + return (hi << 4) + lo; +} + +static char uint4_to_hex[16] = { + '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f', +}; + +void uint8_to_hex(uint8_t u, char h[2]) { + h[0] = uint4_to_hex[u >> 4]; + h[1] = uint4_to_hex[u & 0xf]; +} + +size_t vmp_evt_serialize(void* evt, uint8_t* dest) { + struct vmp_evt_t* event = evt; + size_t size = 0; + + *((uint16_t*)(dest + size)) = htons((event)->uid); + size += sizeof(uint16_t); + *((uint16_t*)(dest + size)) = htons((event)->line); + size += sizeof(uint16_t); + *((uint32_t*)(dest + size)) = htonl((event)->time); + size += sizeof(uint32_t); + + return size; +} + +size_t vmp_evt_deserialize(uint8_t* src, void* evt) { + struct vmp_evt_t* event = evt; + size_t size = 0; + + event->uid = ntohs(*((uint16_t*)(src + size))); + size += sizeof(uint16_t); + event->line = ntohs(*((uint16_t*)(src + size))); + size += sizeof(uint16_t); + event->time = ntohl(*((uint32_t*)(src + size))); + size += sizeof(uint32_t); + + return size; +} + +size_t vmp_evt_serialized_size() { + return sizeof(uint16_t) + // uid + sizeof(uint16_t) + // line + sizeof(uint32_t) + // time + 0; +} + +size_t vmp_fwrite(void* ptr, size_t size) { + uint8_t* u8 = ptr; + char hex[2]; + + size_t s = 0; + for (int i = 0; i < size; ++i) { + uint8_to_hex(u8[i], hex); + s += printf("%c%c", hex[0], hex[1]); + } + + return s; +} + +size_t vmp_fread(void* ptr, size_t size) { + uint8_t* u8 = ptr; + char hex[2]; + + size_t s = 0; + for (int i = 0; i < size; ++i) { + s += fread(hex, 1, 2, stdin); + u8[i] = hex_to_uint8(hex); + } + + assert(s % 2 == 0); + + return s / 2; +} + +enum vmp_err_t vmp_uid_str_malloc(struct vmp_uid_str_t* uid_str, int capacity) { + assert(capacity > 0); + + uid_str->capacity = capacity; + + char(*strs)[VMP_UID_STR_MAX] = vmp_alloc(capacity * VMP_UID_STR_MAX); + if (!strs) { + return VMP_ERR_MALLOC; + } + + uid_str->strs = strs; + + return VMP_ERR_NONE; +} + +enum vmp_err_t vmp_uid_str_free(struct vmp_uid_str_t* uid_str) { + if (!uid_str->strs) { + return VMP_ERR_FREE; + } + + vmp_dealloc(uid_str->strs); + uid_str->strs = NULL; + + return VMP_ERR_NONE; +} + +size_t vmp_write_u8_str(vmp_fwrite_t write, char* s) { + size_t len = strlen(s); + if (len > VMP_UID_STR_MAX - 1) { + return 0; + } + + size_t size = 0; + + uint8_t u8 = len; + size += write(&u8, sizeof(uint8_t)); + + size += write(s, u8); + + return size; +} + +size_t vmp_read_u8_str(vmp_fread_t read, char* s) { + size_t size = 0; + + uint8_t u8; + size += read(&u8, sizeof(uint8_t)); + + size += read(s, u8); + s[u8] = '\0'; + + return size; +} + +size_t vmp_uid_str_serialize_and_write(int uids, char** strs, struct vmp_reg_t* reg) { + if (!reg->evt_serialize) { + return 0; + } + if (!reg->fwrite) { + return 0; + } + + size_t size = 0; + + uint32_t u32; + + u32 = htonl(uids); + size += reg->fwrite(&u32, sizeof(uint32_t)); + + for (int i = 0; i < uids; ++i) { + size += vmp_write_u8_str(reg->fwrite, strs[i]); + } + + return size; +} + +size_t vmp_uid_str_read_and_deserialize(struct vmp_uid_str_t* uid_str, struct vmp_reg_t* reg) { + if (!reg->evt_deserialize) { + return 0; + } + if (!reg->fread) { + return 0; + } + + size_t size = 0; + + uint32_t u32; + + size += reg->fread(&u32, sizeof(uint32_t)); + int capacity = ntohl(u32); + + enum vmp_err_t err = vmp_uid_str_malloc(uid_str, capacity); + if (err != VMP_ERR_NONE) { + return 0; + } + + for (int i = 0; i < capacity; ++i) { + size += vmp_read_u8_str(reg->fread, uid_str->strs[i]); + } + + return size; +} + +size_t vmp_serialize_start(struct vmp_reg_t* reg) { + if (!reg->fwrite) { + return 0; + } + + return reg->fwrite(VMP_START, strlen(VMP_START)); +} + +size_t vmp_serialize_close(struct vmp_reg_t* reg) { + if (!reg->fwrite) { + return 0; + } + + return reg->fwrite(VMP_CLOSE, strlen(VMP_CLOSE)); +} + +enum vmp_err_t vmp_evt_to_vis(struct vmp_buf_t* src, struct vmp_buf_t* dest) { + if (src->capacity != dest->capacity) { + return VMP_ERR_PARAM; + } + if (src->typesize != sizeof(struct vmp_evt_t)) { + return VMP_ERR_PARAM; + } + if (dest->typesize != sizeof(struct vmp_vis_t)) { + return VMP_ERR_PARAM; + } + + if (dest->size != 0) { + return VMP_ERR_PARAM; + } + if (src->size < 1) { + return VMP_ERR_PARAM; + } + + struct vmp_evt_t* zero_evt = VMP_EVT(vmp_buf_get(src, 0)); + *VMP_VIS(vmp_buf_get(dest, 0)) = (struct vmp_vis_t){ + .evt = *zero_evt, + .time_abs = 0, + .time_rel = 0, + }; + + for (int i = 1; i < src->capacity; ++i) { + struct vmp_evt_t* prev_evt = VMP_EVT(vmp_buf_get(src, i - 1)); + struct vmp_evt_t* evt = VMP_EVT(vmp_buf_get(src, i)); + *VMP_VIS(vmp_buf_get(dest, i)) = (struct vmp_vis_t){ + .evt = *evt, + .time_abs = evt->time - zero_evt->time, + .time_rel = evt->time - prev_evt->time, + }; + vmp_buf_incr(dest); + } + + return VMP_ERR_NONE; +} diff --git a/grid_pico/main/vmp/vmp_def.h b/grid_pico/main/vmp/vmp_def.h new file mode 100644 index 00000000..62ac657f --- /dev/null +++ b/grid_pico/main/vmp/vmp_def.h @@ -0,0 +1,66 @@ +#ifndef VMP_DEF_H +#define VMP_DEF_H + +#include +#include +#include + +#include "vmp.h" + +extern struct vmp_buf_t vmp; + +#define vmp_push(...) vmp_buf_push(&vmp, __VA_ARGS__) + +struct vmp_evt_t { + uint16_t uid; + uint16_t line; + uint32_t time; +}; + +#define VMP_EVT(ptr) ((struct vmp_evt_t*)(ptr)) + +#define VMP_EVT_WRITE(ptr, id) \ + { \ + VMP_EVT(ptr)->uid = (id); \ + VMP_EVT(ptr)->line = __LINE__; \ + VMP_EVT(ptr)->time = grid_pico_time(); \ + } + +size_t vmp_evt_serialize(void* evt, uint8_t* dest); +size_t vmp_evt_deserialize(uint8_t* src, void* evt); +size_t vmp_evt_serialized_size(); + +size_t vmp_fwrite(void* ptr, size_t size); +size_t vmp_fread(void* ptr, size_t size); + +enum { + VMP_UID_STR_MAX = 0x100, +}; + +struct vmp_uid_str_t { + int capacity; + char (*strs)[VMP_UID_STR_MAX]; +}; + +enum vmp_err_t vmp_uid_str_malloc(struct vmp_uid_str_t* uid_str, int capacity); +enum vmp_err_t vmp_uid_str_free(struct vmp_uid_str_t* uid_str); +size_t vmp_uid_str_serialize_and_write(int uids, char** strs, struct vmp_reg_t* reg); +size_t vmp_uid_str_read_and_deserialize(struct vmp_uid_str_t* uid_str, struct vmp_reg_t* reg); + +#define VMP_START "\xab\xad\xca\xfe" +#define VMP_CLOSE "\xab\xad\xc0\xde" + +size_t vmp_serialize_start(struct vmp_reg_t* reg); +size_t vmp_serialize_close(struct vmp_reg_t* reg); + +struct vmp_vis_t { + struct vmp_evt_t evt; + uint32_t time_abs; + uint32_t time_rel; +}; + +#define VMP_VIS(ptr) ((struct vmp_vis_t*)(ptr)) + +enum vmp_err_t vmp_evt_to_vis(struct vmp_buf_t* src, struct vmp_buf_t* dest); + +#endif /* VMP_DEF_H */ diff --git a/grid_pico/main/vmp/vmp_tag.h b/grid_pico/main/vmp/vmp_tag.h new file mode 100644 index 00000000..a7d0984e --- /dev/null +++ b/grid_pico/main/vmp/vmp_tag.h @@ -0,0 +1,19 @@ +#ifndef VMP_TAG_H +#define VMP_TAG_H + +#include "vmp.h" + +enum { + MAIN = 0, + UART_RX, + UART_TX, + SPI_RX, + SPI_TX, + VMP_UID_COUNT, +}; + +char* VMP_ASSOC[] = { + VMP_NAME_EQ_STR_ENTRY(MAIN), VMP_NAME_EQ_STR_ENTRY(UART_RX), VMP_NAME_EQ_STR_ENTRY(UART_TX), VMP_NAME_EQ_STR_ENTRY(SPI_RX), VMP_NAME_EQ_STR_ENTRY(SPI_TX), +}; + +#endif /* VMP_TAG_H */ diff --git a/vmp/build.sh b/vmp/build.sh new file mode 100755 index 00000000..fea36473 --- /dev/null +++ b/vmp/build.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +mkdir -p build +gcc -o build/recv recv.c diff --git a/vmp/recv.c b/vmp/recv.c new file mode 100644 index 00000000..3b339eed --- /dev/null +++ b/vmp/recv.c @@ -0,0 +1,243 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// Baudrate settings are defined in , +// which is included by +#define BAUDRATE B2000000 + +char* path = NULL; + +void process_args(int argc, char** argv) { + int idx = 0; + while (idx < argc) { + + // Input file + if (strcmp(argv[idx], "-i") == 0 && argc - idx > 1) { + path = argv[idx + 1]; + ++idx; + } + + ++idx; + } +} + +void setup_termios(struct termios* tio) { + // BAUDRATE: bps rate (you could also use cfsetispeed or cfsetospeed + // CRTSCTS: output hardware flow control (only used if the cable has + // all necessary lines + // CS8: 8n1 (8bit, no parity, 1 stopbit) + // CLOCAL: local connection, no modem control + // CREAD: enable receiving characters + tio->c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD; + + // IGNPAR: ignore bytes with parity errors + // ICRML: map CR to NL (otherwise a CR input on the other computer + // will not terminate input) + // otherwise make device raw (no other input processing) + tio->c_iflag = IGNPAR /*| ICRNL*/; + + // Raw output + tio->c_oflag = 0; + + // ICANON: enable canonical input + // ECHO: disable all echo functionality, + // and don't send signals to calling program + tio->c_lflag = 0; + + // Initialize all control characters + // Default values can be found in /usr/include/termios.h, + // and are given in the comments, but we don't need them here + tio->c_cc[VINTR] = 0; /* Ctrl-c */ + tio->c_cc[VQUIT] = 0; /* Ctrl-\ */ + tio->c_cc[VERASE] = 0; /* del */ + tio->c_cc[VKILL] = 0; /* @ */ + tio->c_cc[VEOF] = 0; /* Ctrl-d */ + tio->c_cc[VTIME] = 0; /* inter-character timer unused */ + tio->c_cc[VMIN] = 1; /* blocking read until 1 character arrives */ + tio->c_cc[VSWTC] = 0; /* '\0' */ + tio->c_cc[VSTART] = 0; /* Ctrl-q */ + tio->c_cc[VSTOP] = 0; /* Ctrl-s */ + tio->c_cc[VSUSP] = 0; /* Ctrl-z */ + tio->c_cc[VEOL] = 0; /* '\0' */ + tio->c_cc[VREPRINT] = 0; /* Ctrl-r */ + tio->c_cc[VDISCARD] = 0; /* Ctrl-u */ + tio->c_cc[VWERASE] = 0; /* Ctrl-w */ + tio->c_cc[VLNEXT] = 0; /* Ctrl-v */ + tio->c_cc[VEOL2] = 0; /* '\0' */ +} + +int fd = -1; +struct termios oldtio; + +void terminate() { + // Restore the old port settings + int ret = tcsetattr(fd, TCSANOW, &oldtio); + if (ret < 0) { + fprintf(stderr, "tcsetattr() failed\n"); + exit(1); + } + + if (!(fd < 0)) { + close(fd); + } + + exit(0); +} + +void sigint_handler(int signum) { terminate(); } + +bool byte_pattern(char* pattern, int size, char rx, int* pos) { + assert(size > 0); + assert(*pos >= 0); + assert(*pos < size); + + if (pattern[*pos] == rx) { + *pos += 1; + } else { + *pos = pattern[0] == rx; + } + + return *pos == size; +} + +#define SEQ_START "abadcafe" +#define SEQ_CLOSE "abadc0de" + +int process_char(char rx) { + static char* seqs[] = {SEQ_START, SEQ_CLOSE}; + static int lens[] = {sizeof(SEQ_START) - 1, sizeof(SEQ_CLOSE) - 1}; + + assert(lens[0] <= lens[1]); + + static char buf[sizeof(SEQ_CLOSE) - 1]; + static int phase = 0; + static int match = 0; + + // Sequence continues to match + if (rx == seqs[phase][match]) { + + // Buffer undecided character + buf[match++] = rx; + } + // Sequence stops matching + else { + + // Flush buffered characters if necessary + if (phase && match) { + write(1, buf, match); + } + + // Reset match length + match = 0; + + // Sequence start matches + if (rx == seqs[phase][match]) { + + // Buffer undecided character + buf[match++] = rx; + } + // Sequence start does not match + else { + + // Write current character if necessary + if (phase) { + write(1, &rx, 1); + } + } + } + + // Upon full match + if (match == lens[phase]) { + + // If the second sequence was matched, return nonzero status + if (phase && match == lens[phase]) { + return 1; + } + + // Otherwise, start matching the next sequence + ++phase; + match = 0; + } + + return 0; +} + +enum { + BUF_SIZE = 0x100, +}; + +int main(int argc, char** argv) { + int ret; + + // Set signal handler for SIGINT + signal(SIGINT, sigint_handler); + + // Process arguments + process_args(argc, argv); + + // Open modem device for reading and writing and not as controlling tty, + // because we don't want to get killed upon receiving CTRL-C. + fd = open(path, O_RDWR | O_NOCTTY); + if (fd < 0) { + perror(path); + exit(1); + } + + // Save current serial port settings + ret = tcgetattr(fd, &oldtio); + if (ret < 0) { + fprintf(stderr, "tcgetattr() failed\n"); + exit(1); + } + + // Clear struct, setup new port settings + struct termios newtio; + memset(&newtio, 0, sizeof(struct termios)); + setup_termios(&newtio); + + // Clean the modem line + ret = tcflush(fd, TCIFLUSH); + if (ret < 0) { + fprintf(stderr, "tcflush() failed\n"); + exit(1); + } + + // Activate the settings for the port + ret = tcsetattr(fd, TCSANOW, &newtio); + if (ret < 0) { + fprintf(stderr, "tcsetattr() failed\n"); + exit(1); + } + + char buf[BUF_SIZE]; + while (true) { + + // Read up to as many bytes as the buffer can hold + int res = read(fd, buf, BUF_SIZE); + if (res < 0) { + fprintf(stderr, "read() returned < 0\n"); + exit(1); + } + + // Pass bytes to receiver, quit upon nonzero status + for (int i = 0; i < res; ++i) { + if (process_char(buf[i])) { + terminate(); + } + } + } + + // Handle termination + terminate(); + + return 0; +} diff --git a/vmp/vmp.c b/vmp/vmp.c new file mode 100644 index 00000000..5f11e96d --- /dev/null +++ b/vmp/vmp.c @@ -0,0 +1,120 @@ +#include "vmp.h" + +uint32_t htonl(uint32_t hostlong) { return hostlong; } +uint16_t htons(uint16_t hostshort) { return hostshort; } +uint32_t ntohl(uint32_t netlong) { return netlong; } +uint16_t ntohs(uint16_t netshort) { return netshort; } + +enum vmp_err_t vmp_buf_malloc(struct vmp_buf_t* buf, int capacity, int typesize) { + assert(capacity > 0); + assert(typesize > 0); + + buf->capacity = capacity; + buf->typesize = typesize; + + void* evts = vmp_alloc(capacity * typesize); + if (!evts) { + return VMP_ERR_MALLOC; + } + + buf->evts = evts; + + vmp_buf_init(buf); + + return VMP_ERR_NONE; +} + +enum vmp_err_t vmp_buf_free(struct vmp_buf_t* buf) { + if (!buf->evts) { + return VMP_ERR_FREE; + } + + vmp_dealloc(buf->evts); + buf->evts = NULL; + + return VMP_ERR_NONE; +} + +void vmp_buf_init(struct vmp_buf_t* buf) { + buf->size = 0; + buf->end = 0; +} + +size_t vmp_buf_serialize_and_write(struct vmp_buf_t* buf, struct vmp_reg_t* reg) { + if (!reg->evt_serialized_size) { + return 0; + } + if (!reg->evt_serialize) { + return 0; + } + if (!reg->fwrite) { + return 0; + } + + size_t size = 0; + + uint32_t u32; + + u32 = htonl(buf->size); + size += reg->fwrite(&u32, sizeof(uint32_t)); + u32 = htonl(buf->typesize); + size += reg->fwrite(&u32, sizeof(uint32_t)); + + size_t evt_size = reg->evt_serialized_size(); + uint8_t* evt_buf = vmp_alloc(evt_size); + if (!evt_buf) { + return 0; + } + + for (int i = 0; i < buf->size; ++i) { + size_t s = reg->evt_serialize(vmp_buf_get(buf, i), evt_buf); + size += reg->fwrite(evt_buf, s); + } + + vmp_dealloc(evt_buf); + + return size; +} + +size_t vmp_buf_read_and_deserialize(struct vmp_buf_t* buf, struct vmp_reg_t* reg) { + if (!reg->evt_serialized_size) { + return 0; + } + if (!reg->evt_deserialize) { + return 0; + } + if (!reg->fread) { + return 0; + } + + size_t size = 0; + + uint32_t u32; + + size += reg->fread(&u32, sizeof(uint32_t)); + int capacity = ntohl(u32); + size += reg->fread(&u32, sizeof(uint32_t)); + int typesize = ntohl(u32); + + enum vmp_err_t err = vmp_buf_malloc(buf, capacity, typesize); + if (err != VMP_ERR_NONE) { + return 0; + } + + size_t evt_size = reg->evt_serialized_size(); + uint8_t* evt_buf = vmp_alloc(evt_size); + if (!evt_buf) { + vmp_buf_free(buf); + return 0; + } + + for (int i = 0; i < capacity; ++i) { + size += reg->fread(evt_buf, evt_size); + reg->evt_deserialize(evt_buf, vmp_buf_at(buf, i)); + vmp_buf_incr(buf); + } + + vmp_dealloc(evt_buf); + + return size; +} diff --git a/vmp/vmp.h b/vmp/vmp.h new file mode 100644 index 00000000..c91902e0 --- /dev/null +++ b/vmp/vmp.h @@ -0,0 +1,73 @@ +#ifndef VMP_H +#define VMP_H + +#include +#include +#include +#include + +static void* (*VMP_ALLOC)(size_t) = malloc; +static void (*VMP_DEALLOC)(void*) = free; + +#define vmp_alloc(a) VMP_ALLOC(a) +#define vmp_dealloc(a) VMP_DEALLOC(a) + +typedef size_t (*vmp_evt_serialized_size_t)(); +typedef size_t (*vmp_serialize_t)(void* self, uint8_t* dest); +typedef size_t (*vmp_deserialize_t)(uint8_t* src, void* self); +typedef size_t (*vmp_fread_t)(void* ptr, size_t size); +typedef size_t (*vmp_fwrite_t)(void* ptr, size_t size); + +struct vmp_reg_t { + vmp_evt_serialized_size_t evt_serialized_size; + vmp_serialize_t evt_serialize; + vmp_deserialize_t evt_deserialize; + vmp_fwrite_t fwrite; + vmp_fread_t fread; +}; + +uint32_t htonl(uint32_t hostlong); +uint16_t htons(uint16_t hostshort); +uint32_t ntohl(uint32_t netlong); +uint16_t ntohs(uint16_t netshort); + +enum vmp_err_t { + VMP_ERR_NONE = 0, + VMP_ERR_MALLOC, + VMP_ERR_FREE, + VMP_ERR_PARAM, +}; + +struct vmp_buf_t { + int capacity; + int typesize; + int size; + int end; + void* evts; +}; + +enum vmp_err_t vmp_buf_malloc(struct vmp_buf_t* buf, int capacity, int typesize); +enum vmp_err_t vmp_buf_free(struct vmp_buf_t* buf); +void vmp_buf_init(struct vmp_buf_t* buf); +size_t vmp_buf_serialize_and_write(struct vmp_buf_t* buf, struct vmp_reg_t* reg); +size_t vmp_buf_read_and_deserialize(struct vmp_buf_t* buf, struct vmp_reg_t* reg); + +#define vmp_buf_at(buf, i) (((uint8_t*)((buf)->evts)) + (i) * (buf)->typesize) + +#define vmp_buf_get(buf, i) (vmp_buf_at(buf, ((buf)->capacity + (buf)->end - (buf)->size + (i)) % (buf)->capacity)) + +#define vmp_buf_incr(buf) \ + { \ + (buf)->end = ((buf)->end + 1) % (buf)->capacity; \ + (buf)->size += (buf)->size < (buf)->capacity; \ + } + +#define vmp_buf_push(buf, ...) \ + { \ + VMP_EVT_WRITE(vmp_buf_at(buf, (buf)->end), __VA_ARGS__) \ + vmp_buf_incr(buf) \ + } + +#define VMP_NAME_EQ_STR_ENTRY(name) [name] = #name + +#endif /* VMP_H */ From 2b2fbc696a4ff407c959a3ab1141cf807e9c67e6 Mon Sep 17 00:00:00 2001 From: ben Date: Thu, 2 Jan 2025 15:10:10 +0100 Subject: [PATCH 2/3] BENB format.sh --- grid_pico/main/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/grid_pico/main/main.c b/grid_pico/main/main.c index b33b0786..aab8dcff 100644 --- a/grid_pico/main/main.c +++ b/grid_pico/main/main.c @@ -609,10 +609,10 @@ int main() { .fwrite = vmp_fwrite, }; - //bool vmp_flushed = false; + // bool vmp_flushed = false; while (1) { - //vmp_push(MAIN); + // vmp_push(MAIN); for (int i = 0; i < 4; ++i) { grid_pico_task_uart_tx(&uart_ports[i], &timer_uart_tx[i]); From c7257a63fa46426b4a0cda3f81191fc0b5b2a690 Mon Sep 17 00:00:00 2001 From: ben Date: Mon, 6 Jan 2025 10:48:10 +0100 Subject: [PATCH 3/3] BENB add convenience script for profiler execution --- grid_pico/main/vmp/execute.sh | 5 +++++ 1 file changed, 5 insertions(+) create mode 100755 grid_pico/main/vmp/execute.sh diff --git a/grid_pico/main/vmp/execute.sh b/grid_pico/main/vmp/execute.sh new file mode 100755 index 00000000..ddbadc0d --- /dev/null +++ b/grid_pico/main/vmp/execute.sh @@ -0,0 +1,5 @@ +#!/bin/sh + +DEVICE="/dev/ttyUSB1" + +../../../vmp/build/recv -i "${DEVICE}" | ./build/proc