Initial commit

diff --git a/nmrpd.c b/nmrpd.c
new file mode 100644 (file)
index 0000000..ea45cf1
--- /dev/null
+++ b/nmrpd.c
@@ -0,0 +1,218 @@
+#include <netinet/ether.h>
+#include <linux/if_packet.h>
+#include <arpa/inet.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+#define ETH_P_NMRP 0x0912
+#define NMRP_MAX_OPT 6
+#define PACKED __attribute__((__packed__))
+#define NMRP_HDR_LEN 6
+#define IP_LEN 4
+
+enum nmrp_code {
+       NMRP_ADVERTISE = 1,
+       NMRP_CONF_REQ = 2,
+       NMRP_CONF_ACK = 3,
+       NMRP_CLOSE_REQ = 4,
+       NMRP_CLOSE_ACK = 5,
+       NMRP_KEEP_ALIVE_REQ = 6,
+       NMRP_KEEP_ALIVE_ACK = 7,
+       NMRP_TFTP_UPLOAD_REQ = 16
+};
+
+enum nmrp_opt_type {
+       NMRP_MAGIC_NO = 1,
+       NMRP_DEV_IP = 2
+};
+
+struct nmrp_opt {
+       uint16_t type;
+       uint16_t len;
+       union {
+               uint8_t magic[4];
+               struct {
+                       uint8_t addr[IP_LEN];
+                       uint8_t mask[IP_LEN];
+               } ip;
+       } value;
+} PACKED;
+
+struct nmrp_msg {
+       uint16_t reserved;
+       uint8_t code;
+       uint8_t id;
+       uint16_t len;
+       struct nmrp_opt opts[6];
+       uint32_t num_opts;
+} PACKED;
+
+struct nmrp_msg_pkt {
+       struct ether_header eth;
+       struct nmrp_msg msg;
+} PACKED;
+
+static uint16_t msg_len_htons(struct nmrp_msg *msg)
+{
+       uint16_t len = NMRP_HDR_LEN;
+       uint32_t i = 0;
+
+       for (; i != msg->num_opts; ++i) {
+               len += ntohs(msg->opts[i].len);
+       }
+
+       printf("msg: len=%u\n", len);
+
+       return htons(len);
+}
+
+static int intf_to_index(int fd, const char *interface)
+{
+       struct ifreq ifr;
+
+       memset(&ifr, 0, sizeof(ifr));
+       strncpy(ifr.ifr_name, interface, IFNAMSIZ - 1);
+       if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
+               perror("ioctl: SIOCGIFINDEX");
+               return -1;
+       }
+
+       return ifr.ifr_ifindex;
+}
+
+static int get_intf_info(int fd, const char *name, int *index, uint8_t *hwaddr)
+{
+       struct ifreq ifr;
+
+       memset(&ifr, 0, sizeof(ifr));
+       strncpy(ifr.ifr_name, name, IFNAMSIZ - 1);
+
+       if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
+               perror("ioctl(SIOCGIFINDEX)");
+               return -1;
+       }
+       *index = ifr.ifr_ifindex;
+
+       if (ioctl(fd, SIOCGIFHWADDR, &ifr) < 0) {
+               perror("ioctl(SIOCGIFHWADDR)");
+               return -1;
+       }
+       memcpy(hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
+
+       return 0;
+}
+
+static int send_msg(int fd, struct sockaddr_ll *addr, struct nmrp_msg *msg)
+{
+       return sendto(fd, msg, ntohs(msg->len), 0, (struct sockaddr*)addr, sizeof(*addr));
+}
+
+static void dump_msg(struct nmrp_msg *msg)
+{
+       struct nmrp_opt *opt;
+       int remain_len, len, i;
+
+       printf("res=0x%04x, code=%u, id=0x%02x, len=%u\n",
+                       msg->reserved, msg->code, msg->id, ntohs(msg->len));
+
+       remain_len = ntohs(msg->len) - NMRP_HDR_LEN;
+       opt = msg->opts;
+
+       while (remain_len > 0) {
+               len = ntohs(opt->len);
+               printf("  opt type=%u, len=%u\n  ", ntohs(opt->type), len);
+               for (i = 0; i != len - 4; ++i) {
+                       printf("%02x ", ((char*)&opt->value)[i] & 0xff);
+               }
+               printf("\n");
+               remain_len -= len;
+               opt = (struct nmrp_opt*)(((char*)opt) + len);
+       }
+       printf("remain_len=%d\n", remain_len);
+}
+
+static uint8_t addr[4] = { 192, 168, 2, 2 };
+static uint8_t mask[4] = { 255, 255, 255, 0 };
+
+static const char *interface = "enp4s0";
+static uint8_t target[ETH_ALEN] = { 0xa4, 0x2b, 0x8c, 0x10, 0xc2, 0x96 };
+
+static const char *spinner = "\\|/-";
+
+int main(int argc, char **argv)
+{
+       struct nmrp_msg_pkt pkt;
+       struct sockaddr_ll addr;
+       uint8_t hwaddr[ETH_ALEN];
+       int hwindex;
+       int i;
+       int fd;
+
+       fd = socket(AF_PACKET, SOCK_DGRAM, ETH_P_NMRP);
+       if (fd == -1) {
+               perror("socket");
+               return 1;
+       }
+
+       if (get_intf_info(fd, interface, &addr.sll_ifindex, hwaddr)) {
+               return 1;
+       }
+
+       addr.sll_halen = ETH_ALEN;
+       memcpy(addr.sll_addr, target, ETH_ALEN);
+
+       pkt.msg.reserved = 0;
+       pkt.msg.code = NMRP_ADVERTISE;
+       pkt.msg.id = 0;
+       pkt.msg.opts[0].type = NMRP_MAGIC_NO;
+       pkt.msg.opts[0].len = htons(8);
+       pkt.msg.opts[0].value.magic[0] = 'N';
+       pkt.msg.opts[0].value.magic[1] = 'T';
+       pkt.msg.opts[0].value.magic[2] = 'G';
+       pkt.msg.opts[0].value.magic[3] = 'R';
+       pkt.msg.num_opts = 1;
+       pkt.msg.len = msg_len_htons(&pkt.msg);
+
+       dump_msg(&pkt.msg);
+
+       i = 0;
+
+#if 1
+       while (1) {
+               printf("\rFlooding %s with NMRP_ADVERTISE ... %c", 
+                               interface, spinner[i]);
+               fflush(stdout);
+               i = (i + 1) & 3;
+
+               if (send_msg(fd, &addr, &pkt.msg) < 0) {
+                       perror("sendto");
+                       return 1;
+               }
+
+               usleep(10);
+               //sleep(5);
+       }
+#else
+       printf("\nuint8_t pkt[] = {");
+
+       for (; i != ntohs(msg.len); ++i) {
+               if (i) {
+                       printf(",");
+               }
+
+               if (!(i % 8)) {
+                       printf("\n\t");
+               }
+
+               printf(" 0x%02x", ((char*)&msg)[i]);
+       }
+
+       printf("\n};\n");
+#endif
+
+       return 0;
+}