1
0
mirror of https://github.com/trezor/trezor-firmware.git synced 2024-12-23 06:48:16 +00:00
trezor-firmware/micropython/bootloader/protobuf.c
2017-05-02 19:49:56 +02:00

97 lines
2.3 KiB
C

#include <string.h>
#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->pos = 9;
ctx->len = 0;
}
void pb_end(PB_CTX *ctx)
{
ctx->buf[5] = (ctx->len >> 24) & 0xFF;
ctx->buf[6] = (ctx->len >> 16) & 0xFF;
ctx->buf[7] = (ctx->len >> 8) & 0xFF;
ctx->buf[8] = ctx->len & 0xFF;
// align to 64 bytes
ctx->pos += (-ctx->pos) & 63;
}
inline static void pb_append(PB_CTX *ctx, uint8_t b)
{
ctx->buf[ctx->pos] = b;
ctx->pos++;
if (ctx->pos % 64 == 0) {
ctx->buf[ctx->pos] = '?';
ctx->pos++;
}
ctx->len++;
}
static void pb_varint(PB_CTX *ctx, uint32_t val)
{
for (;;) {
if (val < 0x80) {
pb_append(ctx, val & 0x7F);
break;
} else {
pb_append(ctx, (val & 0x7F) | 0x80);
val >>= 7;
}
}
}
void pb_add_bool(PB_CTX *ctx, uint32_t field_number, bool val)
{
pb_add_varint(ctx, field_number, val);
}
void pb_add_bytes(PB_CTX *ctx, uint32_t field_number, const uint8_t *val, uint32_t len)
{
field_number = (field_number << 3) | 2;
pb_varint(ctx, field_number);
pb_varint(ctx, len);
for (uint32_t i = 0; i < len; i++) {
pb_append(ctx, val[i]);
}
}
void pb_add_string(PB_CTX *ctx, uint32_t field_number, const char *val)
{
pb_add_bytes(ctx, field_number, (const uint8_t *)val, strlen(val));
}
void pb_add_varint(PB_CTX *ctx, uint32_t field_number, uint32_t val)
{
field_number = (field_number << 3) | 0;
pb_varint(ctx, field_number);
pb_varint(ctx, val);
}
bool pb_parse_header(const uint8_t *buf, uint16_t *msg_id, uint32_t *msg_size)
{
if (buf[0] != '?' || buf[1] != '#' || buf[2] != '#') {
return false;
}
*msg_id = (buf[3] << 8) + buf[4];
*msg_size = (buf[5] << 24) + (buf[6] << 16) + (buf[7] << 8) + buf[8];
return true;
}
uint32_t pb_read_varint(const uint8_t *buf, uint32_t *num)
{
uint32_t offset = 0;
*num = 0;
do {
*num += ((*(buf + offset)) & 0x7F) << (7 * offset);
offset++;
} while ((*(buf + offset)) & 0x80);
return offset;
}