matthew/ostinato
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
8b80b4c3ef
ostinato/server/device.cpp

1119 lines
32 KiB
C++
Raw Blame History

/*
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 "device.h"
#include "../common/emulproto.pb.h"
#include "devicemanager.h"
#include "packetbuffer.h"
#include <QHostAddress>
#include <qendian.h>
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:
* 1. Device Key is (VLANS + MAC) - is assumed to be unique for a device
* 2. Device clients/users (viz. DeviceManager) should take care when
* setting params that change the key, if the key is used elsewhere
* (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;
for (int i = 0; i < kMaxVlan; i++)
vlan_[i] = 0;
numVlanTags_ = 0;
mac_ = 0;
hasIp4_ = false;
hasIp6_ = false;
clearKey();
}
void Device::setVlan(int index, quint16 vlan, quint16 tpid)
{
int ofs;
if ((index < 0) || (index >= kMaxVlan)) {
qWarning("%s: vlan index %d out of range (0 - %d)", __FUNCTION__,
index, kMaxVlan - 1);
return;
}
vlan_[index] = (tpid << 16) | vlan;
ofs = index * sizeof(quint16);
key_[ofs] = vlan >> 8;
key_[ofs+1] = vlan & 0xff;
if (index >= numVlanTags_)
numVlanTags_ = index + 1;
}
quint64 Device::mac()
{
return mac_;
}
void Device::setMac(quint64 mac)
{
int ofs = kMaxVlan * sizeof(quint16);
mac_ = mac & ~(0xffffULL << 48);
memcpy(key_.data() + ofs, (char*)&mac_, sizeof(mac_));
}
void Device::setIp4(quint32 address, int prefixLength, quint32 gateway)
{
ip4_ = address;
ip4PrefixLength_ = prefixLength;
ip4Gateway_ = gateway;
hasIp4_ = true;
}
void Device::setIp6(UInt128 address, int prefixLength, UInt128 gateway)
{
ip6_ = address;
ip6PrefixLength_ = prefixLength;
ip6Gateway_ = gateway;
hasIp6_ = true;
}
void Device::getConfig(OstEmul::Device *deviceConfig)
{
for (int i = 0; i < numVlanTags_; i++)
deviceConfig->add_vlan(vlan_[i]);
deviceConfig->set_mac(mac_);
if (hasIp4_) {
deviceConfig->set_ip4(ip4_);
deviceConfig->set_ip4_prefix_length(ip4PrefixLength_);
deviceConfig->set_ip4_default_gateway(ip4Gateway_);
}
if (hasIp6_) {
deviceConfig->mutable_ip6()->set_hi(ip6_.hi64());
deviceConfig->mutable_ip6()->set_lo(ip6_.lo64());
deviceConfig->set_ip6_prefix_length(ip6PrefixLength_);
deviceConfig->mutable_ip6_default_gateway()->set_hi(ip6Gateway_.hi64());
deviceConfig->mutable_ip6_default_gateway()->set_lo(ip6Gateway_.lo64());
}
}
QString Device::config()
{
QString config;
for (int i = 0; i < numVlanTags_; i++) {
config.append(i == 0 ? "vlans=" : "|");
config.append(
(vlan_[i] >> 16) != kVlanTpid ?
QString("0x%1-%2")
.arg(vlan_[i] >> 16, 4, kBaseHex, QChar('0'))
.arg(vlan_[i] & 0xFFFF) :
QString("%1")
.arg(vlan_[i] & 0xFFFF));
}
config.append(QString(" mac=%1")
.arg(mac_, 12, kBaseHex, QChar('0')));
if (hasIp4_)
config.append(QString(" ip4=%1/%2")
.arg(QHostAddress(ip4_).toString())
.arg(ip4PrefixLength_));
if (hasIp6_)
config.append(QString(" ip6=%1/%2")
.arg(QHostAddress(ip6_.toArray()).toString())
.arg(ip6PrefixLength_));
return config;
}
DeviceKey Device::key()
{
return key_;
}
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);
}
void Device::transmitPacket(PacketBuffer *pktBuf)
{
deviceManager_->transmitPacket(pktBuf);
}
void Device::resolveGateway()
{
if (hasIp4_)
sendArpRequest(ip4Gateway_);
if (hasIp6_)
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)
{
quint16 ethType = qFromBigEndian<quint16>(pktBuf->data());
pktBuf->pull(2);
qDebug("%s: ethType 0x%x", __PRETTY_FUNCTION__, ethType);
switch(ethType)
{
case 0x0800: // IPv4
if (hasIp4_)
sendArpRequest(pktBuf);
break;
case 0x86dd: // IPv6
if (hasIp6_)
sendNeighborSolicit(pktBuf);
break;
default:
break;
}
// FIXME: temporary hack till DeviceManager clones pbufs
pktBuf->push(2);
}
// Append this device's neighbors to the list
void Device::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));
}
}
// Are we the source of the given packet?
// We expect pktBuf to point to EthType on entry
bool Device::isOrigin(const PacketBuffer *pktBuf)
{
const uchar *pktData = pktBuf->data();
quint16 ethType = qFromBigEndian<quint16>(pktData);
qDebug("%s: ethType 0x%x", __PRETTY_FUNCTION__, ethType);
pktData += 2;
// 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+2)) {
qDebug("incomplete IPv4 header: expected %d, actual %d",
ipHdrLen, pktBuf->length());
return false;
}
srcIp = qFromBigEndian<quint32>(pktData + ipHdrLen - 8);
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;
}
// Return the mac address corresponding to the dstIp of the given packet
// We expect pktBuf to point to EthType on entry
quint64 Device::neighborMac(const PacketBuffer *pktBuf)
{
const uchar *pktData = pktBuf->data();
quint16 ethType = qFromBigEndian<quint16>(pktData);
qDebug("%s: ethType 0x%x", __PRETTY_FUNCTION__, ethType);
pktData += 2;
// We know only about IP packets
if ((ethType == 0x0800) && hasIp4_) { // IPv4
int ipHdrLen = (pktData[0] & 0x0F) << 2;
quint32 dstIp, tgtIp, mask;
if (pktBuf->length() < ipHdrLen) {
qDebug("incomplete IPv4 header: expected %d, actual %d",
ipHdrLen, pktBuf->length());
return false;
}
dstIp = qFromBigEndian<quint32>(pktData + ipHdrLen - 4);
mask = ~0 << (32 - ip4PrefixLength_);
qDebug("dst %x mask %x self %x", dstIp, mask, ip4_);
tgtIp = ((dstIp & mask) == (ip4_ & mask)) ? dstIp : ip4Gateway_;
return arpTable_.value(tgtIp);
}
else if ((ethType == kEthTypeIp6) && hasIp6_) { // IPv6
UInt128 dstIp, tgtIp, mask;
if (pktBuf->length() < (kIp6HdrLen+2)) {
qDebug("incomplete IPv6 header: expected %d, actual %d",
kIp6HdrLen, pktBuf->length()-2);
return false;
}
dstIp = qFromBigEndian<UInt128>(pktData + 24);
mask = ~UInt128(0, 0) << (128 - ip6PrefixLength_);
qDebug("dst %s mask %s self %s",
qPrintable(QHostAddress(dstIp.toArray()).toString()),
qPrintable(QHostAddress(mask.toArray()).toString()),
qPrintable(QHostAddress(ip6_.toArray()).toString()));
tgtIp = ((dstIp & mask) == (ip6_ & mask)) ? dstIp : ip6Gateway_;
return ndpTable_.value(tgtIp);
}
return false;
}
//
// Private Methods
//
/*
* ---------------------------------------------------------
* IPv4 related private methods
* ---------------------------------------------------------
*/
void Device::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;
}
// Send ARP request for the IPv4 packet in pktBuf
// pktBuf points to start of IP header
void Device::sendArpRequest(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
int ipHdrLen = (pktData[0] & 0x0F) << 2;
quint32 srcIp = ip4_, dstIp, mask, tgtIp;
if (pktBuf->length() < ipHdrLen) {
qDebug("incomplete IPv4 header: expected %d, actual %d",
ipHdrLen, pktBuf->length());
return;
}
dstIp = qFromBigEndian<quint32>(pktData + ipHdrLen - 4);
mask = ~0 << (32 - ip4PrefixLength_);
qDebug("dst %x src %x mask %x", dstIp, srcIp, mask);
tgtIp = ((dstIp & mask) == (srcIp & mask)) ? dstIp : ip4Gateway_;
sendArpRequest(tgtIp);
}
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;
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
if (!arpTable_.contains(dstIp)) {
qWarning("%s: mac not found for %s; unable to send IPv4 packet",
__FUNCTION__, qPrintable(QHostAddress(dstIp).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(dstIp), 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 = ndpTable_.value(dstIp);
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);
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;
// Swap src/dst IP addresses
dstIp = qFromBigEndian<UInt128>(pktData + 8); // srcIp in original pkt
srcIp = qFromBigEndian<UInt128>(pktData + 24); // dstIp in original pkt
if (!ndpTable_.contains(dstIp)) {
qWarning("%s: mac not found for %s; unable to send IPv6 packet",
__FUNCTION__,
qPrintable(QHostAddress(dstIp.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(dstIp), 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
void Device::sendNeighborSolicit(PacketBuffer *pktBuf)
{
uchar *pktData = pktBuf->data();
UInt128 srcIp = ip6_, dstIp, mask, tgtIp;
if (pktBuf->length() < kIp6HdrLen) {
qDebug("incomplete IPv6 header: expected %d, actual %d",
kIp6HdrLen, pktBuf->length());
return;
}
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_;
sendNeighborSolicit(tgtIp);
}
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()));
}
bool operator<(const DeviceKey &a1, const DeviceKey &a2)
{
int i = 0;
while (i < a1.size()) {
if (uchar(a1.at(i)) < uchar(a2.at(i)))
return true;
if (uchar(a1.at(i)) > uchar(a2.at(i)))
return false;
i++;
}
return false;
}
Reference in New Issue View Git Blame Copy Permalink