diff --git a/include/tins/utils/radiotap_parser.h b/include/tins/utils/radiotap_parser.h index b297a7b..44e2744 100644 --- a/include/tins/utils/radiotap_parser.h +++ b/include/tins/utils/radiotap_parser.h @@ -162,17 +162,21 @@ public: bool has_field(RadioTap::PresentFlags flag) const; private: const uint8_t* find_options_start() const; - bool advance_to_next_field(bool start_from_zero); + bool advance_to_first_field(); + bool advance_to_next_field(); + bool skip_current_field(); bool advance_to_next_namespace(); const RadioTapFlags* get_flags_ptr() const; + void load_current_flags(); bool is_field_set(uint32_t bit, const RadioTapFlags* flags) const; const uint8_t* start_; const uint8_t* end_; const uint8_t* current_ptr_; - NamespaceType current_namespace_; - uint32_t current_bit_; + uint64_t current_bit_; + uint32_t current_flags_; uint32_t namespace_index_; + NamespaceType current_namespace_; }; } // Utils diff --git a/src/radiotap.cpp b/src/radiotap.cpp index 5842c2b..a71bc8c 100644 --- a/src/radiotap.cpp +++ b/src/radiotap.cpp @@ -98,19 +98,19 @@ RadioTap::RadioTap(const uint8_t* buffer, uint32_t total_sz) { total_sz = input.size(); RadioTapParser parser(options_payload_); if (parser.skip_to_field(FLAGS)) { - const uint8_t flags_value = parser.current_option().to(); + const uint8_t flags_value = *parser.current_option_ptr(); if ((flags_value & FCS) != 0) { - if (total_sz < sizeof(uint32_t)) { + if (TINS_UNLIKELY(total_sz < sizeof(uint32_t))) { throw malformed_packet(); } total_sz -= sizeof(uint32_t); - if ((flags_value & FAILED_FCS) !=0) { + if (TINS_UNLIKELY((flags_value & FAILED_FCS) != 0)) { throw malformed_packet(); } } } - if (total_sz) { + if (TINS_LIKELY(total_sz)) { inner_pdu(Dot11::from_bytes(input.pointer(), total_sz)); } } diff --git a/src/utils/radiotap_parser.cpp b/src/utils/radiotap_parser.cpp index 69d3dea..c3babbf 100644 --- a/src/utils/radiotap_parser.cpp +++ b/src/utils/radiotap_parser.cpp @@ -27,6 +27,7 @@ * */ +#include #include "utils/radiotap_parser.h" #include "exceptions.h" @@ -126,7 +127,7 @@ struct RadioTapFlags { #endif void align_buffer(const uint8_t* buffer_start, const uint8_t*& buffer, uint32_t size, size_t n) { - uint32_t offset = ((buffer - buffer_start) % n); + uint32_t offset = (buffer - buffer_start) & (n - 1); if (offset) { offset = n - offset; if (TINS_UNLIKELY(offset > size)) { @@ -137,18 +138,22 @@ void align_buffer(const uint8_t* buffer_start, const uint8_t*& buffer, uint32_t } RadioTapParser::RadioTapParser(const vector& buffer) -: current_namespace_(RADIOTAP_NS), current_bit_(MAX_RADIOTAP_FIELD), namespace_index_(0) { +: current_bit_(MAX_RADIOTAP_FIELD), current_flags_(0), namespace_index_(0), + current_namespace_(RADIOTAP_NS) { if (buffer.empty()) { start_ = 0; end_ = 0; current_ptr_ = start_; + current_flags_ = 0; } else { start_ = &*buffer.begin(); end_ = start_ + buffer.size(); + load_current_flags(); + current_bit_ = 0; current_ptr_ = find_options_start(); // Skip all fields and make this point to the first flags one - advance_to_next_field(true /* start from bit zero */); + advance_to_first_field(); } } @@ -182,17 +187,21 @@ bool RadioTapParser::advance_field() { return false; } // If we manage to advance the field, return true - if (advance_to_next_field(false /* keep going from current */)) { + if (skip_current_field()) { return true; } - // We finished iterating the current namespace (if any). Reset our bit - current_bit_ = MAX_RADIOTAP_FIELD; - // Otherwise, let's try advancing the namespace. If we fail, then we failed + // Try to find the next namespace, as we've exhausted the current one if (!advance_to_next_namespace()) { + current_bit_ = MAX_RADIOTAP_FIELD; return false; } + current_bit_ = 0; // Try to find the first field in this new namespace - return advance_to_next_field(true /* start from 0*/); + if (!advance_to_first_field()) { + current_bit_ = MAX_RADIOTAP_FIELD; + return false; + } + return true; } bool RadioTapParser::advance_namespace() { @@ -203,7 +212,7 @@ bool RadioTapParser::advance_namespace() { } RadioTap::PresentFlags RadioTapParser::namespace_flags() const { - uint32_t output = 0; + uint32_t output; memcpy(&output, get_flags_ptr(), sizeof(output)); return static_cast(Endian::le_to_host(output)); } @@ -252,35 +261,38 @@ const uint8_t* RadioTapParser::find_options_start() const { return reinterpret_cast(flags) + sizeof(RadioTapFlags); } -bool RadioTapParser::advance_to_next_field(bool start_from_zero) { - const RadioTapFlags* flags = get_flags_ptr(); - uint32_t bit; - if (start_from_zero) { - bit = 0; +bool RadioTapParser::advance_to_first_field() { + return advance_to_next_field(); +} + +bool RadioTapParser::advance_to_next_field() { + while ((current_flags_ & 1) == 0 && current_bit_ < MAX_RADIOTAP_FIELD) { + current_bit_++; + current_flags_ >>= 1; } - else { - // Skip the payload - current_ptr_ += RADIOTAP_METADATA[current_bit_].size; - bit = current_bit_ + 1; - } - while (!is_field_set(1 << bit, flags) && bit < MAX_RADIOTAP_FIELD) { - bit++; - } - if (bit < MAX_RADIOTAP_FIELD) { + if (current_bit_ < MAX_RADIOTAP_FIELD) { const uint8_t* radiotap_start = start_ - sizeof(uint32_t); // Skip and align the buffer align_buffer(radiotap_start, current_ptr_, end_ - radiotap_start, - RADIOTAP_METADATA[bit].alignment); - current_bit_ = bit; + RADIOTAP_METADATA[current_bit_].alignment); return true; } return false; } +bool RadioTapParser::skip_current_field() { + // Skip the payload + current_ptr_ += RADIOTAP_METADATA[current_bit_].size; + current_flags_ >>= 1; + current_bit_++; + return advance_to_next_field(); +} + bool RadioTapParser::advance_to_next_namespace() { const uint32_t initial_index = namespace_index_; - while (get_flags_ptr()->ext == 1) { - const RadioTapFlags* flags = get_flags_ptr(); + load_current_flags(); + const RadioTapFlags* flags = (const RadioTapFlags*)¤t_flags_; + while (flags->ext == 1) { if (is_field_set(29, flags)) { current_namespace_ = RADIOTAP_NS; } @@ -291,6 +303,8 @@ bool RadioTapParser::advance_to_next_namespace() { current_namespace_ = UNKNOWN_NS; } namespace_index_++; + load_current_flags(); + flags = (const RadioTapFlags*)¤t_flags_; } return initial_index != namespace_index_; } @@ -309,5 +323,9 @@ const RadioTapFlags* RadioTapParser::get_flags_ptr() const { return (const RadioTapFlags*)(start_ + sizeof(uint32_t) * namespace_index_); } +void RadioTapParser::load_current_flags() { + memcpy(¤t_flags_, get_flags_ptr(), sizeof(current_flags_)); +} + } // Utils } // Tins