Use RadioTapParser/Writer in RadioTap

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<RadioTap::PresentFlags>(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<size_t>(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<RadioTap::PresentFlags>(Endian::le_to_host(output));
+}
+
 bool RadioTapParser::skip_to_field(RadioTap::PresentFlags flag) {
     while (has_fields() && current_field() != flag) {
         advance_field();

src/utils/radiotap_writer.cpp

@@ -29,6 +29,7 @@
 
 #include <cmath>
 #include "utils/radiotap_writer.h"
+#include "utils/radiotap_parser.h"
 #include "exceptions.h"
 
 using std::vector;
@@ -48,7 +49,7 @@ RadioTapWriter::RadioTapWriter(vector<uint8_t>& 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<uint8_t*>(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<uint32_t>(option.option());
     memcpy(&*buffer_.begin(), &flags, sizeof(flags));
-    return true;
 }
 
 // Builds a vector that will contain the padding required for every position.