demo fw WIP

6 files changed, 197 insertions, 60 deletions

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 <global.h>
 
+#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<len-1; i++) {
 			if (in_pos >= 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('<I', crc)
+                        crc_ok = (crc,) == struct.unpack('<I', payload[-4:])
+                    else:
+                        crc_ok = False
+                        ref_crc = ''
+                    print(' '.join(f'{col if col else ""}{c if c else 0:02x}' for col, c in itertools.zip_longest(field_colors,
+                        payload)), 'OK' if crc_ok else f'WRONG')
+                except cobs.DecodeError:
+                    print('COBS framing error')
+                packet = b''
+            else:
+                packet += bytes([c])
+            isprint = lambda c: c in b'0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ!"#$%&\'()*+,-./:;<=>?@[\\]^_`{|}~ '
+            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('<I', seq)
+            seq += 1
+            ser.write(cobs.encode(data + struct.pack('<I', zlib.crc32(data))) + b'\0')
+            last_tx = time.time()
+        #time.sleep(0.01)