nmrp.c

   1 #define _BSD_SOURCE
   2 #include <netinet/ether.h>
   3 #include <linux/if_packet.h>
   4 #include <arpa/inet.h>
   5 #include <sys/socket.h>
   6 #include <sys/ioctl.h>
   7 #include <net/if.h>
   8 #include <stdint.h>
   9 #include <string.h>
  10 #include <unistd.h>
  11 #include <errno.h>
  12 #include <stdio.h>
  13
  14 #define NMRP_HDR_LEN 6
  15 #define NMRP_OPT_LEN 4
  16 #define NMRP_MAX_OPT 6
  17 #define NMRP_MIN_PKT_LEN (sizeof(struct ether_header) +  NMRP_HDR_LEN)
  18
  19 #define ETH_P_NMRP 0x0912
  20 #define IP_LEN 4
  21 #define PACKED __attribute__((__packed__))
  22 #define MAX_LOOP_RECV 1024
  23
  24 #define IS_OOO_CODE(x) (x == NMRP_C_CLOSE_REQ \
  25                 || x == NMRP_C_KEEP_ALIVE_REQ \
  26                 || x == NMRP_C_TFTP_UL_REQ)
  27
  28 extern int tftp_put(const char *filename, const char *ipaddr, uint16_t port);
  29 extern int sock_set_rx_timeout(int fd, unsigned msec);
  30
  31 enum nmrp_code {
  32         NMRP_C_NONE = 0,
  33         NMRP_C_ADVERTISE = 1,
  34         NMRP_C_CONF_REQ = 2,
  35         NMRP_C_CONF_ACK = 3,
  36         NMRP_C_CLOSE_REQ = 4,
  37         NMRP_C_CLOSE_ACK = 5,
  38         NMRP_C_KEEP_ALIVE_REQ = 6,
  39         NMRP_C_KEEP_ALIVE_ACK = 7,
  40         NMRP_C_TFTP_UL_REQ = 16
  41 };
  42
  43 enum nmrp_opt_type {
  44         NMRP_O_MAGIC_NO = 0x0001,
  45         NMRP_O_DEV_IP = 0x0002,
  46         NMRP_O_DEV_REGION = 0x0004,
  47         NMRP_O_FW_UP = 0x0101,
  48         NMRP_O_ST_UP = 0x0102,
  49         NMRP_O_FILE_NAME = 0x0181
  50 };
  51
  52 struct nmrp_opt {
  53         uint16_t type;
  54         uint16_t len;
  55         union {
  56                 uint8_t magic[4];
  57                 struct {
  58                         uint8_t addr[IP_LEN];
  59                         uint8_t mask[IP_LEN];
  60                 } ip;
  61         } val;
  62 } PACKED;
  63
  64 struct nmrp_msg {
  65         uint16_t reserved;
  66         uint8_t code;
  67         uint8_t id;
  68         uint16_t len;
  69         struct nmrp_opt opts[6];
  70         uint32_t num_opts;
  71 } PACKED;
  72
  73 struct nmrp_pkt {
  74         struct ether_header eh;
  75         struct nmrp_msg msg;
  76 } PACKED;
  77
  78 static void msg_update_len(struct nmrp_msg *msg)
  79 {
  80         uint32_t i = 0;
  81         msg->len = NMRP_HDR_LEN;
  82         for (; i != msg->num_opts; ++i) {
  83                 msg->len += msg->opts[i].len;
  84         }
  85 }
  86
  87 static void msg_hton(struct nmrp_msg *msg)
  88 {
  89         uint32_t i = 0;
  90
  91         msg->reserved = htons(msg->reserved);
  92         msg->len = htons(msg->len);
  93
  94         for (; i != msg->num_opts; ++i) {
  95                 msg->opts[i].len = htons(msg->opts[i].len);
  96                 msg->opts[i].type = htons(msg->opts[i].type);
  97         }
  98 }
  99
 100 static void msg_hdr_ntoh(struct nmrp_msg *msg)
 101 {
 102         msg->reserved = ntohs(msg->reserved);
 103         msg->len = ntohs(msg->len);
 104 }
 105
 106 static int msg_ntoh(struct nmrp_msg *msg)
 107 {
 108         struct nmrp_opt *opt = msg->opts;
 109         int remaining;
 110
 111         msg_hdr_ntoh(msg);
 112         remaining = msg->len - NMRP_HDR_LEN;
 113
 114         while (remaining > 0) {
 115                 if (remaining < NMRP_OPT_LEN) {
 116                         fprintf(stderr, "malformed message (rem=%d)\n", remaining);
 117                         return 1;
 118                 }
 119
 120                 opt->type = ntohs(opt->type);
 121                 opt->len = ntohs(opt->len);
 122
 123                 remaining -= opt->len;
 124         }
 125
 126         return 0;
 127 }
 128
 129 static void msg_dump(struct nmrp_msg *msg)
 130 {
 131         struct nmrp_opt *opt;
 132         int remain_len, len, i;
 133
 134         printf("res=0x%04x, code=0x%02x, id=0x%02x, len=%u", msg->reserved,
 135                         msg->code, msg->id, msg->len);
 136
 137         remain_len = msg->len - NMRP_HDR_LEN;
 138         printf("%s\n", remain_len ? "" : " (no opts)");
 139
 140         opt = msg->opts;
 141
 142         while (remain_len > 0) {
 143                 len = opt->len;
 144                 printf("  opt type=%u, len=%u", opt->type, len);
 145                 for (i = 0; i != len - NMRP_OPT_LEN; ++i) {
 146                         if (!(i % 16)) {
 147                                 printf("\n  ");
 148                         }
 149
 150                         printf("%02x ", ((char*)&opt->val)[i] & 0xff);
 151                 }
 152                 printf("\n");
 153                 remain_len -= len;
 154                 opt = (struct nmrp_opt*)(((char*)opt) + len);
 155         }
 156 }
 157
 158 static int intf_get_index_and_addr(int fd, const char *name, int *index,
 159                 uint8_t *hwaddr)
 160 {
 161         struct ifreq ifr;
 162
 163         memset(&ifr, 0, sizeof(ifr));
 164         strncpy(ifr.ifr_name, name, IFNAMSIZ - 1);
 165
 166         if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
 167                 perror("ioctl(SIOCGIFINDEX)");
 168                 return -1;
 169         }
 170         *index = ifr.ifr_ifindex;
 171
 172         if (ioctl(fd, SIOCGIFHWADDR, &ifr) < 0) {
 173                 perror("ioctl(SIOCGIFHWADDR)");
 174                 return -1;
 175         }
 176         memcpy(hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
 177
 178         return 0;
 179 }
 180
 181 static int pkt_send(int fd, struct sockaddr_ll *addr, struct nmrp_pkt *pkt)
 182 {
 183         size_t len = ntohs(pkt->msg.len) + sizeof(pkt->eh);
 184         return sendto(fd, pkt, len, 0, (struct sockaddr*)addr, sizeof(*addr));
 185 }
 186
 187 static int pkt_recv(int fd, struct nmrp_pkt *pkt)
 188 {
 189         struct sockaddr_ll from;
 190         socklen_t addrlen;
 191         ssize_t bytes, len;
 192
 193         memset(pkt, 0, sizeof(*pkt));
 194         bytes = recvfrom(fd, pkt, NMRP_MIN_PKT_LEN, MSG_PEEK,
 195                         (struct sockaddr*)&from, &addrlen);
 196
 197         if (bytes < 0) {
 198                 if (errno == EAGAIN) {
 199                         return 2;
 200                 }
 201                 perror("recvfrom(pkt)");
 202                 return 1;
 203         } else if (ntohs(pkt->eh.ether_type) != ETH_P_NMRP) {
 204                 return 3;
 205         } else if (bytes < NMRP_MIN_PKT_LEN) {
 206                 fprintf(stderr, "short packet (%zi bytes)\n", bytes);
 207                 return 1;
 208         }
 209
 210         msg_hdr_ntoh(&pkt->msg);
 211         len = pkt->msg.len + sizeof(pkt->eh);
 212
 213         bytes = recvfrom(fd, pkt, len, MSG_DONTWAIT, NULL, NULL);
 214         if (bytes < 0) {
 215                 perror("recvfrom(msg)");
 216                 return 1;
 217         } else if (bytes != len) {
 218                 fprintf(stderr, "short message (%zi bytes)\n", len);
 219                 return 1;
 220         } else {
 221                 if (msg_ntoh(&pkt->msg) != 0) {
 222                         return 1;
 223                 }
 224                 msg_dump(&pkt->msg);
 225
 226                 return 0;
 227         }
 228
 229         return 1;
 230 }
 231
 232 static int sock_bind_to_intf(int fd, const char *name)
 233 {
 234         struct ifreq ifr;
 235
 236         strncpy(ifr.ifr_name, name, IFNAMSIZ - 1);
 237         if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr)) < 0) {
 238                 perror("setsockopt(SO_BINDTODEVICE)");
 239                 return 1;
 240         }
 241
 242         return 0;
 243 }
 244
 245 //static const char *arg_filename = "EX2700-V1.0.1.8.img";
 246 static unsigned arg_rx_timeout = 250;
 247 static unsigned arg_ul_timeout = 60000;
 248 static const char *arg_filename = "bad.img";
 249 static const char *arg_ipaddr = "192.168.2.2";
 250 static const char *arg_ipmask = "255.255.255.0";
 251 static const char *arg_intf = "enp4s0";
 252 static uint16_t arg_port = 69;
 253 #if 0
 254 static uint8_t target[ETH_ALEN] = { 0xa4, 0x2b, 0x8c, 0x10, 0xc2, 0x96 };
 255 #else
 256 static uint8_t target[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 257 #endif
 258
 259 static const char *spinner = "\\|/-";
 260
 261 int main(int argc, char **argv)
 262 {
 263         struct nmrp_pkt tx, rx;
 264         struct sockaddr_ll addr;
 265         uint8_t hwaddr[ETH_ALEN];
 266         int i, fd, err, ulreqs, expect;
 267
 268         err = 1;
 269
 270         fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_NMRP));
 271         if (fd == -1) {
 272                 perror("socket");
 273                 return 1;
 274         }
 275
 276         if (intf_get_index_and_addr(fd, arg_intf, &addr.sll_ifindex, hwaddr)) {
 277                 return 1;
 278         }
 279
 280         if (sock_bind_to_intf(fd, arg_intf)) {
 281                 return 1;
 282         }
 283
 284         if (sock_set_rx_timeout(fd, arg_rx_timeout)) {
 285                 return 1;
 286         }
 287
 288         addr.sll_family = PF_PACKET;
 289         //addr.sll_hatype = ARPHRD_ETHER;
 290         //addr.sll_pkttype = PACKET_OTHERHOST;
 291         addr.sll_protocol = htons(ETH_P_NMRP);
 292         addr.sll_halen = ETH_ALEN;
 293         memcpy(addr.sll_addr, target, ETH_ALEN);
 294
 295         memcpy(tx.eh.ether_shost, hwaddr, ETH_ALEN);
 296         memcpy(tx.eh.ether_dhost, target, ETH_ALEN);
 297         tx.eh.ether_type = htons(ETH_P_NMRP);
 298
 299         tx.msg.reserved = 0;
 300         tx.msg.code = NMRP_C_ADVERTISE;
 301         tx.msg.id = 0;
 302         tx.msg.num_opts = 1;
 303         tx.msg.opts[0].type = NMRP_O_MAGIC_NO;
 304         tx.msg.opts[0].len = NMRP_OPT_LEN + 4;
 305         tx.msg.opts[0].val.magic[0] = 'N';
 306         tx.msg.opts[0].val.magic[1] = 'T';
 307         tx.msg.opts[0].val.magic[2] = 'G';
 308         tx.msg.opts[0].val.magic[3] = 'R';
 309
 310         msg_update_len(&tx.msg);
 311         msg_hton(&tx.msg);
 312
 313         i = 0;
 314
 315         while (1) {
 316                 printf("\rAdvertising NMRP server on %s ... %c", arg_intf, spinner[i]);
 317                 fflush(stdout);
 318                 i = (i + 1) & 3;
 319
 320                 if (pkt_send(fd, &addr, &tx) < 0) {
 321                         perror("sendto");
 322                         goto out;
 323                 }
 324
 325                 err = pkt_recv(fd, &rx);
 326                 if (err == 0) {
 327                         break;
 328                 } else if (err == 1) {
 329                         printf("ERR\n");
 330                         goto out;
 331                 }
 332         }
 333
 334         printf("\n");
 335
 336         expect = NMRP_C_CONF_REQ;
 337         ulreqs = 0;
 338
 339         do {
 340                 if (expect != NMRP_C_NONE && rx.msg.code != expect) {
 341                         fprintf(stderr, "Received code 0x%02x while waiting for 0x%02x!\n",
 342                                         rx.msg.code, expect);
 343                 }
 344
 345                 tx.msg.code = NMRP_C_NONE;
 346                 tx.msg.reserved = 0;
 347                 tx.msg.id = 0;
 348                 tx.msg.num_opts = 0;
 349                 tx.msg.len = 0;
 350
 351                 err = 1;
 352
 353                 switch (rx.msg.code) {
 354                         case NMRP_C_CONF_REQ:
 355                                 tx.msg.code = NMRP_C_CONF_ACK;
 356                                 tx.msg.num_opts = 2;
 357
 358                                 tx.msg.opts[0].type = NMRP_O_DEV_IP;
 359                                 tx.msg.opts[0].len = NMRP_OPT_LEN + 2 * IP_LEN;
 360
 361                                 inet_aton(arg_ipaddr,
 362                                                 (struct in_addr*)tx.msg.opts[0].val.ip.addr);
 363                                 inet_aton(arg_ipmask,
 364                                                 (struct in_addr*)tx.msg.opts[0].val.ip.mask);
 365
 366                                 tx.msg.opts[1].type = NMRP_O_FW_UP;
 367                                 tx.msg.opts[1].len = NMRP_OPT_LEN;
 368
 369                                 expect = NMRP_C_TFTP_UL_REQ;
 370
 371                                 printf("Configuration request received from "
 372                                                 "%02x:%02x:%02x:%02x:%02x:%02x.\n",
 373                                                 rx.eh.ether_shost[0], rx.eh.ether_shost[1],
 374                                                 rx.eh.ether_shost[2], rx.eh.ether_shost[3],
 375                                                 rx.eh.ether_shost[4], rx.eh.ether_shost[5]);
 376                                 printf("Sending configuration: ip %s, mask %s.\n", arg_ipaddr,
 377                                                 arg_ipmask);
 378
 379                                 break;
 380                         case NMRP_C_TFTP_UL_REQ:
 381                                 if (++ulreqs > 5) {
 382                                         fprintf(stderr, "Device re-requested file upload %d "
 383                                                         "times; aborting.\n", ulreqs);
 384                                         tx.msg.code = NMRP_C_CLOSE_REQ;
 385                                         break;
 386                                 }
 387                                 printf("Uploading %s ... ", arg_filename);
 388                                 fflush(stdout);
 389                                 err = tftp_put(arg_filename, arg_ipaddr, arg_port);
 390                                 if (!err) {
 391                                         printf("OK\nWaiting for router to respond.\n");
 392                                         sock_set_rx_timeout(fd, arg_ul_timeout);
 393                                         expect = NMRP_C_CLOSE_REQ;
 394                                 } else if (err != -3) {
 395                                         goto out;
 396                                 }
 397                                 break;
 398                         case NMRP_C_KEEP_ALIVE_REQ:
 399                                 tx.msg.code = NMRP_C_KEEP_ALIVE_ACK;
 400                                 break;
 401                         case NMRP_C_CLOSE_REQ:
 402                                 tx.msg.code = NMRP_C_CLOSE_ACK;
 403                                 break;
 404                         case NMRP_C_CLOSE_ACK:
 405                                 err = 0;
 406                                 goto out;
 407                         default:
 408                                 fprintf(stderr, "Unhandled message code 0x%02x!\n",
 409                                                 rx.msg.code);
 410                 }
 411
 412                 if (tx.msg.code != NMRP_C_NONE) {
 413                         msg_update_len(&tx.msg);
 414                         msg_hton(&tx.msg);
 415
 416                         if (pkt_send(fd, &addr, &tx) < 0) {
 417                                 perror("sendto");
 418                                 goto out;
 419                         }
 420                 }
 421
 422                 if (rx.msg.code == NMRP_C_CLOSE_REQ) {
 423                         printf("Remote requested to close connection.\n");
 424                         break;
 425                 }
 426
 427                 err = pkt_recv(fd, &rx);
 428                 if (err) {
 429                         if (err == 2) {
 430                                 fprintf(stderr, "Timeout while waiting for 0x%02x.\n", expect);
 431                         }
 432                         goto out;
 433                 }
 434
 435                 sock_set_rx_timeout(fd, arg_rx_timeout);
 436
 437         } while (1);
 438
 439         err = 0;
 440
 441 out:
 442         close(fd);
 443
 444         return err;
 445 }