#include <net/if.h>
#include <stdint.h>
#include <string.h>
+#include <errno.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_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,
};
enum nmrp_opt_type {
- NMRP_MAGIC_NO = 1,
- NMRP_DEV_IP = 2
+ NMRP_MAGIC_NO = 0x0001,
+ NMRP_DEV_IP = 0x0002
};
struct nmrp_opt {
uint8_t addr[IP_LEN];
uint8_t mask[IP_LEN];
} ip;
- } value;
+ } val;
} PACKED;
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)
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;
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;
}
projects / oweals / nmrpflash.git / commitdiff