From 44cb3fb122afb75f6f05a0df3875218b605fe52f Mon Sep 17 00:00:00 2001 From: Ian Gulliver Date: Wed, 17 Feb 2016 18:01:39 -0800 Subject: [PATCH] Beast send support. --- beast.c | 81 ++++++++++++++++++++++++++++++++++++++++++++++++++++---- beast.h | 1 + common.c | 20 ++++++++++++-- common.h | 4 ++- json.c | 3 --- send.c | 9 ++++++- 6 files changed, 106 insertions(+), 12 deletions(-) diff --git a/beast.c b/beast.c index 57f43be..47ab91d 100644 --- a/beast.c +++ b/beast.c @@ -40,6 +40,15 @@ static uint64_t beast_parse_mlat(uint8_t *mlat_timestamp) { ((uint64_t) mlat_timestamp[5])); } +static void beast_write_mlat(uint64_t timestamp, uint8_t *mlat_timestamp) { + mlat_timestamp[0] = (timestamp >> 40) & 0xff; + mlat_timestamp[1] = (timestamp >> 32) & 0xff; + mlat_timestamp[2] = (timestamp >> 24) & 0xff; + mlat_timestamp[3] = (timestamp >> 16) & 0xff; + mlat_timestamp[4] = (timestamp >> 8) & 0xff; + mlat_timestamp[5] = (timestamp) & 0xff; +} + static ssize_t beast_unescape(struct buf *out, const struct buf *in, size_t out_bytes) { int o = 0, i = 0; for (; i < in->length && o < out_bytes; i++, o++) { @@ -58,8 +67,13 @@ static ssize_t beast_unescape(struct buf *out, const struct buf *in, size_t out_ } } -static bool beast_parse_mode_ac(struct buf *buf, struct packet *packet, struct beast_parser_state *state) { - return false; +static void beast_escape(struct buf *out, const struct buf *in) { + for (int i = 0; i < in->length; i++, out->length++) { + buf_chr(out, out->length) = buf_chr(in, i); + if (i > 0 && buf_chr(in, i) == 0x1a) { + buf_chr(out, ++(out->length)) = 0x1a; + } + } } static bool beast_parse_mode_s_short(struct buf *buf, struct packet *packet, struct beast_parser_state *state) { @@ -110,14 +124,71 @@ bool beast_parse(struct buf *buf, struct packet *packet, void *state_in) { 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); + + default: + fprintf(stderr, "unknown beast type %x\n", overlay->type); + return false; } return false; } + +void beast_serialize_mode_s_short(struct packet *packet, struct buf *buf) { + struct buf buf2 = BUF_INIT; + struct beast_mode_s_short_overlay *overlay = (struct beast_mode_s_short_overlay *) buf_at(&buf2, 0); + overlay->common.one_a = 0x1a; + overlay->common.type = 0x32; + memcpy(overlay->payload, packet->payload, sizeof(overlay->payload)); + beast_write_mlat( + mlat_timestamp_scale_out(packet->mlat_timestamp, UINT64_C(0xffffffffffff), 12), + overlay->mlat_timestamp); + + if (packet->rssi) { + overlay->rssi = rssi_scale_out(packet->rssi, UINT8_MAX); + } else { + overlay->rssi = UINT8_MAX; + } + + buf2.length = sizeof(*overlay); + beast_escape(buf, &buf2); +} + +void beast_serialize_mode_s_long(struct packet *packet, struct buf *buf) { + struct buf buf2 = BUF_INIT; + struct beast_mode_s_long_overlay *overlay = (struct beast_mode_s_long_overlay *) buf_at(&buf2, 0); + overlay->common.one_a = 0x1a; + overlay->common.type = 0x33; + memcpy(overlay->payload, packet->payload, sizeof(overlay->payload)); + beast_write_mlat( + mlat_timestamp_scale_out(packet->mlat_timestamp, UINT64_C(0xffffffffffff), 12), + overlay->mlat_timestamp); + + if (packet->rssi) { + overlay->rssi = rssi_scale_out(packet->rssi, UINT8_MAX); + } else { + overlay->rssi = UINT8_MAX; + } + + buf2.length = sizeof(*overlay); + beast_escape(buf, &buf2); +} + +void beast_serialize(struct packet *packet, struct buf *buf) { + if (!packet) { + return; + } + + switch (packet->type) { + case MODE_S_SHORT: + beast_serialize_mode_s_short(packet, buf); + break; + + case MODE_S_LONG: + beast_serialize_mode_s_long(packet, buf); + break; + } +} diff --git a/beast.h b/beast.h index 244ee8c..832385b 100644 --- a/beast.h +++ b/beast.h @@ -7,3 +7,4 @@ struct packet; void beast_init(); bool beast_parse(struct buf *, struct packet *, void *); +void beast_serialize(struct packet *, struct buf *); diff --git a/common.c b/common.c index 4092458..d9b9692 100644 --- a/common.c +++ b/common.c @@ -83,11 +83,11 @@ char *packet_type_names[] = { }; -uint64_t mlat_timestamp_scale_mhz_in(uint64_t timestamp, uint32_t mhz) { +static uint64_t mlat_timestamp_scale_mhz_in(uint64_t timestamp, uint32_t mhz) { return timestamp * (MLAT_MHZ / mhz); } -uint64_t mlat_timestamp_scale_width_in(uint64_t timestamp, uint64_t max, struct mlat_state *state) { +static uint64_t mlat_timestamp_scale_width_in(uint64_t timestamp, uint64_t max, struct mlat_state *state) { if (timestamp < state->timestamp_last) { // Counter reset state->timestamp_generation += max; @@ -101,11 +101,27 @@ uint64_t mlat_timestamp_scale_in(uint64_t timestamp, uint64_t max, uint16_t mhz, return mlat_timestamp_scale_mhz_in(mlat_timestamp_scale_width_in(timestamp, max, state), mhz); } +static uint64_t mlat_timestamp_scale_mhz_out(uint64_t timestamp, uint64_t mhz) { + return timestamp / (MLAT_MHZ / mhz); +} + +static uint64_t mlat_timestamp_scale_width_out(uint64_t timestamp, uint64_t max) { + return timestamp % max; +} + +uint64_t mlat_timestamp_scale_out(uint64_t timestamp, uint64_t max, uint16_t mhz) { + return mlat_timestamp_scale_width_out(mlat_timestamp_scale_mhz_out(timestamp, mhz), max); +} + uint32_t rssi_scale_in(uint32_t value, uint32_t max) { return value * (RSSI_MAX / max); } +uint32_t rssi_scale_out(uint32_t value, uint32_t max) { + return value / (RSSI_MAX / max); +} + static uint8_t hex_table[256] = {0}; static char hex_char_table[16] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', }; diff --git a/common.h b/common.h index c70085e..a189b7e 100644 --- a/common.h +++ b/common.h @@ -48,8 +48,8 @@ struct packet { enum { MODE_S_SHORT, MODE_S_LONG, - NUM_TYPES, } type; + #define NUM_TYPES 2 uint8_t payload[DATA_LEN_MAX]; uint64_t mlat_timestamp; uint32_t rssi; @@ -70,11 +70,13 @@ struct mlat_state { }; uint64_t mlat_timestamp_scale_in(uint64_t, uint64_t, uint16_t, struct mlat_state *); +uint64_t mlat_timestamp_scale_out(uint64_t, uint64_t, uint16_t); //////// rssi uint32_t rssi_scale_in(uint32_t, uint32_t); +uint32_t rssi_scale_out(uint32_t, uint32_t); //////// hex diff --git a/json.c b/json.c index 4918634..9758bd5 100644 --- a/json.c +++ b/json.c @@ -67,9 +67,6 @@ void json_serialize(struct packet *packet, struct buf *buf) { case MODE_S_LONG: json_serialize_mode_s_long(packet, buf); break; - - case NUM_TYPES: - break; } } diff --git a/send.c b/send.c index 52bf204..4f98557 100644 --- a/send.c +++ b/send.c @@ -8,9 +8,12 @@ #include #include "common.h" + +#include "send.h" + +#include "beast.h" #include "json.h" #include "stats.h" -#include "send.h" struct send { struct peer peer; @@ -26,6 +29,10 @@ struct serializer { serializer serialize; struct send *send_head; } serializers[] = { + { + .name = "beast", + .serialize = beast_serialize, + }, { .name = "json", .serialize = json_serialize,