99 lines
2.5 KiB
C++
99 lines
2.5 KiB
C++
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <vector>
|
|
#include "pico/stdlib.h"
|
|
#include "pico/bootrom.h"
|
|
#include "msgpackpp.h"
|
|
#include "halfsiphash.h"
|
|
#include "static_vector.h"
|
|
|
|
static constexpr uint8_t hash_key[8] = {};
|
|
|
|
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 {
|
|
static constexpr int8_t ext_id = 0;
|
|
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) {
|
|
uint32_t checksum = halfsiphash::hash32(payload.data(), payload.size(), hash_key);
|
|
msgpackpp::packer p;
|
|
p.pack(Envelope{checksum, payload});
|
|
return p.get_payload();
|
|
}
|
|
|
|
static void send_bytes(const std::vector<uint8_t> &data) {
|
|
for (auto b : data) {
|
|
putchar(b);
|
|
}
|
|
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 == 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;
|
|
}
|
|
}
|
|
}
|