mirror of
https://github.com/mfontanini/libtins
synced 2026-01-23 18:55:58 +01:00
560 lines
16 KiB
C++
560 lines
16 KiB
C++
/*
|
|
* Copyright (c) 2014, 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 "radiotap.h"
|
|
|
|
#ifdef HAVE_DOT11
|
|
|
|
#include <cstring>
|
|
#ifdef TINS_DEBUG
|
|
#include <cassert>
|
|
#endif
|
|
#include <stdexcept>
|
|
#include "macros.h"
|
|
#ifndef WIN32
|
|
#if defined(BSD) || defined(__FreeBSD_kernel__)
|
|
#include <net/if_dl.h>
|
|
#else
|
|
#include <netpacket/packet.h>
|
|
#endif
|
|
#include <net/ethernet.h>
|
|
#endif
|
|
#include "dot11/dot11_base.h"
|
|
#include "utils.h"
|
|
#include "packet_sender.h"
|
|
#include "exceptions.h"
|
|
|
|
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 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()
|
|
{
|
|
std::memset(&_radio, 0, sizeof(_radio));
|
|
init();
|
|
}
|
|
|
|
RadioTap::RadioTap(const uint8_t *buffer, uint32_t total_sz)
|
|
{
|
|
check_size(total_sz, sizeof(_radio));
|
|
const uint8_t *buffer_start = buffer;
|
|
std::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() {
|
|
channel(Utils::channel_to_mhz(1), 0xa0);
|
|
flags(FCS);
|
|
tsft(0);
|
|
dbm_signal(0xce);
|
|
rx_flags(0);
|
|
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;
|
|
}
|
|
|
|
return (const uint8_t*)ptr - buffer;
|
|
}
|
|
|
|
// Setter for RadioTap fields
|
|
void RadioTap::version(uint8_t new_version) {
|
|
_radio.it_version = new_version;
|
|
}
|
|
|
|
void RadioTap::padding(uint8_t new_padding) {
|
|
_radio.it_pad = new_padding;
|
|
}
|
|
|
|
void RadioTap::length(uint16_t new_length) {
|
|
_radio.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;
|
|
}
|
|
|
|
void RadioTap::flags(FrameFlags new_flags) {
|
|
_flags = (uint8_t)new_flags;
|
|
_radio.flags.flags = 1;
|
|
}
|
|
|
|
void RadioTap::rate(uint8_t new_rate) {
|
|
_rate = new_rate;
|
|
_radio.flags.rate = 1;
|
|
}
|
|
|
|
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;
|
|
}
|
|
void RadioTap::dbm_signal(int8_t new_dbm_signal) {
|
|
_dbm_signal = new_dbm_signal;
|
|
_radio.flags.dbm_signal = 1;
|
|
}
|
|
|
|
void RadioTap::dbm_noise(int8_t new_dbm_noise) {
|
|
_dbm_noise = new_dbm_noise;
|
|
_radio.flags.dbm_noise = 1;
|
|
}
|
|
|
|
void RadioTap::signal_quality(uint8_t new_signal_quality) {
|
|
_signal_quality = new_signal_quality;
|
|
_radio.flags.lock_quality = 1;
|
|
}
|
|
|
|
void RadioTap::data_retries(uint8_t new_data_retries) {
|
|
_data_retries = new_data_retries;
|
|
_radio.flags.data_retries = 1;
|
|
}
|
|
|
|
void RadioTap::antenna(uint8_t new_antenna) {
|
|
_antenna = new_antenna;
|
|
_radio.flags.antenna = 1;
|
|
}
|
|
|
|
void RadioTap::db_signal(uint8_t new_db_signal) {
|
|
_db_signal = new_db_signal;
|
|
_radio.flags.db_signal = 1;
|
|
}
|
|
|
|
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::tx_flags(uint16_t new_tx_flag) {
|
|
_tx_flags = Endian::host_to_le(new_tx_flag);
|
|
_radio.flags.tx_flags = 1;
|
|
}
|
|
|
|
void RadioTap::mcs(const mcs_type& new_mcs) {
|
|
_mcs = new_mcs;
|
|
_radio.flags.mcs = 1;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
// Getter for RadioTap fields
|
|
uint8_t RadioTap::version() const {
|
|
return _radio.it_version;
|
|
}
|
|
|
|
uint8_t RadioTap::padding() const {
|
|
return _radio.it_pad;
|
|
}
|
|
|
|
uint16_t RadioTap::length() const {
|
|
return Endian::le_to_host(_radio.it_len);
|
|
}
|
|
|
|
uint64_t RadioTap::tsft() const {
|
|
if(!_radio.flags.tsft)
|
|
throw field_not_present();
|
|
return Endian::le_to_host(_tsft);
|
|
}
|
|
|
|
RadioTap::FrameFlags RadioTap::flags() const {
|
|
if(!_radio.flags.flags)
|
|
throw field_not_present();
|
|
return (FrameFlags)_flags;
|
|
}
|
|
|
|
uint8_t RadioTap::rate() const {
|
|
if(!_radio.flags.rate)
|
|
throw field_not_present();
|
|
return _rate;
|
|
}
|
|
|
|
uint16_t RadioTap::channel_freq() const {
|
|
if(!_radio.flags.channel)
|
|
throw field_not_present();
|
|
return Endian::le_to_host(_channel_freq);
|
|
}
|
|
|
|
uint16_t RadioTap::channel_type() const {
|
|
if(!_radio.flags.channel)
|
|
throw field_not_present();
|
|
return Endian::le_to_host(_channel_type);
|
|
}
|
|
|
|
int8_t RadioTap::dbm_signal() const {
|
|
if(!_radio.flags.dbm_signal)
|
|
throw field_not_present();
|
|
return _dbm_signal;
|
|
}
|
|
|
|
int8_t RadioTap::dbm_noise() const {
|
|
if(!_radio.flags.dbm_noise)
|
|
throw field_not_present();
|
|
return _dbm_noise;
|
|
}
|
|
|
|
uint16_t RadioTap::signal_quality() const {
|
|
if(!_radio.flags.lock_quality)
|
|
throw field_not_present();
|
|
return _signal_quality;
|
|
}
|
|
|
|
uint8_t RadioTap::antenna() const {
|
|
if(!_radio.flags.antenna)
|
|
throw field_not_present();
|
|
return _antenna;
|
|
}
|
|
|
|
RadioTap::mcs_type RadioTap::mcs() const {
|
|
if(!_radio.flags.mcs)
|
|
throw field_not_present();
|
|
return _mcs;
|
|
}
|
|
|
|
uint8_t RadioTap::db_signal() const {
|
|
if(!_radio.flags.db_signal)
|
|
throw field_not_present();
|
|
return _db_signal;
|
|
}
|
|
|
|
uint32_t RadioTap::channel_plus() const {
|
|
if(!_radio.flags.channel_plus)
|
|
throw field_not_present();
|
|
return Endian::le_to_host<uint32_t>(_channel_type);
|
|
}
|
|
|
|
uint16_t RadioTap::rx_flags() const {
|
|
if(!_radio.flags.rx_flags)
|
|
throw field_not_present();
|
|
return Endian::le_to_host(_rx_flags);
|
|
}
|
|
|
|
uint16_t RadioTap::tx_flags() const {
|
|
if(!_radio.flags.tx_flags)
|
|
throw field_not_present();
|
|
return Endian::le_to_host(_tx_flags);
|
|
}
|
|
|
|
uint8_t RadioTap::data_retries() const {
|
|
if(!_radio.flags.data_retries)
|
|
throw field_not_present();
|
|
return _data_retries;
|
|
}
|
|
|
|
#ifndef WIN32
|
|
void RadioTap::send(PacketSender &sender, const NetworkInterface &iface) {
|
|
if(!iface)
|
|
throw invalid_interface();
|
|
|
|
#if !defined(BSD) && !defined(__FreeBSD_kernel__)
|
|
struct sockaddr_ll addr;
|
|
|
|
memset(&addr, 0, sizeof(struct sockaddr_ll));
|
|
|
|
addr.sll_family = Endian::host_to_be<uint16_t>(PF_PACKET);
|
|
addr.sll_protocol = Endian::host_to_be<uint16_t>(ETH_P_ALL);
|
|
addr.sll_halen = 6;
|
|
addr.sll_ifindex = iface.id();
|
|
|
|
const Tins::Dot11 *wlan = tins_cast<Tins::Dot11*>(inner_pdu());
|
|
if(wlan) {
|
|
Tins::Dot11::address_type dot11_addr(wlan->addr1());
|
|
std::copy(dot11_addr.begin(), dot11_addr.end(), addr.sll_addr);
|
|
}
|
|
|
|
sender.send_l2(*this, (struct sockaddr*)&addr, (uint32_t)sizeof(addr));
|
|
#else
|
|
sender.send_l2(*this, 0, 0, iface);
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
bool RadioTap::matches_response(const uint8_t *ptr, uint32_t total_sz) const {
|
|
if(sizeof(_radio) < total_sz)
|
|
return false;
|
|
const radiotap_hdr *radio_ptr = (const radiotap_hdr*)ptr;
|
|
if(radio_ptr->it_len <= total_sz) {
|
|
ptr += radio_ptr->it_len;
|
|
total_sz -= radio_ptr->it_len;
|
|
return inner_pdu() ? inner_pdu()->matches_response(ptr, total_sz) : true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void RadioTap::write_serialization(uint8_t *buffer, uint32_t total_sz, const PDU *parent) {
|
|
uint32_t sz = header_size();
|
|
uint8_t *buffer_start = buffer;
|
|
#ifdef TINS_DEBUG
|
|
assert(total_sz >= sz);
|
|
#endif
|
|
_radio.it_len = Endian::host_to_le<uint16_t>(sz);
|
|
memcpy(buffer, &_radio, sizeof(_radio));
|
|
buffer += sizeof(_radio);
|
|
if(_radio.flags.tsft) {
|
|
memcpy(buffer, &_tsft, sizeof(_tsft));
|
|
buffer += sizeof(_tsft);
|
|
}
|
|
if(_radio.flags.flags) {
|
|
memcpy(buffer, &_flags, sizeof(_flags));
|
|
buffer += sizeof(_flags);
|
|
}
|
|
if(_radio.flags.rate) {
|
|
memcpy(buffer, &_rate, sizeof(_rate));
|
|
buffer += sizeof(_rate);
|
|
}
|
|
if(_radio.flags.channel) {
|
|
if(((buffer - buffer_start) & 1) == 1)
|
|
*(buffer++) = 0;
|
|
memcpy(buffer, &_channel_freq, sizeof(_channel_freq));
|
|
buffer += sizeof(_channel_freq);
|
|
memcpy(buffer, &_channel_type, sizeof(_channel_type));
|
|
buffer += sizeof(_channel_type);
|
|
}
|
|
if(_radio.flags.dbm_signal) {
|
|
memcpy(buffer, &_dbm_signal, sizeof(_dbm_signal));
|
|
buffer += sizeof(_dbm_signal);
|
|
}
|
|
if(_radio.flags.dbm_noise) {
|
|
memcpy(buffer, &_dbm_noise, sizeof(_dbm_noise));
|
|
buffer += sizeof(_dbm_noise);
|
|
}
|
|
if(_radio.flags.lock_quality) {
|
|
if(((buffer - buffer_start) & 1) == 1)
|
|
*(buffer++) = 0;
|
|
memcpy(buffer, &_signal_quality, sizeof(_signal_quality));
|
|
buffer += sizeof(_signal_quality);
|
|
}
|
|
if(_radio.flags.antenna) {
|
|
memcpy(buffer, &_antenna, sizeof(_antenna));
|
|
buffer += sizeof(_antenna);
|
|
}
|
|
if(_radio.flags.db_signal) {
|
|
memcpy(buffer, &_db_signal, sizeof(_db_signal));
|
|
buffer += sizeof(_db_signal);
|
|
}
|
|
if(_radio.flags.rx_flags) {
|
|
if(((buffer - buffer_start) & 1) == 1)
|
|
*(buffer++) = 0;
|
|
memcpy(buffer, &_rx_flags, sizeof(_rx_flags));
|
|
buffer += sizeof(_rx_flags);
|
|
}
|
|
if(_radio.flags.channel_plus) {
|
|
uint32_t offset = ((buffer - buffer_start) % 4);
|
|
if(offset) {
|
|
offset = 4 - offset;
|
|
while(offset--) {
|
|
*buffer++ = 0;
|
|
}
|
|
}
|
|
uint32_t dummy = _channel_type;
|
|
// nasty Big Endian fix
|
|
dummy = Endian::le_to_host<uint32_t>(Endian::host_to_le<uint16_t>(dummy));
|
|
memcpy(buffer, &dummy, sizeof(dummy));
|
|
buffer += sizeof(dummy);
|
|
memcpy(buffer, &_channel_freq, sizeof(_channel_freq));
|
|
buffer += sizeof(_channel_freq);
|
|
memcpy(buffer, &_channel, sizeof(_channel));
|
|
buffer += sizeof(_channel);
|
|
memcpy(buffer, &_max_power, sizeof(_max_power));
|
|
buffer += sizeof(_max_power);
|
|
}
|
|
if((_flags & 0x10) != 0 && inner_pdu()) {
|
|
uint32_t crc32 = Endian::host_to_le(
|
|
Utils::crc32(buffer, inner_pdu()->size())
|
|
);
|
|
memcpy(buffer + inner_pdu()->size(), &crc32, sizeof(uint32_t));
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // HAVE_DOT11
|