Add MPLS PDU and hook it up with ICMP extensions

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