matthew/ostinato
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
0fb81e4d8e
ostinato/server/emuldevice.cpp

787 lines
23 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 "emuldevice.h"
#include "devicemanager.h"
#include "netdefs.h"
#include "packetbuffer.h"
#include <QHostAddress>
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 kEthTypeArp:
if (hasIp4_)
receiveArp(pktBuf);
break;
case kEthTypeIp4:
if (hasIp4_)
receiveIp4(pktBuf);
break;
case kEthTypeIp6:
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 != kEthTypeIp4)
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, kEthTypeArp);
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;
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, kEthTypeArp);
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), kEthTypeIp4);
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), kEthTypeIp6);
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;
// 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()));
}
Reference in New Issue View Git Blame Copy Permalink