From: Joseph C. Lehner Date: Sat, 12 Nov 2016 11:17:58 +0000 (+0100) Subject: Add ARPinger X-Git-Tag: v0.9.7~16 X-Git-Url: https://git.librecmc.org/?a=commitdiff_plain;h=f38833e940bfe7580a4afec8d77ea00c9ecae057;p=oweals%2Fnmrpflash.git Add ARPinger --- diff --git a/Makefile b/Makefile index db98f59..c69469f 100644 --- a/Makefile +++ b/Makefile @@ -7,8 +7,8 @@ LDFLAGS += $(LIBS) .PHONY: clean install release release/osx release/linux release/win32 -nmrpflash: nmrp.o tftp.o ethsock.o main.o - $(CC) $(CFLAGS) -o nmrpflash nmrp.o tftp.o ethsock.o main.o $(LDFLAGS) +nmrpflash: nmrp.o tftp.o ethsock.o main.o arp.o + $(CC) $(CFLAGS) -o nmrpflash nmrp.o tftp.o ethsock.o arp.o main.o $(LDFLAGS) nmrp.o: nmrp.c nmrpd.h $(CC) $(CFLAGS) -c -o nmrp.o nmrp.c @@ -19,11 +19,14 @@ tftp.o: tftp.c nmrpd.h ethsock.o: ethsock.c nmrpd.h $(CC) $(CFLAGS) -c -o ethsock.o ethsock.c +arp.o: arp.c nmrpd.h + $(CC) $(CFLAGS) -c -o arp.o arp.c + main.o: main.c nmrpd.h $(CC) $(CFLAGS) -c -o main.o main.c clean: - rm -f nmrp.o tftp.o main.o ethsock.o nmrpflash + rm -f nmrp.o tftp.o main.o ethsock.o arp.o nmrpflash install: nmrpflash install -m 755 nmrpflash $(PREFIX)/bin diff --git a/arp.c b/arp.c new file mode 100644 index 0000000..4bd2f35 --- /dev/null +++ b/arp.c @@ -0,0 +1,197 @@ +#include +#include +#include +#include "nmrpd.h" + +#ifndef PACKED +#define PACKED __attribute__((packed)) +#endif + +#ifndef ETH_P_ARP +#define ETH_P_ARP 0x0806 +#endif + +#define REQUEST_COUNT 256 + +struct arp +{ + uint16_t htype; + uint16_t ptype; + uint8_t hlen; + uint8_t plen; + uint16_t oper; + uint8_t sha[6]; + uint32_t spa; + uint8_t tha[6]; + uint32_t tpa; +} PACKED; + +struct arppkt +{ + struct eth_hdr eth; + struct arp arp; + uint8_t padding[18]; +} PACKED; + +static bool is_arp(void *pktbuf, size_t len) +{ + if (len < 28) { + return false; + } + + len -= 14; + pktbuf += 14; + + struct arp *pkt = pktbuf; + return ntohs(pkt->htype) == 1 && ntohs(pkt->ptype) == 0x0800 + && pkt->hlen == 6 && pkt->plen == 4; +} + +static bool is_reply(void *pktbuf, size_t len, struct ethsock *sock) +{ + struct arppkt *pkt = pktbuf; + return is_arp(pktbuf, len) && htons(pkt->arp.oper) == 2 + && !memcmp(ethsock_get_hwaddr(sock), pkt->arp.tha, 6); +} + +static const char *u32toa(uint32_t u32) +{ + struct in_addr addr = { .s_addr = u32 }; + return inet_ntoa(addr); +} + +static int ip_callback(struct ethsock_ip_callback_args *args) +{ + uint32_t *ip = args->arg; + ip[0] = args->ipaddr->s_addr; + ip[1] = args->ipmask->s_addr; + + return 0; +} + +static void init_request(struct arppkt *pkt, struct ethsock *sock, uint32_t spa, uint32_t tpa) +{ + memcpy(pkt->eth.ether_shost, ethsock_get_hwaddr(sock), 6); + memset(pkt->eth.ether_dhost, 0xff, 6); + pkt->eth.ether_type = htons(0x0806); + memset(pkt->padding, 0, sizeof(pkt->padding)); + + pkt->arp.htype = htons(1); + pkt->arp.ptype = htons(0x0800); + pkt->arp.hlen = 6; + pkt->arp.plen = 4; + pkt->arp.oper = htons(1); + + memcpy(pkt->arp.sha, ethsock_get_hwaddr(sock), 6); + pkt->arp.spa = htonl(spa); + + memset(pkt->arp.tha, 0xff, 6); + pkt->arp.tpa = htonl(tpa); +} + +int arp_find_free_ip(const char *intf, uint32_t *addr) +{ + struct arppkt pkt; + uint32_t srcip[2]; + struct ethsock *arpsock = NULL; + uint32_t min, max, ip; + int i, timeouts; + bool replies[REQUEST_COUNT] = { 0 }; + int ret = -1; + + arpsock = ethsock_create(intf, ETH_P_ARP); + if (!arpsock) { + return -1; + } + + if (ethsock_set_timeout(arpsock, 1000) != 0) { + goto out; + } + + if (ethsock_for_each_ip(arpsock, &ip_callback, srcip) != 0) { + goto out; + } + + printf("IP is %s/", u32toa(srcip[0])); + printf("%s", u32toa(srcip[1])); + + srcip[0] = ntohl(srcip[0]); + srcip[1] = ntohl(srcip[1]); + + printf(" aka 0x%08x/0x%08x\n", srcip[0], srcip[1]); + + if (~srcip[1]) { + min = srcip[0] & srcip[1]; + // highest possible address, minus 1 (e.g. for 192.168.0.1/24, + // set value to 192.168.0.254) + max = min | (~srcip[1] - 1); + ip = max; + + if (verbosity) { + printf("ARPinging range %s-", u32toa(htonl(min))); + printf("%s\n", u32toa(htonl(max))); + } + + for (i = 0; i < REQUEST_COUNT && ip > min; --ip, ++i) { + if (ip == srcip[0] || replies[i]) { + continue; + } + + init_request(&pkt, arpsock, srcip[0], ip); + if (ethsock_send(arpsock, &pkt, sizeof(pkt)) != 0) { + goto out; + } + } + + min = ip; + timeouts = 0; + + while (1) { + ssize_t bytes = ethsock_recv(arpsock, &pkt, sizeof(pkt)); + if (bytes < 0) { + goto out; + } else if (!bytes) { + if (++timeouts >= 5) { + break; + } + continue; + } + + timeouts = 0; + + if (!is_reply(&pkt, sizeof(pkt), arpsock)) { + continue; + } + + uint32_t spa = ntohl(pkt.arp.spa); + + if (spa > min && spa <= max) { + replies[spa - min] = true; + if (verbosity > 1) { + printf("Got ARP reply for %s from %s.\n", u32toa(pkt.arp.spa), mac_to_str(pkt.arp.sha)); + } + } else if (verbosity > 1) { + printf("Got unexpected ARP reply for %s (min=", u32toa(pkt.arp.spa)); + printf("%s, max=", u32toa(htonl(min))); + printf("%s)\n", u32toa(htonl(max))); + } + } + + for (; i; --i) { + if (!replies[i - 1]) { + *addr = htonl(min + i); + printf("Found free address %s.\n", u32toa(*addr)); + ret = 0; + break; + } + } + } + +out: + ethsock_close(arpsock); + if (ret != 0) { + fprintf(stderr, "Failed to find free ip address on %s\n", intf); + } + + return ret; +} diff --git a/nmrp.c b/nmrp.c index cd62503..459a646 100644 --- a/nmrp.c +++ b/nmrp.c @@ -88,12 +88,6 @@ struct nmrp_msg { uint32_t num_opts; } PACKED; -struct eth_hdr { - uint8_t ether_dhost[6]; - uint8_t ether_shost[6]; - uint16_t ether_type; -} PACKED; - struct nmrp_pkt { struct eth_hdr eh; struct nmrp_msg msg; @@ -404,7 +398,7 @@ int nmrp_do(struct nmrpd_args *args) uint16_t len, region; char *filename; time_t beg; - int i, status, ulreqs, expect, upload_ok; + int i, status, ulreqs, expect, upload_ok, autoip; struct ethsock *sock; void (*sigh_orig)(int); struct { @@ -423,8 +417,11 @@ int nmrp_do(struct nmrpd_args *args) } if ((ipconf.addr.s_addr = inet_addr(args->ipaddr)) == INADDR_NONE) { - fprintf(stderr, "Invalid IP address '%s'.\n", args->ipaddr); - return 1; + autoip = 1; + //fprintf(stderr, "Invalid IP address '%s'.\n", args->ipaddr); + //return 1; + } else { + autoip = 0; } if ((ipconf.mask.s_addr = inet_addr(args->ipmask)) == INADDR_NONE) { @@ -457,6 +454,12 @@ int nmrp_do(struct nmrpd_args *args) status = 1; + if (autoip) { + if (arp_find_free_ip(args->intf, &ipconf.addr.s_addr) != 0) { + return 1; + } + } + sock = ethsock_create(args->intf, ETH_P_NMRP); if (!sock) { return 1; diff --git a/nmrpd.h b/nmrpd.h index ef25a69..77cf727 100644 --- a/nmrpd.h +++ b/nmrpd.h @@ -53,6 +53,16 @@ #define MIN(a, b) ((a) < (b) ? (a) : (b)) #endif +#ifndef PACKED +#define PACKED __attribute__((packed)) +#endif + +struct eth_hdr { + uint8_t ether_dhost[6]; + uint8_t ether_shost[6]; + uint16_t ether_type; +} PACKED; + enum nmrp_op { NMRP_UPLOAD_FW = 0, NMRP_UPLOAD_ST = 1, @@ -114,4 +124,6 @@ struct ethsock_ip_callback_args typedef int (*ethsock_ip_callback_t)(struct ethsock_ip_callback_args *args); int ethsock_for_each_ip(struct ethsock *sock, ethsock_ip_callback_t callback, void *arg); + +int arp_find_free_ip(const char *intf, uint32_t *addr); #endif