Device Emulation (contd.) - Implemented sending of IPv6 Neighbor Solicitation packets for IPv6 resolution

common/uint128.h

@@ -20,6 +20,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>
 #ifndef _UINT128_H
 #define _UINT128_H
 
+#include <QHash>
+
 #include <QtGlobal>
 #include <qendian.h>
 
@@ -33,8 +35,12 @@ public:
     quint64 lo64() const;
     quint8* toArray() const;
 
-    UInt128 operator+(const UInt128 &other);
+    bool operator==(const UInt128 &other) const;
-    UInt128 operator*(const uint &other);
+    UInt128 operator+(const UInt128 &other) const;
+    UInt128 operator*(const uint &other) const;
+    UInt128 operator<<(const int &shift) const;
+    UInt128 operator~() const;
+    UInt128 operator&(const UInt128 &other) const;
 
 private:
     quint64 hi_;
@@ -71,7 +77,12 @@ inline quint8* UInt128::toArray() const
     return (quint8*)array_;
 }
 
-inline UInt128 UInt128::operator+(const UInt128 &other)
+inline bool UInt128::operator==(const UInt128 &other) const
+{
+    return ((hi_ == other.hi_) && (lo_ == other.lo_));
+}
+
+inline UInt128 UInt128::operator+(const UInt128 &other) const
 {
     UInt128 sum;
 
@@ -81,7 +92,7 @@ inline UInt128 UInt128::operator+(const UInt128 &other)
     return sum;
 }
 
-inline UInt128 UInt128::operator*(const uint &other)
+inline UInt128 UInt128::operator*(const uint &other) const
 {
     UInt128 product;
   
@@ -92,4 +103,49 @@ inline UInt128 UInt128::operator*(const uint &other)
     return product;
 }
 
+inline UInt128 UInt128::operator<<(const int &shift) const
+{
+    UInt128 shifted;
+
+    if (shift < 64)
+        return UInt128((hi_<<shift) | (lo_>>(64-shift)), lo_ << shift);
+
+    return UInt128(hi_<<(shift-64), 0);
+}
+
+inline UInt128 UInt128::operator~() const
+{
+    return UInt128(~hi_, ~lo_);
+}
+
+inline UInt128 UInt128::operator&(const UInt128 &other) const
+{
+    return UInt128(hi_ & other.hi_, lo_ & other.lo_);
+}
+
+template <> inline UInt128 qFromBigEndian<UInt128>(const uchar *src)
+{
+    quint64 hi, lo;
+
+    hi = qFromBigEndian<quint64>(src);
+    lo = qFromBigEndian<quint64>(src+8);
+
+    return UInt128(hi, lo);
+}
+
+template <> inline UInt128 qToBigEndian<UInt128>(const UInt128 src)
+{
+    quint64 hi, lo;
+
+    hi = qToBigEndian<quint64>(src.hi64());
+    lo = qToBigEndian<quint64>(src.lo64());
+
+    return UInt128(hi, lo);
+}
+
+inline uint qHash(const UInt128 &key)
+{
+    return qHash(key.hi64()) ^ qHash(key.lo64());
+}
+
 #endif

server/device.cpp

@@ -28,6 +28,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>
 
 const int kBaseHex = 16;
 const quint64 kBcastMac = 0xffffffffffffULL;
+const quint16 kEthTypeIp4 = 0x0800;
+const quint16 kEthTypeIp6 = 0x86dd;
+const int kIp6HdrLen = 40;
+const quint8 kIpProtoIcmp6 = 58;
 
 /*
  * NOTE:
@@ -37,6 +41,17 @@ const quint64 kBcastMac = 0xffffffffffffULL;
  *    (e.g. in a hash)
  */
 
