diff --git a/include/tins/utils/radiotap_writer.h b/include/tins/utils/radiotap_writer.h new file mode 100644 index 0000000..e729a23 --- /dev/null +++ b/include/tins/utils/radiotap_writer.h @@ -0,0 +1,79 @@ +/* + * Copyright (c) 2017, Matias Fontanini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following disclaimer + * in the documentation and/or other materials provided with the + * distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef TINS_RADIOTAP_WRITER_H +#define TINS_RADIOTAP_WRITER_H + +#include +#include +#include "radiotap_parser.h" + +namespace Tins { +namespace Utils { + +/** + * \brief Writes RadioTap options into a buffer + * + * This class can write RadioTap options into a buffer, respecting the alignment + * of each of them. + * + * Note that RadioTap options are ordered. Writing multiple of them in a non + * ascending order will involve several memory moves around the buffer so it + * will be less efficient. + */ +class RadioTapWriter { +public: + /** + * \brief Constructs a RadioTapWriter object + * + * Note that a reference to the buffer will be kept and updated so it must + * be kept in scope while writing options to it + */ + RadioTapWriter(std::vector& buffer); + + /** + * \brief Writes an option, adding/removing padding as needed + * + * The function returns true iff the option was added successfully. This will + * always be the case, unless an option having that type is already set. + * + * \param option The option to be written + */ + bool write_option(const RadioTapParser::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); + + std::vector& buffer_; +}; + +} // Utils +} // Tins + +#endif // TINS_RADIOTAP_WRITER_H diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8068b7c..07b0540 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -77,6 +77,7 @@ set(SOURCES utils/checksum_utils.cpp utils/frequency_utils.cpp utils/radiotap_parser.cpp + utils/radiotap_writer.cpp utils/routing_utils.cpp utils/resolve_utils.cpp utils/pdu_utils.cpp diff --git a/src/utils/radiotap_writer.cpp b/src/utils/radiotap_writer.cpp new file mode 100644 index 0000000..7d76b47 --- /dev/null +++ b/src/utils/radiotap_writer.cpp @@ -0,0 +1,168 @@ +/* + * Copyright (c) 2017, Matias Fontanini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following disclaimer + * in the documentation and/or other materials provided with the + * distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include "utils/radiotap_writer.h" +#include "exceptions.h" + +using std::vector; + +namespace Tins { +namespace Utils { + +uint32_t calculate_padding(uint32_t alignment, uint32_t offset) { + return offset % alignment; +} + +uint32_t get_bit(uint32_t value) { + return log(value) / log(2); +} + +RadioTapWriter::RadioTapWriter(vector& buffer) +: buffer_(buffer) { +} + +bool RadioTapWriter::write_option(const RadioTapParser::option& option) { + const uint32_t bit = get_bit(option.option()); + if (bit > RadioTapParser::MAX_RADIOTAP_FIELD) { + throw malformed_option(); + } + const bool is_empty = buffer_.empty(); + RadioTapParser parser(buffer_); + const uint8_t* candidate_ptr = parser.current_option_ptr(); + // Loop while we find lower fields and we're still in the first namespace + while (parser.has_fields()) { + if (parser.current_field() > option.option()) { + break; + } + else if (parser.current_field() == option.option()) { + // We can't add the same option twice + return false; + } + else { + const uint32_t bit = get_bit(parser.current_field()); + const RadioTapParser::FieldMetadata& meta = RadioTapParser::RADIOTAP_METADATA[bit]; + candidate_ptr = parser.current_option_ptr() + meta.size; + } + parser.advance_field(); + } + size_t offset = candidate_ptr - &*buffer_.begin(); + const RadioTapParser::FieldMetadata& meta = RadioTapParser::RADIOTAP_METADATA[bit]; + + vector paddings = build_padding_vector(candidate_ptr, parser); + + // Calculate the offset based on the RadioTap header (add 4 bytes) + const uint32_t padding = calculate_padding(meta.alignment, offset + sizeof(uint32_t)); + + // Now actually insert our new field (padding first) + buffer_.insert(buffer_.begin() + offset, padding, 0); + buffer_.insert(buffer_.begin() + offset + padding, option.data_ptr(), + option.data_ptr() + option.data_size()); + + update_paddings(paddings, offset + padding + option.data_size()); + + // Finally, update the flags + uint32_t flags = 0; + if (is_empty) { + buffer_.insert(buffer_.begin(), sizeof(flags), 0); + } + else { + memcpy(&flags, &*buffer_.begin(), sizeof(flags)); + } + 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. +// e.g. if a 2 byte field is found, then in those 2 indexes we'll have the values [2, 1]. +// 2 to indicate that the first field requires 16 bit padding and the second one is fine +// with 1 byte padding as long as the first one is as well. +// +// Padding bytes are filled with the value 0 to indicate a special index that can be +// compacted/removed if less/more padding is required +vector RadioTapWriter::build_padding_vector(const uint8_t* last_ptr, + RadioTapParser& parser) { + vector paddings; + while (parser.has_fields()) { + const uint32_t flag = static_cast(parser.current_field()); + const uint32_t bit = get_bit(flag); + const RadioTapParser::FieldMetadata& meta = RadioTapParser::RADIOTAP_METADATA[bit]; + const uint8_t* current_ptr = parser.current_option_ptr(); + // These are just paddings + paddings.insert(paddings.end(), current_ptr - last_ptr, 0); + // Say this byte has to be padded to whatever the alignment is for this field + paddings.push_back(meta.alignment); + // The rest of the bytes for this field don't really need alignment + for (size_t i = 0; i < meta.size - 1; ++i) { + paddings.push_back(1); + } + last_ptr = current_ptr + meta.size; + parser.advance_field(); + } + return paddings; +} + +// Iterates the padding vector and extends/compacts the paddings as needed +void RadioTapWriter::update_paddings(const vector& paddings, uint32_t offset) { + size_t i = 0; + while (i != paddings.size()) { + // Skip everything that doesn't need padding + while (i != paddings.size() && paddings[i] == 1) { + ++i; + } + const size_t start = i; + // Find the next field + while (i != paddings.size() && paddings[i] == 0) { + ++i; + } + if (i == paddings.size()) { + break; + } + offset += start; + const uint8_t needed_padding = calculate_padding(paddings[i], offset + sizeof(uint32_t)); + const size_t existing_padding = i - start; + // Remove padding if there's too much + if (existing_padding > needed_padding) { + buffer_.erase(buffer_.begin() + offset, + buffer_.begin() + offset + (existing_padding - needed_padding)); + offset -= existing_padding - needed_padding; + } + // Add padding if there's too little + else if (existing_padding < needed_padding) { + buffer_.insert(buffer_.begin() + offset, needed_padding - existing_padding, 0); + offset += needed_padding - existing_padding; + } + offset += i - start; + ++i; + } +} + +} // Utils +} // Tins diff --git a/tests/src/radiotap_test.cpp b/tests/src/radiotap_test.cpp index 59c5a1b..be7a37d 100644 --- a/tests/src/radiotap_test.cpp +++ b/tests/src/radiotap_test.cpp @@ -13,10 +13,12 @@ #include "eapol.h" #include "utils.h" #include "utils/radiotap_parser.h" +#include "utils/radiotap_writer.h" using namespace std; using namespace Tins; using Tins::Utils::RadioTapParser; +using Tins::Utils::RadioTapWriter; class RadioTapTest : public testing::Test { public: @@ -412,7 +414,7 @@ TEST_F(RadioTapTest, RadioTapParsing) { EXPECT_TRUE(parser.advance_field()); EXPECT_EQ(RadioTap::FLAGS, parser.current_field()); - EXPECT_EQ((uint32_t)RadioTap::FCS, parser.current_option().to()); + EXPECT_EQ((uint8_t)RadioTap::FCS, parser.current_option().to()); EXPECT_TRUE(parser.advance_field()); EXPECT_EQ(RadioTap::RATE, parser.current_field()); @@ -441,9 +443,13 @@ TEST_F(RadioTapTest, RadioTapParsingMultipleNamespaces) { vector buffer(expected_packet4+4, expected_packet4 + sizeof(expected_packet4)-4); RadioTapParser parser(buffer); EXPECT_EQ(RadioTapParser::RADIOTAP_NS, parser.current_namespace()); - while (parser.current_field() != RadioTap::MCS) { - ASSERT_TRUE(parser.advance_field()); - } + // Skip to MCS, which is teh last one on the first set of flags + parser.skip_to_field(RadioTap::MCS); + // Check if a specific option is set + EXPECT_TRUE(parser.has_field(RadioTap::MCS)); + // Check if we can find this one which is in the second namespace + EXPECT_TRUE(parser.has_field(RadioTap::ANTENNA)); + // MCS is the last option in this namespace. After this, we should jump to the next one EXPECT_TRUE(parser.advance_field()); EXPECT_TRUE(parser.has_fields()); @@ -470,6 +476,80 @@ TEST_F(RadioTapTest, RadioTapParsingUsingEmptyBuffer) { EXPECT_FALSE(parser.has_fields()); EXPECT_FALSE(parser.advance_field()); EXPECT_FALSE(parser.has_fields()); + EXPECT_FALSE(parser.has_field(RadioTap::ANTENNA)); +} + +TEST_F(RadioTapTest, RadioTapWritingEmptyBuffer) { + vector buffer; + RadioTapWriter writer(buffer); + { + const uint8_t value = 0xca; + writer.write_option(RadioTapParser::option(RadioTap::ANTENNA, sizeof(value), &value)); + } + { + const uint8_t value = (uint8_t)RadioTap::FCS; + writer.write_option(RadioTapParser::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)); + } + { + 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)); + } + { + 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))); + } + { + const uint8_t value = 0xf7; + writer.write_option(RadioTapParser::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)); + } + + RadioTapParser parser(buffer); + EXPECT_EQ(RadioTap::TSTF, parser.current_field()); + EXPECT_EQ(616089172U, parser.current_option().to()); + EXPECT_TRUE(parser.advance_field()); + + EXPECT_EQ(RadioTap::FLAGS, parser.current_field()); + EXPECT_EQ((uint8_t)RadioTap::FCS, parser.current_option().to()); + EXPECT_TRUE(parser.advance_field()); + + EXPECT_EQ(RadioTap::RATE, parser.current_field()); + EXPECT_EQ(0xab, parser.current_option().to()); + EXPECT_TRUE(parser.advance_field()); + + EXPECT_EQ(RadioTap::FHSS, parser.current_field()); + EXPECT_EQ(0x1234, parser.current_option().to()); + EXPECT_TRUE(parser.advance_field()); + + EXPECT_EQ(RadioTap::DBM_SIGNAL, parser.current_field()); + EXPECT_EQ(0xf7, parser.current_option().to()); + EXPECT_TRUE(parser.advance_field()); + + EXPECT_EQ(RadioTap::ANTENNA, parser.current_field()); + EXPECT_EQ(0xca, parser.current_option().to()); + EXPECT_TRUE(parser.advance_field()); + + EXPECT_EQ(RadioTap::RX_FLAGS, parser.current_field()); + EXPECT_EQ(0x4321, parser.current_option().to()); + + EXPECT_FALSE(parser.advance_field()); + } #endif // TINS_HAVE_DOT11