Add ARPinger

diff --git a/Makefile b/Makefile
index db98f59e0d472590e561a451022a912c52357989..c69469f2de6955253ae4688e0c8df462eea8755c 100644 (file)
--- 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 (file)
index 0000000..4bd2f35
--- /dev/null
+++ b/arp.c
@@ -0,0 +1,197 @@
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#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 cd62503e2d2478579f31e1c81f6101e8da407296..459a646e0741a0aa86d2f5ff7e46970a0d0e8580 100644 (file)
--- 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 ef25a690f48a14d1585a8ad0ad5fb91e03397560..77cf7271660d94bdd832882906f0d4c2e4243e88 100644 (file)
--- 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