+inline quint32 sumUInt128(UInt128 value)
+{
+    quint8 *arr = value.toArray();
+    quint32 sum = 0;
+
+    for (int i = 0; i < 16; i += 2)
+        sum += qToBigEndian(*((quint16*)(arr + i)));
+
+    return sum;
+}
+
 Device::Device(DeviceManager *deviceManager)
 {
     deviceManager_ = deviceManager;
@@ -81,8 +96,8 @@ void Device::setMac(quint64 mac)
 {
     int ofs = kMaxVlan * sizeof(quint16);
 
-    mac_ = mac;
+    mac_ = mac & ~(0xffffULL << 48);
-    memcpy(key_.data() + ofs, (char*)&mac, sizeof(mac));
+    memcpy(key_.data() + ofs, (char*)&mac_, sizeof(mac_));
 }
 
 void Device::setIp4(quint32 address, int prefixLength, quint32 gateway)
@@ -234,7 +249,7 @@ void Device::transmitPacket(PacketBuffer *pktBuf)
 
 void Device::clearNeighbors()
 {
-    arpTable.clear();
+    arpTable_.clear();
 }
 
 // Resolve the Neighbor IP address for this to-be-transmitted pktBuf
@@ -268,8 +283,8 @@ void Device::resolveNeighbor(PacketBuffer *pktBuf)
 // Append this device's neighbors to the list
 void Device::getNeighbors(OstEmul::DeviceNeighborList *neighbors)
 {
-    QList<quint32> ipList = arpTable.keys();
+    QList<quint32> ipList = arpTable_.keys();
-    QList<quint64> macList = arpTable.values();
+    QList<quint64> macList = arpTable_.values();
 
     Q_ASSERT(ipList.size() == macList.size());
 
@@ -290,12 +305,13 @@ bool Device::isOrigin(const PacketBuffer *pktBuf)
     qDebug("%s: ethType 0x%x", __PRETTY_FUNCTION__, ethType);
     pktData += 2;
 
-    // We know only about IP packets
+    // We know only about IP packets - adjust for ethType length (2 bytes)
+    // when checking that we have a complete IP header
     if ((ethType == 0x0800) && hasIp4_) { // IPv4
         int ipHdrLen = (pktData[0] & 0x0F) << 2;
         quint32 srcIp;
 
-        if (pktBuf->length() < ipHdrLen) {
+        if (pktBuf->length() < (ipHdrLen+2)) {
             qDebug("incomplete IPv4 header: expected %d, actual %d",
                     ipHdrLen, pktBuf->length());
             return false;
@@ -305,6 +321,19 @@ bool Device::isOrigin(const PacketBuffer *pktBuf)
         qDebug("%s: pktSrcIp/selfIp = 0x%x/0x%x", __FUNCTION__, srcIp, ip4_);
         return (srcIp == ip4_);
     }
+    else if ((ethType == kEthTypeIp6) && hasIp6_) { // IPv6
+        UInt128 srcIp;
+        if (pktBuf->length() < (kIp6HdrLen+2)) {
+            qDebug("incomplete IPv6 header: expected %d, actual %d",
+                    kIp6HdrLen, pktBuf->length()-2);
+            return false;
+        }
+
+        srcIp = qFromBigEndian<UInt128>(pktData + 8);
+        qDebug("%s: pktSrcIp6/selfIp6 = %llx-%llx/%llx-%llx", __FUNCTION__,
+                srcIp.hi64(), srcIp.lo64(), ip6_.hi64(), ip6_.lo64());
+        return (srcIp == ip6_);
+    }
 
     return false;
 }
@@ -335,7 +364,7 @@ quint64 Device::neighborMac(const PacketBuffer *pktBuf)
         qDebug("dst %x self %x mask %x", dstIp, ip4_, mask);
         tgtIp = ((dstIp & mask) == (ip4_ & mask)) ? dstIp : ip4Gateway_;
 
-        return arpTable.value(tgtIp);
+        return arpTable_.value(tgtIp);
     }
 
     return false;
@@ -344,6 +373,11 @@ quint64 Device::neighborMac(const PacketBuffer *pktBuf)
 //
 // Private Methods
 //
+/*
+ * ---------------------------------------------------------
+ * IPv4 related private methods
+ * ---------------------------------------------------------
+ */
 void Device::receiveArp(PacketBuffer *pktBuf)
 {
     PacketBuffer *rspPkt;
@@ -404,7 +438,7 @@ void Device::receiveArp(PacketBuffer *pktBuf)
     switch (opCode)
     {
     case 1:  // ARP Request
-        arpTable.insert(srcIp, srcMac);
+        arpTable_.insert(srcIp, srcMac);
 
         rspPkt = new PacketBuffer;
         rspPkt->reserve(encapSize());
@@ -432,7 +466,7 @@ void Device::receiveArp(PacketBuffer *pktBuf)
                 qPrintable(QHostAddress(tgtIp).toString()));
         break;
     case 2: // ARP Response
-        arpTable.insert(srcIp, srcMac);
+        arpTable_.insert(srcIp, srcMac);
         break;
 
     default:
@@ -462,6 +496,7 @@ void Device::sendArpRequest(PacketBuffer *pktBuf)
         return;
     }
 
+    // FIXME: not required - caller is checking for origin anyway
     // Extract srcIp first to check quickly that this packet originates
     // from this device
     srcIp = qFromBigEndian<quint32>(pktData + ipHdrLen - 8);
@@ -480,7 +515,7 @@ void Device::sendArpRequest(PacketBuffer *pktBuf)
 
     // Do we already have a ARP entry (resolved or unresolved)?
     // FIXME: do we need a timer to resend ARP for unresolved entries?
-    if (arpTable.contains(tgtIp))
+    if (arpTable_.contains(tgtIp))
         return;
 
     reqPkt = new PacketBuffer;
@@ -503,7 +538,7 @@ void Device::sendArpRequest(PacketBuffer *pktBuf)
 
     encap(reqPkt, kBcastMac, 0x0806);
     transmitPacket(reqPkt);
-    arpTable.insert(tgtIp, 0);
+    arpTable_.insert(tgtIp, 0);
 
     qDebug("Sent ARP Request for srcIp/tgtIp=%s/%s",
             qPrintable(QHostAddress(srcIp).toString()),
@@ -567,7 +602,7 @@ void Device::sendIp4Reply(PacketBuffer *pktBuf)
     dstIp = qFromBigEndian<quint32>(pktData + 12); // srcIp in original pkt
     srcIp = qFromBigEndian<quint32>(pktData + 16); // dstIp in original pkt
 
-    if (!arpTable.contains(dstIp))
+    if (!arpTable_.contains(dstIp))
         return;
 
     *(quint32*)(pktData + 12) = qToBigEndian(srcIp);
@@ -585,7 +620,7 @@ void Device::sendIp4Reply(PacketBuffer *pktBuf)
         sum = (sum & 0xFFFF) + (sum >> 16);
     *(quint16*)(pktData + 10) = qToBigEndian(quint16(~sum));
 
-    encap(pktBuf, arpTable.value(dstIp), 0x0800);
+    encap(pktBuf, arpTable_.value(dstIp), 0x0800);
     transmitPacket(pktBuf);
 }
 
@@ -617,11 +652,123 @@ void Device::receiveIcmp4(PacketBuffer *pktBuf)
     qDebug("Sent ICMP Echo Reply");
 }
 
+/*
+ * ---------------------------------------------------------
+ * IPV6 related private methods
+ * ---------------------------------------------------------
+ */
+
+// pktBuf should point to start of IP payload
+bool Device::sendIp6(PacketBuffer *pktBuf, UInt128 dstIp, quint8 protocol)
+{
+    int payloadLen = pktBuf->length();
+    uchar *p = pktBuf->push(kIp6HdrLen);
+    quint64 dstMac = kBcastMac;
+
+    if (!p) {
+        qWarning("%s: failed to push %d bytes [0x%p, 0x%p]", __FUNCTION__,
+                kIp6HdrLen, pktBuf->head(), pktBuf->data());
+        goto _error_exit;
+    }
+
+    // Ver(4), TrfClass(8), FlowLabel(8)
+    *(quint32*)(p   ) = qToBigEndian(quint32(0x60000000));
+    *(quint16*)(p+ 4) = qToBigEndian(quint16(payloadLen));
+    p[6] = protocol;
+    p[7] = 255; // HopLimit
+    memcpy(p+ 8,  ip6_.toArray(), 16); // Source IP
+    memcpy(p+24, dstIp.toArray(), 16); // Destination IP
+
+    // In case of mcast, derive dstMac
+    if ((dstIp.hi64() >> 56) == 0xff)
+        dstMac = (quint64(0x3333) << 32) | (dstIp.lo64() & 0xffffffff);
+
+    // FIXME: both these functions should return success/failure
+    encap(pktBuf, dstMac, kEthTypeIp6);
+    transmitPacket(pktBuf);
+
+    return true;
+
+_error_exit:
+    return false;
+}
+
 // Send NS for the IPv6 packet in pktBuf
-// pktBuf points to start of IP header
+// caller is responsible to check that pktBuf originates from this device
+// pktBuf should point to start of IP header
 void Device::sendNeighborSolicit(PacketBuffer *pktBuf)
 {
-    // TODO
+    PacketBuffer *reqPkt;
+    uchar *pktData = pktBuf->data();
+    UInt128 srcIp, dstIp, mask, tgtIp;
+
+    if (pktBuf->length() < kIp6HdrLen) {
+        qDebug("incomplete IPv6 header: expected %d, actual %d",
+                kIp6HdrLen, pktBuf->length());
+        return;
+    }
+
+    srcIp = qFromBigEndian<UInt128>(pktData +  8);
+    dstIp = qFromBigEndian<UInt128>(pktData + 24);
+
+    mask =  ~UInt128(0, 0) << (128 - ip6PrefixLength_);
+    qDebug("%s: dst %s src %s mask %s", __FUNCTION__,
+            qPrintable(QHostAddress(dstIp.toArray()).toString()),
+            qPrintable(QHostAddress(srcIp.toArray()).toString()),
+            qPrintable(QHostAddress(mask.toArray()).toString()));
+    tgtIp = ((dstIp & mask) == (srcIp & mask)) ? dstIp : ip6Gateway_;
+
+    // Do we already have a NDP entry (resolved or unresolved)?
+    // FIXME: do we need a timer to resend NS for unresolved entries?
+    if (ndpTable_.contains(tgtIp))
+        return;
+
+    // Form the solicited node address to be used as dstIp
+    // ff02::1:ffXX:XXXX/104
+    dstIp = UInt128((quint64(0xff02) << 48),
+                    (quint64(0x01ff) << 24) | (tgtIp.lo64() & 0xFFFFFF));
+
+    reqPkt = new PacketBuffer;
+    reqPkt->reserve(encapSize() + kIp6HdrLen);
+    pktData = reqPkt->put(32);
+    if (pktData) {
+        // Calculate checksum first -
+        // start with fixed fields in ICMP Header and IPv6 Pseudo Header ...
+        quint32 sum = 0x8700 + 0x0101 + 32 + kIpProtoIcmp6;
+
+        // then variable fields from ICMP header ...
+        sum += sumUInt128(tgtIp);
+        sum += (mac_ >> 32) + ((mac_ >> 16) & 0xffff) + (mac_ & 0xffff);
+
+        // and variable fields from IPv6 pseudo header
+        sum += sumUInt128(ip6_);
+        sum += sumUInt128(dstIp);
+
+        while(sum >> 16)
+            sum = (sum & 0xFFFF) + (sum >> 16);
+
+        // Type, Code
+        *(quint16*)(pktData   ) = qToBigEndian(quint16(0x8700));
+        // Checksum
+        *(quint16*)(pktData+ 2) = qToBigEndian(quint16(~sum));
+        // Reserved
+        *(quint32*)(pktData+ 4) = qToBigEndian(quint32(0));
+        // Target IP
+        memcpy(pktData+ 8, tgtIp.toArray(), 16);
+        // Source Addr TLV + MacAddr
+        *(quint16*)(pktData+24) = qToBigEndian(quint16(0x0101));
+        *(quint32*)(pktData+26) = qToBigEndian(quint32(mac_ >> 16));
+        *(quint16*)(pktData+30) = qToBigEndian(quint16(mac_ & 0xffff));
+    }
+
+    if (!sendIp6(reqPkt, dstIp , kIpProtoIcmp6))
+        return;
+
+    ndpTable_.insert(tgtIp, 0);
+
+    qDebug("Sent NDP Request for srcIp/tgtIp=%s/%s",
+            qPrintable(QHostAddress(srcIp.toArray()).toString()),
+            qPrintable(QHostAddress(tgtIp.toArray()).toString()));
 }
 
 bool operator<(const DeviceKey &a1, const DeviceKey &a2)

server/device.h

@@ -75,8 +75,9 @@ private: // methods
 
     void receiveIcmp4(PacketBuffer *pktBuf);
 
+    bool sendIp6(PacketBuffer *pktBuf, UInt128 dstIp, quint8 protocol);
+
     void sendNeighborSolicit(PacketBuffer *pktBuf);
-    void sendIp6(PacketBuffer *pktBuf);
 
 private: // data
     static const int kMaxVlan = 4;
@@ -99,7 +100,8 @@ private: // data
 
     DeviceKey key_;
 
-    QHash<quint32, quint64> arpTable;
+    QHash<quint32, quint64> arpTable_;
+    QHash<UInt128, quint64> ndpTable_;
 };
 
 bool operator<(const DeviceKey &a1, const DeviceKey &a2);

