diff --git a/common/uint128.h b/common/uint128.h
index 6d3f307..80ccf78 100644
--- a/common/uint128.h
+++ b/common/uint128.h
@@ -20,6 +20,8 @@ along with this program. If not, see
#ifndef _UINT128_H
#define _UINT128_H
+#include
+
#include
#include
@@ -33,8 +35,12 @@ public:
quint64 lo64() const;
quint8* toArray() const;
- UInt128 operator+(const UInt128 &other);
- UInt128 operator*(const uint &other);
+ bool operator==(const UInt128 &other) const;
+ 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_<>(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(const uchar *src)
+{
+ quint64 hi, lo;
+
+ hi = qFromBigEndian(src);
+ lo = qFromBigEndian(src+8);
+
+ return UInt128(hi, lo);
+}
+
+template <> inline UInt128 qToBigEndian(const UInt128 src)
+{
+ quint64 hi, lo;
+
+ hi = qToBigEndian(src.hi64());
+ lo = qToBigEndian(src.lo64());
+
+ return UInt128(hi, lo);
+}
+
+inline uint qHash(const UInt128 &key)
+{
+ return qHash(key.hi64()) ^ qHash(key.lo64());
+}
+
#endif
diff --git a/server/device.cpp b/server/device.cpp
index f736de4..0fa731a 100644
--- a/server/device.cpp
+++ b/server/device.cpp
@@ -28,6 +28,10 @@ along with this program. If not, see
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;
- memcpy(key_.data() + ofs, (char*)&mac, sizeof(mac));
+ mac_ = mac & ~(0xffffULL << 48);
+ 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 ipList = arpTable.keys();
- QList macList = arpTable.values();
+ QList ipList = arpTable_.keys();
+ QList 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(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(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(pktData + 12); // srcIp in original pkt
srcIp = qFromBigEndian(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(pktData + 8);
+ dstIp = qFromBigEndian(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)
diff --git a/server/device.h b/server/device.h
index e2bb69d..e0c1455 100644
--- a/server/device.h
+++ b/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 arpTable;
+ QHash arpTable_;
+ QHash ndpTable_;
};
bool operator<(const DeviceKey &a1, const DeviceKey &a2);
diff --git a/test/emultest.py b/test/emultest.py
index 5a114e1..a788a12 100644
--- a/test/emultest.py
+++ b/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.)' \
+ ' && (arp.dst.proto_ipv4 == 10.10.1.1)' \
+ ' && !expert.severity'
+ elif ip_versions[i] == 'ip6':
+ filter = '(icmpv6.type == 135)' \
+ ' && (ipv6.src == 1234:1::)' \
+ ' && (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)' \
- ' && (arp.src.proto_ipv4 == 10.10.1.' \
- + str(101+j*ip_step) + ')' \
- ' && (arp.dst.proto_ipv4 == 10.10.1.1)'
+ filter = filter.replace('', str(101+j*ip_step))
elif ip_versions[i] == 'ip6':
- filter = '(icmpv6.type == 135)' \
- ' && (icmpv6.nd.ns.target_address == 1234:1::1)' \
- ' && (icmpv6.nd.ns.target_address == 1234:1::' \
- + format(0x65+i*ip_step, 'x')+')'
- print filter
- else:
- assert False # unreachable
+ filter = filter.replace('', format(0x65+j*ip_step, 'x'))
+ #print filter
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.)' \
+ ' && (arp.dst.proto_ipv4 == 10.10.2.1)' \
+ ' && !expert.severity'
+ elif ip_versions[i] == 'ip6':
+ filter = '(icmpv6.type == 135)' \
+ ' && (ipv6.src == 1234:2::)' \
+ ' && (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)' \
- ' && (arp.src.proto_ipv4 == 10.10.2.' \
- + str(101+j*ip_step) + ')' \
- ' && (arp.dst.proto_ipv4 == 10.10.2.1)'
+ filter = filter.replace('', str(101+j*ip_step))
elif ip_versions[i] == 'ip6':
- filter = '(icmpv6.type == 135)' \
- ' && (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
+ filter = filter.replace('', format(0x65+j*ip_step, 'x'))
print filter
cap_pkts = subprocess.check_output([tshark, '-nr', 'capture.pcap',
'-Y', filter])