3rd-party/libtins
1
0
Fork 0
mirror of https://github.com/mfontanini/libtins synced 2026-01-27 12:14:26 +01:00
Code Issues Releases Wiki Activity

Use RadioTapParser/Writer in RadioTap

Browse Source
This commit is contained in:
Matias Fontanini
2017-05-25 07:56:23 -07:00
parent 8c7bf7d779
commit e1571e19a8
7 changed files with 224 additions and 472 deletions
Download Patch File Download Diff File Expand all files Collapse all files

479
src/radiotap.cpp
View File

@@ -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 <typename T>
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<typename T>
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<uint16_t>(Endian::host_to_le<uint32_t>(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<uint32_t>((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<uint8_t>();
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<PresentFlags>(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<uint8_t>();
// 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<uint64_t>();
}
RadioTap::FrameFlags RadioTap::flags() const {
if (!radio_.flags.flags) {
throw field_not_present();
}
return (FrameFlags)flags_;
return static_cast<FrameFlags>(do_find_option(FLAGS).to<uint8_t>());
}
uint8_t RadioTap::rate() const {
if (!radio_.flags.rate) {
throw field_not_present();
}
return rate_;
return do_find_option(RATE).to<uint8_t>();
}
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>();
}
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<int8_t>();
}
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<uint16_t>();
}
uint8_t RadioTap::antenna() const {
if (!radio_.flags.antenna) {
throw field_not_present();
}
return antenna_;
return do_find_option(ANTENNA).to<uint8_t>();
}
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<uint8_t>();
}
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<uint32_t>(channel_type_);
return Endian::le_to_host<uint32_t>(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>();
}
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<uint16_t>();
}
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<uint8_t>();
}
#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<uint16_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(0);
}
stream.write(rx_flags_);
}
if (radio_.flags.channel_plus) {
header_.it_len = Endian::host_to_le<uint16_t>(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