diff --git a/adsbus/adsbus.c b/adsbus/adsbus.c index e491e86..0ee3dbe 100644 --- a/adsbus/adsbus.c +++ b/adsbus/adsbus.c @@ -12,6 +12,7 @@ #include "opts.h" #include "outgoing.h" #include "peer.h" +#include "proto.h" #include "rand.h" #include "receive.h" #include "resolve.h" @@ -121,6 +122,7 @@ int main(int argc, char *argv[]) { peer_loop(); json_cleanup(); + proto_cleanup(); receive_cleanup(); send_cleanup(); diff --git a/adsbus/proto.c b/adsbus/proto.c index 060ac04..fdfd1b8 100644 --- a/adsbus/proto.c +++ b/adsbus/proto.c @@ -11,6 +11,16 @@ #define PROTO_MAGIC "aDsB" +struct proto_parser_state { + struct packet_mlat_state mlat_state; + uint16_t mlat_timestamp_mhz; + uint64_t mlat_timestamp_max; + uint32_t rssi_max; + bool have_header; +}; + +static Adsb *proto_prev = NULL; + static void proto_obj_to_buf(Adsb *wrapper, struct buf *buf) { assert(!buf->length); assert(adsb__get_packed_size(wrapper) <= BUF_LEN_MAX); @@ -44,26 +54,96 @@ static void proto_serialize_mode_s_long(struct packet *packet, struct buf *buf) AdsbPacket packet_out = ADSB_PACKET__INIT; proto_serialize_packet(packet, &packet_out, 14); Adsb wrapper = ADSB__INIT; - wrapper.mode_s_short = &packet_out; + wrapper.mode_s_long = &packet_out; proto_obj_to_buf(&wrapper, buf); } +static bool proto_parse_header(AdsbHeader *header, struct packet *packet, struct proto_parser_state *state) { + if (strcmp(header->magic, PROTO_MAGIC)) { + return false; + } + + state->mlat_timestamp_mhz = header->mlat_timestamp_mhz; + state->mlat_timestamp_max = header->mlat_timestamp_max; + state->rssi_max = header->rssi_max; + + if (!strcmp(header->server_id, server_id)) { + fprintf(stderr, "R %s: Attempt to receive proto data from our own server ID (%s); loop!\n", packet->source_id, server_id); + return false; + } + + fprintf(stderr, "R %s: Connected to server ID: %s\n", packet->source_id, header->server_id); + return true; +} + +static bool proto_parse_packet(AdsbPacket *in, struct packet *packet, struct proto_parser_state *state, size_t len) { + if (in->payload.len != len) { + return false; + } + + packet->source_id = in->source_id; + memcpy(packet->payload, in->payload.data, len); + + if (in->has_mlat_timestamp) { + packet->mlat_timestamp = packet_mlat_timestamp_scale_in( + in->mlat_timestamp, + state->mlat_timestamp_max, + state->mlat_timestamp_mhz, + &state->mlat_state); + } + + if (in->has_rssi) { + packet->rssi = packet_rssi_scale_in(in->rssi, state->rssi_max); + } + + return true; +} + +void proto_cleanup() { + if (proto_prev) { + adsb__free_unpacked(proto_prev, NULL); + } +} + bool proto_parse(struct buf *buf, struct packet *packet, void *state_in) { + struct proto_parser_state *state = (struct proto_parser_state *) state_in; + + if (proto_prev) { + adsb__free_unpacked(proto_prev, NULL); + proto_prev = NULL; + } + Adsb *wrapper = adsb__unpack(NULL, buf->length, (uint8_t *) buf_at(buf, 0)); if (!wrapper) { return false; } - if (!wrapper->header && - !wrapper->mode_s_short && - !wrapper->mode_s_long) { + + if (wrapper->header) { + if (!proto_parse_header(wrapper->header, packet, state)) { + adsb__free_unpacked(wrapper, NULL); + return false; + } + packet->type = PACKET_TYPE_NONE; + } else if (wrapper->mode_s_short) { + if (!proto_parse_packet(wrapper->mode_s_short, packet, state, 7)) { + adsb__free_unpacked(wrapper, NULL); + return false; + } + packet->type = PACKET_TYPE_MODE_S_SHORT; + } else if (wrapper->mode_s_long) { + if (!proto_parse_packet(wrapper->mode_s_long, packet, state, 14)) { + adsb__free_unpacked(wrapper, NULL); + return false; + } + packet->type = PACKET_TYPE_MODE_S_LONG; + } else { // "oneof" is actually "zero or oneof" adsb__free_unpacked(wrapper, NULL); return false; } + proto_prev = wrapper; buf_consume(buf, adsb__get_packed_size(wrapper)); - adsb__free_unpacked(wrapper, NULL); - packet->type = PACKET_TYPE_NONE; // XXX return true; } diff --git a/adsbus/proto.h b/adsbus/proto.h index 73020a0..31b24e2 100644 --- a/adsbus/proto.h +++ b/adsbus/proto.h @@ -5,5 +5,6 @@ struct buf; struct packet; +void proto_cleanup(); bool proto_parse(struct buf *, struct packet *, void *); void proto_serialize(struct packet *, struct buf *);