Parse beast.

beast.c

@@ -1,9 +1,121 @@
+#include <assert.h>
+#include <stdio.h>
+#include <string.h>
+
 #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;
 }

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];
 	}
 }
 

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