2 files changed, 71 insertions, 2 deletions

diff --git a/fw/main.c b/fw/main.c
index 4ba6457..663f7a4 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -350,9 +350,12 @@ void uart_config(void) {
 
 static unsigned int overruns = 0;
 static unsigned int frame_overruns = 0;
+#define SYNC_LENGTH 32 /* Must be a power of two */
 void USART1_IRQHandler(void) {
     static uint8_t expect_framing = 1;
     static int rxpos = 0;
+    static int resync = SYNC_LENGTH+1;
+    static int sync_chars = 0;
 
     GPIOA->BSRR = GPIO_BSRR_BS_0; // Debug
 
@@ -364,16 +367,27 @@ void USART1_IRQHandler(void) {
         USART1->ICR = USART_ICR_ORECF;
     } else { /* RXNE */
         uint8_t data = USART1->RDR;
-        if (expect_framing) {
+
+        if (data == 0x23)
+            sync_chars++;
+        else
+            sync_chars = 0;
+
+        if (resync) {
+            if (sync_chars == SYNC_LENGTH+1) {
+                resync = 0;
+            }
+        } else if (expect_framing) {
             if (data == 0x42) {
                 expect_framing = 0;
             } else {
                 rxpos = 0;
+                resync = 1;
             }
         } else {
             rx_buf.byte_data[rxpos] = data;
             rxpos++;
-            if ((rxpos&0x1F) == 0) {
+            if ((rxpos&(SYNC_LENGTH-1)) == 0) {
                 expect_framing = 1;
             }
             if (rxpos >= sizeof(rx_buf.set_fb_rq)) {
@@ -524,6 +538,10 @@ int main(void) {
             led_state = (led_state+1)&7;
         }
         if (fb_op == FB_FORMAT) {
+            for (int i=0; i<sizeof(rx_buf.set_fb_rq); i++) {
+                if (rx_buf.byte_data[i] == 0x42)
+                    asm("bkpt");
+            }
             transpose_data(rx_buf.byte_data, write_fb);
             fb_op = FB_UPDATE;
         }
diff --git a/fw/test.py b/fw/test.py
new file mode 100755
index 0000000..2f0da3f
--- /dev/null
+++ b/fw/test.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+import serial
+
+def chunked(data, chunk_size):
+    for i in range(0, len(data), chunk_size):
+        yield data[i:i+chunk_size]
+
+def frame_packet(data, chunk_size=32, frame_char=b'\x42'):
+    return frame_char + frame_char.join(chunked(data, chunk_size))
+
+def sync_frame(sync_char=b'\x23', chunk_size=32):
+    return sync_char*(chunk_size+1)
+
+def format_packet(data):
+    out = b''
+    for a, b, c, d in chunked(data, 4):
+        ah, bh, ch, dh = a>>8,   b>>8,   c>>8,   d>>8
+        al, bl, cl, dl = a&0xff, b&0xff, c&0xff, d&0xff
+        # FIXME check order of high bits
+        out += bytes([al, bl, cl, dl, (ah<<6 | bh<<4 | ch<<2 | dh<<0)&0xff])
+    return out
+
+if __name__ == '__main__':
+    import argparse
+    import time
+    
+    parser = argparse.ArgumentParser()
+    parser.add_argument('serial')
+    args = parser.parse_args()
+
+    ser = serial.Serial(args.serial, 2000000)
+
+    frame_len = 4*8*8
+    black, red = [0]*frame_len, [255]*frame_len
+    frames = \
+            [black]*10 +\
+            [red]*10 +\
+            [[i]*frame_len for i in range(0, 256, 4)] +\
+            [[(i + (d//8)*8) % 256*8 for d in range(frame_len)] for i in range(0, 256, 16)]
+
+    frames = [red, black]*5
+    while True:
+        print('Sending sync structure')
+        ser.write(sync_frame())
+        for i, frame in enumerate(frames):
+            formatted = format_packet(frame)
+            #formatted = format_packet(list(range(256)))
+            framed = frame_packet(formatted)
+            print('sending', i, len(frame), len(formatted), len(framed))
+            ser.write(framed)
+            time.sleep(0.1)