diff options
| -rw-r--r-- | .gitignore | 3 | ||||
| -rw-r--r-- | .gitmodules | 3 | ||||
| -rw-r--r-- | CMakeLists.txt | 13 | ||||
| -rw-r--r-- | main.cpp | 369 | ||||
| m--------- | third-party/radiotap-library | 0 |
5 files changed, 388 insertions, 0 deletions
diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3e07682 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build +cmake-build-* +.idea diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..6f93fa6 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "third-party/radiotap-library"] + path = third-party/radiotap-library + url = https://github.com/radiotap/radiotap-library diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..9712cca --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.6) +project(wifi_triangulator) + +set(CMAKE_CXX_STANDARD 14) + +set(SOURCE_FILES main.cpp + third-party/radiotap-library/radiotap.c + third-party/radiotap-library/radiotap.h + third-party/radiotap-library/radiotap_iter.h + third-party/radiotap-library/platform.h) +add_executable(wifi_triangulator ${SOURCE_FILES}) + +target_link_libraries(wifi_triangulator PUBLIC pcap) diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..eac6aeb --- /dev/null +++ b/main.cpp @@ -0,0 +1,369 @@ +#include <stdlib.h> +#include <iostream> +#include <pcap.h> +#include <cinttypes> +#include <sstream> +#include <iomanip> +#include "third-party/radiotap-library/radiotap.h" + +struct eth_mac { + uint8_t a, b, c, d, e, f; + + const std::string to_string() { + std::stringstream s; + s << std::hex << std::setw(2) << std::setfill('0') + << (int) a << ":" + << std::setw(2) << std::setfill('0') << (int) b << ":" + << std::setw(2) << std::setfill('0') << (int) c << ":" + << std::setw(2) << std::setfill('0') << (int) d << ":" + << std::setw(2) << std::setfill('0') << (int) e << ":" + << std::setw(2) << std::setfill('0') << (int) f; + return s.str(); + } +}; + +class data { +public: + time_t sec; + suseconds_t usec; + + int rssi; + struct eth_mac src, dst; + + data() : sec(0), usec(0), src(), rssi(), dst() { + } +}; + +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; + } +}; + +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); + + struct ieee80211_radiotap_header *rtaphdr = (struct ieee80211_radiotap_header *) packet; + + class data data; + data.sec = header->ts.tv_sec; + data.usec = header->ts.tv_usec; + + if (rtaphdr->it_version != 0) { + return; + } + + int present_count = 0; + { + uint32_t *present_ptr = &rtaphdr->it_present; + do { + present_count++; + } while ((*present_ptr++) & 1 << IEEE80211_RADIOTAP_EXT); + } + + radio_tap_it it(packet + 4 + 4 * present_count); + + uint32_t *present_ptr = &rtaphdr->it_present; + bool is_radiotap = true; + 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(); + + data.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; + + const char *type_str; + switch (type) { + case 0x40: + type_str = "probe request"; + break; + case 0x80: + type_str = "beacon"; + break; + case 0x50: + type_str = "probe response"; + break; + case 0x48: + type_str = "null"; + break; + case 0xd4: + type_str = "ack"; + break; + case 0x08: + type_str = "data"; + break; + case 0xc4: + type_str = "cts"; + break; + case 0xb4: + type_str = "rts"; + break; + case 0x1e: + type_str = "cf-end"; + break; + case 0x1f: + type_str = "cf-end+cf-ack"; + break; + case 0x1a: + type_str = "ps-poll"; + break; + default: + type_str = nullptr; + } + +// printf("ieee802_11_header->frame_ctl=%02x, %s\n", type, type_str ? type_str : "???"); + + if (!ieee802_11_header->is_from_ds()) { + if (!ieee802_11_header->is_to_ds()) { + data.dst = ieee802_11_header->addr1; + data.src = ieee802_11_header->addr2; + } else { + data.dst = ieee802_11_header->addr1; + data.src = ieee802_11_header->addr3; + } + } else { + if (!ieee802_11_header->is_to_ds()) { + data.dst = ieee802_11_header->addr3; + data.src = ieee802_11_header->addr2; + } else { + data.dst = ieee802_11_header->addr3; + data.src = ieee802_11_header->addr4; + } + } + + auto src = data.src.to_string(); + auto dst = data.dst.to_string(); + printf("data: %lu.%lu, rssi=%d, src=%s, dst=%s\n", data.sec, data.usec, data.rssi, src.c_str(), dst.c_str()); + + fflush(stdout); +} + +int main(int argc, char *argv[]) { + char *dev = argv[1]; + char errbuf[1000]; + + pcap_t *handle; + + handle = pcap_open_live(dev, BUFSIZ, 1, 1000, errbuf); +// handle = pcap_open_offline(dev, errbuf); + + if (!handle) { + fprintf(stderr, "Could not open %s: %s\n", dev, 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); + return (2); + } + + /* 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(2); +// } +// if (pcap_setfilter(handle, &fp) == -1) { +// fprintf(stderr, "Couldn't install filter %s: %s\n", filter_exp, pcap_geterr(handle)); +// return(2); +// } + + while (true) { + auto ret = pcap_loop(handle, 1000, got_packet, nullptr); + + 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 (0); +} diff --git a/third-party/radiotap-library b/third-party/radiotap-library new file mode 160000 +Subproject 9fafde1c85e2705488b7fffdf6275b3889ec513 |