From 7f9492f77a96d18b51b53f9567b1ec3843d91b91 Mon Sep 17 00:00:00 2001
From: jaseg <git@jaseg.de>
Date: Wed, 14 Jul 2021 18:59:30 +0200
Subject: [PATCH] Integration test WORKS!!1!

---
 demo/fw/src/main.c | 61 ++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 53 insertions(+), 8 deletions(-)

diff --git a/demo/fw/src/main.c b/demo/fw/src/main.c
index e8710bd..65ab856 100644
--- a/demo/fw/src/main.c
+++ b/demo/fw/src/main.c
@@ -25,6 +25,7 @@
 #include <assert.h>
 #include <string.h>
 #include <math.h>
+#include <platform/endian.h>
 
 #include <tinyalloc.h>
 #include <stm32f407xx.h>
@@ -225,11 +226,27 @@ unsigned long strtoul(const char *nptr, char **endptr, int base) {
     }
 }
 
+static uint8_t unfuck_channel_val(uint8_t val) {
+    if ((val & 0xf) == 0xf)
+        return val + 1;
+    return val;
+}
+
+static void unpack_px_data(uint8_t *buf, size_t size) {
+    assert (size%2 == 0);
+
+    for (size_t i=0, j=0; i<size; i+=1, j+=2) {
+        buf[i] = (unfuck_channel_val(buf[j]) & 0xf0) | (unfuck_channel_val(buf[j+1]) >> 4);
+    }
+}
+
 void *malloc(size_t size) { return ta_alloc(size); }
 void free(void *ptr) { ta_free(ptr); }
 void *calloc(size_t nmemb, size_t size) { return ta_calloc(nmemb, size); }
 
 unsigned char ta_heap[0x10000];
+uint8_t payload_buf[16384];
+uint8_t dec_buf[16384];
 
 int main(void)
 {
@@ -285,7 +302,7 @@ int main(void)
     err = stream_decrypt(decrypted, sizeof(cage_startup_test_data), &decrypted_size, cage_startup_test_data, sizeof(cage_startup_test_data)-1, file_key);
     assert (!err);
     assert (decrypted_size > 0 && decrypted_size < sizeof(cage_startup_test_data));
-    decrypted[decrypted_size-1] = '\0';
+    decrypted[decrypted_size-1] = '\0'; /* overwrite trailing \n */
 
     con_printf("%s\r\n", decrypted);
 
@@ -307,6 +324,7 @@ int main(void)
         */
 
         con_printf_blocking(" #%u", i);
+        /*
         if (k == 0)
             spif_printf(&spif, "\33[1m");
         if (k == 1)
@@ -318,20 +336,19 @@ int main(void)
         if (k >= 12 && k < 20)
             spif_printf(&spif, "\033[9%dm", k-12+1);
         spif_printf(&spif, "loop #%u  \033[4%dmloop #%u  \033[10%dmloop #%u\033[0m\n", i, k%8, (k+1)%8);
+        */
         i++;
         k = (k + 1) % 40;
         if (k == 0) {
-            spif_printf(&spif, "\033[H\033[0m\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\033[H");
+            spif_capture_read(&spif, sizeof(payload_buf), payload_buf);
 
-            uint8_t buf[4096];
-            spif_capture_read(&spif, sizeof(buf), buf);
             con_printf_blocking("\r\n");
             con_printf_blocking("\r\n");
             con_printf_blocking("buffer capture:\r\n");
-            for (size_t e=0; e<sizeof(buf); e += 27) {
-                uint8_t *p = buf + e;
+            for (size_t e=0; e<256; e += 27) {
+                uint8_t *p = payload_buf + e;
                 con_printf_blocking("   ");
-                for (size_t j=0; j<27 && e+j < sizeof(buf); j++) {
+                for (size_t j=0; j<27 && e+j < sizeof(payload_buf); j++) {
                     if (j%3 == 0)
                         con_printf_blocking("\033[91m");
                     else if (j%3 == 1)
@@ -350,7 +367,35 @@ int main(void)
                 }
             }
             con_printf_blocking("\033[0m\r\n");
-            con_printf_blocking("loop");
+
+            unpack_px_data(payload_buf, sizeof(payload_buf));
+
+            size_t payload_len = be32toh(((uint32_t *)payload_buf)[0]);
+            if (payload_len < 0 || payload_len > sizeof(payload_buf) - sizeof(uint32_t)) {
+                con_printf_blocking("Invalid payload size %zx\r\n", payload_len);
+                continue;
+            }
+
+            unsigned char file_key[16];
+            err = parse_age_buf(&ks, payload_buf + sizeof(uint32_t), payload_len+1, file_key);
+            if (err) {
+                con_printf_blocking("Error parsing payload age header: %d\r\n", err);
+                continue;
+            }
+
+            size_t decrypted_size = 0;
+            err = stream_decrypt(dec_buf, sizeof(dec_buf), &decrypted_size, payload_buf + sizeof(uint32_t), payload_len, file_key);
+            if (err) {
+                con_printf_blocking("Error decrypting payload: %d\r\n", err);
+                continue;
+            }
+
+            assert (decrypted_size > 0 && decrypted_size < sizeof(dec_buf));
+            dec_buf[decrypted_size-1] = '\0'; /* overwrite trailing \n */
+            con_printf_blocking("decrypted payload: %s\r\n", dec_buf);
+
+            spif_printf(&spif, "\033[H\033[0m\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\033[H");
+            spif_printf(&spif, "\033[1;91mDecrypted data:\033[0m\n\n%s\n", dec_buf);
         }
 
         for (int j=0; j<1000000; j++) {