/* 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 */ #include "device.h" #include "../common/emulproto.pb.h" #include "devicemanager.h" #include "packetbuffer.h" #include #include const int kBaseHex = 16; const quint16 kEthTypeIp4 = 0x0800; const quint16 kEthTypeIp6 = 0x86dd; const int kIp6HdrLen = 40; /* * NOTE: * 1. Device Key is (VLANS [without prio/cfi] + 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) */ 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) & 0x0f; // Vlan prio/cfi should not be part of key 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); qToBigEndian(mac_, (uchar*)key_.data()+ofs); } void Device::setIp4(quint32 address, int prefixLength, quint32 gateway) { ip4_ = address; ip4PrefixLength_ = prefixLength; ip4Gateway_ = gateway; hasIp4_ = true; // Precalculate our mask 'n subnet to avoid doing so at pkt rx/tx time ip4Mask_ = ~0 << (32 - ip4PrefixLength_); ip4Subnet_ = ip4_ & ip4Mask_; } void Device::setIp6(UInt128 address, int prefixLength, UInt128 gateway) { ip6_ = address; ip6PrefixLength_ = prefixLength; ip6Gateway_ = gateway; hasIp6_ = true; // Precalculate our mask 'n subnet to avoid doing so at pkt rx/tx time ip6Mask_ = ~UInt128(0, 0) << (128 - ip6PrefixLength_); ip6Subnet_ = ip6_ & ip6Mask_; } 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)); } /* void Device::receivePacket(PacketBuffer *pktBuf) { // XXX: Pure Virtual: Subclass should implement // We expect pktBuf to point to EthType on entry } */ void Device::transmitPacket(PacketBuffer *pktBuf) { deviceManager_->transmitPacket(pktBuf); } void Device::resolveGateway() { if (hasIp4_ && ip4Gateway_ && !isResolved(ip4Gateway_)) sendArpRequest(ip4Gateway_); if (hasIp6_ && (ip6Gateway_ != 0) && !isResolved(ip6Gateway_)) sendNeighborSolicit(ip6Gateway_); } // 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(pktBuf->data()); pktBuf->pull(2); qDebug("%s: ethType 0x%x", __PRETTY_FUNCTION__, ethType); switch(ethType) { case kEthTypeIp4: // IPv4 if (hasIp4_) sendArpRequest(pktBuf); break; case kEthTypeIp6: // IPv6 if (hasIp6_) sendNeighborSolicit(pktBuf); break; default: break; } // FIXME: temporary hack till DeviceManager clones pbufs pktBuf->push(2); } /* void Device::clearNeighbors(Device::NeighborSet set) { // 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) { const uchar *pktData = pktBuf->data(); quint16 ethType = qFromBigEndian(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 == kEthTypeIp4) && 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(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(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) { quint64 mac = 0; const uchar *pktData = pktBuf->data(); quint16 ethType = qFromBigEndian(pktData); qDebug("%s: ethType 0x%x", __PRETTY_FUNCTION__, ethType); pktData += 2; // We know only about IP packets if ((ethType == kEthTypeIp4) && hasIp4_) { // IPv4 int ipHdrLen = (pktData[0] & 0x0F) << 2; quint32 dstIp, tgtIp; if (pktBuf->length() < ipHdrLen) { qDebug("incomplete IPv4 header: expected %d, actual %d", ipHdrLen, pktBuf->length()); return mac; } dstIp = qFromBigEndian(pktData + ipHdrLen - 4); if ((dstIp & 0xF0000000) == 0xE0000000) { // Mcast IP? mac = (quint64(0x01005e) << 24) | (dstIp & 0x7FFFFF); qDebug("mcast dst %08x, mac: %012llx", dstIp, mac); } else { tgtIp = ((dstIp & ip4Mask_) == ip4Subnet_) ? dstIp : ip4Gateway_; mac = arpLookup(tgtIp); qDebug("tgtIp: %08x, mac: %012llx", tgtIp, mac); } } else if ((ethType == kEthTypeIp6) && hasIp6_) { // IPv6 UInt128 dstIp, tgtIp; if (pktBuf->length() < (kIp6HdrLen+2)) { qDebug("incomplete IPv6 header: expected %d, actual %d", kIp6HdrLen, pktBuf->length()-2); return mac; } dstIp = qFromBigEndian(pktData + 24); if (dstIp.toArray()[0] == 0xFF) { // Mcast IP? mac = (quint64(0x3333) << 32) | (dstIp.lo64() & 0xFFFFFFFF); qDebug("mcast dst %s, mac: %012llx", qPrintable(QHostAddress(dstIp.toArray()).toString()), mac); } else { tgtIp = ((dstIp & ip6Mask_) == ip6Subnet_) ? dstIp : ip6Gateway_; mac = ndpLookup(tgtIp); qDebug("tgtIp %s, mac: %012llx", qPrintable(QHostAddress(dstIp.toArray()).toString()), mac); } } return mac; } /* quint64 Device::arpLookup(quint32 ip) { // 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) { uchar *pktData = pktBuf->data(); int ipHdrLen = (pktData[0] & 0x0F) << 2; quint32 dstIp, tgtIp; if (pktBuf->length() < ipHdrLen) { qDebug("incomplete IPv4 header: expected %d, actual %d", ipHdrLen, pktBuf->length()); return; } dstIp = qFromBigEndian(pktData + ipHdrLen - 4); tgtIp = ((dstIp & ip4Mask_) == ip4Subnet_) ? dstIp : ip4Gateway_; if (!isResolved(tgtIp)) sendArpRequest(tgtIp); } // 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 dstIp, tgtIp; if (pktBuf->length() < kIp6HdrLen) { qDebug("incomplete IPv6 header: expected %d, actual %d", kIp6HdrLen, pktBuf->length()); return; } dstIp = qFromBigEndian(pktData + 24); tgtIp = ((dstIp & ip6Mask_) == ip6Subnet_) ? dstIp : ip6Gateway_; if (!isResolved(tgtIp)) sendNeighborSolicit(tgtIp); } bool Device::isResolved(quint32 ip) { return arpLookup(ip) > 0; } bool Device::isResolved(UInt128 ip) { return ndpLookup(ip) > 0; } // // Protected Methods // /* void Device::sendArpRequest(quint32 tgtIp) { // XXX: Pure virtual: Subclass should implement } void Device::sendNeighborSolicit(UInt128 tgtIp) { // XXX: Pure virtual: Subclass should implement } */ 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; }