From 5fcde410239a80673ea76702c64ba7ccf04040a8 Mon Sep 17 00:00:00 2001 From: Matias Fontanini Date: Thu, 25 May 2017 07:56:23 -0700 Subject: [PATCH] Use RadioTapParser/Writer in RadioTap --- include/tins/radiotap.h | 126 ++----- include/tins/utils/radiotap_parser.h | 19 +- include/tins/utils/radiotap_writer.h | 6 +- src/radiotap.cpp | 479 ++++++++------------------- src/utils/radiotap_parser.cpp | 21 +- src/utils/radiotap_writer.cpp | 9 +- tests/src/radiotap_test.cpp | 36 +- 7 files changed, 224 insertions(+), 472 deletions(-) diff --git a/include/tins/radiotap.h b/include/tins/radiotap.h index 14cf6e9..22b322f 100644 --- a/include/tins/radiotap.h +++ b/include/tins/radiotap.h @@ -35,6 +35,7 @@ #include "macros.h" #include "pdu.h" #include "endianness.h" +#include "pdu_option.h" namespace Tins { class PacketSender; @@ -82,7 +83,7 @@ public: * \sa RadioTap::present() */ enum PresentFlags { - TSTF = 1 << 0, + TSFT = 1 << 0, FLAGS = 1 << 1, RATE = 1 << 2, CHANNEL = 1 << 3, @@ -127,6 +128,11 @@ public: uint8_t mcs; } TINS_END_PACK; + /** + * The type used to store RadioTap options + */ + typedef PDUOption option; + /** * \brief Default constructor. */ @@ -369,10 +375,7 @@ public: * will be undefined. It is only safe to use the getter of a field * if its corresponding bit flag is set in the present field. */ - PresentFlags present() const { - //return (PresentFlags)*(uint32_t*)(&radio_.it_len + 1); - return (PresentFlags)Endian::le_to_host(radio_.flags_32); - } + PresentFlags present() const; /** \brief Check whether ptr points to a valid response for this PDU. * @@ -396,6 +399,13 @@ public: */ uint32_t trailer_size() const; + /** + * Adds the given option + * + * \param option The option to be added. + */ + void add_option(const option& opt); + /** * \sa PDU::clone */ @@ -412,70 +422,7 @@ public: } private: TINS_BEGIN_PACK - #if TINS_IS_LITTLE_ENDIAN - struct flags_type { - uint32_t - tsft:1, - flags:1, - rate:1, - channel:1, - fhss:1, - dbm_signal:1, - dbm_noise:1, - lock_quality:1, - - tx_attenuation:1, - db_tx_attenuation:1, - dbm_tx_power:1, - antenna:1, - db_signal:1, - db_noise:1, - rx_flags:1, - tx_flags:1, - - reserved1:1, - data_retries:1, - channel_plus:1, - mcs:1, - reserved2:4, - - reserved3:7, - ext:1; - } TINS_END_PACK; - #else - struct flags_type { - uint32_t - lock_quality:1, - dbm_noise:1, - dbm_signal:1, - fhss:1, - channel:1, - rate:1, - flags:1, - tsft:1, - - tx_flags:1, - rx_flags:1, - db_noise:1, - db_signal:1, - antenna:1, - dbm_tx_power:1, - db_tx_attenuation:1, - tx_attenuation:1, - - reserved2:4, - mcs:1, - channel_plus:1, - data_retries:1, - reserved1:1, - - ext:1, - reserved3:7; - } TINS_END_PACK; - #endif - - TINS_BEGIN_PACK - struct radiotap_hdr { + struct radiotap_header { #if TINS_IS_LITTLE_ENDIAN uint8_t it_version; uint8_t it_pad; @@ -484,48 +431,13 @@ private: uint8_t it_version; #endif // TINS_IS_LITTLE_ENDIAN uint16_t it_len; - union { - flags_type flags; - uint32_t flags_32; - }; } TINS_END_PACK; - void init(); void write_serialization(uint8_t* buffer, uint32_t total_sz); - uint32_t find_extra_flag_fields_size(const uint8_t* buffer, uint32_t total_sz); + option do_find_option(PresentFlags type) const; - template - void align_buffer(const uint8_t* buffer_start, const uint8_t*& buffer, uint32_t& size) { - uint32_t offset = ((buffer - buffer_start) % n); - if (offset) { - offset = n - offset; - if (offset > size) { - throw malformed_packet(); - } - buffer += offset; - size -= offset; - } - } - - - radiotap_hdr radio_; - // present fields... - uint64_t tsft_; - uint16_t channel_type_; - uint16_t channel_freq_; - uint16_t rx_flags_; - uint16_t signal_quality_; - uint16_t tx_flags_; - mcs_type mcs_; - uint8_t antenna_; - uint8_t flags_; - uint8_t rate_; - uint8_t channel_; - uint8_t max_power_; - uint8_t db_signal_; - uint8_t data_retries_; - int8_t dbm_signal_; - int8_t dbm_noise_; + radiotap_header header_; + std::vector options_payload_; }; } diff --git a/include/tins/utils/radiotap_parser.h b/include/tins/utils/radiotap_parser.h index 9a08b85..b297a7b 100644 --- a/include/tins/utils/radiotap_parser.h +++ b/include/tins/utils/radiotap_parser.h @@ -58,8 +58,6 @@ public: UNKNOWN_NS }; - typedef PDUOption option; - /** * Represents the size and alignment of each RadioTap field */ @@ -111,7 +109,7 @@ public: /** * Gets the option the parsed is currently pointing at */ - option current_option(); + RadioTap::option current_option(); /** * Gets the pointer at which the current option is located @@ -123,11 +121,24 @@ public: * * If there's a namespace change, this will handle that as well. * - * \returns true iff advancing was successfull (e.g. false if we reached + * \return true iff advancing was successfull (e.g. false if we reached * the end of the header) */ bool advance_field(); + /** + * \brief Advances to the next namespace + * + * \return true iff advancing was successfull (e.g. false if we're currently + * in the last namespace) + */ + bool advance_namespace(); + + /** + * Gets the current namespace's flags + */ + RadioTap::PresentFlags namespace_flags() const; + /** * \brief Skips all fields until the provided one is found * diff --git a/include/tins/utils/radiotap_writer.h b/include/tins/utils/radiotap_writer.h index e729a23..eb1efd3 100644 --- a/include/tins/utils/radiotap_writer.h +++ b/include/tins/utils/radiotap_writer.h @@ -32,11 +32,13 @@ #include #include -#include "radiotap_parser.h" +#include "radiotap.h" namespace Tins { namespace Utils { +class RadioTapParser; + /** * \brief Writes RadioTap options into a buffer * @@ -65,7 +67,7 @@ public: * * \param option The option to be written */ - bool write_option(const RadioTapParser::option& option); + void write_option(const RadioTap::option& option); private: std::vector build_padding_vector(const uint8_t* last_ptr, RadioTapParser& parser); void update_paddings(const std::vector& paddings, uint32_t offset); diff --git a/src/radiotap.cpp b/src/radiotap.cpp index 8902c0f..5842c2b 100644 --- a/src/radiotap.cpp +++ b/src/radiotap.cpp @@ -48,144 +48,29 @@ #include "memory_helpers.h" #include "utils/checksum_utils.h" #include "utils/frequency_utils.h" +#include "utils/radiotap_parser.h" +#include "utils/radiotap_writer.h" using std::memcpy; +using Tins::Memory::InputMemoryStream; using Tins::Memory::OutputMemoryStream; +using Tins::Utils::RadioTapParser; +using Tins::Utils::RadioTapWriter; + namespace Tins { -void check_size(uint32_t total_sz, size_t field_size) { - if (total_sz < field_size) { - throw malformed_packet(); - } +template +void add_integral_option(RadioTap& radio, RadioTap::PresentFlags type, T value) { + uint8_t buffer[sizeof(value)]; + value = Endian::host_to_le(value); + std::memcpy(buffer, &value, sizeof(value)); + radio.add_option(RadioTap::option(type, sizeof(buffer), buffer)); } -template -void read_field(const uint8_t* &buffer, uint32_t& total_sz, T& field) { - check_size(total_sz, sizeof(field)); - memcpy(&field, buffer, sizeof(field)); - buffer += sizeof(field); - total_sz -= sizeof(field); -} - -RadioTap::RadioTap() : radio_() { - init(); -} - -RadioTap::RadioTap(const uint8_t* buffer, uint32_t total_sz) { - check_size(total_sz, sizeof(radio_)); - const uint8_t* buffer_start = buffer; - memcpy(&radio_, buffer, sizeof(radio_)); - uint32_t radiotap_hdr_size = length(); - check_size(total_sz, radiotap_hdr_size); - // We start on the first flags field, skipping version, pad and length. - const flags_type* current_flags = (const flags_type*)(buffer + sizeof(uint32_t)); - const uint32_t extra_flags_size = find_extra_flag_fields_size( - buffer + sizeof(uint32_t), total_sz); - // Find and skip the extra flag fields. - buffer += extra_flags_size; - radiotap_hdr_size -= extra_flags_size; - // Also skip the header - buffer += sizeof(radio_); - radiotap_hdr_size -= sizeof(radio_); - - while (true) { - radio_.flags_32 |= *(const uint32_t*)current_flags; - if (current_flags->tsft) { - align_buffer<8>(buffer_start, buffer, radiotap_hdr_size); - read_field(buffer, radiotap_hdr_size, tsft_); - } - - if (current_flags->flags) { - read_field(buffer, radiotap_hdr_size, flags_); - } - - if (current_flags->rate) { - read_field(buffer, radiotap_hdr_size, rate_); - } - - if (current_flags->channel) { - align_buffer<2>(buffer_start, buffer, radiotap_hdr_size); - read_field(buffer, radiotap_hdr_size, channel_freq_); - read_field(buffer, radiotap_hdr_size, channel_type_); - } - - if (current_flags->dbm_signal) { - read_field(buffer, radiotap_hdr_size, dbm_signal_); - } - - if (current_flags->dbm_noise) { - read_field(buffer, radiotap_hdr_size, dbm_noise_); - } - - if (current_flags->lock_quality) { - read_field(buffer, radiotap_hdr_size, signal_quality_); - } - - if (current_flags->antenna) { - read_field(buffer, radiotap_hdr_size, antenna_); - } - - if (current_flags->db_signal) { - read_field(buffer, radiotap_hdr_size, db_signal_); - } - - if (current_flags->rx_flags) { - align_buffer<2>(buffer_start, buffer, radiotap_hdr_size); - read_field(buffer, radiotap_hdr_size, rx_flags_); - } - - if (current_flags->tx_flags) { - align_buffer<2>(buffer_start, buffer, radiotap_hdr_size); - read_field(buffer, radiotap_hdr_size, tx_flags_); - } - - if (current_flags->data_retries) { - read_field(buffer, radiotap_hdr_size, data_retries_); - } - - if (current_flags->channel_plus) { - align_buffer<4>(buffer_start, buffer, radiotap_hdr_size); - uint32_t dummy; - read_field(buffer, radiotap_hdr_size, dummy); - // nasty Big Endian fix - channel_type_ = Endian::le_to_host(Endian::host_to_le(dummy)); - read_field(buffer, radiotap_hdr_size, channel_freq_); - read_field(buffer, radiotap_hdr_size, channel_); - read_field(buffer, radiotap_hdr_size, max_power_); - } - if (current_flags->mcs) { - read_field(buffer, radiotap_hdr_size, mcs_.known); - read_field(buffer, radiotap_hdr_size, mcs_.flags); - read_field(buffer, radiotap_hdr_size, mcs_.mcs); - } - // We can do this safely because we checked the size on find_extra_flags... - if (current_flags->ext == 1) { - current_flags++; - } - else { - break; - } - } - - total_sz -= length(); - buffer += radiotap_hdr_size; - - if (radio_.flags.flags && (flags() & FCS) != 0) { - check_size(total_sz, sizeof(uint32_t)); - total_sz -= sizeof(uint32_t); - if ((flags() & FAILED_FCS) !=0) { - throw malformed_packet(); - } - } - - if (total_sz) { - inner_pdu(Dot11::from_bytes(buffer, total_sz)); - } -} - -void RadioTap::init() { +RadioTap::RadioTap() +: header_(), options_payload_(4) { channel(Utils::channel_to_mhz(1), 0xa0); flags(FCS); tsft(0); @@ -194,275 +79,229 @@ void RadioTap::init() { antenna(0); } -// This method finds the extra flags field size, taking into account other -// set of flags that may appear if the "ext" bit is on/. -uint32_t RadioTap::find_extra_flag_fields_size(const uint8_t* buffer, uint32_t total_sz) { - const flags_type* ptr = (const flags_type*)buffer; - while (ptr->ext == 1) { - if (total_sz < sizeof(flags_type)) { - throw malformed_packet(); - } - ++ptr; +RadioTap::RadioTap(const uint8_t* buffer, uint32_t total_sz) { + InputMemoryStream input(buffer, total_sz); + input.read(header_); + uint32_t radiotap_size = length(); + if (TINS_UNLIKELY(radiotap_size < sizeof(header_) + sizeof(uint32_t))) { + throw malformed_packet(); } - return static_cast((const uint8_t*)ptr - buffer); + radiotap_size -= sizeof(header_); + if (TINS_UNLIKELY(radiotap_size + sizeof(uint32_t) > input.size())) { + throw malformed_packet(); + } + + options_payload_.assign(input.pointer(), input.pointer() + radiotap_size); + input.skip(radiotap_size); + + total_sz = input.size(); + RadioTapParser parser(options_payload_); + if (parser.skip_to_field(FLAGS)) { + const uint8_t flags_value = parser.current_option().to(); + if ((flags_value & FCS) != 0) { + if (total_sz < sizeof(uint32_t)) { + throw malformed_packet(); + } + total_sz -= sizeof(uint32_t); + if ((flags_value & FAILED_FCS) !=0) { + throw malformed_packet(); + } + } + } + + if (total_sz) { + inner_pdu(Dot11::from_bytes(input.pointer(), total_sz)); + } } // Setter for RadioTap fields void RadioTap::version(uint8_t new_version) { - radio_.it_version = new_version; + header_.it_version = new_version; } void RadioTap::padding(uint8_t new_padding) { - radio_.it_pad = new_padding; + header_.it_pad = new_padding; } void RadioTap::length(uint16_t new_length) { - radio_.it_len = Endian::host_to_le(new_length); + header_.it_len = Endian::host_to_le(new_length); } void RadioTap::tsft(uint64_t new_tsft) { - tsft_ = Endian::host_to_le(new_tsft); - radio_.flags.tsft = 1; + add_integral_option(*this, TSFT, new_tsft); } void RadioTap::flags(FrameFlags new_flags) { - flags_ = (uint8_t)new_flags; - radio_.flags.flags = 1; + add_integral_option(*this, FLAGS, (uint8_t)new_flags); } void RadioTap::rate(uint8_t new_rate) { - rate_ = new_rate; - radio_.flags.rate = 1; + add_integral_option(*this, RATE, new_rate); } void RadioTap::channel(uint16_t new_freq, uint16_t new_type) { - channel_freq_ = Endian::host_to_le(new_freq); - channel_type_ = Endian::host_to_le(new_type); - radio_.flags.channel = 1; + uint8_t buffer[sizeof(uint16_t) * 2]; + new_freq = Endian::host_to_le(new_freq); + new_type = Endian::host_to_le(new_type); + memcpy(buffer, &new_freq, sizeof(new_freq)); + memcpy(buffer + sizeof(new_freq), &new_type, sizeof(new_type)); + add_option(RadioTap::option(CHANNEL, sizeof(buffer), buffer)); } void RadioTap::dbm_signal(int8_t new_dbm_signal) { - dbm_signal_ = new_dbm_signal; - radio_.flags.dbm_signal = 1; + add_integral_option(*this, DBM_SIGNAL, new_dbm_signal); } void RadioTap::dbm_noise(int8_t new_dbm_noise) { - dbm_noise_ = new_dbm_noise; - radio_.flags.dbm_noise = 1; + add_integral_option(*this, DBM_NOISE, new_dbm_noise); } void RadioTap::signal_quality(uint8_t new_signal_quality) { - signal_quality_ = new_signal_quality; - radio_.flags.lock_quality = 1; + add_integral_option(*this, LOCK_QUALITY, new_signal_quality); } void RadioTap::data_retries(uint8_t new_data_retries) { - data_retries_ = new_data_retries; - radio_.flags.data_retries = 1; + add_integral_option(*this, DATA_RETRIES, new_data_retries); } void RadioTap::antenna(uint8_t new_antenna) { - antenna_ = new_antenna; - radio_.flags.antenna = 1; + add_integral_option(*this, ANTENNA, new_antenna); } void RadioTap::db_signal(uint8_t new_db_signal) { - db_signal_ = new_db_signal; - radio_.flags.db_signal = 1; + add_integral_option(*this, DB_SIGNAL, new_db_signal); } -void RadioTap::rx_flags(uint16_t new_rx_flag) { - rx_flags_ = Endian::host_to_le(new_rx_flag); - radio_.flags.rx_flags = 1; +void RadioTap::rx_flags(uint16_t new_rx_flags) { + add_integral_option(*this, RX_FLAGS, new_rx_flags); } -void RadioTap::tx_flags(uint16_t new_tx_flag) { - tx_flags_ = Endian::host_to_le(new_tx_flag); - radio_.flags.tx_flags = 1; +void RadioTap::tx_flags(uint16_t new_tx_flags) { + add_integral_option(*this, TX_FLAGS, new_tx_flags); } void RadioTap::mcs(const mcs_type& new_mcs) { - mcs_ = new_mcs; - radio_.flags.mcs = 1; + uint8_t buffer[sizeof(new_mcs)]; + memcpy(buffer, &new_mcs, sizeof(new_mcs)); + add_option(RadioTap::option(MCS, sizeof(buffer), buffer)); +} + +RadioTap::PresentFlags RadioTap::present() const { + uint32_t output = 0; + RadioTapParser parser(options_payload_); + do { + output |= parser.namespace_flags(); + } + while (parser.advance_namespace()); + return static_cast(output); } uint32_t RadioTap::header_size() const { - uint32_t total_bytes = 0; - if (radio_.flags.tsft) { - total_bytes += sizeof(tsft_); - } - if (radio_.flags.flags) { - total_bytes += sizeof(flags_); - } - if (radio_.flags.rate) { - total_bytes += sizeof(rate_); - } - if (radio_.flags.channel) { - total_bytes += (total_bytes & 1); - total_bytes += sizeof(uint16_t) * 2; - } - if (radio_.flags.dbm_signal) { - total_bytes += sizeof(dbm_signal_); - } - if (radio_.flags.dbm_noise) { - total_bytes += sizeof(dbm_noise_); - } - if (radio_.flags.lock_quality) { - total_bytes += (total_bytes & 1); - total_bytes += sizeof(signal_quality_); - } - if (radio_.flags.antenna) { - total_bytes += sizeof(antenna_); - } - if (radio_.flags.db_signal) { - total_bytes += sizeof(db_signal_); - } - if (radio_.flags.rx_flags) { - total_bytes += (total_bytes & 1); - total_bytes += sizeof(rx_flags_); - } - if (radio_.flags.tx_flags) { - total_bytes += (total_bytes & 1); - total_bytes += sizeof(tx_flags_); - } - if (radio_.flags.data_retries) { - total_bytes += sizeof(data_retries_); - } - if (radio_.flags.channel_plus) { - uint32_t offset = total_bytes % 4; - if (offset) { - total_bytes += 4 - offset; - } - total_bytes += 8; - } - if (radio_.flags.mcs) { - total_bytes += sizeof(mcs_); - } - - return sizeof(radio_) + total_bytes; + return sizeof(header_) + options_payload_.size(); } uint32_t RadioTap::trailer_size() const { - // will be sizeof(uint32_t) if the FCS-at-the-end bit is on. - return ((flags_ & 0x10) != 0) ? sizeof(uint32_t) : 0; + RadioTapParser parser(options_payload_); + if (parser.skip_to_field(FLAGS)) { + const uint8_t flags_value = parser.current_option().to(); + // If there's FCS at the end, then return its size + if ((flags_value & FCS) != 0) { + return sizeof(uint32_t); + } + } + return 0; +} + +void RadioTap::add_option(const option& opt) { + Utils::RadioTapWriter writer(options_payload_); + writer.write_option(opt); } // Getter for RadioTap fields uint8_t RadioTap::version() const { - return radio_.it_version; + return header_.it_version; } uint8_t RadioTap::padding() const { - return radio_.it_pad; + return header_.it_pad; } uint16_t RadioTap::length() const { - return Endian::le_to_host(radio_.it_len); + return Endian::le_to_host(header_.it_len); } uint64_t RadioTap::tsft() const { - if (!radio_.flags.tsft) { - throw field_not_present(); - } - return Endian::le_to_host(tsft_); + return do_find_option(TSFT).to(); } RadioTap::FrameFlags RadioTap::flags() const { - if (!radio_.flags.flags) { - throw field_not_present(); - } - return (FrameFlags)flags_; + return static_cast(do_find_option(FLAGS).to()); } uint8_t RadioTap::rate() const { - if (!radio_.flags.rate) { - throw field_not_present(); - } - return rate_; + return do_find_option(RATE).to(); } uint16_t RadioTap::channel_freq() const { - if (!radio_.flags.channel) { - throw field_not_present(); - } - return Endian::le_to_host(channel_freq_); + const option opt = do_find_option(CHANNEL); + uint16_t output; + memcpy(&output, opt.data_ptr(), sizeof(uint16_t)); + return Endian::le_to_host(output); } uint16_t RadioTap::channel_type() const { - if (!radio_.flags.channel) { - throw field_not_present(); - } - return Endian::le_to_host(channel_type_); + const option opt = do_find_option(CHANNEL); + uint16_t output; + memcpy(&output, opt.data_ptr() + sizeof(uint16_t), sizeof(uint16_t)); + return Endian::le_to_host(output); } int8_t RadioTap::dbm_signal() const { - if (!radio_.flags.dbm_signal) { - throw field_not_present(); - } - return dbm_signal_; + return do_find_option(DBM_SIGNAL).to(); } int8_t RadioTap::dbm_noise() const { - if (!radio_.flags.dbm_noise) { - throw field_not_present(); - } - return dbm_noise_; + return do_find_option(DBM_NOISE).to(); } uint16_t RadioTap::signal_quality() const { - if (!radio_.flags.lock_quality) { - throw field_not_present(); - } - return signal_quality_; + return do_find_option(DBM_SIGNAL).to(); } uint8_t RadioTap::antenna() const { - if (!radio_.flags.antenna) { - throw field_not_present(); - } - return antenna_; + return do_find_option(ANTENNA).to(); } RadioTap::mcs_type RadioTap::mcs() const { - if (!radio_.flags.mcs) { - throw field_not_present(); - } - return mcs_; + const option opt = do_find_option(MCS); + mcs_type output; + memcpy(&output, opt.data_ptr(), sizeof(mcs_type)); + return output; } uint8_t RadioTap::db_signal() const { - if (!radio_.flags.db_signal) { - throw field_not_present(); - } - return db_signal_; + return do_find_option(DB_SIGNAL).to(); } uint32_t RadioTap::channel_plus() const { - if (!radio_.flags.channel_plus) { + /*if (!header_.flags.channel_plus) { throw field_not_present(); } - return Endian::le_to_host(channel_type_); + return Endian::le_to_host(channel_type_);*/ + // TODO: wat + return 0xdadedade; } uint16_t RadioTap::rx_flags() const { - if (!radio_.flags.rx_flags) { - throw field_not_present(); - } - return Endian::le_to_host(rx_flags_); + return do_find_option(RX_FLAGS).to(); } uint16_t RadioTap::tx_flags() const { - if (!radio_.flags.tx_flags) { - throw field_not_present(); - } - return Endian::le_to_host(tx_flags_); + return do_find_option(TX_FLAGS).to(); } uint8_t RadioTap::data_retries() const { - if (!radio_.flags.data_retries) { - throw field_not_present(); - } - return data_retries_; + return do_find_option(DATA_RETRIES).to(); } #ifndef _WIN32 @@ -495,10 +334,10 @@ void RadioTap::send(PacketSender& sender, const NetworkInterface& iface) { #endif bool RadioTap::matches_response(const uint8_t* ptr, uint32_t total_sz) const { - if (sizeof(radio_) < total_sz) { + if (sizeof(header_) < total_sz) { return false; } - const radiotap_hdr* radio_ptr = (const radiotap_hdr*)ptr; + const radiotap_header* radio_ptr = (const radiotap_header*)ptr; if (radio_ptr->it_len <= total_sz) { ptr += radio_ptr->it_len; total_sz -= radio_ptr->it_len; @@ -509,50 +348,10 @@ bool RadioTap::matches_response(const uint8_t* ptr, uint32_t total_sz) const { void RadioTap::write_serialization(uint8_t* buffer, uint32_t total_sz) { OutputMemoryStream stream(buffer, total_sz); - uint8_t* buffer_start = buffer; - radio_.it_len = Endian::host_to_le(header_size()); - stream.write(radio_); - if (radio_.flags.tsft) { - stream.write(tsft_); - } - if (radio_.flags.flags) { - stream.write(flags_); - } - if (radio_.flags.rate) { - stream.write(rate_); - } - if (radio_.flags.channel) { - if (((stream.pointer() - buffer_start) & 1) == 1) { - stream.write(0); - } - stream.write(channel_freq_); - stream.write(channel_type_); - } - if (radio_.flags.dbm_signal) { - stream.write(dbm_signal_); - } - if (radio_.flags.dbm_noise) { - stream.write(dbm_noise_); - } - if (radio_.flags.lock_quality) { - if (((stream.pointer() - buffer_start) & 1) == 1) { - stream.write(0); - } - stream.write(signal_quality_); - } - if (radio_.flags.antenna) { - stream.write(antenna_); - } - if (radio_.flags.db_signal) { - stream.write(db_signal_); - } - if (radio_.flags.rx_flags) { - if (((stream.pointer() - buffer_start) & 1) == 1) { - stream.write(0); - } - stream.write(rx_flags_); - } - if (radio_.flags.channel_plus) { + header_.it_len = Endian::host_to_le(header_size()); + stream.write(header_); + stream.write(options_payload_.begin(), options_payload_.end()); + /*if (header_.flags.channel_plus) { const uint32_t padding = ((stream.pointer() - buffer_start) % 4); if (padding != 0) { stream.fill(4 - padding, 0); @@ -564,8 +363,10 @@ void RadioTap::write_serialization(uint8_t* buffer, uint32_t total_sz) { stream.write(channel_freq_); stream.write(channel_); stream.write(max_power_); - } - if ((flags_ & 0x10) != 0 && inner_pdu()) { + }*/ + + // If we have a trailer size, then we have the FCS flag on + if (trailer_size() > 0 && inner_pdu()) { uint32_t crc32 = Endian::host_to_le( Utils::crc32(stream.pointer(), inner_pdu()->size()) ); @@ -574,6 +375,14 @@ void RadioTap::write_serialization(uint8_t* buffer, uint32_t total_sz) { } } +RadioTap::option RadioTap::do_find_option(PresentFlags type) const { + RadioTapParser parser(options_payload_); + if (!parser.skip_to_field(type)) { + throw field_not_present(); + } + return parser.current_option(); +} + } // Tins #endif // TINS_HAVE_DOT11 diff --git a/src/utils/radiotap_parser.cpp b/src/utils/radiotap_parser.cpp index 058655a..69d3dea 100644 --- a/src/utils/radiotap_parser.cpp +++ b/src/utils/radiotap_parser.cpp @@ -54,7 +54,7 @@ const RadioTapParser::FieldMetadata RadioTapParser::RADIOTAP_METADATA[] = { { 2, 2 }, // TX_FLAGS { 1, 1 }, // RTS_RETRIES { 1, 1 }, // DATA_RETRIES - { 4, 4 }, // CHANNEL_PLUS + { 8, 4 }, // CHANNEL_PLUS { 3, 1 }, // MCS }; @@ -129,7 +129,7 @@ void align_buffer(const uint8_t* buffer_start, const uint8_t*& buffer, uint32_t uint32_t offset = ((buffer - buffer_start) % n); if (offset) { offset = n - offset; - if (offset > size) { + if (TINS_UNLIKELY(offset > size)) { throw malformed_packet(); } buffer += offset; @@ -164,12 +164,12 @@ RadioTap::PresentFlags RadioTapParser::current_field() const { return static_cast(1 << current_bit_); } -RadioTapParser::option RadioTapParser::current_option() { +RadioTap::option RadioTapParser::current_option() { const uint32_t size = RADIOTAP_METADATA[current_bit_].size; if (TINS_UNLIKELY(current_ptr_ + size > end_)) { throw malformed_packet(); } - return option(current_field(), size, current_ptr_); + return RadioTap::option(current_field(), size, current_ptr_); } const uint8_t* RadioTapParser::current_option_ptr() const { @@ -195,6 +195,19 @@ bool RadioTapParser::advance_field() { return advance_to_next_field(true /* start from 0*/); } +bool RadioTapParser::advance_namespace() { + if (static_cast(end_ - start_) < sizeof(uint32_t)) { + return false; + } + return advance_to_next_namespace(); +} + +RadioTap::PresentFlags RadioTapParser::namespace_flags() const { + uint32_t output = 0; + memcpy(&output, get_flags_ptr(), sizeof(output)); + return static_cast(Endian::le_to_host(output)); +} + bool RadioTapParser::skip_to_field(RadioTap::PresentFlags flag) { while (has_fields() && current_field() != flag) { advance_field(); diff --git a/src/utils/radiotap_writer.cpp b/src/utils/radiotap_writer.cpp index 7d76b47..6bd030e 100644 --- a/src/utils/radiotap_writer.cpp +++ b/src/utils/radiotap_writer.cpp @@ -29,6 +29,7 @@ #include #include "utils/radiotap_writer.h" +#include "utils/radiotap_parser.h" #include "exceptions.h" using std::vector; @@ -48,7 +49,7 @@ RadioTapWriter::RadioTapWriter(vector& buffer) : buffer_(buffer) { } -bool RadioTapWriter::write_option(const RadioTapParser::option& option) { +void RadioTapWriter::write_option(const RadioTap::option& option) { const uint32_t bit = get_bit(option.option()); if (bit > RadioTapParser::MAX_RADIOTAP_FIELD) { throw malformed_option(); @@ -62,8 +63,9 @@ bool RadioTapWriter::write_option(const RadioTapParser::option& option) { break; } else if (parser.current_field() == option.option()) { - // We can't add the same option twice - return false; + memcpy(const_cast(parser.current_option_ptr()), + option.data_ptr(), option.data_size()); + return; } else { const uint32_t bit = get_bit(parser.current_field()); @@ -97,7 +99,6 @@ bool RadioTapWriter::write_option(const RadioTapParser::option& option) { } flags |= Endian::host_to_le(option.option()); memcpy(&*buffer_.begin(), &flags, sizeof(flags)); - return true; } // Builds a vector that will contain the padding required for every position. diff --git a/tests/src/radiotap_test.cpp b/tests/src/radiotap_test.cpp index be7a37d..f915f44 100644 --- a/tests/src/radiotap_test.cpp +++ b/tests/src/radiotap_test.cpp @@ -199,7 +199,14 @@ const uint8_t RadioTapTest::expected_packet4[] = { }; const uint8_t RadioTapTest::expected_packet5[] = { - 0, 0, 14, 0, 0, 128, 10, 0, 0, 0, 0, 7, 0, 5, 136, 65, 0, 0, 76, 158, 255, 127, 13, 247, 144, 162, 218, 245, 28, 56, 84, 4, 166, 180, 209, 35, 208, 207, 0, 0, 105, 29, 0, 32, 1, 0, 0, 0, 170, 170, 3, 0, 0, 0, 8, 0, 69, 16, 0, 100, 217, 93, 64, 0, 64, 6, 220, 39, 192, 168, 1, 245, 192, 168, 1, 185, 0, 22, 132, 209, 139, 84, 229, 243, 73, 225, 122, 44, 128, 24, 14, 36, 163, 203, 0, 0, 1, 1, 8, 10, 0, 95, 217, 190, 0, 30, 133, 52, 220, 143, 231, 158, 151, 126, 243, 67, 163, 172, 214, 109, 192, 190, 238, 160, 95, 63, 206, 71, 230, 59, 143, 105, 105, 172, 142, 15, 17, 139, 55, 70, 232, 71, 84, 12, 235, 224, 159, 132, 178, 117, 5, 43, 177, 190, 152, 170 + 0, 0, 14, 0, 0, 128, 10, 0, 0, 0, 0, 7, 0, 5, 136, 65, 0, 0, 76, 158, 255, 127, 13, + 247, 144, 162, 218, 245, 28, 56, 84, 4, 166, 180, 209, 35, 208, 207, 0, 0, 105, 29, + 0, 32, 1, 0, 0, 0, 170, 170, 3, 0, 0, 0, 8, 0, 69, 16, 0, 100, 217, 93, 64, 0, 64, + 6, 220, 39, 192, 168, 1, 245, 192, 168, 1, 185, 0, 22, 132, 209, 139, 84, 229, 243, + 73, 225, 122, 44, 128, 24, 14, 36, 163, 203, 0, 0, 1, 1, 8, 10, 0, 95, 217, 190, 0, + 30, 133, 52, 220, 143, 231, 158, 151, 126, 243, 67, 163, 172, 214, 109, 192, 190, + 238, 160, 95, 63, 206, 71, 230, 59, 143, 105, 105, 172, 142, 15, 17, 139, 55, 70, + 232, 71, 84, 12, 235, 224, 159, 132, 178, 117, 5, 43, 177, 190, 152, 170 }; TEST_F(RadioTapTest, DefaultConstructor) { @@ -220,7 +227,7 @@ TEST_F(RadioTapTest, ConstructorFromBuffer) { EXPECT_EQ(radio.rate(), 0xc); EXPECT_EQ(radio.flags(), 0x10); - EXPECT_TRUE((radio.present() & RadioTap::TSTF) != 0); + EXPECT_TRUE((radio.present() & RadioTap::TSFT) != 0); EXPECT_TRUE((radio.present() & RadioTap::RATE) != 0); EXPECT_TRUE((radio.present() & RadioTap::DBM_SIGNAL) != 0); EXPECT_TRUE((radio.present() & RadioTap::ANTENNA) != 0); @@ -282,7 +289,7 @@ TEST_F(RadioTapTest, ConstructorFromBuffer3) { TEST_F(RadioTapTest, ConstructorFromBuffer4) { RadioTap radio(expected_packet4, sizeof(expected_packet4)); - EXPECT_TRUE((radio.present() & RadioTap::TSTF) != 0); + EXPECT_TRUE((radio.present() & RadioTap::TSFT) != 0); EXPECT_TRUE((radio.present() & RadioTap::DBM_SIGNAL) != 0); EXPECT_TRUE((radio.present() & RadioTap::CHANNEL) != 0); EXPECT_TRUE((radio.present() & RadioTap::ANTENNA) != 0); @@ -409,7 +416,7 @@ TEST_F(RadioTapTest, SerializationWorksFine) { TEST_F(RadioTapTest, RadioTapParsing) { vector buffer(expected_packet+4, expected_packet + sizeof(expected_packet)-4); RadioTapParser parser(buffer); - EXPECT_EQ(RadioTap::TSTF, parser.current_field()); + EXPECT_EQ(RadioTap::TSFT, parser.current_field()); EXPECT_EQ(616089172U, parser.current_option().to()); EXPECT_TRUE(parser.advance_field()); @@ -434,7 +441,7 @@ TEST_F(RadioTapTest, RadioTapParsing) { EXPECT_TRUE(parser.advance_field()); EXPECT_EQ(RadioTap::CHANNEL_PLUS,parser.current_field()); - EXPECT_EQ(0x140U, parser.current_option().to()); + EXPECT_EQ(0x1124143c00000140ULL, parser.current_option().to()); EXPECT_FALSE(parser.advance_field()); } @@ -484,44 +491,41 @@ TEST_F(RadioTapTest, RadioTapWritingEmptyBuffer) { RadioTapWriter writer(buffer); { const uint8_t value = 0xca; - writer.write_option(RadioTapParser::option(RadioTap::ANTENNA, sizeof(value), &value)); + writer.write_option(RadioTap::option(RadioTap::ANTENNA, sizeof(value), &value)); } { const uint8_t value = (uint8_t)RadioTap::FCS; - writer.write_option(RadioTapParser::option(RadioTap::FLAGS, sizeof(value), &value)); + writer.write_option(RadioTap::option(RadioTap::FLAGS, sizeof(value), &value)); } { const uint64_t value = Endian::host_to_le(616089172U); uint8_t buffer[sizeof(value)]; memcpy(buffer, &value, sizeof(value)); - writer.write_option(RadioTapParser::option(RadioTap::TSTF, sizeof(buffer), buffer)); + writer.write_option(RadioTap::option(RadioTap::TSFT, sizeof(buffer), buffer)); } { const uint16_t value = Endian::host_to_le(0x1234); uint8_t buffer[sizeof(value)]; memcpy(buffer, &value, sizeof(value)); - writer.write_option(RadioTapParser::option(RadioTap::FHSS, sizeof(buffer), buffer)); + writer.write_option(RadioTap::option(RadioTap::FHSS, sizeof(buffer), buffer)); } { const uint8_t value = 0xab; - writer.write_option(RadioTapParser::option(RadioTap::RATE, sizeof(value), &value)); - // We can't add the same option twice - EXPECT_FALSE(writer.write_option(RadioTapParser::option(RadioTap::RATE, sizeof(value), - &value))); + writer.write_option(RadioTap::option(RadioTap::RATE, sizeof(value), &value)); } { const uint8_t value = 0xf7; - writer.write_option(RadioTapParser::option(RadioTap::DBM_SIGNAL, sizeof(value), &value)); + writer.write_option(RadioTap::option(RadioTap::DBM_SIGNAL, sizeof(value), &value)); } { const uint16_t value = Endian::host_to_le(0x4321); uint8_t buffer[sizeof(value)]; memcpy(buffer, &value, sizeof(value)); - writer.write_option(RadioTapParser::option(RadioTap::RX_FLAGS, sizeof(buffer), buffer)); + writer.write_option(RadioTap::option(RadioTap::RX_FLAGS, sizeof(buffer), buffer)); } RadioTapParser parser(buffer); - EXPECT_EQ(RadioTap::TSTF, parser.current_field()); + EXPECT_EQ(RadioTap::TSFT, parser.current_field()); EXPECT_EQ(616089172U, parser.current_option().to()); EXPECT_TRUE(parser.advance_field());