From 36e59a1991f075e36e117a08321d5e4c4dc00eac Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Fri, 7 Apr 2017 19:03:05 +0200 Subject: o Splitting capture into just two, capture and send to stdout and transmit from stdin. o Adding formatter that formats the incoming message. --- CMakeLists.txt | 14 +- capture.cpp | 108 -------------- core.cpp | 364 ------------------------------------------------ misc.cpp | 35 ----- receiver.cpp | 80 ----------- src/capture.cpp | 47 +++++++ src/core.cpp | 354 ++++++++++++++++++++++++++++++++++++++++++++++ src/formatter.cpp | 54 +++++++ src/misc.cpp | 35 +++++ src/receiver.cpp | 71 ++++++++++ src/transmitter.cpp | 79 +++++++++++ wifi-triangulator.proto | 13 +- 12 files changed, 659 insertions(+), 595 deletions(-) delete mode 100644 capture.cpp delete mode 100644 core.cpp delete mode 100644 misc.cpp delete mode 100644 receiver.cpp create mode 100644 src/capture.cpp create mode 100644 src/core.cpp create mode 100644 src/formatter.cpp create mode 100644 src/misc.cpp create mode 100644 src/receiver.cpp create mode 100644 src/transmitter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6fa065d..141beda 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,7 +22,7 @@ include_directories(${Protobuf_INCLUDE_DIRS} ${Protobuf_INCLUDE_DIR}) protobuf_generate_cpp(PROTO_SRC PROTO_HEADER wifi-triangulator.proto) add_library(wifi-triangulator - core.cpp misc.cpp + src/core.cpp src/misc.cpp ${PROTO_SRC} ${PROTO_HEADER} include/wifi-triangulator/core.h @@ -34,9 +34,17 @@ target_include_directories(wifi-triangulator PUBLIC include ${CMAKE_CURRENT_BINA target_link_libraries(wifi-triangulator PUBLIC pcap ${Protobuf_LIBRARIES}) # capture -add_executable(capture capture.cpp) +add_executable(capture src/capture.cpp) target_link_libraries(capture PUBLIC wifi-triangulator) +# transmitter +add_executable(transmitter src/transmitter.cpp) +target_link_libraries(transmitter PUBLIC wifi-triangulator) + # receiver -add_executable(receiver receiver.cpp) +add_executable(receiver src/receiver.cpp) target_link_libraries(receiver PUBLIC wifi-triangulator) + +# formatter +add_executable(formatter src/formatter.cpp) +target_link_libraries(formatter PUBLIC wifi-triangulator) diff --git a/capture.cpp b/capture.cpp deleted file mode 100644 index 7e51fcd..0000000 --- a/capture.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include -#include "wifi-triangulator/core.h" -#include "wifi-triangulator.pb.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace wifi_triangulator; - -class output { -public: - virtual void handle(const data &) = 0; -}; - -class human_output : public 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); - }; -}; - -class protobuf_output : public output { - int s; - struct sockaddr_in addr_si; - struct sockaddr *addr; - socklen_t addr_len; - -public: - protobuf_output(string host, uint16_t port) { - GOOGLE_PROTOBUF_VERIFY_VERSION; - - 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(port); - if (inet_aton(host.c_str(), &addr_si.sin_addr) == 0) { - throw std::runtime_error("inet_aton() failed"); - } - - addr = reinterpret_cast(&addr_si); - addr_len = sizeof(addr_si); - } - - ~protobuf_output() { - google::protobuf::ShutdownProtobufLibrary(); - } - - void handle(const data &data) override { - pb::envelope envelope; - 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(data.dst)); - probe->set_rssi(data.rssi); - } - - std::string str; - envelope.SerializeToString(&str); - - if (sendto(s, str.c_str(), str.length(), 0, addr, addr_len) == -1) { - throw std::runtime_error("sendto failed"); - } - cout << flush; - - static int count = 0; - cerr << "count=" << count << ", len=" << str.length() << "!\r" << flush; - count++; - } -}; - -bool human = false; - -int main(int argc, char *argv[]) { - if (argc != 3) { - fprintf(stderr, "usage: %s [interface] [host]\n", argv[0]); - return EXIT_FAILURE; - } - - string dev = argv[1]; - string host = argv[2]; - - output *output; - if (human) { - output = new human_output; - } else { - cerr << "Sending to stdout" << flush << endl; - output = new protobuf_output(host, 3333); - } - - return launch_capture(dev, [&](const data &data) { - output->handle(data); - }); -} diff --git a/core.cpp b/core.cpp deleted file mode 100644 index 11a20f7..0000000 --- a/core.cpp +++ /dev/null @@ -1,364 +0,0 @@ -#include -#include -#include -#include -#include -#include -#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(&data[offset]); - offset += 2; - offset = (offset + (2 - 1)) & -2; - return value; - } - - uint32_t read32u() { - uint32_t value = *reinterpret_cast(&data[offset]); - offset += 4; - offset = (offset + (4 - 1)) & -4; - return value; - } - - uint64_t read64u() { - uint64_t value = *reinterpret_cast(&data[offset]); - offset += 8; - offset = (offset + (8 - 1)) & -8; - return value; - } -}; - -struct capture_context { - std::function 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(args); - auto *rtaphdr = reinterpret_cast(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(&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 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(&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/misc.cpp b/misc.cpp deleted file mode 100644 index ffe174f..0000000 --- a/misc.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#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/receiver.cpp b/receiver.cpp deleted file mode 100644 index c6c8f72..0000000 --- a/receiver.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "wifi-triangulator/core.h" -#include "wifi-triangulator.pb.h" - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -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(&si_me), sizeof(si_me)) == -1) { - throw std::runtime_error("bind"); - } - - sockaddr *me = reinterpret_cast(&si_me); - - 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(&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/capture.cpp b/src/capture.cpp new file mode 100644 index 0000000..5844b25 --- /dev/null +++ b/src/capture.cpp @@ -0,0 +1,47 @@ +#include +#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(data.dst)); + probe->set_rssi(data.rssi); + } + + envelope.SerializeToString(&str); + cout << static_cast(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 +#include +#include +#include +#include +#include +#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(&data[offset]); + offset += 2; + offset = (offset + (2 - 1)) & -2; + return value; + } + + uint32_t read32u() { + uint32_t value = *reinterpret_cast(&data[offset]); + offset += 4; + offset = (offset + (4 - 1)) & -4; + return value; + } + + uint64_t read64u() { + uint64_t value = *reinterpret_cast(&data[offset]); + offset += 8; + offset = (offset + (8 - 1)) & -8; + return value; + } +}; + +struct capture_context { + std::function 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(args); + auto *rtaphdr = reinterpret_cast(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(&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 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(&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 +#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(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 +#include + +#include +#include + +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(&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(&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 +#include "wifi-triangulator/core.h" + +#include +#include +#include + +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(&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(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; +} diff --git a/wifi-triangulator.proto b/wifi-triangulator.proto index 7daf9bf..959a370 100644 --- a/wifi-triangulator.proto +++ b/wifi-triangulator.proto @@ -18,12 +18,15 @@ enum packet_type { }; message probe { - sint32 rssi = 2; - fixed64 src = 3; - fixed64 dst = 4; + sint32 rssi = 1; + fixed64 src = 2; + fixed64 dst = 3; } message envelope { - packet_type type = 1; - probe probe = 2; + sfixed64 time_s = 1; + sfixed64 time_us = 2; + + packet_type type = 3; + probe probe = 20; } -- cgit v1.2.3