[no changelog]tychovrahe/bluetooth/master
tychovrahe
1 year ago
parent
39635fcf44
commit
ae86dc93ca
@ -0,0 +1,87 @@
|
||||
|
||||
|
||||
#include "protob_helpers.h"
|
||||
|
||||
secbool send_protob_msg(uint8_t iface_num, uint16_t msg_id,
|
||||
const pb_msgdesc_t *fields, const void *msg,
|
||||
bool (*write)(pb_ostream_t *stream,
|
||||
const pb_byte_t *buf, size_t count),
|
||||
void (*write_flush)(write_state *state)) {
|
||||
// determine message size by serializing it into a dummy stream
|
||||
pb_ostream_t sizestream = {.callback = NULL,
|
||||
.state = NULL,
|
||||
.max_size = SIZE_MAX,
|
||||
.bytes_written = 0,
|
||||
.errmsg = NULL};
|
||||
if (false == pb_encode(&sizestream, fields, msg)) {
|
||||
return secfalse;
|
||||
}
|
||||
const uint32_t msg_size = sizestream.bytes_written;
|
||||
|
||||
write_state state = {
|
||||
.iface_num = iface_num,
|
||||
.packet_index = 0,
|
||||
.packet_pos = MSG_HEADER1_LEN,
|
||||
.buf =
|
||||
{
|
||||
'?',
|
||||
'#',
|
||||
'#',
|
||||
(msg_id >> 8) & 0xFF,
|
||||
msg_id & 0xFF,
|
||||
(msg_size >> 24) & 0xFF,
|
||||
(msg_size >> 16) & 0xFF,
|
||||
(msg_size >> 8) & 0xFF,
|
||||
msg_size & 0xFF,
|
||||
},
|
||||
};
|
||||
|
||||
pb_ostream_t stream = {.callback = write,
|
||||
.state = &state,
|
||||
.max_size = SIZE_MAX,
|
||||
.bytes_written = 0,
|
||||
.errmsg = NULL};
|
||||
|
||||
if (false == pb_encode(&stream, fields, msg)) {
|
||||
return secfalse;
|
||||
}
|
||||
|
||||
write_flush(&state);
|
||||
return secfalse;
|
||||
}
|
||||
|
||||
secbool recv_protob_msg(uint8_t iface_num, uint32_t msg_size, uint8_t *buf,
|
||||
const pb_msgdesc_t *fields, void *msg,
|
||||
bool (*read)(pb_istream_t *stream, pb_byte_t *buf,
|
||||
size_t count),
|
||||
void (*read_flush)(read_state *state),
|
||||
uint16_t packet_size) {
|
||||
read_state state = {.iface_num = iface_num,
|
||||
.packet_index = 0,
|
||||
.packet_pos = MSG_HEADER1_LEN,
|
||||
.packet_size = packet_size,
|
||||
.buf = buf};
|
||||
|
||||
pb_istream_t stream = {.callback = read,
|
||||
.state = &state,
|
||||
.bytes_left = msg_size,
|
||||
.errmsg = NULL};
|
||||
|
||||
if (false == pb_decode_noinit(&stream, fields, msg)) {
|
||||
return secfalse;
|
||||
}
|
||||
|
||||
read_flush(&state);
|
||||
|
||||
return sectrue;
|
||||
}
|
||||
|
||||
secbool msg_parse_header(const uint8_t *buf, uint16_t *msg_id,
|
||||
uint32_t *msg_size) {
|
||||
if (buf[0] != '?' || buf[1] != '#' || buf[2] != '#') {
|
||||
return secfalse;
|
||||
}
|
||||
*msg_id = (buf[3] << 8) + buf[4];
|
||||
*msg_size = (buf[5] << 24) + (buf[6] << 16) + (buf[7] << 8) + buf[8];
|
||||
return sectrue;
|
||||
}
|
||||
@ -0,0 +1,87 @@
|
||||
#include <pb_decode.h>
|
||||
#include <pb_encode.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include "secbool.h"
|
||||
|
||||
#define USB_PACKET_SIZE 64
|
||||
#define MSG_HEADER1_LEN 9
|
||||
#define MSG_HEADER2_LEN 1
|
||||
|
||||
#define MSG_SEND_INIT(TYPE) TYPE msg_send = TYPE##_init_default
|
||||
#define MSG_SEND_ASSIGN_REQUIRED_VALUE(FIELD, VALUE) \
|
||||
{ msg_send.FIELD = VALUE; }
|
||||
#define MSG_SEND_ASSIGN_VALUE(FIELD, VALUE) \
|
||||
{ \
|
||||
msg_send.has_##FIELD = true; \
|
||||
msg_send.FIELD = VALUE; \
|
||||
}
|
||||
#define MSG_SEND_ASSIGN_STRING(FIELD, VALUE) \
|
||||
{ \
|
||||
msg_send.has_##FIELD = true; \
|
||||
memzero(msg_send.FIELD, sizeof(msg_send.FIELD)); \
|
||||
strncpy(msg_send.FIELD, VALUE, sizeof(msg_send.FIELD) - 1); \
|
||||
}
|
||||
#define MSG_SEND_ASSIGN_STRING_LEN(FIELD, VALUE, LEN) \
|
||||
{ \
|
||||
msg_send.has_##FIELD = true; \
|
||||
memzero(msg_send.FIELD, sizeof(msg_send.FIELD)); \
|
||||
strncpy(msg_send.FIELD, VALUE, MIN(LEN, sizeof(msg_send.FIELD) - 1)); \
|
||||
}
|
||||
#define MSG_SEND_ASSIGN_BYTES(FIELD, VALUE, LEN) \
|
||||
{ \
|
||||
msg_send.has_##FIELD = true; \
|
||||
memzero(msg_send.FIELD.bytes, sizeof(msg_send.FIELD.bytes)); \
|
||||
memcpy(msg_send.FIELD.bytes, VALUE, \
|
||||
MIN(LEN, sizeof(msg_send.FIELD.bytes))); \
|
||||
msg_send.FIELD.size = MIN(LEN, sizeof(msg_send.FIELD.bytes)); \
|
||||
}
|
||||
#define MSG_SEND_CALLBACK(FIELD, CALLBACK, ARGUMENT) \
|
||||
{ \
|
||||
msg_send.FIELD.funcs.encode = &CALLBACK; \
|
||||
msg_send.FIELD.arg = (void *)ARGUMENT; \
|
||||
}
|
||||
#define MSG_SEND(TYPE, WRITE, WRITE_FLUSH) \
|
||||
send_protob_msg(iface_num, MessageType_MessageType_##TYPE, TYPE##_fields, \
|
||||
&msg_send, WRITE, WRITE_FLUSH)
|
||||
|
||||
#define MSG_RECV_INIT(TYPE) TYPE msg_recv = TYPE##_init_default
|
||||
#define MSG_RECV_CALLBACK(FIELD, CALLBACK, ARGUMENT) \
|
||||
{ \
|
||||
msg_recv.FIELD.funcs.decode = &CALLBACK; \
|
||||
msg_recv.FIELD.arg = (void *)ARGUMENT; \
|
||||
}
|
||||
#define MSG_RECV(TYPE, READ, READ_FLUSH, PACKET_SIZE) \
|
||||
recv_protob_msg(iface_num, msg_size, buf, TYPE##_fields, &msg_recv, READ, \
|
||||
READ_FLUSH, PACKET_SIZE)
|
||||
|
||||
typedef struct {
|
||||
uint8_t iface_num;
|
||||
uint8_t packet_index;
|
||||
uint8_t packet_pos;
|
||||
uint8_t buf[USB_PACKET_SIZE];
|
||||
} write_state;
|
||||
|
||||
typedef struct {
|
||||
uint8_t iface_num;
|
||||
uint8_t packet_index;
|
||||
uint8_t packet_pos;
|
||||
uint16_t packet_size;
|
||||
uint8_t *buf;
|
||||
} read_state;
|
||||
|
||||
secbool send_protob_msg(uint8_t iface_num, uint16_t msg_id,
|
||||
const pb_msgdesc_t *fields, const void *msg,
|
||||
bool (*write_fnc)(pb_ostream_t *stream,
|
||||
const pb_byte_t *buf, size_t count),
|
||||
void (*write_flush)(write_state *state));
|
||||
|
||||
secbool recv_protob_msg(uint8_t iface_num, uint32_t msg_size, uint8_t *buf,
|
||||
const pb_msgdesc_t *fields, void *msg,
|
||||
bool (*read)(pb_istream_t *stream, pb_byte_t *buf,
|
||||
size_t count),
|
||||
void (*read_flush)(read_state *state),
|
||||
uint16_t packet_size);
|
||||
|
||||
secbool msg_parse_header(const uint8_t *buf, uint16_t *msg_id,
|
||||
uint32_t *msg_size);
|
||||
@ -1 +0,0 @@
|
||||
../trezorhal/secbool.h
|
||||
Loading…
Reference in new issue