From fffe8abd0201500f0d9a7e94bfee8000286c98be Mon Sep 17 00:00:00 2001 From: Pavol Rusnak Date: Sat, 15 Apr 2017 17:44:19 +0200 Subject: [PATCH] bootloader: rework message creation --- Makefile.bootloader | 1 + micropython/bootloader/main.c | 21 ++++---- micropython/bootloader/messages.c | 90 ++++++++++++++++--------------- micropython/bootloader/messages.h | 10 ++-- micropython/bootloader/protobuf.c | 60 +++++++++++++++++++++ micropython/bootloader/protobuf.h | 19 +++++++ micropython/firmware/main.c | 2 +- 7 files changed, 145 insertions(+), 58 deletions(-) create mode 100644 micropython/bootloader/protobuf.c create mode 100644 micropython/bootloader/protobuf.h diff --git a/Makefile.bootloader b/Makefile.bootloader index 6486b47caa..817570613d 100644 --- a/Makefile.bootloader +++ b/Makefile.bootloader @@ -59,6 +59,7 @@ OBJ_FW += $(addprefix $(BUILD_FW)/, \ bootloader/header.o \ bootloader/main.o \ bootloader/messages.o \ + bootloader/protobuf.o \ extmod/modtrezorui/display.o \ extmod/modtrezorui/inflate.o \ extmod/modtrezorui/font_bitmap.o \ diff --git a/micropython/bootloader/main.c b/micropython/bootloader/main.c index dd346fd7db..c06651a189 100644 --- a/micropython/bootloader/main.c +++ b/micropython/bootloader/main.c @@ -174,27 +174,23 @@ void mainloop(void) switch (msg_id) { case 0: // Initialize DPRINTLN("received Initialize"); - send_msg_features(iface, false); + send_msg_Features(iface, false); break; case 1: // Ping DPRINTLN("received Ping"); - send_msg_success(iface); + send_msg_Success(iface); break; case 6: // FirmwareErase DPRINTLN("received FirmwareErase"); - send_msg_failure(iface); + send_msg_Failure(iface); break; case 7: // FirmwareUpload DPRINTLN("received FirmwareUpload"); - send_msg_failure(iface); - break; - case 27: // ButtonAck - DPRINTLN("received ButtonAck"); - send_msg_failure(iface); + send_msg_Failure(iface); break; default: DPRINTLN("received garbage"); - send_msg_failure(iface); + send_msg_Failure(iface); break; } } @@ -229,3 +225,10 @@ int main(void) return 0; } + +#ifndef NDEBUG +void __assert_func(const char *file, int line, const char *func, const char *expr) { + // printf("Assertion '%s' failed, at file %s:%d\n", expr, file, line); + __fatal_error("Assertion failed"); +} +#endif diff --git a/micropython/bootloader/messages.c b/micropython/bootloader/messages.c index 70023c0bd1..94bb2a8441 100644 --- a/micropython/bootloader/messages.c +++ b/micropython/bootloader/messages.c @@ -1,56 +1,58 @@ +#include +#include + #include "usb.h" #include "version.h" +#include "protobuf.h" #include "messages.h" -void send_msg_success(int iface) +void send_msg_Success(int iface) { - // send response: Success message (id 2), payload len 0 - usb_hid_write_blocking(iface, (const uint8_t *) - "?##" // header - "\x00\x02" // msg_id - "\x00\x00\x00\x00" // payload_len - "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" - , 64, 1); + // response: Success message (id 2), payload len 0 + PB_CTX ctx; + pb_start(&ctx, 2); + usb_hid_write_blocking(iface, pb_build(&ctx), 64, 1); } -void send_msg_failure(int iface) +void send_msg_Failure(int iface) { - // send response: Failure message (id 3), payload len 2 - // code = 99 (Failure_FirmwareError) - usb_hid_write_blocking(iface, (const uint8_t *) - "?##" // header - "\x00\x03" // msg_id - "\x00\x00\x00\x02" // payload_len - "\x08\x63" // data - "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" - , 64, 1); + // response: Failure message (id 3), payload len 2 + // - code = 99 (Failure_FirmwareError) + PB_CTX ctx; + pb_start(&ctx, 3); + pb_add_varint(&ctx, "\x08", 99); + usb_hid_write_blocking(iface, pb_build(&ctx), 64, 1); } -void send_msg_features(int iface, bool firmware_present) +void send_msg_Features(int iface, bool firmware_present) { - // send response: Features message (id 17), payload len 22 - // vendor = "trezor.io" - // major_version = VERSION_MAJOR - // minor_version = VERSION_MINOR - // patch_version = VERSION_PATCH - // bootloader_mode = True - // firmware_present = True/False - if (firmware_present) { - usb_hid_write_blocking(iface, (const uint8_t *) - "?##" // header - "\x00\x11" // msg_id - "\x00\x00\x00\x16" // payload_len - "\x0a\x09" "trezor.io\x10" VERSION_MAJOR_CHAR "\x18" VERSION_MINOR_CHAR " " VERSION_PATCH_CHAR "(\x01" // data - "\x90\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" - , 64, 1); - } else { - usb_hid_write_blocking(iface, (const uint8_t *) - "?##" // header - "\x00\x11" // msg_id - "\x00\x00\x00\x16" // payload_len - "\x0a\x09" "trezor.io\x10" VERSION_MAJOR_CHAR "\x18" VERSION_MINOR_CHAR " " VERSION_PATCH_CHAR "(\x01" // data - "\x90\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" - , 64, 1); - } -} \ No newline at end of file + // response: Features message (id 17), payload len 22 + // - vendor = "trezor.io" + // - major_version = VERSION_MAJOR + // - minor_version = VERSION_MINOR + // - patch_version = VERSION_PATCH + // - bootloader_mode = True + // - firmware_present = True/False + PB_CTX ctx; + pb_start(&ctx, 17); + pb_add_string(&ctx, "\x0a", "trezor.io"); + pb_add_varint(&ctx, "\x10", VERSION_MAJOR); + pb_add_varint(&ctx, "\x18", VERSION_MINOR); + pb_add_varint(&ctx, "\x20", VERSION_PATCH); + pb_add_bool(&ctx, "\x28", true); + pb_add_bool(&ctx, "\x90\x01", firmware_present); + usb_hid_write_blocking(iface, pb_build(&ctx), 64, 1); +} + +void send_msg_FirmwareRequest(int iface, uint32_t offset, uint32_t length) +{ + // response: FirmwareRequest message (id 8), payload len X + // - offset = offset + // - length = length + PB_CTX ctx; + pb_start(&ctx, 8); + pb_add_varint(&ctx, "\x08", offset); + pb_add_varint(&ctx, "\x10", length); + usb_hid_write_blocking(iface, pb_build(&ctx), 64, 1); +} diff --git a/micropython/bootloader/messages.h b/micropython/bootloader/messages.h index b680f1f696..6e2cb3f9c7 100644 --- a/micropython/bootloader/messages.h +++ b/micropython/bootloader/messages.h @@ -2,9 +2,11 @@ #define __MESSAGES_H__ #include +#include -void send_msg_success(int iface); -void send_msg_failure(int iface); -void send_msg_features(int iface, bool firmware_present); +void send_msg_Success(int iface); +void send_msg_Failure(int iface); +void send_msg_Features(int iface, bool firmware_present); +void send_msg_FirmwareRequest(int iface, uint32_t offset, uint32_t length); -#endif \ No newline at end of file +#endif diff --git a/micropython/bootloader/protobuf.c b/micropython/bootloader/protobuf.c new file mode 100644 index 0000000000..8304d23d73 --- /dev/null +++ b/micropython/bootloader/protobuf.c @@ -0,0 +1,60 @@ +#include +#include "protobuf.h" + +void pb_start(PB_CTX *ctx, uint16_t msg_id) +{ + memset(ctx->buf, 0, sizeof(ctx->buf)); + ctx->buf[0] = '?'; + ctx->buf[1] = '#'; + ctx->buf[2] = '#'; + ctx->buf[3] = (msg_id >> 8) & 0xFF; + ctx->buf[4] = msg_id & 0xFF; + ctx->size = 9; +} + +const uint8_t *pb_build(PB_CTX *ctx) +{ + ctx->buf[5] = (ctx->size >> 24) & 0xFF; + ctx->buf[6] = (ctx->size >> 16) & 0xFF; + ctx->buf[7] = (ctx->size >> 8) & 0xFF; + ctx->buf[8] = ctx->size & 0xFF; + return ctx->buf; +} + +static void pb_add_id(PB_CTX *ctx, const char *id) +{ + size_t len = strlen(id); + memcpy(ctx->buf + ctx->size, id, len); + ctx->buf[ctx->size] += len; +} + +void pb_add_bool(PB_CTX *ctx, const char *id, bool val) +{ + pb_add_id(ctx, id); + ctx->buf[ctx->size] = val; + ctx->size++; +} + +void pb_add_string(PB_CTX *ctx, const char *id, const char *val) +{ + pb_add_varint(ctx, id, strlen(val)); + size_t len = strlen(val); + memcpy(ctx->buf + ctx->size, val, len); + ctx->buf[ctx->size] += len; +} + +void pb_add_varint(PB_CTX *ctx, const char *id, uint32_t val) +{ + pb_add_id(ctx, id); + for (;;) { + if (val < 0x80) { + ctx->buf[ctx->size] = val; + ctx->size++; + break; + } else { + ctx->buf[ctx->size] = (val & 0x7F) | 0x80; + ctx->size++; + val >>= 7; + } + } +} diff --git a/micropython/bootloader/protobuf.h b/micropython/bootloader/protobuf.h new file mode 100644 index 0000000000..2f961f9ef4 --- /dev/null +++ b/micropython/bootloader/protobuf.h @@ -0,0 +1,19 @@ +#ifndef __PROTOBUF_H__ +#define __PROTOBUF_H__ + +#include +#include + +typedef struct { + uint8_t buf[64]; + uint32_t size; +} PB_CTX; + +void pb_start(PB_CTX *ctx, uint16_t msg_id); +const uint8_t *pb_build(PB_CTX *ctx); + +void pb_add_bool(PB_CTX *ctx, const char *id, bool val); +void pb_add_string(PB_CTX *ctx, const char *id, const char *val); +void pb_add_varint(PB_CTX *ctx, const char *id, uint32_t val); + +#endif diff --git a/micropython/firmware/main.c b/micropython/firmware/main.c index 5b2ad6c4e8..ce3a9f43c1 100644 --- a/micropython/firmware/main.c +++ b/micropython/firmware/main.c @@ -161,7 +161,7 @@ int main(void) { } #ifndef NDEBUG -void MP_WEAK __assert_func(const char *file, int line, const char *func, const char *expr) { +void __assert_func(const char *file, int line, const char *func, const char *expr) { printf("Assertion '%s' failed, at file %s:%d\n", expr, file, line); __fatal_error("Assertion failed"); }