test/emultest.py

@@ -159,6 +159,7 @@ def ports(request, drone):
 def dut(request):
     # Enable IP forwarding on the DUT (aka make it a router)
     sudo('sysctl -w net.ipv4.ip_forward=1')
+    sudo('sysctl -w net.ipv6.conf.all.forwarding=1')
 
 @pytest.fixture(scope='module')
 def dut_ports(request):
@@ -559,20 +560,22 @@ def test_multiEmulDevNoVlan(drone, ports, dut, dut_ports, dut_ip,
     print(cap_pkts)
     log.info('dumping Tx capture buffer (filtered)')
     for i in range(len(ip_versions)):
+        if ip_versions[i] == 'ip4':
+            filter = '(arp.opcode == 1)' \
+                ' && (arp.src.proto_ipv4 == 10.10.1.<x>)' \
+                ' && (arp.dst.proto_ipv4 == 10.10.1.1)' \
+                ' && !expert.severity'
+        elif ip_versions[i] == 'ip6':
+            filter = '(icmpv6.type == 135)' \
+                ' && (ipv6.src == 1234:1::<x>)' \
+                ' && (icmpv6.nd.ns.target_address == 1234:1::1)' \
+                ' && !expert.severity'
         for j in range(num_devs):
             if ip_versions[i] == 'ip4':
-                filter = '(arp.opcode == 1)' \
+                filter = filter.replace('<x>', str(101+j*ip_step))
-                    ' && (arp.src.proto_ipv4 == 10.10.1.' \
-                            + str(101+j*ip_step) + ')' \
-                    ' && (arp.dst.proto_ipv4 == 10.10.1.1)'
             elif ip_versions[i] == 'ip6':
-                filter = '(icmpv6.type == 135)' \
+                filter = filter.replace('<x>', format(0x65+j*ip_step, 'x'))
-                    ' && (icmpv6.nd.ns.target_address == 1234:1::1)' \
+            #print filter
-                    ' && (icmpv6.nd.ns.target_address == 1234:1::' \
-                        + format(0x65+i*ip_step, 'x')+')'
-                print filter
-            else:
-                assert False # unreachable
             cap_pkts = subprocess.check_output([tshark, '-nr', 'capture.pcap',
                         '-Y', filter])
             print(cap_pkts)
@@ -586,19 +589,21 @@ def test_multiEmulDevNoVlan(drone, ports, dut, dut_ports, dut_ip,
     print(cap_pkts)
     log.info('dumping Rx capture buffer (filtered)')
     for i in range(len(ip_versions)):
+        if ip_versions[i] == 'ip4':
+            filter = '(arp.opcode == 1)' \
+                ' && (arp.src.proto_ipv4 == 10.10.2.<x>)' \
+                ' && (arp.dst.proto_ipv4 == 10.10.2.1)' \
+                ' && !expert.severity'
+        elif ip_versions[i] == 'ip6':
+            filter = '(icmpv6.type == 135)' \
+                ' && (ipv6.src == 1234:2::<x>)' \
+                ' && (icmpv6.nd.ns.target_address == 1234:2::1)' \
+                ' && !expert.severity'
         for j in range(num_devs):
             if ip_versions[i] == 'ip4':
-                filter = '(arp.opcode == 1)' \
+                filter = filter.replace('<x>', str(101+j*ip_step))
-                    ' && (arp.src.proto_ipv4 == 10.10.2.' \
-                        + str(101+j*ip_step) + ')' \
-                    ' && (arp.dst.proto_ipv4 == 10.10.2.1)'
             elif ip_versions[i] == 'ip6':
-                filter = '(icmpv6.type == 135)' \
+                filter = filter.replace('<x>', format(0x65+j*ip_step, 'x'))
-                    ' && (icmpv6.nd.ns.target_address == 1234:2::1)' \
-                    ' && (icmpv6.nd.ns.target_address == 1234:2::' \
-                        + format(0x65+i*ip_step, 'x')+')'
-            else:
-                assert False # unreachable
             print filter
             cap_pkts = subprocess.check_output([tshark, '-nr', 'capture.pcap',
                         '-Y', filter])