diff --git a/beast.c b/beast.c index 47651b4..5d21ace 100644 --- a/beast.c +++ b/beast.c @@ -1,9 +1,121 @@ +#include +#include +#include + #include "beast.h" +struct __attribute__((packed)) beast_common_overlay { + uint8_t one_a; + uint8_t type; +}; + +struct __attribute__((packed)) beast_mode_s_short_overlay { + struct beast_common_overlay common; + uint8_t mlat_timestamp[6]; + uint8_t rssi; + uint8_t payload[7]; +}; + +struct __attribute__((packed)) beast_mode_s_long_overlay { + struct beast_common_overlay common; + uint8_t mlat_timestamp[6]; + uint8_t rssi; + uint8_t payload[14]; +}; + +struct beast_parser_state { + struct mlat_state mlat_state; +}; + void beast_init() { + assert(sizeof(struct beast_parser_state) <= PARSER_STATE_LEN); +} + +static uint64_t beast_parse_mlat(uint8_t *mlat_timestamp) { + return ( + ((uint64_t) mlat_timestamp[0]) << 40 | + ((uint64_t) mlat_timestamp[1]) << 32 | + ((uint64_t) mlat_timestamp[2]) << 24 | + ((uint64_t) mlat_timestamp[3]) << 16 | + ((uint64_t) mlat_timestamp[4]) << 8 | + ((uint64_t) mlat_timestamp[5])); + // mlat_timestamp_scale_in(source_mlat, UINT64_C(0xffffffffffff), 12, &state->mlat_state); +} + +ssize_t beast_unescape(struct buf *out, struct buf *in, size_t out_bytes) { + int o = 0, i = 0; + for (; i < in->length && o < out_bytes; i++, o++) { + if (i > 0 && buf_chr(in, i) == 0x1a) { + if (i == in->length - 1 || buf_chr(in, i + 1) != 0x1a) { + return -1; + } + i++; + } + buf_chr(out, o) = buf_chr(in, i); + } + if (o == out_bytes) { + return i; + } else { + return -1; + } +} + +static bool beast_parse_mode_ac(struct buf *buf, struct packet *packet, struct beast_parser_state *state) { + return false; +} + +static bool beast_parse_mode_s_short(struct buf *buf, struct packet *packet, struct beast_parser_state *state) { + struct buf buf2 = BUF_INIT; + ssize_t in_bytes = beast_unescape(&buf2, buf, sizeof(struct beast_mode_s_short_overlay)); + if (in_bytes < 0) { + return false; + } + struct beast_mode_s_short_overlay *overlay = (struct beast_mode_s_short_overlay *) buf_at(&buf2, 0); + packet->type = MODE_S_SHORT; + uint64_t source_mlat = beast_parse_mlat(overlay->mlat_timestamp); + packet->mlat_timestamp = mlat_timestamp_scale_in(source_mlat, UINT64_C(0xffffffffffff), 12, &state->mlat_state); + packet->rssi = rssi_scale_in(overlay->rssi, UINT8_MAX); + memcpy(packet->payload, overlay->payload, sizeof(overlay->payload)); + buf_consume(buf, in_bytes); + return true; +} + +static bool beast_parse_mode_s_long(struct buf *buf, struct packet *packet, struct beast_parser_state *state) { + struct buf buf2 = BUF_INIT; + ssize_t in_bytes = beast_unescape(&buf2, buf, sizeof(struct beast_mode_s_long_overlay)); + if (in_bytes < 0) { + return false; + } + struct beast_mode_s_long_overlay *overlay = (struct beast_mode_s_long_overlay *) buf_at(&buf2, 0); + packet->type = MODE_S_LONG; + uint64_t source_mlat = beast_parse_mlat(overlay->mlat_timestamp); + packet->mlat_timestamp = mlat_timestamp_scale_in(source_mlat, UINT64_C(0xffffffffffff), 12, &state->mlat_state); + packet->rssi = rssi_scale_in(overlay->rssi, UINT8_MAX); + memcpy(packet->payload, overlay->payload, sizeof(overlay->payload)); + buf_consume(buf, in_bytes); + return true; } bool beast_parse(struct backend *backend, struct packet *packet) { + struct buf *buf = &backend->buf; + struct beast_parser_state *state = (struct beast_parser_state *) backend->parser_state; + + if (buf->length < sizeof(struct beast_common_overlay) || + buf_chr(buf, 0) != 0x1a) { + return false; + } + + struct beast_common_overlay *overlay = (struct beast_common_overlay *) buf_at(buf, 0); + switch (overlay->type) { + case 0x31: + return beast_parse_mode_ac(buf, packet, state); + + case 0x32: + return beast_parse_mode_s_short(buf, packet, state); + + case 0x33: + return beast_parse_mode_s_long(buf, packet, state); + } return false; } diff --git a/common.c b/common.c index 798e8b0..fc2be3e 100644 --- a/common.c +++ b/common.c @@ -80,7 +80,7 @@ void hex_init() { } } -void hex_to_bin(char *out, char *in, size_t bytes) { +void hex_to_bin(uint8_t *out, char *in, size_t bytes) { uint8_t *in2 = (uint8_t *) in; for (size_t i = 0, j = 0; i < bytes; i++, j += 2) { out[i] = (hex_table[in2[j]] << 4) | hex_table[in2[j + 1]]; @@ -98,11 +98,10 @@ uint64_t hex_to_int(char *in, size_t bytes) { return ret; } -void hex_from_bin(char *out, char *in, size_t bytes) { - uint8_t *in2 = (uint8_t *) in; +void hex_from_bin(char *out, uint8_t *in, size_t bytes) { for (size_t i = 0, j = 0; i < bytes; i++, j += 2) { - out[j] = hex_char_table[in2[i] >> 4]; - out[j + 1] = hex_char_table[in2[i] & 0xf]; + out[j] = hex_char_table[in[i] >> 4]; + out[j + 1] = hex_char_table[in[i] & 0xf]; } } diff --git a/common.h b/common.h index c12ea7c..f5f0f39 100644 --- a/common.h +++ b/common.h @@ -23,6 +23,10 @@ struct buf { size_t start; size_t length; }; +#define BUF_INIT { \ + .start = 0, \ + .length = 0, \ +} #define buf_chr(buff, at) ((buff)->buf[(buff)->start + (at)]) #define buf_at(buff, at) (&buf_chr(buff, at)) @@ -41,7 +45,7 @@ struct packet { MODE_S_SHORT, MODE_S_LONG, } type; - char payload[DATA_LEN_MAX]; + uint8_t payload[DATA_LEN_MAX]; uint64_t mlat_timestamp; uint32_t rssi; }; @@ -69,9 +73,9 @@ uint32_t rssi_scale_in(uint32_t, uint32_t); //////// hex void hex_init(); -void hex_to_bin(char *, char *, size_t); +void hex_to_bin(uint8_t *, char *, size_t); uint64_t hex_to_int(char *, size_t); -void hex_from_bin(char *, char *, size_t); +void hex_from_bin(char *, uint8_t *, size_t); ///////// uuid