From f9838c83d82a1ab3ddd597881b7615c083931b46 Mon Sep 17 00:00:00 2001 From: "Joseph C. Lehner" Date: Wed, 20 Jan 2016 17:47:26 +0100 Subject: [PATCH] It's working... --- nmrpd.c | 303 +++++++++++++++++++++++++++++++++++++++++++------------- 1 file changed, 232 insertions(+), 71 deletions(-) diff --git a/nmrpd.c b/nmrpd.c index ea45cf1..152ae3d 100644 --- a/nmrpd.c +++ b/nmrpd.c @@ -6,13 +6,18 @@ #include #include #include +#include #include -#define ETH_P_NMRP 0x0912 -#define NMRP_MAX_OPT 6 -#define PACKED __attribute__((__packed__)) #define NMRP_HDR_LEN 6 +#define NMRP_OPT_LEN 4 +#define NMRP_MAX_OPT 6 +#define NMRP_MIN_PKT_LEN (sizeof(struct ether_header) + NMRP_HDR_LEN) + +#define ETH_P_NMRP 0x0912 #define IP_LEN 4 +#define PACKED __attribute__((__packed__)) +#define MAX_LOOP_RECV 1024 enum nmrp_code { NMRP_ADVERTISE = 1, @@ -26,8 +31,8 @@ enum nmrp_code { }; enum nmrp_opt_type { - NMRP_MAGIC_NO = 1, - NMRP_DEV_IP = 2 + NMRP_MAGIC_NO = 0x0001, + NMRP_DEV_IP = 0x0002 }; struct nmrp_opt { @@ -39,7 +44,7 @@ struct nmrp_opt { uint8_t addr[IP_LEN]; uint8_t mask[IP_LEN]; } ip; - } value; + } val; } PACKED; struct nmrp_msg { @@ -51,37 +56,83 @@ struct nmrp_msg { uint32_t num_opts; } PACKED; -struct nmrp_msg_pkt { - struct ether_header eth; +struct nmrp_pkt { + struct ether_header eh; struct nmrp_msg msg; } PACKED; -static uint16_t msg_len_htons(struct nmrp_msg *msg) +static void msg_hton(struct nmrp_msg *msg) { uint16_t len = NMRP_HDR_LEN; uint32_t i = 0; + msg->reserved = htons(msg->reserved); + for (; i != msg->num_opts; ++i) { - len += ntohs(msg->opts[i].len); + len += msg->opts[i].len; + msg->opts[i].len = htons(msg->opts[i].len); + msg->opts[i].type = htons(msg->opts[i].type); } - printf("msg: len=%u\n", len); + msg->len = htons(len); +} - return htons(len); +static void msg_hdr_ntoh(struct nmrp_msg *msg) +{ + msg->reserved = ntohs(msg->reserved); + msg->len = ntohs(msg->len); } -static int intf_to_index(int fd, const char *interface) +static int msg_ntoh(struct nmrp_msg *msg) { - struct ifreq ifr; + struct nmrp_opt *opt = msg->opts; + int remaining; + + msg_hdr_ntoh(msg); + remaining = msg->len - NMRP_HDR_LEN; + + while (remaining > 0) { + if (remaining < NMRP_OPT_LEN) { + fprintf(stderr, "malformed message (rem=%d)\n", remaining); + return 1; + } - memset(&ifr, 0, sizeof(ifr)); - strncpy(ifr.ifr_name, interface, IFNAMSIZ - 1); - if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { - perror("ioctl: SIOCGIFINDEX"); - return -1; + opt->type = ntohs(opt->type); + opt->len = ntohs(opt->len); + + remaining -= opt->len; } - return ifr.ifr_ifindex; + return 0; +} + +static void msg_dump(struct nmrp_msg *msg) +{ + struct nmrp_opt *opt; + int remain_len, len, i; + + printf("res=0x%04x, code=%u, id=0x%02x, len=%u", msg->reserved, + msg->code, msg->id, msg->len); + + remain_len = msg->len - NMRP_HDR_LEN; + printf("%s\n", remain_len ? " (no opts)" : ""); + + opt = msg->opts; + + while (remain_len > 0) { + len = opt->len; + printf(" opt type=%u, len=%u", opt->type, len); + for (i = 0; i != len - NMRP_OPT_LEN; ++i) { + if (!(i % 16)) { + printf("\n "); + } + + printf("%02x ", ((char*)&opt->val)[i] & 0xff); + } + printf("\n"); + remain_len -= len; + opt = (struct nmrp_opt*)(((char*)opt) + len); + } } static int get_intf_info(int fd, const char *name, int *index, uint8_t *hwaddr) @@ -106,53 +157,108 @@ static int get_intf_info(int fd, const char *name, int *index, uint8_t *hwaddr) return 0; } -static int send_msg(int fd, struct sockaddr_ll *addr, struct nmrp_msg *msg) +static int pkt_send(int fd, struct sockaddr_ll *addr, struct nmrp_pkt *pkt) { - return sendto(fd, msg, ntohs(msg->len), 0, (struct sockaddr*)addr, sizeof(*addr)); + size_t len = ntohs(pkt->msg.len) + sizeof(pkt->eh); + return sendto(fd, pkt, len, 0, (struct sockaddr*)addr, sizeof(*addr)); } -static void dump_msg(struct nmrp_msg *msg) +static int pkt_recv(int fd, struct nmrp_pkt *pkt) { - struct nmrp_opt *opt; - int remain_len, len, i; + struct sockaddr_ll from; + socklen_t addrlen; + ssize_t bytes, len; - printf("res=0x%04x, code=%u, id=0x%02x, len=%u\n", - msg->reserved, msg->code, msg->id, ntohs(msg->len)); + memset(pkt, 0, sizeof(*pkt)); + bytes = recvfrom(fd, pkt, NMRP_MIN_PKT_LEN, MSG_PEEK, + (struct sockaddr*)&from, &addrlen); - remain_len = ntohs(msg->len) - NMRP_HDR_LEN; - opt = msg->opts; + if (bytes < 0) { + if (errno == EAGAIN) { + return 2; + } + perror("recvfrom(pkt)"); + return 1; + } else if (ntohs(pkt->eh.ether_type) != ETH_P_NMRP) { + return 3; + } else if (bytes < NMRP_MIN_PKT_LEN) { + fprintf(stderr, "short packet (%zi bytes)\n", bytes); + return 1; + } - 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); + msg_hdr_ntoh(&pkt->msg); + len = pkt->msg.len + sizeof(pkt->eh); + + bytes = recvfrom(fd, pkt, len, MSG_DONTWAIT, NULL, NULL); + if (bytes < 0) { + perror("recvfrom(msg)"); + return 1; + } else if (bytes != len) { + fprintf(stderr, "short message (%zi bytes)\n", len); + return 1; + } else { + if (msg_ntoh(&pkt->msg) != 0) { + return 1; + } + msg_dump(&pkt->msg); + + return 0; + } + + return 1; +} + +static int sock_set_rx_timeout(int fd, unsigned msec) +{ + struct timeval tv; + + if (msec) { + tv.tv_sec = 0; + tv.tv_usec = msec * 1000; + if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { + perror("setsockopt(SO_RCVTIMEO)"); + return 1; } - printf("\n"); - remain_len -= len; - opt = (struct nmrp_opt*)(((char*)opt) + len); } - printf("remain_len=%d\n", remain_len); + + return 0; +} + +static int sock_bind(int fd, const char *name) +{ + struct ifreq ifr; + + strncpy(ifr.ifr_name, name, IFNAMSIZ - 1); + if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr)) < 0) { + perror("setsockopt(SO_BINDTODEVICE)"); + return 1; + } + + return 0; } -static uint8_t addr[4] = { 192, 168, 2, 2 }; -static uint8_t mask[4] = { 255, 255, 255, 0 }; +static uint8_t ipaddr[4] = { 192, 168, 2, 2 }; +static uint8_t ipmask[4] = { 255, 255, 255, 0 }; static const char *interface = "enp4s0"; +#if 1 static uint8_t target[ETH_ALEN] = { 0xa4, 0x2b, 0x8c, 0x10, 0xc2, 0x96 }; +#else +static uint8_t target[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; +#endif static const char *spinner = "\\|/-"; int main(int argc, char **argv) { - struct nmrp_msg_pkt pkt; + struct nmrp_pkt pkt, rx; struct sockaddr_ll addr; uint8_t hwaddr[ETH_ALEN]; - int hwindex; - int i; - int fd; + int i, fd, err, status, expect; + + err = 1; - fd = socket(AF_PACKET, SOCK_DGRAM, ETH_P_NMRP); + fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_NMRP)); if (fd == -1) { perror("socket"); return 1; @@ -162,57 +268,112 @@ int main(int argc, char **argv) return 1; } + if (sock_bind(fd, interface)) { + return 1; + } + +#if 1 + if (sock_set_rx_timeout(fd, 10)) { + return 1; + } +#endif + addr.sll_family = PF_PACKET; + //addr.sll_hatype = ARPHRD_ETHER; + //addr.sll_pkttype = PACKET_OTHERHOST; + addr.sll_protocol = htons(ETH_P_NMRP); addr.sll_halen = ETH_ALEN; memcpy(addr.sll_addr, target, ETH_ALEN); + memcpy(pkt.eh.ether_shost, hwaddr, ETH_ALEN); + memcpy(pkt.eh.ether_dhost, target, ETH_ALEN); + pkt.eh.ether_type = htons(ETH_P_NMRP); + 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); + pkt.msg.opts[0].type = NMRP_MAGIC_NO; + pkt.msg.opts[0].len = NMRP_OPT_LEN + 4; + pkt.msg.opts[0].val.magic[0] = 'N'; + pkt.msg.opts[0].val.magic[1] = 'T'; + pkt.msg.opts[0].val.magic[2] = 'G'; + pkt.msg.opts[0].val.magic[3] = 'R'; - dump_msg(&pkt.msg); + msg_hton(&pkt.msg); i = 0; -#if 1 while (1) { - printf("\rFlooding %s with NMRP_ADVERTISE ... %c", + printf("\rAdvertising NMRP server on %s ... %c", interface, spinner[i]); fflush(stdout); i = (i + 1) & 3; - if (send_msg(fd, &addr, &pkt.msg) < 0) { + if (pkt_send(fd, &addr, &pkt) < 0) { perror("sendto"); - return 1; + break; } - usleep(10); - //sleep(5); + status = pkt_recv(fd, &rx); + if (status == 0) { + break; + } else if (status == 1) { + printf("ERR\n"); + goto out; + } } -#else - printf("\nuint8_t pkt[] = {"); - for (; i != ntohs(msg.len); ++i) { - if (i) { - printf(","); + printf("\n"); + + expect = NMRP_CONF_REQ; + + do { + if (rx.msg.code == expect || rx.msg.code == NMRP_KEEP_ALIVE_REQ) { + pkt.msg.reserved = 0; + pkt.msg.id = 0; + + switch (rx.msg.code) { + case NMRP_KEEP_ALIVE_REQ: + pkt.msg.code = NMRP_KEEP_ALIVE_ACK; + pkt.msg.num_opts = 0; + break; + case NMRP_CONF_REQ: + pkt.msg.code = NMRP_CONF_ACK; + pkt.msg.num_opts = 1; + pkt.msg.opts[0].type = NMRP_DEV_IP; + pkt.msg.opts[0].len = NMRP_OPT_LEN + 2 * IP_LEN; + memcpy(pkt.msg.opts[0].val.ip.addr, ipaddr, IP_LEN); + memcpy(pkt.msg.opts[0].val.ip.mask, ipmask, IP_LEN); + expect = -1; + break; + default: + fprintf(stderr, "Unhandled message code %02x!\n", + rx.msg.code); + } + + if (pkt_send(fd, &addr, &pkt) < 0) { + perror("sendto"); + break; + } + } else if (rx.msg.code != NMRP_KEEP_ALIVE_REQ) { + fprintf(stderr, "Received code %02x while waiting for %02x!", + rx.msg.code, expect); } - if (!(i % 8)) { - printf("\n\t"); + i = 0; + + while ((status = pkt_recv(fd, &rx)) != 0) { + if (++i == MAX_LOOP_RECV) { + fprintf(stderr, "Timeout while waiting for %02x.\n", expect); + goto out; + } } + } while (status != 1); - printf(" 0x%02x", ((char*)&msg)[i]); - } + err = 0; - printf("\n};\n"); -#endif +out: + close(fd); - return 0; + return err; } -- 2.25.1