pogmommy/nmrpflash
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

It's working...

Browse source
This commit is contained in:
Joseph C. Lehner 2016-01-20 17:47:26 +01:00
parent ad4a19a7da
commit f9838c83d8
1 changed files with 238 additions and 77 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

315
nmrpd.c
View file

@ -6,13 +6,18 @@
#include <net/if.h> #include <net/if.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <errno.h>
#include <stdio.h> #include <stdio.h>
#define ETH_P_NMRP 0x0912
#define NMRP_MAX_OPT 6
#define PACKED __attribute__((__packed__))
#define NMRP_HDR_LEN 6 #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 IP_LEN 4
#define PACKED __attribute__((__packed__))
#define MAX_LOOP_RECV 1024
enum nmrp_code { enum nmrp_code {
NMRP_ADVERTISE = 1, NMRP_ADVERTISE = 1,
@ -26,8 +31,8 @@ enum nmrp_code {
}; };
enum nmrp_opt_type { enum nmrp_opt_type {
NMRP_MAGIC_NO = 1, NMRP_MAGIC_NO = 0x0001,
NMRP_DEV_IP = 2 NMRP_DEV_IP = 0x0002
}; };
struct nmrp_opt { struct nmrp_opt {
@ -39,7 +44,7 @@ struct nmrp_opt {
uint8_t addr[IP_LEN]; uint8_t addr[IP_LEN];
uint8_t mask[IP_LEN]; uint8_t mask[IP_LEN];
} ip; } ip;
} value; } val;
} PACKED; } PACKED;
struct nmrp_msg { struct nmrp_msg {
@ -51,37 +56,83 @@ struct nmrp_msg {
uint32_t num_opts; uint32_t num_opts;
} PACKED; } PACKED;
struct nmrp_msg_pkt { struct nmrp_pkt {
struct ether_header eth; struct ether_header eh;
struct nmrp_msg msg; struct nmrp_msg msg;
} PACKED; } 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; uint16_t len = NMRP_HDR_LEN;
uint32_t i = 0; uint32_t i = 0;
msg->reserved = htons(msg->reserved);
for (; i != msg->num_opts; ++i) { 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 int intf_to_index(int fd, const char *interface) static void msg_hdr_ntoh(struct nmrp_msg *msg)
{ {
struct ifreq ifr; msg->reserved = ntohs(msg->reserved);
msg->len = ntohs(msg->len);
}
memset(&ifr, 0, sizeof(ifr)); static int msg_ntoh(struct nmrp_msg *msg)
strncpy(ifr.ifr_name, interface, IFNAMSIZ - 1); {
if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { struct nmrp_opt *opt = msg->opts;
perror("ioctl: SIOCGIFINDEX"); int remaining;
return -1;
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;
}
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) 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; 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; struct sockaddr_ll from;
int remain_len, len, i; socklen_t addrlen;
ssize_t bytes, len;
printf("res=0x%04x, code=%u, id=0x%02x, len=%u\n", memset(pkt, 0, sizeof(*pkt));
msg->reserved, msg->code, msg->id, ntohs(msg->len)); bytes = recvfrom(fd, pkt, NMRP_MIN_PKT_LEN, MSG_PEEK,
(struct sockaddr*)&from, &addrlen);
remain_len = ntohs(msg->len) - NMRP_HDR_LEN; if (bytes < 0) {
opt = msg->opts; if (errno == EAGAIN) {
return 2;
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"); perror("recvfrom(pkt)");
remain_len -= len; return 1;
opt = (struct nmrp_opt*)(((char*)opt) + len); } 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;
} }
printf("remain_len=%d\n", remain_len);
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 uint8_t addr[4] = { 192, 168, 2, 2 }; static int sock_set_rx_timeout(int fd, unsigned msec)
static uint8_t mask[4] = { 255, 255, 255, 0 }; {
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;
}
}
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 ipaddr[4] = { 192, 168, 2, 2 };
static uint8_t ipmask[4] = { 255, 255, 255, 0 };
static const char *interface = "enp4s0"; static const char *interface = "enp4s0";
#if 1
static uint8_t target[ETH_ALEN] = { 0xa4, 0x2b, 0x8c, 0x10, 0xc2, 0x96 }; 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 = "\\|/-"; static const char *spinner = "\\|/-";
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
struct nmrp_msg_pkt pkt; struct nmrp_pkt pkt, rx;
struct sockaddr_ll addr; struct sockaddr_ll addr;
uint8_t hwaddr[ETH_ALEN]; uint8_t hwaddr[ETH_ALEN];
int hwindex; int i, fd, err, status, expect;
int i;
int fd;
fd = socket(AF_PACKET, SOCK_DGRAM, ETH_P_NMRP); err = 1;
fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_NMRP));
if (fd == -1) { if (fd == -1) {
perror("socket"); perror("socket");
return 1; return 1;
@ -162,57 +268,112 @@ int main(int argc, char **argv)
return 1; 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; addr.sll_halen = ETH_ALEN;
memcpy(addr.sll_addr, target, 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.reserved = 0;
pkt.msg.code = NMRP_ADVERTISE; pkt.msg.code = NMRP_ADVERTISE;
pkt.msg.id = 0; 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.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; i = 0;
#if 1
while (1) { while (1) {
printf("\rFlooding %s with NMRP_ADVERTISE ... %c", printf("\rAdvertising NMRP server on %s ... %c",
interface, spinner[i]); interface, spinner[i]);
fflush(stdout); fflush(stdout);
i = (i + 1) & 3; i = (i + 1) & 3;
if (send_msg(fd, &addr, &pkt.msg) < 0) { if (pkt_send(fd, &addr, &pkt) < 0) {
perror("sendto"); perror("sendto");
return 1; break;
} }
usleep(10); status = pkt_recv(fd, &rx);
//sleep(5); if (status == 0) {
} break;
#else } else if (status == 1) {
printf("\nuint8_t pkt[] = {"); printf("ERR\n");
goto out;
for (; i != ntohs(msg.len); ++i) {
if (i) {
printf(",");
} }
if (!(i % 8)) {
printf("\n\t");
}
printf(" 0x%02x", ((char*)&msg)[i]);
} }
printf("\n};\n"); printf("\n");
#endif
return 0; 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);
}
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);
err = 0;
out:
close(fd);
return err;
} }