From 424310d957cb04bfacba9c711ee7f074fd41f962 Mon Sep 17 00:00:00 2001 From: jaseg Date: Fri, 27 Nov 2020 13:38:56 +0100 Subject: demo fw WIP --- prototype/fw/Makefile | 2 + prototype/fw/generic_stm32.ld | 20 ------- prototype/fw/src/crc32.h | 6 +- prototype/fw/src/main.c | 131 +++++++++++++++++++++++++++++++++-------- prototype/fw/src/microcobs.c | 25 ++++---- prototype/fw/tools/ser_test.py | 73 +++++++++++++++++++++++ 6 files changed, 197 insertions(+), 60 deletions(-) create mode 100644 prototype/fw/tools/ser_test.py diff --git a/prototype/fw/Makefile b/prototype/fw/Makefile index 33288b8..32ded36 100644 --- a/prototype/fw/Makefile +++ b/prototype/fw/Makefile @@ -37,6 +37,8 @@ STARTUP_FILE := $(CMSIS_DEVICE_DIR)/Source/Templates/gcc/startup_stm32f302xc.s ASM_SOURCES := $(STARTUP_FILE) C_SOURCES := src/main.c +C_SOURCES += src/microcobs.c +C_SOURCES += src/crc32.c C_SOURCES += upstream/tinyprintf/tinyprintf.c CPP_SOURCES := # - none - diff --git a/prototype/fw/generic_stm32.ld b/prototype/fw/generic_stm32.ld index 8fe2da8..72a9c72 100644 --- a/prototype/fw/generic_stm32.ld +++ b/prototype/fw/generic_stm32.ld @@ -94,26 +94,6 @@ SECTIONS _edata = .; /* define a global symbol at data end */ } >RAM AT> FLASH - _siccmram = LOADADDR(.ccmram); - - /* CCM-RAM section - * - * IMPORTANT NOTE! - * If initialized variables will be placed in this section, - * the startup code needs to be modified to copy the init-values. - */ - .ccmram : - { - . = ALIGN(4); - _sccmram = .; /* create a global symbol at ccmram start */ - *(.ccmram) - *(.ccmram*) - - . = ALIGN(4); - _eccmram = .; /* create a global symbol at ccmram end */ - } >CCMRAM AT> FLASH - - /* Uninitialized data section */ . = ALIGN(4); .bss : diff --git a/prototype/fw/src/crc32.h b/prototype/fw/src/crc32.h index 7a3a729..c115820 100644 --- a/prototype/fw/src/crc32.h +++ b/prototype/fw/src/crc32.h @@ -5,8 +5,10 @@ typedef uint32_t crc32_t; -inline static uint32_t crc32_reset() { return ~0; } +inline static uint32_t crc32_reset(void); +uint32_t crc32_reset() { return ~0; } uint32_t crc32_update(uint32_t old_state, uint8_t c); -inline static uint32_t crc32_finalize(uint32_t state) { return ~state; } +inline static uint32_t crc32_finalize(uint32_t state); +uint32_t crc32_finalize(uint32_t state) { return ~state; } #endif /* __CRC_32_H__ */ diff --git a/prototype/fw/src/main.c b/prototype/fw/src/main.c index de920eb..fb3c302 100644 --- a/prototype/fw/src/main.c +++ b/prototype/fw/src/main.c @@ -1,49 +1,128 @@ #include +#include "microcobs.h" +#include "crc32.h" + +struct __attribute__((packed)) ll_pkt_trailer { + uint32_t crc32; +}; + +struct __attribute__((packed)) req_pkt { + uint32_t req_seq; + struct ll_pkt_trailer trailer; +}; + +struct __attribute__((packed)) res_pkt { + uint32_t req_seq; + uint32_t res_seq; + struct ll_pkt_trailer trailer; +}; + +struct tx_state { + uint8_t *tx_char; + int remaining_bytes; +}; + +static crc32_t pkt_crc(void *pkt, struct ll_pkt_trailer *trailer); +crc32_t pkt_crc(void *pkt, struct ll_pkt_trailer *trailer) { + crc32_t crc = crc32_reset(); + for (uint8_t *in = (uint8_t *)pkt; in < (uint8_t *)trailer; in++) { + crc = crc32_update(crc, *in); + } + return crc32_finalize(crc); +} + +static void packetize(void *pkt, struct ll_pkt_trailer *trailer); +void packetize(void *pkt, struct ll_pkt_trailer *trailer) { + trailer->crc32 = pkt_crc(pkt, trailer); +} + int main(void) { RCC->AHBENR |= RCC_AHBENR_GPIOAEN; RCC->APB2ENR |= RCC_APB2ENR_USART1EN; - GPIOA->MODER |= (2 << GPIO_MODER_MODER9_Pos); - GPIOA->AFR[1] = (7 << (9-8)*4); + GPIOA->MODER |= (2 << GPIO_MODER_MODER9_Pos) | (2 << GPIO_MODER_MODER10_Pos); + GPIOA->AFR[1] = (7 << (9-8)*4) | (7 << (10-8)*4); SystemCoreClockUpdate(); int apb2_clock = SystemCoreClock / APB2_PRESC; int baudrate = 115200; - USART1->CR1 = USART_CR1_MME | USART_CR1_TE; + USART1->CR1 = USART_CR1_TE | USART_CR1_RE; USART1->BRR = (apb2_clock + baudrate/2) / baudrate; + USART1->CR1 |= USART_CR2_RXINV; USART1->CR1 |= USART_CR1_UE; - char s[12] = { - '0', '0', '0', '0', '0', - ' ', 'T', 'E', 'S', 'T', - '\n', '\0'}; - int line = 0; - char *c = s; - USART1->TDR = *(c++); /* Kick off transmission */ + int req_seq = 0; + int res_seq = 0; + struct req_pkt req_buf = { 0 }; + struct tx_state tx_st = { 0 }; + struct res_pkt res_buf = { 0 }; + uint8_t rx_buf[512]; + uint8_t tx_buf[512]; + size_t rx_char = 0; + unsigned int rx_overrun = 0; + unsigned int rx_cobs_error = 0; + unsigned int rx_framing_error = 0; + unsigned int rx_crc_error = 0; + + USART1->TDR = 0; /* Kick off transmission */ while (23) { - if (USART1->ISR & USART_ISR_TXE) { + if (tx_st.remaining_bytes == 0) { + res_buf.req_seq = req_seq; + res_buf.res_seq = res_seq; + res_seq += 1; + packetize(&res_buf, &res_buf.trailer); + tx_st.tx_char = tx_buf; + tx_st.remaining_bytes = cobs_encode((uint8_t *)&res_buf, sizeof(res_buf), tx_buf, sizeof(tx_buf));; + } + + if (USART1->ISR & USART_ISR_TXE && tx_st.remaining_bytes > 0) { for (int i=0; i<100; i++) asm volatile ("nop"); - USART1->TDR = *(c++); - if (!*c) { - c = s; - line++; - int tmp = line; - tmp %= 100000; - s[0] = '0' + tmp/10000; - tmp %= 10000; - s[1] = '0' + tmp/1000; - tmp %= 1000; - s[2] = '0' + tmp/100; - tmp %= 100; - s[3] = '0' + tmp/10; - tmp %= 10; - s[4] = '0' + tmp; + USART1->TDR = *(tx_st.tx_char); + tx_st.tx_char += 1; + tx_st.remaining_bytes -= 1; + } + + if (USART1->ISR & USART_ISR_ORE) + USART1->ICR = USART_ICR_ORECF; + + if (USART1->ISR & USART_ISR_NE) + USART1->ICR = USART_ICR_NCF; + + if (USART1->ISR & USART_ISR_FE) + USART1->ICR = USART_ICR_FECF; + + if (USART1->ISR & USART_ISR_RXNE) { + uint8_t c = USART1->RDR; + if (!c) { + int rc = cobs_decode(rx_buf, rx_char, (uint8_t *)&req_buf, sizeof(req_buf)); + if (rc < 0) { + rx_cobs_error += 1; + } else { + if (rc == sizeof(req_buf)) { + crc32_t check_crc = pkt_crc(&req_buf, &req_buf.trailer); + if (check_crc != req_buf.trailer.crc32) { + rx_crc_error += 1; + } else { + req_seq = req_buf.req_seq; + } + } else { + rx_framing_error += 1; + } + } + rx_char = 0; + } else { + if (rx_char < sizeof(rx_buf)) { + rx_buf[rx_char] = c; + rx_char += 1; + } else { + rx_overrun += 1; + } } } } diff --git a/prototype/fw/src/microcobs.c b/prototype/fw/src/microcobs.c index aea199e..5007344 100644 --- a/prototype/fw/src/microcobs.c +++ b/prototype/fw/src/microcobs.c @@ -7,15 +7,15 @@ ssize_t cobs_encode_sg(const struct sg_entry input[], uint8_t *output, size_t ou { size_t idx_pos = 0; size_t out_pos = 1; - size_t sg_idx = 0; + ssize_t sg_idx = 0; const struct sg_entry *e = input; - fprintf(stderr, "\nsg_entry %016x %zd\n", e->target, e->size); + /* fprintf(stderr, "\nsg_entry %016x %zd\n", e->target, e->size); */ while (e->size >= 0) { if (sg_idx == e->size) { sg_idx = 0; e++; - fprintf(stderr, "\nsg_entry %016x %zd\n", e->target, e->size); + /* fprintf(stderr, "\nsg_entry %016x %zd\n", e->target, e->size); */ continue; } @@ -23,7 +23,7 @@ ssize_t cobs_encode_sg(const struct sg_entry input[], uint8_t *output, size_t ou return -1; uint8_t inbyte = e->target[sg_idx]; - fprintf(stderr, "%02x ", inbyte); + /* fprintf(stderr, "%02x ", inbyte); */ sg_idx += 1; if (out_pos - idx_pos >= 255) { @@ -49,8 +49,8 @@ ssize_t cobs_encode_sg(const struct sg_entry input[], uint8_t *output, size_t ou return -1; output[idx_pos] = out_pos - idx_pos; output[out_pos] = 0x00; - fprintf(stderr, "\n"); - fprintf(stderr, "Finishing %d %d %d\n", idx_pos, out_pos, out_pos - idx_pos); + /* fprintf(stderr, "\n"); */ + /* fprintf(stderr, "Finishing %d %d %d\n", idx_pos, out_pos, out_pos - idx_pos); */ return out_pos + 1; } @@ -66,7 +66,7 @@ ssize_t cobs_encode(const uint8_t *input, size_t input_len, uint8_t *output, siz return -1; uint8_t inbyte = input[in_pos]; - fprintf(stderr, "%02x ", inbyte); + /* fprintf(stderr, "%02x ", inbyte); */ in_pos += 1; if (out_pos - idx_pos >= 255) { @@ -92,12 +92,13 @@ ssize_t cobs_encode(const uint8_t *input, size_t input_len, uint8_t *output, siz return -1; output[idx_pos] = out_pos - idx_pos; output[out_pos] = 0x00; - fprintf(stderr, "\n"); - fprintf(stderr, "Finishing %d %d %d\n", idx_pos, out_pos, out_pos - idx_pos); + /* fprintf(stderr, "\n"); */ + /* fprintf(stderr, "Finishing %d %d %d\n", idx_pos, out_pos, out_pos - idx_pos); */ return out_pos + 1; } +/* input and output may overlap. */ ssize_t cobs_decode(const uint8_t *input, size_t input_len, uint8_t *output, size_t output_len) { size_t out_pos = 0; @@ -108,20 +109,20 @@ ssize_t cobs_decode(const uint8_t *input, size_t input_len, uint8_t *output, siz while (in_pos < input_len) { size_t len = input[in_pos]; - fprintf(stderr, "Length @%03d = %03d\n", in_pos, len); + /* fprintf(stderr, "Length @%03d = %03d\n", in_pos, len); */ in_pos += 1; for (size_t i=0; i= input_len) break; - fprintf(stderr, "Copy %03d -> %03d %02x\n", in_pos, out_pos, input[in_pos]); + /* fprintf(stderr, "Copy %03d -> %03d %02x\n", in_pos, out_pos, input[in_pos]); */ output[out_pos] = input[in_pos]; in_pos += 1; out_pos += 1; } if (in_pos < input_len && len < 255) { - fprintf(stderr, "Zero %03d %02x\n", out_pos); + /* fprintf(stderr, "Zero %03d %02x\n", out_pos); */ output[out_pos] = 0; out_pos += 1; } diff --git a/prototype/fw/tools/ser_test.py b/prototype/fw/tools/ser_test.py new file mode 100644 index 0000000..191124b --- /dev/null +++ b/prototype/fw/tools/ser_test.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +import struct +import itertools +import binascii +import string +import serial +import time +import zlib + +from cobs import cobs + +if __name__ == '__main__': + import argparse + parser = argparse.ArgumentParser() + parser.add_argument('port') + parser.add_argument('-b', '--baudrate', type=int, default=115200) + args = parser.parse_args() + + ser = serial.Serial(args.port, args.baudrate, timeout=0) + + byte_count = 0 + line = b'' + packet = b'' + start_time = time.time() + seq = 0 + + make_color = lambda x: f'\033[38;5;{x}m' + field_colors = [ make_color(x) for x in [ 48, 48, 48, 48, 220, 220, 220, 220, 207, 207, 207, 207 ] ] + last_tx = time.time() + while True: + data = ser.read() + for c in data: + if byte_count == 0: + print(f'\033[38;5;244m{time.time() - start_time: 8.3f} \033[0m', end='') + + col = '\033[91m' if c == 0 else '\033[0m' + print(f'{col}{c:02x}', end=' ') + line += bytes([c]) + byte_count += 1 + if c == 0: + print(' ' * (16 - byte_count), end='\033[0m\n') + byte_count = 16 + try: + payload = cobs.decode(packet) + if len(payload) > 4: + crc = zlib.crc32(payload[:-4]) + ref_crc = struct.pack('?@[\\]^_`{|}~ ' + if byte_count == 16: + printable = ''.join( chr(c) if isprint(c) else '.' for c in line ) + print(f'\033[93m | {printable}\033[0m') + byte_count = 0 + line = b'' + + + if time.time() - last_tx > 0.01: + data = struct.pack('