Add RadioTapWriter class

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 <vector>
+#include <stdint.h>
+#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<uint8_t>& 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<uint8_t> build_padding_vector(const uint8_t* last_ptr, RadioTapParser& parser);
+    void update_paddings(const std::vector<uint8_t>& paddings, uint32_t offset);
+
+    std::vector<uint8_t>& buffer_;
+};
+
+} // Utils
+} // Tins
+
+#endif // TINS_RADIOTAP_WRITER_H

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

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 <cmath>
+#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<uint8_t>& 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<uint8_t> 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<uint32_t>(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<uint8_t> RadioTapWriter::build_padding_vector(const uint8_t* last_ptr,
+                                                     RadioTapParser& parser) {
+    vector<uint8_t> paddings;
+    while (parser.has_fields()) {
+        const uint32_t flag = static_cast<uint32_t>(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<uint8_t>& 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

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<uint8_t>());
+    EXPECT_EQ((uint8_t)RadioTap::FCS, parser.current_option().to<uint8_t>());
     EXPECT_TRUE(parser.advance_field());
 
     EXPECT_EQ(RadioTap::RATE, parser.current_field());
@@ -441,9 +443,13 @@ TEST_F(RadioTapTest, RadioTapParsingMultipleNamespaces) {
     vector<uint8_t> 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<uint8_t> 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<uint64_t>(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<uint16_t>(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<uint16_t>(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<uint64_t>());
+    EXPECT_TRUE(parser.advance_field());
+
+    EXPECT_EQ(RadioTap::FLAGS, parser.current_field());
+    EXPECT_EQ((uint8_t)RadioTap::FCS, parser.current_option().to<uint8_t>());
+    EXPECT_TRUE(parser.advance_field());
+
+    EXPECT_EQ(RadioTap::RATE, parser.current_field());
+    EXPECT_EQ(0xab, parser.current_option().to<uint8_t>());
+    EXPECT_TRUE(parser.advance_field());
+
+    EXPECT_EQ(RadioTap::FHSS, parser.current_field());
+    EXPECT_EQ(0x1234, parser.current_option().to<uint16_t>());
+    EXPECT_TRUE(parser.advance_field());
+
+    EXPECT_EQ(RadioTap::DBM_SIGNAL, parser.current_field());
+    EXPECT_EQ(0xf7, parser.current_option().to<uint8_t>());
+    EXPECT_TRUE(parser.advance_field());
+
+    EXPECT_EQ(RadioTap::ANTENNA, parser.current_field());
+    EXPECT_EQ(0xca, parser.current_option().to<uint8_t>());
+    EXPECT_TRUE(parser.advance_field());
+
+    EXPECT_EQ(RadioTap::RX_FLAGS, parser.current_field());
+    EXPECT_EQ(0x4321, parser.current_option().to<uint16_t>());
+
+    EXPECT_FALSE(parser.advance_field());
+
 }
 
 #endif // TINS_HAVE_DOT11