o Splitting capture into just two, capture and send to stdout and transmit from stdin.

o Adding formatter that formats the incoming message.

6 files changed, 640 insertions, 0 deletions

diff --git a/src/capture.cpp b/src/capture.cpp
new file mode 100644
index 0000000..5844b25
--- /dev/null
+++ b/src/capture.cpp
@@ -0,0 +1,47 @@
+#include <iostream>
+#include "wifi-triangulator/core.h"
+
+using namespace std;
+using namespace wifi_triangulator;
+
+int main(int argc, char *argv[]) {
+    if (argc != 2) {
+        fprintf(stderr, "usage: %s [interface]\n", argv[0]);
+        return EXIT_FAILURE;
+    }
+
+    string dev = argv[1];
+
+    GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+    string str;
+    str.reserve(1 << 16);
+
+    int ret = launch_capture(dev, [&](const data &data) {
+        pb::envelope envelope;
+        envelope.set_time_s(data.sec);
+        envelope.set_time_us(data.usec);
+        envelope.set_type(data.type);
+
+        if (data.type == pb::packet_type::probe_request) {
+            pb::probe *probe = envelope.mutable_probe();
+            probe->set_src(data.src);
+            probe->set_dst(static_cast<uint64_t>(data.dst));
+            probe->set_rssi(data.rssi);
+        }
+
+        envelope.SerializeToString(&str);
+        cout << static_cast<uint16_t>(str.length()) << str << flush;
+
+//        static int count = 0;
+//        cerr << "count=" << count << "!\r" << flush;
+//        count++;
+    });
+
+    google::protobuf::ShutdownProtobufLibrary();
+
+    cerr << "Capture exiting" << endl;
+    cout.flush();
+
+    return ret;
+}
diff --git a/src/core.cpp b/src/core.cpp
new file mode 100644
index 0000000..8cb595f
--- /dev/null
+++ b/src/core.cpp
@@ -0,0 +1,354 @@
+#include <stdlib.h>
+#include <iostream>
+#include <pcap.h>
+#include <cinttypes>
+#include <sstream>
+#include <iomanip>
+#include "../third-party/radiotap-library/radiotap.h"
+#include "wifi-triangulator/core.h"
+
+using std::string;
+using namespace wifi_triangulator::pb;
+
+namespace wifi_triangulator {
+
+struct ieee802_11_header {
+    uint8_t frame_1;
+    uint8_t frame_2;
+    uint16_t duration_id;
+
+    eth_mac addr1, addr2, addr3;
+    uint16_t seq_ctl;
+    eth_mac addr4;
+
+    bool is_to_ds() const {
+        return (frame_2 & 0x80) > 0;
+    }
+
+    bool is_from_ds() const {
+        return (frame_2 & 0x40) > 0;
+    }
+} __attribute__ ((packed));
+
+struct radio_tap_it {
+    radio_tap_it(const uint8_t *data) : offset(0), data(data) {
+    }
+
+    const uint8_t *data;
+    int offset;
+
+    uint8_t read8u() {
+        uint8_t value = data[offset];
+        offset += 1;
+        return value;
+    }
+
+    int8_t read8s() {
+        int8_t value = data[offset];
+        offset += 1;
+        return value;
+    }
+
+    uint16_t read16u() {
+        uint16_t value = *reinterpret_cast<const uint16_t *>(&data[offset]);
+        offset += 2;
+        offset = (offset + (2 - 1)) & -2;
+        return value;
+    }
+
+    uint32_t read32u() {
+        uint32_t value = *reinterpret_cast<const uint32_t *>(&data[offset]);
+        offset += 4;
+        offset = (offset + (4 - 1)) & -4;
+        return value;
+    }
+
+    uint64_t read64u() {
+        uint64_t value = *reinterpret_cast<const uint64_t *>(&data[offset]);
+        offset += 8;
+        offset = (offset + (8 - 1)) & -8;
+        return value;
+    }
+};
+
+struct capture_context {
+    std::function<void(const data &)> data_consumer;
+};
+
+void got_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *packet) {
+//    fprintf(stdout, "%lu.%lu, caplen=%d, len=%d\n", header->ts.tv_sec, header->ts.tv_usec, header->caplen, header->len);
+
+    auto *ctx = reinterpret_cast<capture_context *>(args);
+    auto *rtaphdr = reinterpret_cast<const struct ieee80211_radiotap_header *>(packet);
+
+    if (rtaphdr->it_version != 0) {
+        return;
+    }
+
+    int present_count = 0;
+    {
+        auto *present_ptr = &rtaphdr->it_present;
+        do {
+            present_count++;
+        } while ((*present_ptr++) & 1 << IEEE80211_RADIOTAP_EXT);
+    }
+
+    radio_tap_it it(packet + 4 + 4 * present_count);
+
+    auto *present_ptr = &rtaphdr->it_present;
+    bool is_radiotap = true;
+    int rssi = 0;
+    do {
+        uint32_t present = *present_ptr;
+        bool next_is_radiotap = (present & 1 << IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE) != 0;
+        // printf("present: %08x\n", present);
+
+        present &= ~(1 << IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE);
+        present &= ~(1 << IEEE80211_RADIOTAP_VENDOR_NAMESPACE);
+        present &= ~(1 << IEEE80211_RADIOTAP_EXT);
+
+        if (is_radiotap) {
+            char buf[101];
+
+            if (present & 1 << IEEE80211_RADIOTAP_TSFT) {
+                auto tsft = it.read64u();
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_TSFT: %" PRIu64 "\n", tsft);
+                present &= ~(1 << IEEE80211_RADIOTAP_TSFT);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_FLAGS) {
+                uint8_t flags = it.read8u();
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_FLAGS: %02x\n", flags);
+                present &= ~(1 << IEEE80211_RADIOTAP_FLAGS);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_RATE) {
+                uint8_t rate = it.read8u();
+
+                double r = rate / 2;
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_RATE: %.1lf\n", r);
+                present &= ~(1 << IEEE80211_RADIOTAP_RATE);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_CHANNEL) {
+                uint16_t mhz = it.read16u();
+                uint16_t bitmap = it.read16u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_CHANNEL: %d MHz, flags=%04x\n", mhz, bitmap);
+                present &= ~(1 << IEEE80211_RADIOTAP_CHANNEL);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_FHSS) {
+                uint8_t hop_set = it.read8u();
+                uint8_t pattern = it.read8u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_FHSS: hop_set=%02x, pattern=%02x\n", hop_set, pattern);
+                present &= ~(1 << IEEE80211_RADIOTAP_FHSS);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL) {
+                int8_t antsignal_dbm = it.read8s();
+
+                rssi = antsignal_dbm;
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_DBM_ANTSIGNAL: dbm=%d\n", antsignal_dbm);
+                present &= ~(1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_DBM_ANTNOISE) {
+                int8_t antnoise_dbm = it.read8s();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_DBM_ANTNOISE: dbm=%d\n", antnoise_dbm);
+                present &= ~(1 << IEEE80211_RADIOTAP_DBM_ANTNOISE);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_LOCK_QUALITY) {
+                uint16_t lock_quality = it.read16u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_LOCK_QUALITY: lock_quality=%d\n", lock_quality);
+                present &= ~(1 << IEEE80211_RADIOTAP_LOCK_QUALITY);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_TX_ATTENUATION) {
+                uint16_t tx_attenuation = it.read16u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_TX_ATTENUATION: tx_attenuation=%d\n", tx_attenuation);
+                present &= ~(1 << IEEE80211_RADIOTAP_TX_ATTENUATION);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_DB_TX_ATTENUATION) {
+                uint16_t db_tx_attenuation = it.read16u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_DB_TX_ATTENUATION: db_tx_attenuation=%d\n", db_tx_attenuation);
+                present &= ~(1 << IEEE80211_RADIOTAP_DB_TX_ATTENUATION);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_DBM_TX_POWER) {
+                int8_t tx_power_dbm = it.read8s();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_DBM_TX_POWER: tx_power_dbm=%d\n", tx_power_dbm);
+                present &= ~(1 << IEEE80211_RADIOTAP_DBM_TX_POWER);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_ANTENNA) {
+                uint8_t antenna = it.read8u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_ANTENNA: antenna=%d\n", antenna);
+                present &= ~(1 << IEEE80211_RADIOTAP_ANTENNA);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_DB_ANTSIGNAL) {
+                uint8_t antenna_signal_db = it.read8u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_DB_ANTSIGNAL: antenna_signal_db=%d\n", antenna_signal_db);
+                present &= ~(1 << IEEE80211_RADIOTAP_DB_ANTSIGNAL);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_DB_ANTNOISE) {
+                uint8_t antenna_noise_db = it.read8u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_DB_ANTNOISE: antenna_noise_db=%d\n", antenna_noise_db);
+                present &= ~(1 << IEEE80211_RADIOTAP_DB_ANTNOISE);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_RX_FLAGS) {
+                uint16_t rx_flags = it.read16u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_RX_FLAGS: rx_flags=%d\n", rx_flags);
+                present &= ~(1 << IEEE80211_RADIOTAP_RX_FLAGS);
+            }
+            if (present & 1 << IEEE80211_RADIOTAP_TX_FLAGS) {
+                uint16_t tx_flags = it.read16u();
+
+                snprintf(buf, 100, "IEEE80211_RADIOTAP_TX_FLAGS: tx_flags=%d\n", tx_flags);
+                present &= ~(1 << IEEE80211_RADIOTAP_TX_FLAGS);
+            }
+
+            if (present > 0) {
+                snprintf(buf, 100, "Unknown fields remain: %08x\n", present);
+                break;
+            }
+
+//            printf(buf);
+            /*
+            int bit = 0;
+            uint32_t mask = 1;
+            for (int i = 0; i < 29; i++) {
+                if (present & mask) {
+                    printf("bit %d\n", bit);
+                }
+
+                bit++;
+                mask <<= 1;
+            }
+            */
+        }
+        is_radiotap = next_is_radiotap;
+    } while ((*present_ptr++) & 1 << IEEE80211_RADIOTAP_EXT);
+
+    const struct ieee802_11_header *ieee802_11_header = reinterpret_cast<const struct ieee802_11_header *>(&packet[rtaphdr->it_len]);
+
+    uint8_t type = ieee802_11_header->frame_1;
+
+    packet_type t;
+    switch (type) {
+        case 0x40:
+            t = packet_type::probe_request;
+            break;
+        case 0x80:
+            t = packet_type::beacon;
+            break;
+        case 0x50:
+            t = packet_type::probe_response;
+            break;
+        case 0x48:
+            t = packet_type::null;
+            break;
+        case 0xd4:
+            t = packet_type::ack;
+            break;
+        case 0x08:
+            t = packet_type::data;
+            break;
+        case 0xc4:
+            t = packet_type::cts;
+            break;
+        case 0xb4:
+            t = packet_type::rts;
+            break;
+        case 0x1e:
+            t = packet_type::cf_end;
+            break;
+        case 0x1f:
+            t = packet_type::cf_end_cf_ack;
+            break;
+        case 0x1a:
+            t = packet_type::ps_poll;
+            break;
+        default:
+            t = packet_type::unknown;
+    }
+
+//    printf("ieee802_11_header->frame_ctl=%02x, %s\n", type, type_str ? type_str : "???");
+
+    eth_mac src, dst;
+
+    if (!ieee802_11_header->is_from_ds()) {
+        if (!ieee802_11_header->is_to_ds()) {
+            dst = ieee802_11_header->addr1;
+            src = ieee802_11_header->addr2;
+        } else {
+            dst = ieee802_11_header->addr1;
+            src = ieee802_11_header->addr3;
+        }
+    } else {
+        if (!ieee802_11_header->is_to_ds()) {
+            dst = ieee802_11_header->addr3;
+            src = ieee802_11_header->addr2;
+        } else {
+            dst = ieee802_11_header->addr3;
+            src = ieee802_11_header->addr4;
+        }
+    }
+
+    ctx->data_consumer(data{
+            t,
+            header->ts.tv_sec, header->ts.tv_usec,
+            rssi, src, dst});
+}
+
+int launch_capture(string dev, std::function<void(const class data &)> data_consumer) {
+    char errbuf[1000];
+
+    pcap_t *handle;
+
+    handle = pcap_open_live(dev.c_str(), BUFSIZ, 1, 1000, errbuf);
+//    handle = pcap_open_offline(dev, errbuf);
+
+    if (!handle) {
+        fprintf(stderr, "Could not open %s: %s\n", dev.c_str(), errbuf);
+        return EXIT_FAILURE;
+    }
+
+    int i = pcap_datalink(handle);
+    if (i != DLT_IEEE802_11_RADIO) {
+        fprintf(stderr, "Device %s doesn't provide IEEE 802.11 radio headers\n", dev.c_str());
+        return EXIT_FAILURE;
+    }
+
+/* Compile and apply the filter */
+//    if (pcap_compile(handle, &fp, filter_exp, 0, net) == -1) {
+//        fprintf(stderr, "Couldn't parse filter %s: %s\n", filter_exp, pcap_geterr(handle));
+//        return EXIT_FAILURE;
+//    }
+//    if (pcap_setfilter(handle, &fp) == -1) {
+//        fprintf(stderr, "Couldn't install filter %s: %s\n", filter_exp, pcap_geterr(handle));
+//        return EXIT_FAILURE;
+//    }
+
+    capture_context ctx{data_consumer};
+    while (true) {
+        auto ret = pcap_loop(handle, 1000, got_packet, reinterpret_cast<u_char *>(&ctx));
+
+        if (ret == -1) {
+            printf("pcap failed: %s\n",
+                   pcap_geterr(handle)
+            );
+            break;
+        } else if (ret == -2) {
+// someone called break loop
+            break;
+        }
+    }
+
+    pcap_close(handle);
+
+    return EXIT_SUCCESS;
+}
+
+} // namespace wifi_triangulator
diff --git a/src/formatter.cpp b/src/formatter.cpp
new file mode 100644
index 0000000..b165a89
--- /dev/null
+++ b/src/formatter.cpp
@@ -0,0 +1,54 @@
+#include <iostream>
+#include "wifi-triangulator/core.h"
+
+using namespace std;
+using namespace wifi_triangulator;
+
+void on_probe_request(const pb::probe &p) {
+    cout << "PROBE REQUEST"
+         << ", src=" << eth_mac(p.src()).to_string()
+         << ", dst=" << eth_mac(p.dst()).to_string()
+         << ", rssi=" << p.rssi()
+         << flush
+         << endl;
+}
+
+void on_envelope(pb::envelope envelope) {
+    if (envelope.type() == pb::probe_request) {
+        on_probe_request(envelope.probe());
+    } else {
+        cout << to_string(envelope.type()) << endl;
+    }
+}
+
+int main(int argc, char *argv[]) {
+    GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+    if (argc != 1) {
+        fprintf(stderr, "usage: %s\n", argv[0]);
+        return EXIT_FAILURE;
+    }
+
+    int count = 0;
+    uint8_t buffer[1 << 16];
+    while (!feof(stdin)) {
+        pb::envelope envelope;
+
+        uint16_t size;
+        cin >> size;
+        cin.read(reinterpret_cast<char *>(buffer), size);
+        bool ok = envelope.ParseFromArray(buffer, size);
+        if (!ok) {
+            cerr << "bad read" << endl;
+            continue;
+        }
+
+//        cerr << "count=" << count++ << endl;
+
+        on_envelope(envelope);
+    }
+
+    google::protobuf::ShutdownProtobufLibrary();
+
+    return EXIT_SUCCESS;
+}
diff --git a/src/misc.cpp b/src/misc.cpp
new file mode 100644
index 0000000..ffe174f
--- /dev/null
+++ b/src/misc.cpp
@@ -0,0 +1,35 @@
+#include "wifi-triangulator/core.h"
+
+namespace std {
+string to_string(wifi_triangulator::pb::packet_type t) {
+    using wifi_triangulator::pb::packet_type;
+
+    switch (t) {
+        case packet_type::probe_request:
+            return "probe request";
+        case packet_type::beacon:
+            return "beacon";
+        case packet_type::probe_response:
+            return "probe response";
+        case packet_type::null:
+            return "null";
+        case packet_type::ack:
+            return "ack";
+        case packet_type::data:
+            return "data";
+        case packet_type::cts:
+            return "cts";
+        case packet_type::rts:
+            return "rts";
+        case packet_type::cf_end:
+            return "cf-end";
+        case packet_type::cf_end_cf_ack:
+            return "cf-end+cf-ack";
+        case packet_type::ps_poll:
+            return "ps-poll";
+        case packet_type::unknown:
+        default:
+            return "unknown";
+    }
+}
+} // namespace std
diff --git a/src/receiver.cpp b/src/receiver.cpp
new file mode 100644
index 0000000..c6c5bdc
--- /dev/null
+++ b/src/receiver.cpp
@@ -0,0 +1,71 @@
+#include "wifi-triangulator/core.h"
+
+#include <iostream>
+#include <unistd.h>
+
+#include <arpa/inet.h>
+#include <cstring>
+
+using namespace std;
+using namespace wifi_triangulator;
+
+void on_probe_request(const pb::probe &p) {
+    cerr << "PROBE REQUEST"
+         << ", src=" << eth_mac(p.src()).to_string()
+         << ", dst=" << eth_mac(p.dst()).to_string()
+         << ", rssi=" << p.rssi()
+         << endl;
+}
+
+int main(int argc, char *argv[]) {
+    if (argc != 1) {
+        fprintf(stderr, "usage: %s\n", argv[0]);
+        return EXIT_FAILURE;
+    }
+
+    GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+    uint16_t port = 3333;
+    int s;
+    struct sockaddr_in si_me;
+    if ((s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
+        throw std::runtime_error("socket");
+
+    memset((char *) &si_me, 0, sizeof(si_me));
+    si_me.sin_family = AF_INET;
+    si_me.sin_port = htons(port);
+    si_me.sin_addr.s_addr = htonl(INADDR_ANY);
+    if (::bind(s, reinterpret_cast<struct sockaddr *>(&si_me), sizeof(si_me)) == -1) {
+        throw std::runtime_error("bind");
+    }
+
+    pb::envelope envelope;
+    while (true) {
+        size_t len = 64 * 1024;
+        uint8_t bytes[len];
+        ssize_t n_read;
+
+        struct sockaddr_in si_other;
+        socklen_t addr_len;
+        if ((n_read = ::recvfrom(s, bytes, len, 0, reinterpret_cast<struct sockaddr *>(&si_other), &addr_len)) == -1) {
+            throw std::runtime_error("recvfrom");
+        }
+
+        string str;
+        for (socklen_t i = 0; i < n_read; i++) {
+            str += bytes[i];
+        }
+
+        istringstream ss{str};
+        bool ok = envelope.ParseFromIstream(&ss);
+        if (ok) {
+            if (envelope.type() == pb::probe_request) {
+                on_probe_request(envelope.probe());
+            }
+        } else {
+            cerr << "FAIL n_read=" << n_read << flush << endl;
+        }
+    }
+
+    google::protobuf::ShutdownProtobufLibrary();
+}
diff --git a/src/transmitter.cpp b/src/transmitter.cpp
new file mode 100644
index 0000000..047d37d
--- /dev/null
+++ b/src/transmitter.cpp
@@ -0,0 +1,79 @@
+#include <iostream>
+#include "wifi-triangulator/core.h"
+
+#include <arpa/inet.h>
+#include <cstring>
+#include <unistd.h>
+
+using namespace std;
+using namespace wifi_triangulator;
+
+class human_output {
+public:
+    void handle(const data &data) {
+        printf("timestamp=%lu, rssi=%d, src=%s, dst=%s, type=%s\n", ((data.sec * 1000 * 1000) + data.usec) * 1000,
+               data.rssi, data.src.to_string().c_str(), data.dst.to_string().c_str(), to_string(data.type).c_str());
+        fflush(stdout);
+    };
+};
+
+int main(int argc, char *argv[]) {
+    if (argc != 2) {
+        fprintf(stderr, "usage: %s [host]\n", argv[0]);
+        return EXIT_FAILURE;
+    }
+
+    string host = argv[1];
+
+    GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+    int s;
+    struct sockaddr_in addr_si;
+    struct sockaddr *addr;
+    socklen_t addr_len;
+
+    if ((s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
+        throw std::runtime_error("socket");
+    }
+
+    memset((char *) &addr_si, 0, sizeof(addr_si));
+    addr_si.sin_family = AF_INET;
+    addr_si.sin_port = htons(3333);
+    if (inet_aton(host.c_str(), &addr_si.sin_addr) == 0) {
+        throw std::runtime_error("Could not resolve " + host);
+    }
+
+    addr = reinterpret_cast<struct sockaddr *>(&addr_si);
+    addr_len = sizeof(addr_si);
+
+    int count = 0;
+    uint8_t buffer[1 << 16];
+    while (!feof(stdin)) {
+        pb::envelope envelope;
+
+        uint16_t size;
+
+        cin >> size;
+        cin.read(reinterpret_cast<char *>(buffer), size);
+
+        /*
+        bool ok = envelope.ParseFromArray(buffer, size);
+        if (!ok) {
+            cerr << "bad read" << endl;
+            continue;
+        }
+        */
+
+        if (sendto(s, buffer, size, 0, addr, addr_len) == -1) {
+            throw std::runtime_error("sendto failed");
+        }
+
+        cerr << "count=" << count++ << endl;
+    }
+
+    cerr << "transmitter exiting" << endl;
+
+    google::protobuf::ShutdownProtobufLibrary();
+
+    return EXIT_SUCCESS;
+}