Bidirectional msgpack wire protocol with unpack support

picomap.cpp

@@ -5,12 +5,20 @@
 #include "pico/bootrom.h"
 #include "msgpackpp.h"
 #include "halfsiphash.h"
+#include "static_vector.h"
 
 static constexpr uint8_t hash_key[8] = {};
 
-struct RebootingBootsel {
+struct ResponseBOOTSEL {
     static constexpr int8_t ext_id = 1;
     auto as_tuple() const { return std::tie(); }
+    auto as_tuple() { return std::tie(); }
+};
+
+struct RequestBOOTSEL {
+    static constexpr int8_t ext_id = 2;
+    auto as_tuple() const { return std::tie(); }
+    auto as_tuple() { return std::tie(); }
 };
 
 struct Envelope {
@@ -18,6 +26,7 @@ struct Envelope {
     uint32_t checksum;
     std::vector<uint8_t> payload;
     auto as_tuple() const { return std::tie(checksum, payload); }
+    auto as_tuple() { return std::tie(checksum, payload); }
 };
 
 static std::vector<uint8_t> pack_envelope(const std::vector<uint8_t> &payload) {
@@ -34,20 +43,56 @@ static void send_bytes(const std::vector<uint8_t> &data) {
     stdio_flush();
 }
 
+template <typename T>
+static void send_message(const T &msg) {
+    msgpackpp::packer inner;
+    inner.pack(msg);
+    auto envelope = pack_envelope(inner.get_payload());
+    send_bytes(envelope);
+}
+
+static int8_t try_decode(const static_vector<uint8_t, 256> &buf) {
+    msgpackpp::parser p(buf.data(), static_cast<int>(buf.size()));
+
+    Envelope env;
+    if (!msgpackpp::unpack(p, env)) return -1;
+
+    uint32_t expected = halfsiphash::hash32(env.payload.data(), env.payload.size(), hash_key);
+    if (env.checksum != expected) return -1;
+
+    msgpackpp::parser inner(env.payload.data(), static_cast<int>(env.payload.size()));
+    if (!inner.is_ext()) return -1;
+    auto ext = inner.get_ext();
+    if (!ext) return -1;
+
+    return std::get<0>(*ext);
+}
+
 int main() {
     stdio_init_all();
 
+    static static_vector<uint8_t, 256> rx_buf;
+
     while (true) {
         int c = getchar_timeout_us(100000);
-        if (c == 'p') {
-            printf("p");
-        } else if (c == 'b') {
-            msgpackpp::packer inner;
-            inner.pack(RebootingBootsel{});
-            auto msg = pack_envelope(inner.get_payload());
-            send_bytes(msg);
+        if (c == PICO_ERROR_TIMEOUT) continue;
+
+        rx_buf.push_back(static_cast<uint8_t>(c));
+
+        int8_t msg_type = try_decode(rx_buf);
+        if (msg_type < 0) {
+            if (rx_buf.full()) rx_buf.clear();
+            continue;
+        }
+
+        rx_buf.clear();
+
+        switch (msg_type) {
+        case RequestBOOTSEL::ext_id:
+            send_message(ResponseBOOTSEL{});
             sleep_ms(100);
             reset_usb_boot(0, 0);
+            break;
         }
     }
 }