2 * nmrpflash - Netgear Unbrick Utility
3 * Copyright (C) 2016 Joseph Lehner <joseph.c.lehner@gmail.com>
5 * nmrpflash is free software: you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation, either version 3 of the License, or
8 * (at your option) any later version.
10 * nmrpflash is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with nmrpflash. If not, see <http://www.gnu.org/licenses/>.
20 #include <sys/types.h>
29 #if defined(NMRPFLASH_WINDOWS)
30 #define NMRPFLASH_NETALIAS_PREFIX "net"
34 #include <sys/ioctl.h>
39 #if defined(NMRPFLASH_LINUX)
40 #define NMRPFLASH_AF_PACKET AF_PACKET
41 #include <linux/if_packet.h>
42 #include <netlink/route/addr.h>
43 #include <netlink/route/neighbour.h>
45 #define NMRPFLASH_AF_PACKET AF_LINK
46 #include <net/if_types.h>
47 #include <net/if_media.h>
55 #ifndef NMRPFLASH_WINDOWS
57 #ifdef NMRPFLASH_LINUX
68 struct ethsock_arp_undo
74 struct ethsock_ip_undo
76 #ifndef NMRPFLASH_WINDOWS
83 const char *mac_to_str(uint8_t *mac)
86 snprintf(buf, sizeof(buf), "%02x:%02x:%02x:%02x:%02x:%02x",
87 mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
91 static int x_pcap_findalldevs(pcap_if_t **devs)
93 char errbuf[PCAP_ERRBUF_SIZE];
94 if (pcap_findalldevs(devs, errbuf) != 0) {
95 fprintf(stderr, "%s.\n", errbuf);
102 #ifndef NMRPFLASH_LINUX
103 static int systemf(const char *fmt, ...)
110 ret = vsnprintf(cmd, sizeof(cmd) - 1, fmt, va);
111 if (ret >= sizeof(cmd) - 1) {
122 #ifndef NMRPFLASH_WINDOWS
123 static inline bool sockaddr_get_hwaddr(struct sockaddr *sa, uint8_t *hwaddr)
127 if (sa->sa_family != NMRPFLASH_AF_PACKET) {
131 #ifndef NMRPFLASH_LINUX
132 if (((struct sockaddr_dl*)sa)->sdl_type != IFT_ETHER) {
135 src = LLADDR((struct sockaddr_dl*)sa);
137 src = ((struct sockaddr_ll*)sa)->sll_addr;
140 memcpy(hwaddr, src, 6);
144 #ifdef NMRPFLASH_LINUX
145 static int bridge_stp_state(const char *intf)
148 snprintf(name, sizeof(name), "/sys/class/net/%s/bridge/stp_state", intf);
149 return open(name, O_RDWR, 0644);
152 static bool bridge_stp_enabled(const char *intf)
155 int fd = bridge_stp_state(intf);
160 if (read(fd, &c, 1) != 1) {
168 static bool bridge_stp(const char *intf, bool enabled)
171 const char *s = enabled ? "1\n" : "0\n";
172 int fd = bridge_stp_state(intf);
177 ret = (write(fd, s, 2) == 2);
183 static struct nl_addr *build_ip(uint32_t ip)
185 struct nl_addr *na = nl_addr_build(AF_INET, &ip, 4);
187 xperror("nl_addr_build");
193 static struct nl_sock *xnl_socket_route()
196 struct nl_sock *sk = nl_socket_alloc();
198 if (!(err = nl_connect(sk, NETLINK_ROUTE))) {
202 nl_perror(err, "nl_connect");
204 xperror("nl_socket_alloc");
210 static bool intf_add_del_ip(const char *intf, uint32_t ipaddr, uint32_t ipmask, bool add)
212 struct rtnl_addr *ra = NULL;
213 struct nl_sock *sk = NULL;
214 struct nl_addr *na = NULL;
217 if (!(sk = xnl_socket_route())) {
221 if (!(ra = rtnl_addr_alloc())) {
222 xperror("rtnl_addr_alloc");
226 rtnl_addr_set_ifindex(ra, if_nametoindex(intf));
228 if (!(na = build_ip(ipaddr))) {
232 nl_addr_set_prefixlen(na, bitcount(ipmask));
233 rtnl_addr_set_local(ra, na);
236 if (!(na = build_ip((ipaddr & ipmask) | ~ipmask))) {
240 rtnl_addr_set_broadcast(ra, na);
243 if ((err = add ? rtnl_addr_add(sk, ra, 0) : rtnl_addr_delete(sk, ra, 0)) < 0) {
244 if (add && err == -NLE_EXIST) {
246 } else if (add || verbosity > 1) {
247 nl_perror(err, add ? "rtnl_addr_add" : "rtnl_addr_delete");
258 static bool intf_add_del_arp(const char *intf, uint32_t ipaddr, uint8_t *hwaddr, bool add)
262 memset(&arp, 0, sizeof(arp));
263 arp.arp_ha.sa_family = ARPHRD_ETHER;
264 memcpy(&arp.arp_ha.sa_data, hwaddr, 6);
265 arp.arp_flags = ATF_PERM | ATF_COM;
267 struct sockaddr_in *in = (struct sockaddr_in*)&req.arp_pa;
268 in->sin_addr.s_addr = htonl(ipaddr);
269 in->sin_family = AF_INET;
271 int fd = socket(AF_INET, SOCK_DGRAM, 0);
279 if (ioctl(fd, add ? SIOCSARP : SIOCDARP, &req) < 0) {
280 perror(add ? "ioctl(SIOCSARP)" : "ioctl(SIOCDARP");
288 struct rtnl_neigh *neigh;
289 struct nl_addr *mac, *ip;
296 if (!(sk = xnl_socket_route())) {
300 if (!(neigh = rtnl_neigh_alloc())) {
301 xperror("rtnl_neigh_alloc");
305 if (!(mac = nl_addr_build(AF_PACKET, hwaddr, 6))) {
306 xperror("nl_addr_build");
310 if (!(ip = nl_addr_build(AF_INET, &ipaddr, 4))) {
311 xperror("nl_addr_build");
315 rtnl_neigh_set_ifindex(neigh, if_nametoindex(intf));
316 rtnl_neigh_set_dst(neigh, ip);
318 err = rtnl_neigh_delete(sk, neigh, 0);
321 rtnl_neigh_set_lladdr(neigh, mac);
322 rtnl_neigh_set_state(neigh, NUD_PERMANENT);
323 err = rtnl_neigh_add(sk, neigh, NLM_F_CREATE);
326 if (err && (add || verbosity > 1)) {
327 nl_perror(err, add ? "rtnl_neigh_add" : "rtnl_neigh_delete");
333 rtnl_neigh_put(neigh);
342 static bool intf_get_info(const char *intf, uint8_t *hwaddr, bool *bridge)
344 struct ifaddrs *ifas, *ifa;
347 if (getifaddrs(&ifas) != 0) {
348 xperror("getifaddrs");
358 for (ifa = ifas; ifa; ifa = ifa->ifa_next) {
359 if (!strcmp(ifa->ifa_name, intf)) {
360 if (sockaddr_get_hwaddr(ifa->ifa_addr, hwaddr)) {
363 *bridge = ((struct if_data*) ifa->ifa_data)->ifi_type == IFT_BRIDGE;
378 void win_perror2(const char *msg, DWORD err)
381 FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER |
382 FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
383 NULL, err, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
384 (LPTSTR)&buf, 0, NULL);
387 /* FormatMessageA terminates buf with CRLF! */
388 fprintf(stderr, "%s: %s", msg, buf);
391 fprintf(stderr, "%s: error %d\n", msg, (int)err);
395 static bool intf_get_info(const char *intf, uint8_t *hwaddr, DWORD *index)
397 PIP_ADAPTER_INFO adapters, adapter;
402 if ((ret = GetAdaptersInfo(NULL, &bufLen)) != ERROR_BUFFER_OVERFLOW) {
403 win_perror2("GetAdaptersInfo", ret);
407 adapters = malloc(bufLen);
413 if ((ret = GetAdaptersInfo(adapters, &bufLen) == NO_ERROR)) {
414 for (adapter = adapters; adapter; adapter = adapter->Next) {
415 if (adapter->Type != MIB_IF_TYPE_ETHERNET && adapter->Type != IF_TYPE_IEEE80211) {
419 /* Interface names from WinPcap are "\Device\NPF_{GUID}", while
420 * AdapterName from GetAdaptersInfo is just "{GUID}".*/
421 if (strstr(intf, adapter->AdapterName)) {
422 if (adapter->AddressLength == 6) {
423 memcpy(hwaddr, adapter->Address, 6);
425 *index = adapter->Index;
433 win_perror2("GetAdaptersInfo", ret);
440 static const char *intf_alias_to_wpcap(const char *intf)
442 static char buf[128];
443 pcap_if_t *devs, *dev;
444 unsigned i = 0, dev_num = 0;
446 if (intf[0] == '\\') {
448 } else if (sscanf(intf, NMRPFLASH_NETALIAS_PREFIX "%u", &dev_num) != 1) {
449 fprintf(stderr, "Invalid interface alias.\n");
453 if (x_pcap_findalldevs(&devs) != 0) {
457 for (dev = devs; dev; dev = dev->next, ++i) {
460 printf("%s%u: %s\n", NMRPFLASH_NETALIAS_PREFIX, i, dev->name);
462 strncpy(buf, dev->name, sizeof(buf) - 1);
463 buf[sizeof(buf) - 1] = '\0';
468 pcap_freealldevs(devs);
471 fprintf(stderr, "Interface alias not found.\n");
478 static const char *intf_get_pretty_name(const char *intf)
480 static char buf[512];
486 guid = strstr(intf, "NPF_{");
493 snprintf(buf, sizeof(buf),
494 "System\\CurrentControlSet\\Control\\Network\\"
495 "{4D36E972-E325-11CE-BFC1-08002BE10318}\\"
496 "%s\\Connection", guid);
497 err = RegOpenKeyExA(HKEY_LOCAL_MACHINE, buf, 0, KEY_READ, &hkey);
498 if (err != ERROR_SUCCESS) {
500 win_perror2("RegOpenKeyExA", err);
506 err = RegQueryValueExA(hkey, "Name", NULL, NULL, (LPBYTE)buf, &len);
507 if (err == ERROR_SUCCESS) {
511 win_perror2("RegQueryValueExA", err);
521 inline uint8_t *ethsock_get_hwaddr(struct ethsock *sock)
526 struct ethsock *ethsock_create(const char *intf, uint16_t protocol)
528 char buf[PCAP_ERRBUF_SIZE];
529 struct bpf_program fp;
530 struct ethsock *sock;
534 #ifdef NMRPFLASH_WINDOWS
535 intf = intf_alias_to_wpcap(intf);
541 sock = malloc(sizeof(struct ethsock));
550 sock->pcap = pcap_open_live(sock->intf, BUFSIZ, 1, 1, buf);
552 fprintf(stderr, "%s.\n", buf);
557 fprintf(stderr, "Warning: %s.\n", buf);
560 if (pcap_datalink(sock->pcap) != DLT_EN10MB) {
561 fprintf(stderr, "%s is not an ethernet interface.\n",
566 #ifndef NMRPFLASH_WINDOWS
567 err = !intf_get_info(intf, sock->hwaddr, &is_bridge);
569 err = !intf_get_info(intf, sock->hwaddr, &sock->index);
572 fprintf(stderr, "Failed to get interface info.\n");
576 #ifndef NMRPFLASH_WINDOWS
577 sock->fd = pcap_get_selectable_fd(sock->pcap);
578 if (sock->fd == -1) {
579 pcap_perror(sock->pcap, "pcap_get_selectable_fd");
583 sock->handle = pcap_getevent(sock->pcap);
585 pcap_perror(sock->pcap, "pcap_getevent");
589 err = pcap_setmintocopy(sock->pcap, 1);
591 pcap_perror(sock->pcap, "pcap_setmintocopy");
596 snprintf(buf, sizeof(buf), "ether proto 0x%04x and not ether src %s",
597 protocol, mac_to_str(sock->hwaddr));
599 err = pcap_compile(sock->pcap, &fp, buf, 0, 0);
601 pcap_perror(sock->pcap, "pcap_compile");
605 err = pcap_setfilter(sock->pcap, &fp);
609 pcap_perror(sock->pcap, "pcap_setfilter");
613 #ifdef NMRPFLASH_LINUX
614 // nmrpflash does not work on bridge interfaces with STP enabled
615 if ((sock->stp = bridge_stp_enabled(intf))) {
616 if (!bridge_stp(intf, false)) {
617 fprintf(stderr, "Warning: failed to disable STP on %s.\n", intf);
622 fprintf(stderr, "Warning: bridge interfaces are not fully "
623 "supported on this platform.\n");
634 int select_fd(int fd, unsigned timeout)
643 tv.tv_sec = timeout / 1000000;
644 tv.tv_usec = timeout % 1000000;
646 status = select(fd + 1, &fds, NULL, NULL, &tv);
648 sock_perror("select");
654 ssize_t ethsock_recv(struct ethsock *sock, void *buf, size_t len)
656 struct pcap_pkthdr* hdr;
657 const u_char *capbuf;
659 #ifdef NMRPFLASH_WINDOWS
663 ret = WaitForSingleObject(sock->handle, sock->timeout);
664 if (ret == WAIT_TIMEOUT) {
666 } else if (ret != WAIT_OBJECT_0) {
667 win_perror2("WaitForSingleObject", ret);
673 status = select_fd(sock->fd, sock->timeout);
676 } else if (status == 0) {
682 status = pcap_next_ex(sock->pcap, &hdr, &capbuf);
685 memcpy(buf, capbuf, MIN(len, hdr->caplen));
690 pcap_perror(sock->pcap, "pcap_next_ex");
693 fprintf(stderr, "pcap_next_ex: returned %d.\n", status);
698 int ethsock_send(struct ethsock *sock, void *buf, size_t len)
700 #ifdef NMRPFLASH_WINDOWS
701 if (pcap_sendpacket(sock->pcap, buf, len) == 0) {
704 pcap_perror(sock->pcap, "pcap_sendpacket");
708 if (pcap_inject(sock->pcap, buf, len) == len) {
711 pcap_perror(sock->pcap, "pcap_inject");
717 int ethsock_close(struct ethsock *sock)
723 #ifdef NMRPFLASH_LINUX
725 bridge_stp(sock->intf, true);
729 pcap_close(sock->pcap);
736 inline int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
738 sock->timeout = msec;
742 static int ethsock_arp(struct ethsock *sock, uint8_t *hwaddr, uint32_t ipaddr, struct ethsock_arp_undo **undo)
744 #if defined(NMRPFLASH_UNIX) && !defined(NMRPFLASH_LINUX)
745 struct in_addr addr = { .s_addr = ipaddr };
746 #elif defined(NMRPFLASH_WINDOWS)
749 .dwIndex = sock->index,
752 .dwType = MIB_IPNET_TYPE_STATIC
755 memcpy(arp.bPhysAddr, hwaddr, 6);
759 #if defined(NMRPFLASH_LINUX)
760 if (!intf_add_del_arp(sock->intf, ipaddr, hwaddr, true)) {
763 #elif defined(NMRPFLASH_WINDOWS)
764 err = CreateIpNetEntry(&arp);
765 if (err != NO_ERROR) {
766 win_perror2("CreateIpNetEntry", err);
770 if (systemf("arp -s %s %s", inet_ntoa(addr), mac_to_str(hwaddr)) != 0) {
775 *undo = malloc(sizeof(struct ethsock_arp_undo));
781 (*undo)->ipaddr = ipaddr;
782 memcpy((*undo)->hwaddr, hwaddr, 6);
784 #if defined(NMRPFLASH_LINUX)
785 if (!intf_add_del_arp(sock->intf, ipaddr, hwaddr, false)) {
788 #elif defined(NMRPFLASH_WINDOWS)
789 return DeleteIpNetEntry(&arp) ? 0 : -1;
791 return systemf("arp -d %s", inet_ntoa(addr));
798 int ethsock_arp_add(struct ethsock *sock, uint8_t *hwaddr, uint32_t ipaddr, struct ethsock_arp_undo **undo)
800 ethsock_arp(sock, hwaddr, ipaddr, NULL);
801 return undo ? ethsock_arp(sock, hwaddr, ipaddr, undo) : -1;
804 int ethsock_arp_del(struct ethsock *sock, struct ethsock_arp_undo **undo)
810 int ret = ethsock_arp(sock, (*undo)->hwaddr, (*undo)->ipaddr, NULL);
816 static bool get_hwaddr_from_pcap(const pcap_if_t *dev, uint8_t *hwaddr)
818 #ifndef NMRPFLASH_WINDOWS
822 for (addr = dev->addresses; addr; addr = addr->next) {
824 printf("%s: sa_family=%d, sa_data={ ", dev->name,
825 addr->addr->sa_family);
826 for (i = 0; i != sizeof(addr->addr->sa_data); ++i) {
827 printf("%02x ", addr->addr->sa_data[i] & 0xff);
832 if (sockaddr_get_hwaddr(addr->addr, hwaddr)) {
838 return intf_get_info(dev->name, hwaddr, NULL);
841 int ethsock_list_all(void)
843 pcap_if_t *devs, *dev;
846 unsigned dev_num = 0, dev_ok = 0;
847 #ifdef NMRPFLASH_WINDOWS
851 if (x_pcap_findalldevs(&devs) != 0) {
855 memset(hwaddr, 0, 6);
857 for (dev = devs; dev; dev = dev->next, ++dev_num) {
858 if (dev->flags & PCAP_IF_LOOPBACK) {
860 printf("%-15s (loopback device)\n", dev->name);
865 if (!get_hwaddr_from_pcap(dev, hwaddr)) {
867 printf("%-15s (not an ethernet device)\n",
873 #ifndef NMRPFLASH_WINDOWS
874 printf("%-15s", dev->name);
876 /* Call this here so *_perror() calls don't happen within a line */
877 pretty = intf_get_pretty_name(dev->name);
880 printf("%s%-2u", NMRPFLASH_NETALIAS_PREFIX, dev_num);
882 printf("%s", dev->name);
886 for (addr = dev->addresses; addr; addr = addr->next) {
887 if (addr->addr->sa_family == AF_INET) {
889 inet_ntoa(((struct sockaddr_in*)addr->addr)->sin_addr));
895 printf(" %-15s", "0.0.0.0");
898 printf(" %s", mac_to_str(hwaddr));
900 #ifdef NMRPFLASH_WINDOWS
902 printf(" (%s)", pretty);
903 } else if (dev->description) {
904 printf(" (%s)", dev->description);
913 printf("No suitable network interfaces found.\n");
919 int ethsock_for_each_ip(struct ethsock *sock, ethsock_ip_callback_t callback,
922 struct ethsock_ip_callback_args args;
923 pcap_if_t *devs, *dev;
927 if (x_pcap_findalldevs(&devs) != 0) {
933 for (dev = devs; dev; dev = dev->next) {
934 if (strcmp(sock->intf, dev->name)) {
938 for (addr = dev->addresses; addr; addr = addr->next) {
939 if (addr->addr->sa_family == AF_INET) {
940 args.ipaddr = &((struct sockaddr_in*)addr->addr)->sin_addr;
941 args.ipmask = &((struct sockaddr_in*)addr->netmask)->sin_addr;
943 status = callback(&args);
953 pcap_freealldevs(devs);
955 return status <= 0 ? status : 0;
958 static inline void set_addr(void *p, uint32_t addr)
960 struct sockaddr_in* sin = p;
961 sin->sin_family = AF_INET;
962 sin->sin_addr.s_addr = addr;
964 ((struct sockaddr*)p)->sa_len = sizeof(struct sockaddr_in);
968 #if !defined(NMRPFLASH_WINDOWS) && !defined(NMRPFLASH_LINUX)
969 static bool intf_up(int fd, const char *intf, bool up)
972 strncpy(ifr.ifr_name, intf, IFNAMSIZ);
974 if (ioctl(fd, SIOCGIFFLAGS, &ifr) != 0) {
976 xperror("ioctl(SIOCGIFFLAGS)");
982 ifr.ifr_flags &= ~(IFF_UP | IFF_RUNNING);
984 ifr.ifr_flags |= IFF_UP | IFF_RUNNING;
987 if (ioctl(fd, SIOCSIFFLAGS, &ifr) != 0) {
989 xperror("ioctl(SIOCSIFFLAGS)");
998 static int ethsock_ip_add_del(struct ethsock *sock, uint32_t ipaddr, uint32_t ipmask, struct ethsock_ip_undo **undo, bool add)
1003 if (!(*undo = malloc(sizeof(struct ethsock_ip_undo)))) {
1008 memset(*undo, 0, sizeof(**undo));
1012 fd = socket(AF_INET, SOCK_DGRAM, 0);
1014 sock_perror("socket");
1018 #ifndef NMRPFLASH_WINDOWS
1019 #ifdef NMRPFLASH_LINUX
1021 (*undo)->ip[0] = ipaddr;
1022 (*undo)->ip[1] = ipmask;
1025 if (!intf_add_del_ip(sock->intf, (*undo)->ip[0], (*undo)->ip[1], add)) {
1028 #else // NMRPFLASH_OSX (or any other BSD)
1029 struct ifaliasreq ifra;
1030 memset(&ifra, 0, sizeof(ifra));
1031 strncpy(ifra.ifra_name, sock->intf, IFNAMSIZ);
1033 set_addr(&ifra.ifra_addr, ipaddr);
1034 set_addr(&ifra.ifra_mask, ipmask);
1035 //set_addr(&ifra.ifra_broadaddr, (ipaddr & ipmask) | ~ipmask);
1037 if (ioctl(fd, add ? SIOCAIFADDR : SIOCDIFADDR, &ifra) != 0) {
1039 xperror("ioctl(SIOCAIFADDR");
1045 (*undo)->ip[0] = ipaddr;
1046 (*undo)->ip[1] = ipmask;
1047 intf_up(fd, ifra.ifra_name, true);
1051 #else // NMRPFLASH_WINDOWS
1052 struct sockaddr_in sin;
1055 (*undo)->context = 0;
1057 DWORD err = AddIPAddress(ipaddr, ipmask, sock->index, &(*undo)->context, &instance);
1058 if (err != NO_ERROR && err != ERROR_DUP_DOMAINNAME && err != ERROR_OBJECT_ALREADY_EXISTS) {
1059 win_perror2("AddIPAddress", err);
1063 set_addr(&sin, ipaddr);
1064 time_t beg = time_monotonic();
1066 /* Wait until the new IP has actually been added */
1068 while (bind(fd, (struct sockaddr*)&sin, sizeof(sin)) != 0) {
1069 if ((time_monotonic() - beg) >= 5) {
1070 fprintf(stderr, "Failed to bind after 5 seconds: ");
1071 sock_perror("bind");
1072 DeleteIPAddress((*undo)->context);
1080 #ifndef NMRPFLASH_WINDOWS
1085 if (ret != 0 && undo) {
1093 int ethsock_ip_add(struct ethsock *sock, uint32_t ipaddr, uint32_t ipmask, struct ethsock_ip_undo **undo)
1095 return ethsock_ip_add_del(sock, ipaddr, ipmask, undo, true);
1098 int ethsock_ip_del(struct ethsock *sock, struct ethsock_ip_undo **undo)
1106 #ifndef NMRPFLASH_WINDOWS
1107 if ((*undo)->ip[0] != INADDR_NONE) {
1108 ret = ethsock_ip_add_del(sock, (*undo)->ip[0], (*undo)->ip[1], undo, false);
1113 ret = DeleteIPAddress((*undo)->context) ? 0 : -1;