matthew/ostinato
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

HostDev: Refactored Device into base Device and EmulDevice

Browse Source
This commit is contained in:
Srivats P 2018-07-19 18:54:01 +05:30
parent 016694f1fe
commit 0d5e836d3f
6 changed files with 934 additions and 780 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

780
server/device.cpp
View File

@ -41,22 +41,6 @@ const quint8 kIpProtoIcmp6 = 58;
* (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;
}
inline bool isIp6Mcast(UInt128 ip)
{
return (ip.hi64() >> 56) == 0xff;
}
Device::Device(DeviceManager *deviceManager)
{
deviceManager_ = deviceManager;
@ -189,75 +173,13 @@ void Device::clearKey()
key_.fill(0, kMaxVlan * sizeof(quint16) + sizeof(quint64));
}
int Device::encapSize()
{
// ethernet header + vlans
int size = 14 + 4*numVlanTags_;
return size;
}
void Device::encap(PacketBuffer *pktBuf, quint64 dstMac, quint16 type)
{
int ofs;
quint64 srcMac = mac_;
uchar *p = pktBuf->push(encapSize());
if (!p) {
qWarning("%s: failed to push %d bytes [0x%p, 0x%p]", __FUNCTION__,
encapSize(), pktBuf->head(), pktBuf->data());
goto _exit;
}
*(quint32*)(p ) = qToBigEndian(quint32(dstMac >> 16));
*(quint16*)(p + 4) = qToBigEndian(quint16(dstMac & 0xffff));
*(quint32*)(p + 6) = qToBigEndian(quint32(srcMac >> 16));
*(quint16*)(p + 10) = qToBigEndian(quint16(srcMac & 0xffff));
ofs = 12;
for (int i = 0; i < numVlanTags_; i++) {
*(quint32*)(p + ofs) = qToBigEndian(vlan_[i]);
ofs += 4;
}
*(quint16*)(p + ofs) = qToBigEndian(type);
ofs += 2;
Q_ASSERT(ofs == encapSize());
_exit:
return;
}
// We expect pktBuf to point to EthType on entry
/*
void Device::receivePacket(PacketBuffer *pktBuf)
{
quint16 ethType = qFromBigEndian<quint16>(pktBuf->data());
pktBuf->pull(2);
qDebug("%s: ethType 0x%x", __PRETTY_FUNCTION__, ethType);
switch(ethType)
{
case 0x0806: // ARP
if (hasIp4_)
receiveArp(pktBuf);
break;
case 0x0800: // IPv4
if (hasIp4_)
receiveIp4(pktBuf);
break;
case 0x86dd: // IPv6
if (hasIp6_)
receiveIp6(pktBuf);
break;
default:
break;
}
// FIXME: temporary hack till DeviceManager clones pbufs
pktBuf->push(2);
// XXX: Pure Virtual: Subclass should implement
// We expect pktBuf to point to EthType on entry
}
*/
void Device::transmitPacket(PacketBuffer *pktBuf)
{
@ -273,35 +195,6 @@ void Device::resolveGateway()
sendNeighborSolicit(ip6Gateway_);
}
void Device::clearNeighbors(Device::NeighborSet set)
{
QMutableHashIterator<quint32, quint64> arpIter(arpTable_);
QMutableHashIterator<UInt128, quint64> ndpIter(ndpTable_);
switch (set) {
case kAllNeighbors:
arpTable_.clear();
ndpTable_.clear();
break;
case kUnresolvedNeighbors:
while (arpIter.hasNext()) {
arpIter.next();
if (arpIter.value() == 0)
arpIter.remove();
}
while (ndpIter.hasNext()) {
ndpIter.next();
if (ndpIter.value() == 0)
ndpIter.remove();
}
break;
default:
Q_ASSERT(false); // Unreachable!
}
}
// Resolve the Neighbor IP address for this to-be-transmitted pktBuf
// We expect pktBuf to point to EthType on entry
void Device::resolveNeighbor(PacketBuffer *pktBuf)
@ -330,33 +223,19 @@ void Device::resolveNeighbor(PacketBuffer *pktBuf)
pktBuf->push(2);
}
// Append this device's neighbors to the list
void Device::getNeighbors(OstEmul::DeviceNeighborList *neighbors)
/*
void Device::clearNeighbors(Device::NeighborSet set)
{
QList<quint32> ip4List = arpTable_.keys();
QList<UInt128> ip6List = ndpTable_.keys();
QList<quint64> macList;
macList = arpTable_.values();
Q_ASSERT(ip4List.size() == macList.size());
for (int i = 0; i < ip4List.size(); i++) {
OstEmul::ArpEntry *arp = neighbors->add_arp();
arp->set_ip4(ip4List.at(i));
arp->set_mac(macList.at(i));
}
macList = ndpTable_.values();
Q_ASSERT(ip6List.size() == macList.size());
for (int i = 0; i < ip6List.size(); i++) {
OstEmul::NdpEntry *ndp = neighbors->add_ndp();
ndp->mutable_ip6()->set_hi(ip6List.at(i).hi64());
ndp->mutable_ip6()->set_lo(ip6List.at(i).lo64());
ndp->set_mac(macList.at(i));
}
// XXX: Pure virtual: Subclass should implement
}
// Append this device's neighbors to the list
void Device::getNeighbors(OstEmul::DeviceNeighborList* neighbors)
{
// XXX: Subclass should implement
}
*/
// Are we the source of the given packet?
// We expect pktBuf to point to EthType on entry
bool Device::isOrigin(const PacketBuffer *pktBuf)
@ -428,7 +307,7 @@ quint64 Device::neighborMac(const PacketBuffer *pktBuf)
}
tgtIp = ((dstIp & ip4Mask_) == ip4Subnet_) ? dstIp : ip4Gateway_;
return arpTable_.value(tgtIp);
return arpLookup(tgtIp);
}
else if ((ethType == kEthTypeIp6) && hasIp6_) { // IPv6
UInt128 dstIp, tgtIp;
@ -447,122 +326,24 @@ quint64 Device::neighborMac(const PacketBuffer *pktBuf)
}
tgtIp = ((dstIp & ip6Mask_) == ip6Subnet_) ? dstIp : ip6Gateway_;
return ndpTable_.value(tgtIp);
return ndpLookup(tgtIp);
}
return false;
}
//
// Private Methods
//
/*
* ---------------------------------------------------------
* IPv4 related private methods
* ---------------------------------------------------------
*/
void Device::receiveArp(PacketBuffer *pktBuf)
quint64 Device::arpLookup(quint32 ip)
{
PacketBuffer *rspPkt;
uchar *pktData = pktBuf->data();
int offset = 0;
quint16 hwType, protoType;
quint8 hwAddrLen, protoAddrLen;
quint16 opCode;
quint64 srcMac, tgtMac;
quint32 srcIp, tgtIp;
// Extract tgtIp first to check quickly if this packet is for us or not
tgtIp = qFromBigEndian<quint32>(pktData + 24);
if (tgtIp != ip4_) {
qDebug("tgtIp %s is not me %s",
qPrintable(QHostAddress(tgtIp).toString()),
qPrintable(QHostAddress(ip4_).toString()));
return;
}
// Extract annd verify ARP packet contents
hwType = qFromBigEndian<quint16>(pktData + offset);
offset += 2;
if (hwType != 1) // Mac
goto _invalid_exit;
protoType = qFromBigEndian<quint16>(pktData + offset);
offset += 2;
if (protoType != 0x0800) // IPv4
goto _invalid_exit;
hwAddrLen = pktData[offset];
offset += 1;
if (hwAddrLen != 6)
goto _invalid_exit;
protoAddrLen = pktData[offset];
offset += 1;
if (protoAddrLen != 4)
goto _invalid_exit;
opCode = qFromBigEndian<quint16>(pktData + offset);
offset += 2;
srcMac = qFromBigEndian<quint32>(pktData + offset);
offset += 4;
srcMac = (srcMac << 16) | qFromBigEndian<quint16>(pktData + offset);
offset += 2;
srcIp = qFromBigEndian<quint32>(pktData + offset);
offset += 4;
tgtMac = qFromBigEndian<quint32>(pktData + offset);
offset += 4;
tgtMac = (tgtMac << 16) | qFromBigEndian<quint16>(pktData + offset);
offset += 2;
switch (opCode)
{
case 1: // ARP Request
arpTable_.insert(srcIp, srcMac);
rspPkt = new PacketBuffer;
rspPkt->reserve(encapSize());
pktData = rspPkt->put(28);
if (pktData) {
// HTYP, PTYP
*(quint32*)(pktData ) = qToBigEndian(quint32(0x00010800));
// HLEN, PLEN, OPER
*(quint32*)(pktData+ 4) = qToBigEndian(quint32(0x06040002));
// Source H/W Addr, Proto Addr
*(quint32*)(pktData+ 8) = qToBigEndian(quint32(mac_ >> 16));
*(quint16*)(pktData+12) = qToBigEndian(quint16(mac_ & 0xffff));
*(quint32*)(pktData+14) = qToBigEndian(ip4_);
// Target H/W Addr, Proto Addr
*(quint32*)(pktData+18) = qToBigEndian(quint32(srcMac >> 16));
*(quint16*)(pktData+22) = qToBigEndian(quint16(srcMac & 0xffff));
*(quint32*)(pktData+24) = qToBigEndian(srcIp);
}
encap(rspPkt, srcMac, 0x0806);
transmitPacket(rspPkt);
qDebug("Sent ARP Reply for srcIp/tgtIp=%s/%s",
qPrintable(QHostAddress(srcIp).toString()),
qPrintable(QHostAddress(tgtIp).toString()));
break;
case 2: // ARP Response
arpTable_.insert(srcIp, srcMac);
break;
default:
break;
}
return;
_invalid_exit:
qWarning("Invalid ARP content");
return;
// XXX: Pure virtual: Subclass should implement
}
quint64 Device::ndpLookup(UInt128 ip)
{
// XXX: Pure virtual: Subclass should implement
}
*/
// Send ARP request for the IPv4 packet in pktBuf
// pktBuf points to start of IP header
void Device::sendArpRequest(PacketBuffer *pktBuf)
@ -585,371 +366,6 @@ void Device::sendArpRequest(PacketBuffer *pktBuf)
}
void Device::sendArpRequest(quint32 tgtIp)
{
quint32 srcIp = ip4_;
PacketBuffer *reqPkt;
uchar *pktData;
// Validate target IP
if (!tgtIp)
return;
// This function will be called once per unique stream - which
// may all have the same dst IP; even if dst IP are different the
// gateway for the different dst IP may all be same. However,
// we don't want to send duplicate ARP requests, so we check
// if the tgtIP is already in the cache (resolved or unresolved)
// and if so, we don't resend it
if (arpTable_.contains(tgtIp))
return;
reqPkt = new PacketBuffer;
reqPkt->reserve(encapSize());
pktData = reqPkt->put(28);
if (pktData) {
// HTYP, PTYP
*(quint32*)(pktData ) = qToBigEndian(quint32(0x00010800));
// HLEN, PLEN, OPER
*(quint32*)(pktData+ 4) = qToBigEndian(quint32(0x06040001));
// Source H/W Addr, Proto Addr
*(quint32*)(pktData+ 8) = qToBigEndian(quint32(mac_ >> 16));
*(quint16*)(pktData+12) = qToBigEndian(quint16(mac_ & 0xffff));
*(quint32*)(pktData+14) = qToBigEndian(srcIp);
// Target H/W Addr, Proto Addr
*(quint32*)(pktData+18) = qToBigEndian(quint32(0));
*(quint16*)(pktData+22) = qToBigEndian(quint16(0));
*(quint32*)(pktData+24) = qToBigEndian(tgtIp);
}
encap(reqPkt, kBcastMac, 0x0806);
transmitPacket(reqPkt);
arpTable_.insert(tgtIp, 0);
qDebug("Sent ARP Request for srcIp/tgtIp=%s/%s",
qPrintable(QHostAddress(srcIp).toString()),
qPrintable(QHostAddress(tgtIp).toString()));
}
void Device::receiveIp4(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
uchar ipProto;
quint32 dstIp;
if (pktData[0] != 0x45) {
qDebug("%s: Unsupported IP version or options (%02x) ", __FUNCTION__,
pktData[0]);
goto _invalid_exit;
}
if (pktBuf->length() < 20) {
qDebug("incomplete IPv4 header: expected 20, actual %d",
pktBuf->length());
goto _invalid_exit;
}
// XXX: We don't verify IP Header checksum
dstIp = qFromBigEndian<quint32>(pktData + 16);
if (dstIp != ip4_) {
qDebug("%s: dstIp %x is not me (%x)", __FUNCTION__, dstIp, ip4_);
goto _invalid_exit;
}
ipProto = pktData[9];
qDebug("%s: ipProto = %d", __FUNCTION__, ipProto);
switch (ipProto) {
case 1: // ICMP
pktBuf->pull(20);
receiveIcmp4(pktBuf);
break;
default:
qWarning("%s: Unsupported ipProto %d", __FUNCTION__, ipProto);
break;
}
_invalid_exit:
return;
}
// This function assumes we are replying back to the same IP
// that originally sent us the packet and therefore we can reuse the
// ingress packet for egress; in other words, it assumes the
// original IP header is intact and will just reuse it after
// minimal modifications
void Device::sendIp4Reply(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->push(20);
uchar origTtl = pktData[8];
uchar ipProto = pktData[9];
quint32 srcIp, dstIp, tgtIp;
quint32 sum;
// Swap src/dst IP addresses
dstIp = qFromBigEndian<quint32>(pktData + 12); // srcIp in original pkt
srcIp = qFromBigEndian<quint32>(pktData + 16); // dstIp in original pkt
tgtIp = ((dstIp & ip4Mask_) == ip4Subnet_) ? dstIp : ip4Gateway_;
if (!arpTable_.contains(tgtIp)) {
qWarning("%s: mac not found for %s; unable to send IPv4 packet",
__FUNCTION__, qPrintable(QHostAddress(tgtIp).toString()));
return;
}
*(quint32*)(pktData + 12) = qToBigEndian(srcIp);
*(quint32*)(pktData + 16) = qToBigEndian(dstIp);
// Reset TTL
pktData[8] = 64;
// Incremental checksum update (RFC 1624 [Eqn.3])
// HC' = ~(~HC + ~m + m')
sum = quint16(~qFromBigEndian<quint16>(pktData + 10)); // old cksum
sum += quint16(~quint16(origTtl << 8 | ipProto)); // old value
sum += quint16(pktData[8] << 8 | ipProto); // new value
while(sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
*(quint16*)(pktData + 10) = qToBigEndian(quint16(~sum));
encap(pktBuf, arpTable_.value(tgtIp), 0x0800);
transmitPacket(pktBuf);
}
void Device::receiveIcmp4(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
quint32 sum;
// XXX: We don't verify icmp checksum
// We handle only ping request
if (pktData[0] != 8) { // Echo Request
qDebug("%s: Ignoring non echo request (%d)", __FUNCTION__, pktData[0]);
return;
}
pktData[0] = 0; // Echo Reply
// Incremental checksum update (RFC 1624 [Eqn.3])
// HC' = ~(~HC + ~m + m')
sum = quint16(~qFromBigEndian<quint16>(pktData + 2)); // old cksum
sum += quint16(~quint16(8 << 8 | pktData[1])); // old value
sum += quint16(0 << 8 | pktData[1]); // new value
while(sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
*(quint16*)(pktData + 2) = qToBigEndian(quint16(~sum));
sendIp4Reply(pktBuf);
qDebug("Sent ICMP Echo Reply");
}
/*
* ---------------------------------------------------------
* IPV6 related private methods
* ---------------------------------------------------------
*/
void Device::receiveIp6(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
uchar ipProto;
UInt128 dstIp;
if ((pktData[0] & 0xF0) != 0x60) {
qDebug("%s: Unsupported IP version (%02x) ", __FUNCTION__,
pktData[0]);
goto _invalid_exit;
}
if (pktBuf->length() < kIp6HdrLen) {
qDebug("incomplete IPv6 header: expected %d, actual %d",
kIp6HdrLen, pktBuf->length());
goto _invalid_exit;
}
// FIXME: check for specific mcast address(es) instead of any mcast?
dstIp = qFromBigEndian<UInt128>(pktData + 24);
if (!isIp6Mcast(dstIp) && (dstIp != ip6_)) {
qDebug("%s: dstIp %s is not me (%s)", __FUNCTION__,
qPrintable(QHostAddress(dstIp.toArray()).toString()),
qPrintable(QHostAddress(ip6_.toArray()).toString()));
goto _invalid_exit;
}
ipProto = pktData[6];
switch (ipProto) {
case kIpProtoIcmp6:
pktBuf->pull(kIp6HdrLen);
receiveIcmp6(pktBuf);
break;
default:
break;
}
_invalid_exit:
return;
}
// 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;
if (!p) {
qWarning("%s: failed to push %d bytes [0x%p, 0x%p]", __FUNCTION__,
kIp6HdrLen, pktBuf->head(), pktBuf->data());
goto _error_exit;
}
// In case of mcast, derive dstMac
if ((dstIp.hi64() >> 56) == 0xff)
dstMac = (quint64(0x3333) << 32) | (dstIp.lo64() & 0xffffffff);
else {
UInt128 tgtIp = ((dstIp & ip6Mask_) == ip6Subnet_)? dstIp : ip6Gateway_;
dstMac = ndpTable_.value(tgtIp);
}
if (!dstMac) {
qWarning("%s: mac not found for %s; unable to send IPv6 packet",
__FUNCTION__,
qPrintable(QHostAddress(dstIp.toArray()).toString()));
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
// FIXME: both these functions should return success/failure
encap(pktBuf, dstMac, kEthTypeIp6);
transmitPacket(pktBuf);
return true;
_error_exit:
return false;
}
// This function assumes we are replying back to the same IP
// that originally sent us the packet and therefore we can reuse the
// ingress packet for egress; in other words, it assumes the
// original IP header is intact and will just reuse it after
// minimal modifications
void Device::sendIp6Reply(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->push(kIp6HdrLen);
UInt128 srcIp, dstIp, tgtIp;
// Swap src/dst IP addresses
dstIp = qFromBigEndian<UInt128>(pktData + 8); // srcIp in original pkt
srcIp = qFromBigEndian<UInt128>(pktData + 24); // dstIp in original pkt
tgtIp = ((dstIp & ip6Mask_) == ip6Subnet_) ? dstIp : ip6Gateway_;
if (!ndpTable_.contains(tgtIp)) {
qWarning("%s: mac not found for %s; unable to send IPv6 packet",
__FUNCTION__,
qPrintable(QHostAddress(tgtIp.toArray()).toString()));
return;
}
memcpy(pktData + 8, srcIp.toArray(), 16); // Source IP
memcpy(pktData + 24, dstIp.toArray(), 16); // Destination IP
// Reset TTL
pktData[7] = 64;
encap(pktBuf, ndpTable_.value(tgtIp), 0x86dd);
transmitPacket(pktBuf);
}
void Device::receiveIcmp6(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
quint8 type = pktData[0];
quint32 sum;
// XXX: We don't verify icmp checksum
switch (type) {
case 128: // ICMPv6 Echo Request
pktData[0] = 129; // Echo Reply
// Incremental checksum update (RFC 1624 [Eqn.3])
// HC' = ~(~HC + ~m + m')
sum = quint16(~qFromBigEndian<quint16>(pktData + 2)); // old cksum
sum += quint16(~quint16(128 << 8 | pktData[1])); // old value
sum += quint16(129 << 8 | pktData[1]); // new value
while(sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
*(quint16*)(pktData + 2) = qToBigEndian(quint16(~sum));
sendIp6Reply(pktBuf);
qDebug("Sent ICMPv6 Echo Reply");
break;
case 135: // Neigh Solicit
case 136: // Neigh Advt
receiveNdp(pktBuf);
break;
default:
break;
}
}
void Device::receiveNdp(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
quint8 type = pktData[0];
int len = pktBuf->length();
int minLen = 24 + (type == 136 ? 8 : 0); // NA should have the Target TLV
if (len < minLen) {
qDebug("%s: incomplete NS/NA header: expected %d, actual %d",
__FUNCTION__, minLen, pktBuf->length());
goto _invalid_exit;
}
switch (type)
{
case 135: { // Neigh Solicit
// TODO: Validation as per RFC 4861
sendNeighborAdvertisement(pktBuf);
break;
}
case 136: { // Neigh Advt
quint8 flags = pktData[4];
const quint8 kSFlag = 0x40;
const quint8 kOFlag = 0x20;
UInt128 tgtIp = qFromBigEndian<UInt128>(pktData + 8);
quint64 mac = ndpTable_.value(tgtIp);
// Update NDP table only for solicited responses
if (!(flags & kSFlag))
break;
if ((flags & kOFlag) || (mac == 0)) {
// Check if we have a Target Link-Layer TLV
if ((pktData[24] != 2) || (pktData[25] != 1))
goto _invalid_exit;
mac = qFromBigEndian<quint32>(pktData + 26);
mac = (mac << 16) | qFromBigEndian<quint16>(pktData + 30);
ndpTable_.insert(tgtIp, mac);
}
break;
}
}
_invalid_exit:
return;
}
// Send NS for the IPv6 packet in pktBuf
// caller is responsible to check that pktBuf originates from this device
// pktBuf should point to start of IP header
@ -971,146 +387,20 @@ void Device::sendNeighborSolicit(PacketBuffer *pktBuf)
sendNeighborSolicit(tgtIp);
}
//
// Protected Methods
//
/*
void Device::sendArpRequest(quint32 tgtIp)
{
// XXX: Pure virtual: Subclass should implement
}
void Device::sendNeighborSolicit(UInt128 tgtIp)
{
UInt128 dstIp, srcIp = ip6_;
PacketBuffer *reqPkt;
uchar *pktData;
// Validate target IP
if (tgtIp == UInt128(0, 0))
return;
// Do we already have a NDP entry (resolved or unresolved)?
// If so, don't resend (see note in sendArpRequest())
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()));
}
// Send NA for the NS packet in pktBuf
// pktBuf should point to start of ICMPv6 header
void Device::sendNeighborAdvertisement(PacketBuffer *pktBuf)
{
PacketBuffer *naPkt;
uchar *pktData = pktBuf->data();
quint16 flags = 0x6000; // solicit = 1; overide = 1
uchar *ip6Hdr;
UInt128 tgtIp, srcIp;
tgtIp = qFromBigEndian<UInt128>(pktData + 8);
if (tgtIp != ip6_) {
qDebug("%s: NS tgtIp %s is not us %s", __FUNCTION__,
qPrintable(QHostAddress(tgtIp.toArray()).toString()),
qPrintable(QHostAddress(ip6_.toArray()).toString()));
ip6Hdr = pktBuf->push(kIp6HdrLen);
return;
}
ip6Hdr = pktBuf->push(kIp6HdrLen);
srcIp = qFromBigEndian<UInt128>(ip6Hdr + 8);
if (srcIp == UInt128(0, 0)) {
// reset solicit flag
flags &= ~0x4000;
// NA should be sent to All nodes address
srcIp = UInt128(quint64(0xff02) << 48, quint64(1));
}
else if (pktBuf->length() >= 32) { // have TLVs?
if ((pktData[24] == 0x01) && (pktData[25] == 0x01)) { // Source TLV
quint64 mac;
mac = qFromBigEndian<quint32>(pktData + 26);
mac = (mac << 16) | qFromBigEndian<quint16>(pktData + 30);
ndpTable_.insert(srcIp, mac);
}
}
naPkt = new PacketBuffer;
naPkt->reserve(encapSize() + kIp6HdrLen);
pktData = naPkt->put(32);
if (pktData) {
// Calculate checksum first -
// start with fixed fields in ICMP Header and IPv6 Pseudo Header ...
quint32 sum = (0x8800 + flags + 0x0201) + (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(srcIp);
while(sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
// Type, Code
*(quint16*)(pktData ) = qToBigEndian(quint16(0x8800));
// Checksum
*(quint16*)(pktData+ 2) = qToBigEndian(quint16(~sum));
// Flags-Reserved
*(quint32*)(pktData+ 4) = qToBigEndian(quint32(flags << 16));
// Target IP
memcpy(pktData+ 8, tgtIp.toArray(), 16);
// Target Addr TLV + MacAddr
*(quint16*)(pktData+24) = qToBigEndian(quint16(0x0201));
*(quint32*)(pktData+26) = qToBigEndian(quint32(mac_ >> 16));
*(quint16*)(pktData+30) = qToBigEndian(quint16(mac_ & 0xffff));
}
if (!sendIp6(naPkt, srcIp , kIpProtoIcmp6))
return;
qDebug("Sent Neigh Advt to dstIp for tgtIp=%s/%s",
qPrintable(QHostAddress(srcIp.toArray()).toString()),
qPrintable(QHostAddress(tgtIp.toArray()).toString()));
// XXX: Pure virtual: Subclass should implement
}
*/
bool operator<(const DeviceKey &a1, const DeviceKey &a2)
{

43
server/device.h
View File

@ -47,6 +47,7 @@ public:
public:
Device(DeviceManager *deviceManager);
virtual ~Device() = default;
void setVlan(int index, quint16 vlan, quint16 tpid = kVlanTpid);
quint64 mac();
@ -59,43 +60,25 @@ public:
DeviceKey key();
void clearKey();
int encapSize();
void encap(PacketBuffer *pktBuf, quint64 dstMac, quint16 type);
void receivePacket(PacketBuffer *pktBuf);
virtual void receivePacket(PacketBuffer *pktBuf) = 0;
void transmitPacket(PacketBuffer *pktBuf);
void resolveGateway();
void clearNeighbors(Device::NeighborSet set);
void resolveNeighbor(PacketBuffer *pktBuf);
void getNeighbors(OstEmul::DeviceNeighborList *neighbors);
virtual void clearNeighbors(Device::NeighborSet set) = 0;
virtual void getNeighbors(OstEmul::DeviceNeighborList *neighbors) = 0;
bool isOrigin(const PacketBuffer *pktBuf);
quint64 neighborMac(const PacketBuffer *pktBuf);
private: // methods
void receiveArp(PacketBuffer *pktBuf);
void sendArpRequest(PacketBuffer *pktBuf);
void sendArpRequest(quint32 tgtIp);
protected: // methods
virtual quint64 arpLookup(quint32 ip) = 0;
virtual quint64 ndpLookup(UInt128 ip) = 0;
virtual void sendArpRequest(quint32 tgtIp) = 0;
virtual void sendNeighborSolicit(UInt128 tgtIp) = 0;
void receiveIp4(PacketBuffer *pktBuf);
void sendIp4Reply(PacketBuffer *pktBuf);
void receiveIcmp4(PacketBuffer *pktBuf);
void receiveIp6(PacketBuffer *pktBuf);
bool sendIp6(PacketBuffer *pktBuf, UInt128 dstIp, quint8 protocol);
void sendIp6Reply(PacketBuffer *pktBuf);
void receiveIcmp6(PacketBuffer *pktBuf);
void receiveNdp(PacketBuffer *pktBuf);
void sendNeighborSolicit(PacketBuffer *pktBuf);
void sendNeighborSolicit(UInt128 tgtIp);
void sendNeighborAdvertisement(PacketBuffer *pktBuf);
private: // data
protected: // data
static const int kMaxVlan = 4;
DeviceManager *deviceManager_;
@ -120,8 +103,10 @@ private: // data
DeviceKey key_;
QHash<quint32, quint64> arpTable_;
QHash<UInt128, quint64> ndpTable_;
private: // methods
void sendArpRequest(PacketBuffer *pktBuf);
void sendNeighborSolicit(PacketBuffer *pktBuf);
};
bool operator<(const DeviceKey &a1, const DeviceKey &a2);

13
server/devicemanager.cpp
View File

@ -21,6 +21,7 @@ along with this program. If not, see <http://www.gnu.org/licenses/>
#include "abstractport.h"
#include "device.h"
#include "emuldevice.h"
#include "../common/emulation.h"
#include "packetbuffer.h"
@ -169,7 +170,7 @@ void DeviceManager::receivePacket(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
int offset = 0;
Device dk(this);
EmulDevice dk(this);
Device *device;
quint64 dstMac;
quint16 ethType;
@ -304,7 +305,7 @@ Device* DeviceManager::originDevice(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
int offset = 12; // start parsing after mac addresses
Device dk(this);
EmulDevice dk(this);
quint16 ethType;
quint16 vlan;
int idx = 0;
@ -346,7 +347,7 @@ void DeviceManager::enumerateDevices(
const OstProto::DeviceGroup *deviceGroup,
Operation oper)
{
Device dk(this);
EmulDevice dk(this);
OstEmul::VlanEmulation pbVlan = deviceGroup->encap()
.GetExtension(OstEmul::vlan);
int numTags = pbVlan.stack_size();
@ -458,14 +459,14 @@ void DeviceManager::enumerateDevices(
__FUNCTION__, qPrintable(dk.config()));
break;
}
device = new Device(this);
device = new EmulDevice(this);
*device = dk;
deviceList_.insert(dk.key(), device);
sortedDeviceList_.insert(dk.key(), device);
dk.setMac(kBcastMac);
bcastList_.insert(dk.key(), device);
qDebug("enumerate(add): %s", qPrintable(device->config()));
qDebug("enumerate(add): %p %s", device, qPrintable(device->config()));
break;
case kDelete:
@ -475,7 +476,7 @@ void DeviceManager::enumerateDevices(
__FUNCTION__, qPrintable(dk.config()));
break;
}
qDebug("enumerate(del): %s", qPrintable(device->config()));
qDebug("enumerate(del): %p %s", device, qPrintable(device->config()));
delete device;
sortedDeviceList_.take(dk.key()); // already freed above

1
server/drone.pro
View File

@ -37,6 +37,7 @@ HEADERS += drone.h \
SOURCES += \
devicemanager.cpp \
device.cpp \
emuldevice.cpp \
drone_main.cpp \
drone.cpp \
portmanager.cpp \

805
server/emuldevice.cpp Normal file
View File

@ -0,0 +1,805 @@
/*
Copyright (C) 2015 Srivats P.
This file is part of "Ostinato"
This is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>
*/
#include "emuldevice.h"
#include "devicemanager.h"
#include "packetbuffer.h"
#include <QHostAddress>
const quint64 kBcastMac = 0xffffffffffffULL;
const quint16 kEthTypeIp4 = 0x0800;
const quint16 kEthTypeIp6 = 0x86dd;
const int kIp6HdrLen = 40;
const quint8 kIpProtoIcmp6 = 58;
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;
}
inline bool isIp6Mcast(UInt128 ip)
{
return (ip.hi64() >> 56) == 0xff;
}
EmulDevice::EmulDevice(DeviceManager *deviceManager)
: Device(deviceManager)
{
}
int EmulDevice::encapSize()
{
Q_ASSERT(numVlanTags_ >= 0);
// ethernet header + vlans
int size = 14 + kMaxVlan*numVlanTags_;
return size;
}
void EmulDevice::encap(PacketBuffer *pktBuf, quint64 dstMac, quint16 type)
{
int ofs;
quint64 srcMac = mac_;
uchar *p = pktBuf->push(encapSize());
if (!p) {
qWarning("%s: failed to push %d bytes [0x%p, 0x%p]", __FUNCTION__,
encapSize(), pktBuf->head(), pktBuf->data());
goto _exit;
}
*(quint32*)(p ) = qToBigEndian(quint32(dstMac >> 16));
*(quint16*)(p + 4) = qToBigEndian(quint16(dstMac & 0xffff));
*(quint32*)(p + 6) = qToBigEndian(quint32(srcMac >> 16));
*(quint16*)(p + 10) = qToBigEndian(quint16(srcMac & 0xffff));
ofs = 12;
for (int i = 0; i < numVlanTags_; i++) {
*(quint32*)(p + ofs) = qToBigEndian(vlan_[i]);
ofs += 4;
}
*(quint16*)(p + ofs) = qToBigEndian(type);
ofs += 2;
Q_ASSERT(ofs == encapSize());
_exit:
return;
}
// We expect pktBuf to point to EthType on entry
void EmulDevice::receivePacket(PacketBuffer *pktBuf)
{
quint16 ethType = qFromBigEndian<quint16>(pktBuf->data());
pktBuf->pull(2);
qDebug("%s: ethType 0x%x", __PRETTY_FUNCTION__, ethType);
switch(ethType)
{
case 0x0806: // ARP
if (hasIp4_)
receiveArp(pktBuf);
break;
case 0x0800: // IPv4
if (hasIp4_)
receiveIp4(pktBuf);
break;
case 0x86dd: // IPv6
if (hasIp6_)
receiveIp6(pktBuf);
break;
default:
break;
}
// FIXME: temporary hack till DeviceManager clones pbufs
pktBuf->push(2);
}
void EmulDevice::clearNeighbors(EmulDevice::NeighborSet set)
{
QMutableHashIterator<quint32, quint64> arpIter(arpTable_);
QMutableHashIterator<UInt128, quint64> ndpIter(ndpTable_);
switch (set) {
case kAllNeighbors:
arpTable_.clear();
ndpTable_.clear();
break;
case kUnresolvedNeighbors:
while (arpIter.hasNext()) {
arpIter.next();
if (arpIter.value() == 0)
arpIter.remove();
}
while (ndpIter.hasNext()) {
ndpIter.next();
if (ndpIter.value() == 0)
ndpIter.remove();
}
break;
default:
Q_ASSERT(false); // Unreachable!
}
}
// Append this device's neighbors to the list
void EmulDevice::getNeighbors(OstEmul::DeviceNeighborList *neighbors)
{
QList<quint32> ip4List = arpTable_.keys();
QList<UInt128> ip6List = ndpTable_.keys();
QList<quint64> macList;
macList = arpTable_.values();
Q_ASSERT(ip4List.size() == macList.size());
for (int i = 0; i < ip4List.size(); i++) {
OstEmul::ArpEntry *arp = neighbors->add_arp();
arp->set_ip4(ip4List.at(i));
arp->set_mac(macList.at(i));
}
macList = ndpTable_.values();
Q_ASSERT(ip6List.size() == macList.size());
for (int i = 0; i < ip6List.size(); i++) {
OstEmul::NdpEntry *ndp = neighbors->add_ndp();
ndp->mutable_ip6()->set_hi(ip6List.at(i).hi64());
ndp->mutable_ip6()->set_lo(ip6List.at(i).lo64());
ndp->set_mac(macList.at(i));
}
}
//
// Protected/Private Methods
//
/*
* ---------------------------------------------------------
* IPv4 related protected/private methods
* ---------------------------------------------------------
*/
void EmulDevice::receiveArp(PacketBuffer *pktBuf)
{
PacketBuffer *rspPkt;
uchar *pktData = pktBuf->data();
int offset = 0;
quint16 hwType, protoType;
quint8 hwAddrLen, protoAddrLen;
quint16 opCode;
quint64 srcMac, tgtMac;
quint32 srcIp, tgtIp;
// Extract tgtIp first to check quickly if this packet is for us or not
tgtIp = qFromBigEndian<quint32>(pktData + 24);
if (tgtIp != ip4_) {
qDebug("tgtIp %s is not me %s",
qPrintable(QHostAddress(tgtIp).toString()),
qPrintable(QHostAddress(ip4_).toString()));
return;
}
// Extract annd verify ARP packet contents
hwType = qFromBigEndian<quint16>(pktData + offset);
offset += 2;
if (hwType != 1) // Mac
goto _invalid_exit;
protoType = qFromBigEndian<quint16>(pktData + offset);
offset += 2;
if (protoType != 0x0800) // IPv4
goto _invalid_exit;
hwAddrLen = pktData[offset];
offset += 1;
if (hwAddrLen != 6)
goto _invalid_exit;
protoAddrLen = pktData[offset];
offset += 1;
if (protoAddrLen != 4)
goto _invalid_exit;
opCode = qFromBigEndian<quint16>(pktData + offset);
offset += 2;
srcMac = qFromBigEndian<quint32>(pktData + offset);
offset += 4;
srcMac = (srcMac << 16) | qFromBigEndian<quint16>(pktData + offset);
offset += 2;
srcIp = qFromBigEndian<quint32>(pktData + offset);
offset += 4;
tgtMac = qFromBigEndian<quint32>(pktData + offset);
offset += 4;
tgtMac = (tgtMac << 16) | qFromBigEndian<quint16>(pktData + offset);
offset += 2;
switch (opCode)
{
case 1: // ARP Request
arpTable_.insert(srcIp, srcMac);
rspPkt = new PacketBuffer;
rspPkt->reserve(encapSize());
pktData = rspPkt->put(28);
if (pktData) {
// HTYP, PTYP
*(quint32*)(pktData ) = qToBigEndian(quint32(0x00010800));
// HLEN, PLEN, OPER
*(quint32*)(pktData+ 4) = qToBigEndian(quint32(0x06040002));
// Source H/W Addr, Proto Addr
*(quint32*)(pktData+ 8) = qToBigEndian(quint32(mac_ >> 16));
*(quint16*)(pktData+12) = qToBigEndian(quint16(mac_ & 0xffff));
*(quint32*)(pktData+14) = qToBigEndian(ip4_);
// Target H/W Addr, Proto Addr
*(quint32*)(pktData+18) = qToBigEndian(quint32(srcMac >> 16));
*(quint16*)(pktData+22) = qToBigEndian(quint16(srcMac & 0xffff));
*(quint32*)(pktData+24) = qToBigEndian(srcIp);
}
encap(rspPkt, srcMac, 0x0806);
transmitPacket(rspPkt);
qDebug("Sent ARP Reply for srcIp/tgtIp=%s/%s",
qPrintable(QHostAddress(srcIp).toString()),
qPrintable(QHostAddress(tgtIp).toString()));
break;
case 2: // ARP Response
arpTable_.insert(srcIp, srcMac);
break;
default:
break;
}
return;
_invalid_exit:
qWarning("Invalid ARP content");
return;
}
quint64 EmulDevice::arpLookup(quint32 ip)
{
return arpTable_.value(ip);
}
quint64 EmulDevice::ndpLookup(UInt128 ip)
{
return ndpTable_.value(ip);
}
void EmulDevice::sendArpRequest(quint32 tgtIp)
{
quint32 srcIp = ip4_;
PacketBuffer *reqPkt;
uchar *pktData;
// Validate target IP
if (!tgtIp)
return;
// This function will be called once per unique stream - which
// may all have the same dst IP; even if dst IP are different the
// gateway for the different dst IP may all be same. However,
// we don't want to send duplicate ARP requests, so we check
// if the tgtIP is already in the cache (resolved or unresolved)
// and if so, we don't resend it
if (arpTable_.contains(tgtIp))
return;
reqPkt = new PacketBuffer;
reqPkt->reserve(encapSize());
pktData = reqPkt->put(28);
if (pktData) {
// HTYP, PTYP
*(quint32*)(pktData ) = qToBigEndian(quint32(0x00010800));
// HLEN, PLEN, OPER
*(quint32*)(pktData+ 4) = qToBigEndian(quint32(0x06040001));
// Source H/W Addr, Proto Addr
*(quint32*)(pktData+ 8) = qToBigEndian(quint32(mac_ >> 16));
*(quint16*)(pktData+12) = qToBigEndian(quint16(mac_ & 0xffff));
*(quint32*)(pktData+14) = qToBigEndian(srcIp);
// Target H/W Addr, Proto Addr
*(quint32*)(pktData+18) = qToBigEndian(quint32(0));
*(quint16*)(pktData+22) = qToBigEndian(quint16(0));
*(quint32*)(pktData+24) = qToBigEndian(tgtIp);
}
encap(reqPkt, kBcastMac, 0x0806);
transmitPacket(reqPkt);
arpTable_.insert(tgtIp, 0);
qDebug("Sent ARP Request for srcIp/tgtIp=%s/%s",
qPrintable(QHostAddress(srcIp).toString()),
qPrintable(QHostAddress(tgtIp).toString()));
}
void EmulDevice::receiveIp4(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
uchar ipProto;
quint32 dstIp;
if (pktData[0] != 0x45) {
qDebug("%s: Unsupported IP version or options (%02x) ", __FUNCTION__,
pktData[0]);
goto _invalid_exit;
}
if (pktBuf->length() < 20) {
qDebug("incomplete IPv4 header: expected 20, actual %d",
pktBuf->length());
goto _invalid_exit;
}
// XXX: We don't verify IP Header checksum
dstIp = qFromBigEndian<quint32>(pktData + 16);
if (dstIp != ip4_) {
qDebug("%s: dstIp %x is not me (%x)", __FUNCTION__, dstIp, ip4_);
goto _invalid_exit;
}
ipProto = pktData[9];
qDebug("%s: ipProto = %d", __FUNCTION__, ipProto);
switch (ipProto) {
case 1: // ICMP
pktBuf->pull(20);
receiveIcmp4(pktBuf);
break;
default:
qWarning("%s: Unsupported ipProto %d", __FUNCTION__, ipProto);
break;
}
_invalid_exit:
return;
}
// This function assumes we are replying back to the same IP
// that originally sent us the packet and therefore we can reuse the
// ingress packet for egress; in other words, it assumes the
// original IP header is intact and will just reuse it after
// minimal modifications
void EmulDevice::sendIp4Reply(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->push(20);
uchar origTtl = pktData[8];
uchar ipProto = pktData[9];
quint32 srcIp, dstIp, tgtIp;
quint32 sum;
// Swap src/dst IP addresses
dstIp = qFromBigEndian<quint32>(pktData + 12); // srcIp in original pkt
srcIp = qFromBigEndian<quint32>(pktData + 16); // dstIp in original pkt
tgtIp = ((dstIp & ip4Mask_) == ip4Subnet_) ? dstIp : ip4Gateway_;
if (!arpTable_.contains(tgtIp)) {
qWarning("%s: mac not found for %s; unable to send IPv4 packet",
__FUNCTION__, qPrintable(QHostAddress(tgtIp).toString()));
return;
}
*(quint32*)(pktData + 12) = qToBigEndian(srcIp);
*(quint32*)(pktData + 16) = qToBigEndian(dstIp);
// Reset TTL
pktData[8] = 64;
// Incremental checksum update (RFC 1624 [Eqn.3])
// HC' = ~(~HC + ~m + m')
sum = quint16(~qFromBigEndian<quint16>(pktData + 10)); // old cksum
sum += quint16(~quint16(origTtl << 8 | ipProto)); // old value
sum += quint16(pktData[8] << 8 | ipProto); // new value
while(sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
*(quint16*)(pktData + 10) = qToBigEndian(quint16(~sum));
encap(pktBuf, arpTable_.value(tgtIp), 0x0800);
transmitPacket(pktBuf);
}
void EmulDevice::receiveIcmp4(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
quint32 sum;
// XXX: We don't verify icmp checksum
// We handle only ping request
if (pktData[0] != 8) { // Echo Request
qDebug("%s: Ignoring non echo request (%d)", __FUNCTION__, pktData[0]);
return;
}
pktData[0] = 0; // Echo Reply
// Incremental checksum update (RFC 1624 [Eqn.3])
// HC' = ~(~HC + ~m + m')
sum = quint16(~qFromBigEndian<quint16>(pktData + 2)); // old cksum
sum += quint16(~quint16(8 << 8 | pktData[1])); // old value
sum += quint16(0 << 8 | pktData[1]); // new value
while(sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
*(quint16*)(pktData + 2) = qToBigEndian(quint16(~sum));
sendIp4Reply(pktBuf);
qDebug("Sent ICMP Echo Reply");
}
/*
* ---------------------------------------------------------
* IPV6 related private methods
* ---------------------------------------------------------
*/
void EmulDevice::receiveIp6(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
uchar ipProto;
UInt128 dstIp;
if ((pktData[0] & 0xF0) != 0x60) {
qDebug("%s: Unsupported IP version (%02x) ", __FUNCTION__,
pktData[0]);
goto _invalid_exit;
}
if (pktBuf->length() < kIp6HdrLen) {
qDebug("incomplete IPv6 header: expected %d, actual %d",
kIp6HdrLen, pktBuf->length());
goto _invalid_exit;
}
// FIXME: check for specific mcast address(es) instead of any mcast?
dstIp = qFromBigEndian<UInt128>(pktData + 24);
if (!isIp6Mcast(dstIp) && (dstIp != ip6_)) {
qDebug("%s: dstIp %s is not me (%s)", __FUNCTION__,
qPrintable(QHostAddress(dstIp.toArray()).toString()),
qPrintable(QHostAddress(ip6_.toArray()).toString()));
goto _invalid_exit;
}
ipProto = pktData[6];
switch (ipProto) {
case kIpProtoIcmp6:
pktBuf->pull(kIp6HdrLen);
receiveIcmp6(pktBuf);
break;
default:
break;
}
_invalid_exit:
return;
}
// pktBuf should point to start of IP payload
bool EmulDevice::sendIp6(PacketBuffer *pktBuf, UInt128 dstIp, quint8 protocol)
{
int payloadLen = pktBuf->length();
uchar *p = pktBuf->push(kIp6HdrLen);
quint64 dstMac;
if (!p) {
qWarning("%s: failed to push %d bytes [0x%p, 0x%p]", __FUNCTION__,
kIp6HdrLen, pktBuf->head(), pktBuf->data());
goto _error_exit;
}
// In case of mcast, derive dstMac
if ((dstIp.hi64() >> 56) == 0xff)
dstMac = (quint64(0x3333) << 32) | (dstIp.lo64() & 0xffffffff);
else {
UInt128 tgtIp = ((dstIp & ip6Mask_) == ip6Subnet_)? dstIp : ip6Gateway_;
dstMac = ndpTable_.value(tgtIp);
}
if (!dstMac) {
qWarning("%s: mac not found for %s; unable to send IPv6 packet",
__FUNCTION__,
qPrintable(QHostAddress(dstIp.toArray()).toString()));
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
// FIXME: both these functions should return success/failure
encap(pktBuf, dstMac, kEthTypeIp6);
transmitPacket(pktBuf);
return true;
_error_exit:
return false;
}
// This function assumes we are replying back to the same IP
// that originally sent us the packet and therefore we can reuse the
// ingress packet for egress; in other words, it assumes the
// original IP header is intact and will just reuse it after
// minimal modifications
void EmulDevice::sendIp6Reply(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->push(kIp6HdrLen);
UInt128 srcIp, dstIp, tgtIp;
// Swap src/dst IP addresses
dstIp = qFromBigEndian<UInt128>(pktData + 8); // srcIp in original pkt
srcIp = qFromBigEndian<UInt128>(pktData + 24); // dstIp in original pkt
tgtIp = ((dstIp & ip6Mask_) == ip6Subnet_) ? dstIp : ip6Gateway_;
if (!ndpTable_.contains(tgtIp)) {
qWarning("%s: mac not found for %s; unable to send IPv6 packet",
__FUNCTION__,
qPrintable(QHostAddress(tgtIp.toArray()).toString()));
return;
}
memcpy(pktData + 8, srcIp.toArray(), 16); // Source IP
memcpy(pktData + 24, dstIp.toArray(), 16); // Destination IP
// Reset TTL
pktData[7] = 64;
encap(pktBuf, ndpTable_.value(tgtIp), 0x86dd);
transmitPacket(pktBuf);
}
void EmulDevice::receiveIcmp6(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
quint8 type = pktData[0];
quint32 sum;
// XXX: We don't verify icmp checksum
switch (type) {
case 128: // ICMPv6 Echo Request
pktData[0] = 129; // Echo Reply
// Incremental checksum update (RFC 1624 [Eqn.3])
// HC' = ~(~HC + ~m + m')
sum = quint16(~qFromBigEndian<quint16>(pktData + 2)); // old cksum
sum += quint16(~quint16(128 << 8 | pktData[1])); // old value
sum += quint16(129 << 8 | pktData[1]); // new value
while(sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
*(quint16*)(pktData + 2) = qToBigEndian(quint16(~sum));
sendIp6Reply(pktBuf);
qDebug("Sent ICMPv6 Echo Reply");
break;
case 135: // Neigh Solicit
case 136: // Neigh Advt
receiveNdp(pktBuf);
break;
default:
break;
}
}
void EmulDevice::receiveNdp(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
quint8 type = pktData[0];
int len = pktBuf->length();
int minLen = 24 + (type == 136 ? 8 : 0); // NA should have the Target TLV
if (len < minLen) {
qDebug("%s: incomplete NS/NA header: expected %d, actual %d",
__FUNCTION__, minLen, pktBuf->length());
goto _invalid_exit;
}
switch (type)
{
case 135: { // Neigh Solicit
// TODO: Validation as per RFC 4861
sendNeighborAdvertisement(pktBuf);
break;
}
case 136: { // Neigh Advt
quint8 flags = pktData[4];
const quint8 kSFlag = 0x40;
const quint8 kOFlag = 0x20;
UInt128 tgtIp = qFromBigEndian<UInt128>(pktData + 8);
quint64 mac = ndpTable_.value(tgtIp);
// Update NDP table only for solicited responses
if (!(flags & kSFlag))
break;
if ((flags & kOFlag) || (mac == 0)) {
// Check if we have a Target Link-Layer TLV
if ((pktData[24] != 2) || (pktData[25] != 1))
goto _invalid_exit;
mac = qFromBigEndian<quint32>(pktData + 26);
mac = (mac << 16) | qFromBigEndian<quint16>(pktData + 30);
ndpTable_.insert(tgtIp, mac);
}
break;
}
}
_invalid_exit:
return;
}
void EmulDevice::sendNeighborSolicit(UInt128 tgtIp)
{
UInt128 dstIp, srcIp = ip6_;
PacketBuffer *reqPkt;
uchar *pktData;
// Validate target IP
if (tgtIp == UInt128(0, 0))
return;
// Do we already have a NDP entry (resolved or unresolved)?
// If so, don't resend (see note in sendArpRequest())
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()));
}
// Send NA for the NS packet in pktBuf
// pktBuf should point to start of ICMPv6 header
void EmulDevice::sendNeighborAdvertisement(PacketBuffer *pktBuf)
{
PacketBuffer *naPkt;
uchar *pktData = pktBuf->data();
quint16 flags = 0x6000; // solicit = 1; overide = 1
uchar *ip6Hdr;
UInt128 tgtIp, srcIp;
tgtIp = qFromBigEndian<UInt128>(pktData + 8);
if (tgtIp != ip6_) {
qDebug("%s: NS tgtIp %s is not us %s", __FUNCTION__,
qPrintable(QHostAddress(tgtIp.toArray()).toString()),
qPrintable(QHostAddress(ip6_.toArray()).toString()));
ip6Hdr = pktBuf->push(kIp6HdrLen);
return;
}
ip6Hdr = pktBuf->push(kIp6HdrLen);
srcIp = qFromBigEndian<UInt128>(ip6Hdr + 8);
if (srcIp == UInt128(0, 0)) {
// reset solicit flag
flags &= ~0x4000;
// NA should be sent to All nodes address
srcIp = UInt128(quint64(0xff02) << 48, quint64(1));
}
else if (pktBuf->length() >= 32) { // have TLVs?
if ((pktData[24] == 0x01) && (pktData[25] == 0x01)) { // Source TLV
quint64 mac;
mac = qFromBigEndian<quint32>(pktData + 26);
mac = (mac << 16) | qFromBigEndian<quint16>(pktData + 30);
ndpTable_.insert(srcIp, mac);
}
}
naPkt = new PacketBuffer;
naPkt->reserve(encapSize() + kIp6HdrLen);
pktData = naPkt->put(32);
if (pktData) {
// Calculate checksum first -
// start with fixed fields in ICMP Header and IPv6 Pseudo Header ...
quint32 sum = (0x8800 + flags + 0x0201) + (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(srcIp);
while(sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
// Type, Code
*(quint16*)(pktData ) = qToBigEndian(quint16(0x8800));
// Checksum
*(quint16*)(pktData+ 2) = qToBigEndian(quint16(~sum));
// Flags-Reserved
*(quint32*)(pktData+ 4) = qToBigEndian(quint32(flags << 16));
// Target IP
memcpy(pktData+ 8, tgtIp.toArray(), 16);
// Target Addr TLV + MacAddr
*(quint16*)(pktData+24) = qToBigEndian(quint16(0x0201));
*(quint32*)(pktData+26) = qToBigEndian(quint32(mac_ >> 16));
*(quint16*)(pktData+30) = qToBigEndian(quint16(mac_ & 0xffff));
}
if (!sendIp6(naPkt, srcIp , kIpProtoIcmp6))
return;
qDebug("Sent Neigh Advt to dstIp for tgtIp=%s/%s",
qPrintable(QHostAddress(srcIp.toArray()).toString()),
qPrintable(QHostAddress(tgtIp.toArray()).toString()));
}

72
server/emuldevice.h Normal file
View File

@ -0,0 +1,72 @@
/*
Copyright (C) 2015 Srivats P.
This file is part of "Ostinato"
This is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>
*/
#ifndef _EMUL_DEVICE_H
#define _EMUL_DEVICE_H
#include "device.h"
class EmulDevice: public Device
{
public:
EmulDevice(DeviceManager *deviceManager);
virtual ~EmulDevice() = default;
void receivePacket(PacketBuffer *pktBuf);
virtual void clearNeighbors(Device::NeighborSet set);
virtual void getNeighbors(OstEmul::DeviceNeighborList *neighbors);
bool isOrigin(const PacketBuffer *pktBuf);
quint64 neighborMac(const PacketBuffer *pktBuf);
protected:
virtual quint64 arpLookup(quint32 ip);
virtual quint64 ndpLookup(UInt128 ip);
virtual void sendArpRequest(quint32 tgtIp);
virtual void sendNeighborSolicit(UInt128 tgtIp);
private: // methods
int encapSize();
void encap(PacketBuffer *pktBuf, quint64 dstMac, quint16 type);
void receiveArp(PacketBuffer *pktBuf);
void receiveIp4(PacketBuffer *pktBuf);
void sendIp4Reply(PacketBuffer *pktBuf);
void receiveIcmp4(PacketBuffer *pktBuf);
void receiveIp6(PacketBuffer *pktBuf);
void sendIp6Reply(PacketBuffer *pktBuf);
bool sendIp6(PacketBuffer *pktBuf, UInt128 dstIp, quint8 protocol);
void receiveIcmp6(PacketBuffer *pktBuf);
void receiveNdp(PacketBuffer *pktBuf);
void sendNeighborAdvertisement(PacketBuffer *pktBuf);
private: // data
QHash<quint32, quint64> arpTable_;
QHash<UInt128, quint64> ndpTable_;
};
bool operator<(const DeviceKey &a1, const DeviceKey &a2);
#endif