Add MPLS PDU and hook it up with ICMP extensions

include/tins/constants.h

@@ -78,6 +78,7 @@ namespace Tins {
                 SPRITE = 0x0500,  /* Sprite */
                 IP = 0x0800,      /* IP */
                 ARP = 0x0806,     /* Address resolution */
+                MPLS = 0x8847,    /* MPLS */
                 REVARP = 0x8035,  /* Reverse ARP */
                 AT = 0x809B,      /* AppleTalk protocol */
                 AARP = 0x80F3,    /* AppleTalk ARP */

include/tins/icmp_extension.h

@@ -10,6 +10,8 @@
 
 namespace Tins {
 
+class MPLS;
+
 /**
  * \brief Class that represents an ICMP extension object
  */
@@ -207,10 +209,20 @@ public:
     /**
      * \brief Adds an extension to this structure
      *
-     * \param extension The extension to be added;
+     * \param extension The extension to be added
      */
     void add_extension(const ICMPExtension& extension);
 
+    /**
+     * \brief Adds an MPLS extension to this structure
+     *
+     * This will construct an extension using the provided MPLS packet as
+     * its payload. The class and type fields will be set appropriately.
+     *
+     * \param extension The MPLS payload to be used for the new extension
+     */
+    void add_extension(MPLS& mpls);
+
     /**
      * \brief Gets the size of this ICMP extensions structure
      *

include/tins/memory_helpers.h

@@ -4,6 +4,7 @@
 #include <stdint.h>
 #include <cstring>
 #include <algorithm>
+#include <vector>
 #include "exceptions.h"
 #include "ip_address.h"
 #include "ipv6_address.h"
@@ -36,6 +37,10 @@ public:
     : buffer_(buffer), size_(total_sz) {
     }
 
+    InputMemoryStream(const std::vector<uint8_t>& data)
+    : buffer_(&data[0]), size_(data.size()) {
+    }
+
     void skip(uint32_t size) {
         buffer_ += size;
         size_ -= size;

include/tins/mpls.h

@@ -0,0 +1,154 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef TINS_MPLS_H
+#define TINS_MPLS_H
+
+#include "pdu.h"
+#include "endianness.h"
+#include "macros.h"
+#include "small_uint.h"
+
+namespace Tins {
+
+class ICMPExtension;
+
+/**
+ * \brief Represents an MPLS PDU
+ */
+class MPLS : public PDU {
+public:
+    /**
+     * \brief This PDU's flag.
+     */
+    static const PDU::PDUType pdu_flag = PDU::MPLS;
+
+    /**
+     * \brief Default constructor
+     */
+    MPLS();
+
+    /**
+     * \brief Construct an MPLS layer from an ICMP extension
+     *
+     * This will use the extension's payload to build this packet. 
+     * The extension's class and type are not checked.
+     *
+     */
+    MPLS(const ICMPExtension& extension);
+
+    /**
+     * \brief Constructor from buffer
+     */
+    MPLS(const uint8_t* buffer, uint32_t total_sz);
+
+    /**
+     * \brief Getter for the label field.
+     */
+    small_uint<20> label() const {
+        return (Endian::be_to_host(header_.label_high) << 4) | 
+               ((header_.label_low_and_bottom >> 4) & 0xf);
+    }
+
+    /**
+     * \brief Getter for the bottom of the stack field.
+     */
+    small_uint<1> bottom_of_stack() const {
+        return header_.label_low_and_bottom & 0x1;
+    }
+
+    /**
+     * \brief Getter for the ttl field.
+     */
+    uint8_t ttl() const {
+        return header_.ttl;
+    }
+
+    /**
+     * \brief Setter for the label field
+     *
+     * \param value The new label field value
+     */
+    void label(small_uint<20> value);
+
+    /**
+     * \brief Setter for the bottom of the stack field
+     *
+     * Note that if this MPLS layer is somewhere between an Ethernet and IP
+     * layers, the bottom of the stack field will be overriden and set 
+     * automatically. You should only set this field when constructing ICMP
+     * extensions.
+     *
+     * \param value The new bottom of the stack field value
+     */
+    void bottom_of_stack(small_uint<1> value);
+
+    /**
+     * \brief Setter for the ttl field
+     *
+     * \param value The new ttl field value
+     */
+    void ttl(uint8_t value);
+
+    /**
+     * \brief Returns the MPLS frame's header length.
+     *
+     * \return The header's size.
+     * \sa PDU::header_size()
+     */
+    uint32_t header_size() const;
+
+    /**
+     * \brief Getter for the PDU's type.
+     * \sa PDU::pdu_type
+     */
+    PDUType pdu_type() const { return pdu_flag; }
+
+    /**
+     * \sa PDU::clone
+     */
+    MPLS *clone() const {
+        return new MPLS(*this);
+    }
+private:
+    TINS_BEGIN_PACK
+    struct mpls_header {
+        uint16_t label_high;
+        uint8_t label_low_and_bottom;
+        uint8_t ttl;
+    } TINS_END_PACK;
+
+    void write_serialization(uint8_t *buffer, uint32_t total_sz, const PDU *parent);
+
+    mpls_header header_;
+};
+
+} // Tins
+
+#endif // TINS_MPLS_H

include/tins/pdu.h

@@ -129,6 +129,7 @@ namespace Tins {
             RAW,
             ETHERNET_II,
             IEEE802_3,
+            DOT3 = IEEE802_3,
             RADIOTAP,
             DOT11,
             DOT11_ACK,
@@ -177,6 +178,7 @@ namespace Tins {
             IPSEC_AH,
             IPSEC_ESP,
             PKTAP,
+            MPLS,
             USER_DEFINED_PDU = 1000
         };
         

include/tins/tins.h

@@ -45,6 +45,7 @@
 #include "dot3.h"
 #include "ip.h"
 #include "ipv6.h"
+#include "mpls.h"
 #include "packet_sender.h"
 #include "packet_writer.h"
 #include "pdu.h"

src/CMakeLists.txt

@@ -37,6 +37,7 @@ ADD_LIBRARY(
     ipsec.cpp
     llc.cpp
     loopback.cpp
+    mpls.cpp
     network_interface.cpp
     offline_packet_filter.cpp
     packet_sender.cpp

src/icmp_extension.cpp

@@ -4,6 +4,7 @@
 #include "exceptions.h"
 #include "utils.h"
 #include "memory_helpers.h"
+#include "mpls.h"
 
 using std::runtime_error;
 
@@ -135,6 +136,12 @@ void ICMPExtensionsStructure::add_extension(const ICMPExtension& extension) {
     extensions_.push_back(extension);
 }
 
+void ICMPExtensionsStructure::add_extension(MPLS& mpls) {
+    ICMPExtension extension(1, 1);
+    extension.payload(mpls.serialize());
+    add_extension(extension);
+}
+
 void ICMPExtensionsStructure::serialize(uint8_t* buffer, uint32_t buffer_size) {
     OutputMemoryStream stream(buffer, buffer_size);
     uint8_t* original_ptr = buffer;

src/internals.cpp

@@ -43,6 +43,7 @@
 #include "sll.h"
 #include "ppi.h"
 #include "icmpv6.h"
+#include "mpls.h"
 #include "arp.h"
 #include "eapol.h"
 #include "rawpdu.h"
@@ -101,6 +102,8 @@ Tins::PDU *pdu_from_flag(Constants::Ethernet::e flag, const uint8_t *buffer,
             return EAPOL::from_bytes(buffer, size);
         case Tins::Constants::Ethernet::VLAN:
             return new Dot1Q(buffer, size);
+        case Tins::Constants::Ethernet::MPLS:
+            return new MPLS(buffer, size);
         default:
             {
                 PDU *pdu = Internals::allocate<EthernetII>(
@@ -231,6 +234,8 @@ Constants::Ethernet::e pdu_flag_to_ether_type(PDU::PDUType flag) {
             return Constants::Ethernet::VLAN;
         case PDU::PPPOE:
             return Constants::Ethernet::PPPOED;
+        case PDU::MPLS:
+            return Constants::Ethernet::MPLS;
         case PDU::RSNEAPOL:
         case PDU::RC4EAPOL:
             return Constants::Ethernet::EAPOL;

src/mpls.cpp

@@ -0,0 +1,95 @@
+/*
+ * 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 "mpls.h"
+#include "ip.h"
+#include "memory_helpers.h"
+#include "icmp_extension.h"
+
+using Tins::Memory::InputMemoryStream;
+using Tins::Memory::OutputMemoryStream;
+
+namespace Tins {
+
+MPLS::MPLS() : header_() {
+
+}
+
+MPLS::MPLS(const ICMPExtension& extension) {
+    InputMemoryStream stream(extension.payload());
+    stream.read(header_);
+}
+
+MPLS::MPLS(const uint8_t* buffer, uint32_t total_sz) {
+    InputMemoryStream stream(buffer, total_sz);
+    stream.read(header_);
+    if (stream) {
+        // If this is the last MPLS, then construct an IP
+        if (bottom_of_stack()) {
+            inner_pdu(new Tins::IP(stream.pointer(), stream.size()));
+        }
+        else {
+            inner_pdu(new MPLS(stream.pointer(), stream.size()));
+        }
+    }
+}
+
+void MPLS::label(small_uint<20> value) {
+    const uint32_t label_value = value;
+    const uint16_t label_high = Endian::host_to_be<uint16_t>(label_value >> 4);
+    const uint8_t label_low = (Endian::host_to_be<uint32_t>(label_value) >> 20) & 0xf0;
+    header_.label_high = label_high & 0xffff;
+    header_.label_low_and_bottom = (header_.label_low_and_bottom & 0x0f) | label_low;
+}
+
+void MPLS::bottom_of_stack(small_uint<1> value) {
+    header_.label_low_and_bottom = (header_.label_low_and_bottom & 0xfe) | value;
+}
+
+void MPLS::ttl(uint8_t value) {
+    header_.ttl = value;
+}
+
+uint32_t MPLS::header_size() const {
+    return sizeof(header_);
+}
+
+void MPLS::write_serialization(uint8_t *buffer, uint32_t total_sz, const PDU *parent) {
+    OutputMemoryStream stream(buffer, total_sz);
+    // If we have a parent PDU, we might set the bottom-of-stack field
+    if (parent) {
+        // We'll set it if we either don't have a child or we have one and it's not MPLS
+        if (!inner_pdu() || inner_pdu()->pdu_type() != PDU::MPLS) {
+            bottom_of_stack(1);
+        }
+    }
+    stream.write(header_);
+}
+
+} // Tins

tests/src/CMakeLists.txt

@@ -70,6 +70,7 @@ ADD_CUSTOM_TARGET(
     LLCTest
     LoopbackTest
     MatchesResponseTest
+    MPLSTest
     NetworkInterfaceTest
     OfflinePacketFilterTest
     PDUTest
@@ -112,6 +113,7 @@ ADD_EXECUTABLE(IPv6Test EXCLUDE_FROM_ALL ipv6.cpp)
 ADD_EXECUTABLE(IPv6AddressTest EXCLUDE_FROM_ALL ipv6address.cpp)
 ADD_EXECUTABLE(LLCTest EXCLUDE_FROM_ALL llc.cpp)
 ADD_EXECUTABLE(LoopbackTest EXCLUDE_FROM_ALL loopback.cpp)
+ADD_EXECUTABLE(MPLSTest EXCLUDE_FROM_ALL mpls.cpp)
 ADD_EXECUTABLE(MatchesResponseTest EXCLUDE_FROM_ALL matches_response.cpp)
 ADD_EXECUTABLE(NetworkInterfaceTest EXCLUDE_FROM_ALL network_interface.cpp)
 ADD_EXECUTABLE(OfflinePacketFilterTest EXCLUDE_FROM_ALL offline_packet_filter.cpp)
@@ -193,6 +195,7 @@ ADD_TEST(IPv6Address IPv6AddressTest)
 ADD_TEST(LLC LLCTest)
 ADD_TEST(Loopback LoopbackTest)
 ADD_TEST(MatchesResponse MatchesResponseTest)
+ADD_TEST(MPLS MPLSTest)
 ADD_TEST(NetworkInterface NetworkInterfaceTest)
 ADD_TEST(OfflinePacketFilter OfflinePacketFilterTest)
 ADD_TEST(PDU PDUTest)

tests/src/icmp.cpp

@@ -6,6 +6,7 @@
 #include "ip.h"
 #include "ethernetII.h"
 #include "utils.h"
+#include "mpls.h"
 #include "rawpdu.h"
 
 using namespace std;
@@ -313,6 +314,8 @@ TEST_F(ICMPTest, ExtensionsParsingWithoutALengthField) {
     ICMP icmp(packet_with_extensions, sizeof(packet_with_extensions));
     ICMPExtensionsStructure extensions = icmp.extensions();
     ASSERT_EQ(1, extensions.extensions().size());
+    MPLS mpls(*extensions.extensions().begin());
+    EXPECT_EQ(100704, mpls.label());
     EXPECT_EQ(
         ICMPExtension::payload_type(ext, ext + sizeof(ext)), 
         extensions.extensions().begin()->serialize()

tests/src/icmp_extension.cpp

@@ -1,8 +1,11 @@
 #include <gtest/gtest.h>
 #include "icmp_extension.h"
+#include "mpls.h"
 
 using Tins::ICMPExtension;
 using Tins::ICMPExtensionsStructure;
+using Tins::MPLS;
+using Tins::PDU;
 
 class ICMPExtensionTest : public testing::Test {
 public:
@@ -93,3 +96,20 @@ TEST_F(ICMPExtensionTest, Version) {
     EXPECT_EQ(0xdea, structure.reserved());
     EXPECT_EQ(0xf, structure.version());
 }
+
+TEST_F(ICMPExtensionTest, MPLSExtension) {
+    ICMPExtensionsStructure structure;
+    MPLS mpls1;
+    mpls1.label(10012);
+    mpls1.bottom_of_stack(1);
+    mpls1.ttl(15);
+    structure.add_extension(mpls1);
+    
+    PDU::serialization_type buffer = structure.serialize();
+    ICMPExtensionsStructure new_structure(&buffer[0], buffer.size());
+    ASSERT_EQ(1, new_structure.extensions().size());
+    MPLS mpls2(*new_structure.extensions().begin());
+    EXPECT_EQ(mpls1.label(), mpls2.label());
+    EXPECT_EQ(mpls1.bottom_of_stack(), mpls2.bottom_of_stack());
+    EXPECT_EQ(mpls1.ttl(), mpls2.ttl());
+}

tests/src/mpls.cpp

@@ -0,0 +1,124 @@
+#include <gtest/gtest.h>
+#include <cstring>
+#include <string>
+#include <algorithm>
+#include <stdint.h>
+#include "mpls.h"
+#include "ip.h"
+#include "udp.h"
+#include "rawpdu.h"
+#include "ethernetII.h"
+
+using namespace std;
+using namespace Tins;
+
+class MPLSTest : public testing::Test {
+public:
+    static const uint8_t eth_and_mpls[];
+    static const uint8_t mpls_layer[];
+};
+
+const uint8_t MPLSTest::eth_and_mpls[] = {
+    0, 1, 1, 0, 0, 2, 0, 1, 1, 0, 0, 1, 136, 71, 0, 62, 144, 128, 0, 62, 
+    160, 128, 0, 62, 177, 128, 69, 0, 0, 39, 147, 163, 0, 0, 128, 17, 
+    169, 32, 127, 0, 0, 1, 127, 0, 0, 1, 0, 7, 0, 7, 0, 19, 35, 34, 72, 
+    101, 108, 108, 111, 32, 77, 80, 76, 83, 33
+};
+
+const uint8_t MPLSTest::mpls_layer[] = {
+    24, 150, 1, 1
+};
+
+TEST_F(MPLSTest, ConstructWholePacket) {
+    // This has 3 MPLS labels
+    EthernetII eth(eth_and_mpls, sizeof(eth_and_mpls));
+    const MPLS* mpls1 = eth.find_pdu<MPLS>();
+    ASSERT_TRUE(mpls1 != 0);
+    ASSERT_TRUE(mpls1->inner_pdu() != 0);
+
+    const MPLS* mpls2 = mpls1->inner_pdu()->find_pdu<MPLS>();
+    ASSERT_TRUE(mpls2 != 0);
+    ASSERT_TRUE(mpls2->inner_pdu() != 0);
+
+    const MPLS* mpls3 = mpls2->inner_pdu()->find_pdu<MPLS>();
+    ASSERT_TRUE(mpls3 != 0);
+    ASSERT_TRUE(mpls3->inner_pdu() != 0);
+
+    const IP* ip = mpls3->find_pdu<IP>();
+    ASSERT_TRUE(ip != 0);
+
+    const RawPDU* raw = ip->find_pdu<RawPDU>();
+    ASSERT_TRUE(raw != 0);
+
+    string payload = "Hello MPLS!";
+    EXPECT_EQ(RawPDU::payload_type(payload.begin(), payload.end()), raw->payload());
+
+    MPLS::serialization_type buffer = eth.serialize();
+    EXPECT_EQ(
+        MPLS::serialization_type(eth_and_mpls, eth_and_mpls + sizeof(eth_and_mpls)),
+        buffer
+    );
+}
+
+TEST_F(MPLSTest, ConstructorFromBuffer) {
+    MPLS mpls(mpls_layer, sizeof(mpls_layer));
+    EXPECT_EQ(100704, mpls.label());
+    EXPECT_EQ(1, mpls.bottom_of_stack());
+    EXPECT_EQ(1, mpls.ttl());
+}
+
+TEST_F(MPLSTest, Serialize) {
+    MPLS mpls(mpls_layer, sizeof(mpls_layer));
+    MPLS::serialization_type buffer = mpls.serialize();
+    EXPECT_EQ(
+        MPLS::serialization_type(mpls_layer, mpls_layer + sizeof(mpls_layer)),
+        buffer
+    );
+}
+
+TEST_F(MPLSTest, SerializeAfterEthernet) {
+    EthernetII eth = EthernetII() / MPLS() / IP() / UDP() / RawPDU("hehehe");
+    eth.serialize();
+    // Bottom of stack should be 1
+    EXPECT_EQ(1, eth.rfind_pdu<MPLS>().bottom_of_stack());
+}
+
+TEST_F(MPLSTest, SerializeAfterEthernetUsingTwoMPLSLayers) {
+    EthernetII eth = EthernetII() / MPLS() / MPLS() / IP() / UDP() / RawPDU("hehehe");
+    eth.serialize();
+
+    const MPLS& mpls1 = eth.rfind_pdu<MPLS>();
+    const MPLS& mpls2 = mpls1.inner_pdu()->rfind_pdu<MPLS>();
+    // Bottom of stack should be 0 for the first MPLS layer
+    EXPECT_EQ(0, mpls1.bottom_of_stack());
+    // 1 for the second one
+    EXPECT_EQ(1, mpls2.bottom_of_stack());
+}
+
+TEST_F(MPLSTest, SetAllFields) {
+    MPLS mpls;
+    mpls.ttl(0xde);
+    mpls.bottom_of_stack(1);
+    mpls.label(0xdead8);
+    EXPECT_EQ(0xdead8, mpls.label());
+    EXPECT_EQ(1, mpls.bottom_of_stack());
+    EXPECT_EQ(0xde, mpls.ttl());
+}
+
+TEST_F(MPLSTest, Label) {
+    MPLS mpls;
+    mpls.label(0xdead8);
+    EXPECT_EQ(0xdead8, mpls.label());
+}
+
+TEST_F(MPLSTest, BottomOfStack) {
+    MPLS mpls;
+    mpls.bottom_of_stack(1);
+    EXPECT_EQ(1, mpls.bottom_of_stack());
+}
+
+TEST_F(MPLSTest, TTL) {
+    MPLS mpls;
+    mpls.ttl(0xde);
+    EXPECT_EQ(0xde, mpls.ttl());
+}