123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673 |
- /**
- * nmrpflash - Netgear Unbrick Utility
- * Copyright (C) 2016-2020 Joseph Lehner <joseph.c.lehner@gmail.com>
- *
- * nmrpflash 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.
- *
- * nmrpflash 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 nmrpflash. If not, see <http://www.gnu.org/licenses/>.
- *
- */
- #if 0
- #include <netlink/route/addr.h>
- #endif
- #include <sys/socket.h>
- #include <sys/ioctl.h>
- #include <net/if.h>
- #include <fstream>
- #include <cstdarg>
- #include "ethsock.h"
- #if BOOST_OS_LINUX
- #include <linux/if_packet.h>
- #endif
- using namespace std;
- namespace nmrpflash {
- namespace {
- struct eth_header
- {
- uint8_t dest[6];
- uint8_t src[6];
- uint16_t proto;
- } __attribute__((packed));
- class pcap_devices
- {
- pcap_if_t* m_devs = nullptr;
- public:
- pcap_devices()
- {
- char errbuf[PCAP_ERRBUF_SIZE];
- if (pcap_findalldevs(&m_devs, errbuf) != 0) {
- throw runtime_error(errbuf);
- }
- }
- ~pcap_devices()
- {
- pcap_freealldevs(m_devs);
- }
- list<const pcap_if_t*> get() const
- {
- list<const pcap_if_t*> ret;
- for (pcap_if_t* dev = m_devs; dev; dev = dev->next) {
- ret.push_back(dev);
- }
- return ret;
- }
- const pcap_if_t* get(const string& intf)
- {
- for (pcap_if_t* dev = m_devs; dev; dev = dev->next) {
- if (dev->name == intf) {
- return dev;
- }
- }
- throw invalid_argument("No such interface: " + intf);
- }
- };
- void check_mac_addr(const mac_addr& mac)
- {
- if (mac == mac_addr::none || mac == mac_addr::broadcast) {
- throw invalid_argument("Invalid MAC address: " + stringify(mac));
- }
- }
- void check_ip_addr(const ip_addr& ip)
- {
- if (!ip || ip == ip_addr(255, 255, 255, 255)) {
- throw invalid_argument("Invalid IP address: " + stringify(ip));
- }
- }
- #if BOOST_OS_WINDOWS
- GUID guid_from_str(const string& str)
- {
- GUID guid;
- int n;
- sscanf(str.c_str(),
- "%08X-%04X-%04X-%02X%02X-%02X%02X%02X%02X%02X%02X%n",
- &guid.Data1, &guid.Data2, &guid.Data3,
- &guid.Data4[0], &guid.Data4[1], &guid.Data4[2],
- &guid.Data4[3], &guid.Data4[4], &guid.Data4[5],
- &guid.Data4[6], &guid.Data4[7], &n);
- if (n != 36) {
- throw invalid_argument("Not a valid GUID: " + str);
- }
- return ret;
- }
- string guid_to_str(const GUID& guid)
- {
- string ret('\0', 37);
- snprintf(&ret[0], ret.size(),
- "%08X-%04X-%04X-%02X%02X-%02X%02X%02X%02X%02X%02X",
- guid.Data1, guid.Data2, guid.Data3,
- guid.Data4[0], guid.Data4[1], guid.Data4[2],
- guid.Data4[3], guid.Data4[4], guid.Data4[5],
- guid.Data4[6], guid.Data4[7]);
- return ret;
- }
- void get_if_entry(MIB_IF_ROW2* row, NET_IFINDEX index)
- {
- memset(row, 0, sizeof(*row));
- row->InterfaceIndex = index;
- DWORD err = GetIfEntry2(row);
- if (err) {
- throw winapi_error("GetIfEntry2", err);
- }
- }
- #elif BOOST_OS_LINUX
- bool linux_sys_read_b(const string& intf, const string& file, bool defval)
- {
- ifstream in((boost::format("/sys/class/net/%s/%s") % intf % file).str().c_str());
- return in ? (in.get() == '1') : defval;
- }
- #endif
- #if 0
- #if defined(NMRPFLASH_LINUX) && false
- auto create_nl_route_socket()
- {
- auto sock = wrap_unique(nl_socket_alloc(), &nl_socket_free);
- if (sock) {
- if (nl_connect(sock.get(), NETLINK_ROUTE) != 0) {
- throw libnl_error("nl_connect");
- }
- return sock;
- } else {
- throw libnl_error("nl_socket_alloc");
- }
- }
- auto build_nl_ip(const ip_addr& ip)
- {
- uint32_t raw = ip.to_uint();
- auto ret = wrap_unique(nl_addr_build(AF_INET, &raw, 4), &nl_addr_put);
- if (!ret) {
- throw libnl_error("nl_addr_build");
- }
- if (ip.pfxlen()) {
- nl_addr_set_prefixlen(ret.get(), ip.pfxlen());
- }
- return ret;
- }
- #endif
- static int systemf(const char* fmt, ...) __attribute__((format(printf, 1, 2)));
- static int systemf(const char* fmt, ...)
- {
- va_list va;
- va_start(va, fmt);
- char cmd[1024];
- int n = vsnprintf(cmd, sizeof(cmd), fmt, va);
- if (n >= sizeof(cmd)) {
- return -1;
- }
- int ret = system(cmd);
- va_end(va);
- return ret;
- }
- #endif
- #if !BOOST_OS_WINDOWS && !BOOST_OS_LINUX
- static void bsd_set_intf_up(const string& intf, bool up, int fd = -1)
- {
- scoped_fd sfd;
- if (fd == -1) {
- sfd.reset(xsocket(AF_INET, SOCK_DGRAM, 0));
- fd = *sfd;
- }
- struct ifreq ifr;
- strncpy(ifr.ifr_name, intf.c_str(), IFNAMSIZ);
- if (ioctl(fd, SIOCGIFFLAGS, &ifr) != 0) {
- throw errno_error("ioctl(SIOCGIFFLAGS)");
- }
- if (!up) {
- ifr.ifr_flags &= ~(IFF_UP | IFF_RUNNING);
- } else {
- ifr.ifr_flags |= IFF_UP | IFF_RUNNING;
- }
- if (ioctl(fd, SIOCSIFFLAGS, &ifr) != 0) {
- throw errno_error("ioctl(SIOCSIFFLAGS)");
- }
- }
- #endif
- }
- eth_interface::eth_interface(const string& intf)
- {
- init_from_name(intf);
- pcap_devices devs;
- init_from_pcap(devs.get(m_name));
- }
- eth_interface::eth_interface(const pcap_if_t* dev)
- {
- init_from_name(dev->name);
- init_from_pcap(dev);
- }
- eth_interface::~eth_interface()
- {
- try {
- for (auto ip : m_undo_ip) {
- del_ip(ip);
- }
- for (auto arp : m_undo_arp) {
- del_arp(arp.first);
- }
- } catch (const exception& e) {
- // yum!
- }
- }
- void eth_interface::init_index(const string& intf)
- {
- m_index = if_nametoindex(intf.c_str());
- if (!m_index) {
- throw invalid_argument("Interface does not exist: " + intf);
- }
- }
- void eth_interface::init_from_name(const string& intf)
- {
- // On Windows, `intf` might be either the pcap name, which is
- // `\Device\NPF_{GUID}`, or the ANSI interface name!
- #if !BOOST_OS_WINDOWS
- m_name = intf;
- init_index(intf);
- #else
- if (intf.find("\\Device\\NPF_{") == string::npos) {
- if (intf.find("net") == 0 && intf.find('_') == string::npos) {
- m_index = stoi(intf.substr(3));
- } else {
- init_index(intf);
- }
- DWORD err = ConvertInterfaceIndexToLuid(m_index, &m_luid);
- if (err) {
- throw winapi_error("ConvertInterfaceIndexToLuid", err);
- }
- err = ConvertInterfaceLuidToGuid(index, &m_guid);
- if (err) {
- throw winapi_error("ConvertInterfaceLuidToGuid", err);
- }
- m_name = "\\Device\\NPF_{" + guid_to_str(m_guid) + "}";
- } else {
- m_name = intf;
- m_guid = guid_from_str(intf.substr(13, 36));
- DWORD err = ConvertInterfaceGuidToLuid(&guid, &m_luid);
- if (err) {
- throw winapi_error("ConvertInterfaceGuidToLuid", err);
- }
- err = ConvertInterfaceLuidToIndex(&m_luid, &m_index);
- if (err) {
- throw winapi_error("ConvertInterfaceGuidToLuid", err);
- }
- }
- #endif
- }
- void eth_interface::init_from_pcap(const pcap_if_t* dev)
- {
- #if !BOOST_OS_WINDOWS
- for (pcap_addr_t* addr = dev->addresses; addr; addr = addr->next) {
- if (addr->addr->sa_family == AF_PACKET) {
- void* src;
- #if BOOST_OS_LINUX
- src = reinterpret_cast<sockaddr_ll*>(addr->addr)->sll_addr;
- #else
- src = LLADDR(reinterpret_cast<sockaddr_dl*>(addr->addr));
- #endif
- m_mac = mac_addr(src);
- return;
- }
- }
- throw runtime_error("Failed to get hwaddr of interface");
- #else
- MIB_IF_ROW2 row;
- get_if_entry(&row, m_index);
- memcpy(m_mac, row.PhysicalAddress, sizeof(m_mac));
- char ansi[IF_NAMESIZE];
- m_ansi = if_indextoname(m_index, ansi);
- m_alias = row.Alias;
- #endif
- }
- bool eth_interface::is_unplugged() const
- {
- #if BOOST_OS_WINDOWS
- MIB_IF_ROW2 row;
- get_if_entry(&row, m_index);
- return row.InterfaceAndOperStatusFlags.NotMediaConnected;
- #elif BOOST_OS_LINUX
- return !linux_sys_read_b(m_name, "carrier", true);
- #else
- return false;
- #endif
- }
- void eth_interface::add_del_ip(const ip_addr& ip, bool add)
- {
- check_ip_addr(ip);
- cmdfmt cmd;
- #if BOOST_OS_WINDOWS
- if (add) {
- cmd = cmdfmt(L"netsh interface add address %s addr=%s mask=%s gateway=0.0.0.0")
- % quote(m_alias) % stringify(ip.address()) % stringify(ip.netmask());
- } else {
- cmd = cmdfmt(L"netsh interface delete address %s addr=%s")
- % quote(m_alias) % stringify(ip.address());
- }
- #elif BOOST_OS_LINUX
- cmd = cmdfmt("ip address %s %s dev %s") % (add ? "add" : "del")
- % stringify(ip) % quote(m_name);
- #else
- cmd = cmdfmt("ifconfig %s inet %s netmask %s %s") % quote(m_name)
- % stringify(ip.address()) % stringify(ip.netmask()) % (add ? "add" : "delete");
- #endif
- run(cmd, add);
- #if 0
- #if defined(NMRPFLASH_WINDOWS)
- MIB_UNICASTIPADDRESS_ROW row;
- memset(&row, 0, sizeof(row));
- row.InterfaceIndex = m_index;
- row.PrefixOrigin = IpPrefixOriginManual;
- row.SuffixOrigin = IpPrefixOriginManual;
- row.OnLinkPrefixLength = pfix;
- row.SkipAsSource = false;
- row.PreferredLifetime = 0xffffffff;
- row.ValidLifetime = 0xffffffff;
- ip.apply_to(row.Address.Ipv4);
- if (add) {
- DWORD err = CreateUnicastIpAddressEntry(&row);
- if (err && err != ERROR_OBJECT_ALREADY_EXISTS) {
- throw winapi_error("CreateUnicastIpAddressEntry", err);
- }
- } else {
- DWORD err = DeleteUnicastIpAddressEntry(&row);
- if (err) {
- log::d("DeleteUnicastIpAddressEntry: %d", err);
- }
- }
- #elif defined(NMRPFLASH_LINUX)
- auto ra = wrap_unique(rtnl_addr_alloc(), rtnl_addr_put);
- if (!ra) {
- throw libnl_error("rtnl_addr_alloc");
- }
- rtnl_addr_set_ifindex(ra.get(), m_index);
- rtnl_addr_set_local(ra.get(), build_nl_ip(ip).get());
- rtnl_addr_set_broadcast(ra.get(), build_nl_ip(ip.broadcast()).get());
- auto sk = create_nl_route_socket();
- int err = add ? rtnl_addr_add(*sk, *ra, 0) : rtnl_addr_delete(*sk, *ra, 0);
- if (add) {
- if (err && err != -NLE_EXIST) {
- throw libnl_error("rtnl_addr_add");
- }
- } else if (err) {
- log::d("rtnl_addr_delete: %d", err);
- }
- #else // NMRPFLASH_OSX (or any other BSD)
- struct ifaliasreq ifra;
- memset(&ifra, 0, sizeof(ifra));
- strncpy(ifra.ifra_name, m_name.c_str(), IFNAMSIZ);
- ip.apply_to(ifra.ifra_addr);
- ip.netmask().apply_to(ifra.ifra_mask);
- scoped_fd fd(xsocket(AF_INET, SOCK_DGRAM, 0);
- if (ioctl(*fd, add ? SIOCAIFADDR : SIOCDIFADDR, &ifra) != 0) {
- if (add) {
- throw errno_error("ioctl(SIOCAIFADDR");
- } else {
- log::d("ioctl(SIOCDIFADDR): %d", errno);
- }
- }
- bsd_set_intf_up(m_name, true, *fd);
- #endif
- #endif
- if (add) {
- m_undo_ip.insert(ip);
- } else {
- m_undo_ip.erase(ip);
- }
- }
- void eth_interface::add_del_arp(const mac_addr& mac, const ip_addr& ip, bool add)
- {
- #if 0
- #if defined(NMRPFLASH_WINDOWS)
- MIB_IPNETROW arp = {
- .dwIndex = m_index,
- .dwPhysAddrLen = sizeof(mac),
- .dwAddr = ip,
- .dwType = MIB_IPNET_TYPE_STATIC,
- };
- mac.apply_to(&arp.bPhysAddr);
- DWORD err = add ? CreateIpNetEntry(&arp) : DeleteIpNetEntry(&arp);
- if (err)
- throw winapi_error(add ? "CreateIpNetEntry" : "DelteIpNetEntry", err);
- }
- #elif defined(NMRPFLASH_LINUX)
- arpreq arp;
- memset(&arp, 0, sizeof(arp));
- arp.arp_ha.sa_family = ARPHRD_ETHER;
- arp.arp_flags = ATF_PERM | ATF_COM;
- mac.apply_to(&arp.arp_ha.sa_data);
- ip.apply_to(&arp.arp_pa);
- int fd = socket(AF_INET, SOCK_DGRAM, 0);
- if (fd < 0) {
- throw errno_error("socket");
- }
- bool err = (ioctl(fd, add ? SIOCSARP : SIOCDARP, &req) < 0);
- close(fd);
- if (err) {
- throw errno_error(add ? "ioctl(SIOCSARP)" : "ioctl(SIOCDARP)");
- }
- #else
- #endif
- #endif
- cmdfmt cmd;
- if (add) {
- #if BOOST_OS_WINDOWS
- cmd = cmdfmt(L"netsh add neighbors interface=%s address=%s neighbor=%s store=active"
- % quote(m_alias) % stringify(ip.address()) % mac.to_string('-');
- #else
- cmd = cmdfmt("arp -s %s %s") % stringify(ip.address()) % stringify(mac);
- #endif
- } else {
- #if BOOST_OS_WINDOWS
- cmd = cmdfmt(L"netsh delete neighbors interface=%s address=%s")
- % quote(m_alias) % stringify(ip.address()));
- #else
- cmd = cmdfmt("arp -d %s") % stringify(ip.address());
- #endif
- }
- run(cmd, add);
- if (add) {
- m_undo_arp[ip] = mac;
- } else {
- m_undo_arp.erase(ip);
- }
- }
- list<eth_interface> eth_interface::all()
- {
- list<eth_interface> ret;
- pcap_devices devs;
- for (auto dev : devs.get()) {
- if (if_nametoindex(dev->name)) {
- ret.emplace_back(dev);
- }
- }
- return ret;
- }
- eth_sock::eth_sock(eth_interface& iface, uint16_t proto)
- : m_iface(iface), m_proto(proto), m_timeout(0), m_stp_enabled(iface.is_stp_enabled())
- {
- char errbuf[PCAP_ERRBUF_SIZE];
- m_pcap = pcap_open_live(iface.name().c_str(), BUFSIZ, 1, 1, errbuf);
- if (!m_pcap) {
- throw runtime_error(errbuf);
- } else if (*errbuf) {
- log::w(errbuf);
- }
- try {
- init();
- } catch (const exception& e) {
- shutdown();
- throw e;
- }
- }
- void eth_sock::send(const string& buf, const mac_addr& dest)
- {
- send(buf.data(), buf.size(), dest);
- }
- void eth_sock::send(const void* data, size_t size, const mac_addr& dest)
- {
- size += sizeof(eth_header);
- auto pkt = make_unique<char[]>(size);
- auto hdr = reinterpret_cast<eth_header*>(pkt.get());
- m_iface.hwaddr().apply_to(hdr->src);
- hdr->proto = htons(m_proto);
- if (m_peer == mac_addr::none) {
- dest.apply_to(hdr->dest);
- } else {
- m_peer.apply_to(hdr->dest);
- }
- #if BOOST_OS_WINDOWS
- if (pcap_sendpacket(m_pcap, pkt.get(), size) != 0) {
- throw pcap_error("pcap_sendpacket", m_pcap);
- }
- #else
- if (pcap_inject(m_pcap, pkt.get(), size) != size) {
- throw pcap_error("pcap_inject", m_pcap);
- }
- #endif
- }
- std::string eth_sock::recv(unsigned timeout, mac_addr* src)
- {
- if (!timeout) {
- timeout = m_timeout;
- }
- if (timeout) {
- #if BOOST_OS_WINDOWS
- DWORD ret = WaitForSingleObject(m_handle, timeout);
- if (ret == WAIT_TIMEOUT) {
- return pkt::empty();
- } else if (ret != WAIT_OBJECT_0) {
- throw winapi_error("WaitForSingleObject", ret);
- }
- #else
- if (!select_readfd(m_fd, timeout)) {
- return {};
- }
- #endif
- }
- pcap_pkthdr* hdr;
- const u_char* buf;
- int status = pcap_next_ex(m_pcap, &hdr, &buf);
- if (!status) {
- return {};
- } else if (status == PCAP_ERROR) {
- throw pcap_error("pcap_next_ex", m_pcap);
- } else if (status != 1) {
- throw runtime_error("pcap_next_ex: error " + to_string(status));
- } else if (hdr->caplen < sizeof(eth_header)) {
- throw runtime_error("Received short packet: " + to_string(hdr->caplen) + "b");
- }
- if (src) {
- *src = mac_addr(&buf[6]);
- }
- return string(reinterpret_cast<const char*>(buf + 14), hdr->caplen - 14);
- }
- void eth_sock::init()
- {
- if (pcap_datalink(m_pcap) != DLT_EN10MB) {
- throw invalid_argument("Not an Ethernet interface: " + m_iface.name());
- }
- #if BOOST_OS_WINDOWS
- m_handle = pcap_getevent(m_pcap);
- if (!m_handle) {
- throw pcap_error("pcap_getevent", m_pcap);
- }
- if (pcap_setmintocopy(m_pcap, 1) != 0) {
- throw pcap_error("pcap_setmintocopy", m_pcap);
- }
- #else
- m_fd = pcap_get_selectable_fd(m_pcap);
- if (m_fd == -1) {
- throw pcap_error("pcap_get_selectable_fd", m_pcap);
- }
- #endif
- bpf_program bpf;
- string filter = (boost::format("ether proto 0x%04x and not ether src %s")
- % m_proto % stringify(m_iface.hwaddr())).str();
- if (pcap_compile(m_pcap, &bpf, filter.c_str(), 0, 0) != 0) {
- throw pcap_error("pcap_compile", m_pcap);
- }
- int err = pcap_setfilter(m_pcap, &bpf);
- pcap_freecode(&bpf);
- if (err) {
- throw pcap_error("pcap_setfilter", m_pcap);
- }
- m_iface.enable_stp(false);
- }
- void eth_sock::shutdown()
- {
- if (m_pcap) {
- pcap_close(m_pcap);
- m_pcap = nullptr;
- m_iface.enable_stp(m_stp_enabled);
- }
- }
- }
|