PPPoE::PPPoE(const uint8_t* buffer, uint32_t total_sz)
: tags_size_() {
InputMemoryStream stream(buffer, total_sz);
stream.read(header_);
stream.size(std::min(stream.size(), (size_t)payload_length()));
// If this is a session data packet
if (code() == 0) {
if (stream) {
inner_pdu(
new RawPDU(stream.pointer(), stream.size())
);
}
}
else {
while (stream) {
TagTypes opt_type = static_cast<TagTypes>(stream.read<uint16_t>());
uint16_t opt_len = stream.read_be<uint16_t>();
if (!stream.can_read(opt_len)) {
throw malformed_packet();
}
add_tag(tag(opt_type, opt_len, stream.pointer()));
stream.skip(opt_len);
}
}
}
uint32_t EthernetII::trailer_size() const {
int32_t padding = 60 - sizeof(ethhdr); // EthernetII min size is 60, padding is sometimes needed
if (inner_pdu()) {
padding -= inner_pdu()->size();
padding = std::max(0, padding);
}
return padding;
}
void IPSecAH::write_serialization(uint8_t* buffer, uint32_t total_sz) {
if (inner_pdu()) {
next_header(Internals::pdu_flag_to_ip_type(inner_pdu()->pdu_type()));
}
length(header_size() / sizeof(uint32_t) - 2);
OutputMemoryStream output(buffer, total_sz);
output.write(header_);
output.write(icv_.begin(), icv_.end());
}
uint32_t Dot1Q::trailer_size() const {
if(_append_padding) {
uint32_t total_size = sizeof(_header);
if(inner_pdu())
total_size += inner_pdu()->size();
return (total_size > 50) ? 0 : (50 - total_size);
}
else
return 0;
}
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;
}
bool Dot1Q::matches_response(const uint8_t *ptr, uint32_t total_sz) const {
if(total_sz < sizeof(_header))
return false;
const dot1q_hdr *dot1q_ptr = (const dot1q_hdr*)ptr;
if(get_id(dot1q_ptr) == get_id(&_header)) {
ptr += sizeof(_header);
total_sz -= sizeof(_header);
return inner_pdu() ? inner_pdu()->matches_response(ptr, total_sz) : true;
}
return false;
}
bool EthernetII::matches_response(const uint8_t *ptr, uint32_t total_sz) const {
if(total_sz < sizeof(ethhdr))
return false;
const size_t addr_sz = address_type::address_size;
const ethhdr *eth_ptr = (const ethhdr*)ptr;
if(std::equal(_eth.src_mac, _eth.src_mac + addr_sz, eth_ptr->dst_mac)) {
if(std::equal(_eth.src_mac, _eth.src_mac + addr_sz, eth_ptr->dst_mac) || !dst_addr().is_unicast())
{
return (inner_pdu()) ? inner_pdu()->matches_response(ptr + sizeof(_eth), total_sz - sizeof(_eth)) : true;
}
}
return false;
}
void Tins::SNAP::write_serialization(uint8_t *buffer, uint32_t total_sz, const PDU *parent) {
#ifdef TINS_DEBUG
assert(total_sz >= sizeof(_snap));
#endif
if (inner_pdu()) {
Constants::Ethernet::e flag = Internals::pdu_flag_to_ether_type(
inner_pdu()->pdu_type()
);
_snap.eth_type = Endian::host_to_be(
static_cast<uint16_t>(flag)
);
}
std::memcpy(buffer, &_snap, sizeof(_snap));
}
Dot11Data::Dot11Data(const uint8_t *buffer, uint32_t total_sz)
: Dot11(buffer, total_sz)
{
const uint32_t offset = init(buffer, total_sz);
buffer += offset;
total_sz -= offset;
if(total_sz) {
// If the wep bit is on, then just use a RawPDU
if(wep())
inner_pdu(new Tins::RawPDU(buffer, total_sz));
else
inner_pdu(new Tins::SNAP(buffer, total_sz));
}
}
IPSecESP::IPSecESP(const uint8_t* buffer, uint32_t total_sz) {
InputMemoryStream stream(buffer, total_sz);
stream.read(header_);
if (stream) {
inner_pdu(new RawPDU(stream.pointer(), stream.size()));
}
}
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
}
PPI::PPI(const uint8_t *buffer, uint32_t total_sz) {
if(total_sz < sizeof(_header))
throw malformed_packet();
std::memcpy(&_header, buffer, sizeof(_header));
if(length() > total_sz || length() < sizeof(_header))
throw malformed_packet();
buffer += sizeof(_header);
total_sz -= sizeof(_header);
// There are some options
const size_t options_length = length() - sizeof(_header);
if(options_length > 0) {
_data.assign(buffer, buffer + options_length);
buffer += options_length;
total_sz -= static_cast<uint32_t>(options_length);
}
if(total_sz > 0) {
switch(dlt()) {
case DLT_IEEE802_11:
#ifdef HAVE_DOT11
parse_80211(buffer, total_sz);
#else
throw protocol_disabled();
#endif
break;
case DLT_EN10MB:
if(Internals::is_dot3(buffer, total_sz))
inner_pdu(new Dot3(buffer, total_sz));
else
inner_pdu(new EthernetII(buffer, total_sz));
break;
case DLT_IEEE802_11_RADIO:
#ifdef HAVE_DOT11
inner_pdu(new RadioTap(buffer, total_sz));
#else
throw protocol_disabled();
#endif
break;
case DLT_NULL:
inner_pdu(new Loopback(buffer, total_sz));
break;
case DLT_LINUX_SLL:
inner_pdu(new Tins::SLL(buffer, total_sz));
break;
}
}
}
void Dot1Q::write_serialization(uint8_t *buffer, uint32_t total_sz, const PDU *) {
uint32_t trailer = trailer_size();
#ifdef TINS_DEBUG
assert(total_sz >= sizeof(_header) + trailer);
#endif
if ((payload_type() == 0) && inner_pdu()) {
Constants::Ethernet::e flag = Internals::pdu_flag_to_ether_type(
inner_pdu()->pdu_type()
);
payload_type(static_cast<uint16_t>(flag));
}
std::memcpy(buffer, &_header, sizeof(_header));
buffer += sizeof(_header);
if(inner_pdu())
buffer += inner_pdu()->size();
std::fill(buffer, buffer + trailer, 0);
}
ARP::ARP(const uint8_t *buffer, uint32_t total_sz)
{
if(total_sz < sizeof(arphdr))
throw malformed_packet();
memcpy(&_arp, buffer, sizeof(arphdr));
total_sz -= sizeof(arphdr);
//TODO: Check whether this should be removed or not.
if(total_sz)
inner_pdu(new RawPDU(buffer + sizeof(arphdr), total_sz));
}
Dot11QoSData::Dot11QoSData(const uint8_t *buffer, uint32_t total_sz)
// Am I breaking something? :S
: Dot11Data(buffer, total_sz, no_inner_pdu()) {
uint32_t sz = data_frame_size();
buffer += sz;
total_sz -= sz;
if(total_sz < sizeof(_qos_control))
throw malformed_packet();
_qos_control = *(uint16_t*)buffer;
total_sz -= sizeof(uint16_t);
buffer += sizeof(uint16_t);
if(total_sz) {
// If the wep bit is on, then just use a RawPDU
if(wep())
inner_pdu(new Tins::RawPDU(buffer, total_sz));
else
inner_pdu(new Tins::SNAP(buffer, total_sz));
}
}
RSNEAPOL::RSNEAPOL(const uint8_t* buffer, uint32_t total_sz)
: EAPOL(buffer, total_sz) {
InputMemoryStream stream(buffer, total_sz);
stream.skip(sizeof(eapol_header));
stream.read(header_);
if (stream.size() >= wpa_length()) {
stream.read(key_, wpa_length());
if (stream) {
inner_pdu(new RawPDU(stream.pointer(), stream.size()));
}
}
}
void EthernetII::write_serialization(uint8_t *buffer, uint32_t total_sz, const PDU *parent) {
#ifdef TINS_DEBUG
assert(total_sz >= header_size() + trailer_size());
#endif
/* Inner type defaults to IP */
if (inner_pdu()) {
Constants::Ethernet::e flag = Internals::pdu_flag_to_ether_type(
inner_pdu()->pdu_type()
);
payload_type(static_cast<uint16_t>(flag));
}
memcpy(buffer, &_eth, sizeof(ethhdr));
uint32_t trailer = trailer_size();
if (trailer) {
uint32_t trailer_offset = header_size();
if (inner_pdu())
trailer_offset += inner_pdu()->size();
memset(buffer + trailer_offset, 0, trailer);
}
}
void PPI::parse_80211(const uint8_t* buffer, uint32_t total_sz) {
#ifdef HAVE_DOT11
if (_data.size() >= 13) {
// Is FCS-at-end on?
if ((_data[12] & 1) == 1) {
// We need to reduce the total size since we're skipping the FCS
if (total_sz < sizeof(uint32_t)) {
throw malformed_packet();
}
total_sz -= sizeof(uint32_t);
}
}
inner_pdu(Dot11::from_bytes(buffer, total_sz));
#endif // HAVE_DOT11
}
Tins::SNAP::SNAP(const uint8_t *buffer, uint32_t total_sz)
{
if(total_sz < sizeof(_snap))
throw malformed_packet();
std::memcpy(&_snap, buffer, sizeof(_snap));
buffer += sizeof(_snap);
total_sz -= sizeof(_snap);
if(total_sz) {
inner_pdu(
Internals::pdu_from_flag(
(Constants::Ethernet::e)eth_type(),
buffer,
total_sz
)
);
}
}
PKTAP::PKTAP(const uint8_t* buffer, uint32_t total_sz) {
InputMemoryStream stream(buffer, total_sz);
stream.read(header_);
uint32_t header_length = header_.length;
if (header_length > total_sz || header_length < sizeof(header_)) {
throw malformed_packet();
}
stream.skip(header_length - sizeof(header_));
if (header_.next && stream) {
inner_pdu(
Internals::pdu_from_dlt_flag(
header_.dlt,
stream.pointer(),
stream.size()
)
);
}
}
EthernetII::EthernetII(const uint8_t *buffer, uint32_t total_sz)
{
if(total_sz < sizeof(ethhdr))
throw malformed_packet();
memcpy(&_eth, buffer, sizeof(ethhdr));
buffer += sizeof(ethhdr);
total_sz -= sizeof(ethhdr);
if(total_sz) {
inner_pdu(
Internals::pdu_from_flag(
(Constants::Ethernet::e)payload_type(),
buffer,
total_sz
)
);
}
}
PKTAP::PKTAP(const uint8_t* buffer, uint32_t total_sz) {
if (total_sz < sizeof(pktap_header)) {
throw malformed_packet();
}
memcpy(&header_, buffer, sizeof(header_));
uint32_t header_length = header_.length;
if (header_length > total_sz) {
throw malformed_packet();
}
buffer += header_length;
total_sz -= header_length;
if (header_.next && total_sz > 0) {
inner_pdu(
Internals::pdu_from_dlt_flag(
header_.dlt,
buffer,
total_sz
)
);
}
}
IPSecAH::IPSecAH(const uint8_t* buffer, uint32_t total_sz) {
InputMemoryStream stream(buffer, total_sz);
stream.read(header_);
const uint32_t ah_len = 4 * (static_cast<uint16_t>(length()) + 2);
if (ah_len < sizeof(header_)) {
throw malformed_packet();
}
const uint32_t icv_length = ah_len - sizeof(header_);
if (!stream.can_read(icv_length)) {
throw malformed_packet();
}
stream.read(icv_, icv_length);
if (stream) {
inner_pdu(
Internals::pdu_from_flag(
static_cast<Constants::IP::e>(next_header()),
stream.pointer(),
stream.size(),
true
)
);
}
}
ICMPv6::ICMPv6(const uint8_t *buffer, uint32_t total_sz)
: _options_size(), reach_time(0), retrans_timer(0)
{
if(total_sz < sizeof(_header))
throw malformed_packet();
std::memcpy(&_header, buffer, sizeof(_header));
buffer += sizeof(_header);
total_sz -= sizeof(_header);
if(has_target_addr()) {
if(total_sz < ipaddress_type::address_size)
throw malformed_packet();
target_addr(buffer);
buffer += ipaddress_type::address_size;
total_sz -= ipaddress_type::address_size;
}
if(has_dest_addr()) {
if(total_sz < ipaddress_type::address_size)
throw malformed_packet();
dest_addr(buffer);
buffer += ipaddress_type::address_size;
total_sz -= ipaddress_type::address_size;
}
if(type() == ROUTER_ADVERT) {
if(total_sz < sizeof(uint32_t) * 2)
throw malformed_packet();
const uint32_t *ptr_32 = (const uint32_t*)buffer;
reach_time = *ptr_32++;
retrans_timer = *ptr_32++;
buffer += sizeof(uint32_t) * 2;
total_sz -= sizeof(uint32_t) * 2;
}
if(has_options())
parse_options(buffer, total_sz);
if(total_sz > 0)
inner_pdu(new RawPDU(buffer, total_sz));
}
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));
}
}
void PDU::copy_inner_pdu(const PDU &pdu) {
if(pdu.inner_pdu())
inner_pdu(pdu.inner_pdu()->clone());
}
void PDU::inner_pdu(const PDU &next_pdu) {
inner_pdu(next_pdu.clone());
}
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);
buffer += sizeof(_radio);
radiotap_hdr_size -= sizeof(_radio);
if(_radio.flags.tsft)
read_field(buffer, radiotap_hdr_size, _tsft);
if(_radio.flags.flags)
read_field(buffer, radiotap_hdr_size, _flags);
if(_radio.flags.rate)
read_field(buffer, radiotap_hdr_size, _rate);
if(_radio.flags.channel) {
if(((buffer - buffer_start) & 1) == 1) {
buffer++;
radiotap_hdr_size--;
}
read_field(buffer, radiotap_hdr_size, _channel_freq);
read_field(buffer, radiotap_hdr_size, _channel_type);
}
if(_radio.flags.dbm_signal)
read_field(buffer, radiotap_hdr_size, _dbm_signal);
if(_radio.flags.dbm_noise)
read_field(buffer, radiotap_hdr_size, _dbm_noise);
if(_radio.flags.lock_quality)
read_field(buffer, radiotap_hdr_size, _signal_quality);
if(_radio.flags.antenna)
read_field(buffer, radiotap_hdr_size, _antenna);
if(_radio.flags.db_signal)
read_field(buffer, radiotap_hdr_size, _db_signal);
if(_radio.flags.rx_flags) {
if(((buffer - buffer_start) & 1) == 1) {
buffer++;
radiotap_hdr_size--;
}
read_field(buffer, radiotap_hdr_size, _rx_flags);
}
if(_radio.flags.channel_plus) {
uint32_t offset = ((buffer - buffer_start) % 4);
if(offset) {
offset = 4 - offset;
buffer += offset;
radiotap_hdr_size -= offset;
}
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);
}
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));
}
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)
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) {
if(((buffer - buffer_start) & 1) == 1) {
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) {
if(((buffer - buffer_start) & 1) == 1) {
buffer++;
radiotap_hdr_size--;
}
read_field(buffer, radiotap_hdr_size, _rx_flags);
}
if(current_flags->channel_plus) {
uint32_t offset = ((buffer - buffer_start) % 4);
if(offset) {
offset = 4 - offset;
buffer += offset;
radiotap_hdr_size -= offset;
}
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);
}
// 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));
}
