From 875aad86c872a0d4adebfbcb2a97339519de46d0 Mon Sep 17 00:00:00 2001 From: "Joseph C. Lehner" Date: Sun, 24 Jan 2016 21:56:22 +0100 Subject: [PATCH 1/1] Separate main from NMRP code --- main.c | 44 ++++++++++ nmrp.c | 250 ++++++++++++++++++++++++++++++++++----------------------- tftp.c | 14 ++-- 3 files changed, 202 insertions(+), 106 deletions(-) create mode 100644 main.c diff --git a/main.c b/main.c new file mode 100644 index 0000000..7ee086c --- /dev/null +++ b/main.c @@ -0,0 +1,44 @@ +#include +#include "nmrpd.h" + +void usage() +{ + printf( + "Usage: nmrpd [options] [command] [command args]\n" + "\n" + "Available options:\n" + " -m [mac] MAC address of target device (xx:xx:xx:xx:xx:xx)\n" + " -a [ipaddr] IP address to assign to target device\n" + " -M [netmask] Subnet mask to assign to target device\n" + " -t [timeout] Timeout (in milliseconds) for regular messages\n" + " -T [timeout] Time to wait after successfull TFTP upload\n" + " -p [port] Port to use for TFTP upload\n" + " -i [interface] Network interface directly connected to device\n" + "\n" + "Available commands:\n" + " set-region Set region of device\n" + " upload-firmware Upload new firmware\n" + " upload-strings Upload string table\n" + "\n" + ); +} + + + +int main(int argc, char **argv) +{ + struct nmrpd_args args = { + .rx_timeout = 200, + .ul_timeout = 60000, + .filename = argc >= 2 ? argv[1] : NULL, + .ipaddr = "192.168.2.2", + .ipmask = "255.255.255.0", + .intf = "enp4s0", + .mac = "ff:ff:ff:ff:ff:ff", + .op = NMRP_UPLOAD_FW, + .port = 69, + .force_root = 0 + }; + + return nmrp_do(&args); +} diff --git a/nmrp.c b/nmrp.c index 380277d..eb2305e 100644 --- a/nmrp.c +++ b/nmrp.c @@ -5,15 +5,15 @@ #include #include #include -#include +#include #include #include #include #include +#include "nmrpd.h" #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 @@ -21,13 +21,6 @@ #define PACKED __attribute__((__packed__)) #define MAX_LOOP_RECV 1024 -#define IS_OOO_CODE(x) (x == NMRP_C_CLOSE_REQ \ - || x == NMRP_C_KEEP_ALIVE_REQ \ - || x == NMRP_C_TFTP_UL_REQ) - -extern int tftp_put(const char *filename, const char *ipaddr, uint16_t port); -extern int sock_set_rx_timeout(int fd, unsigned msec); - enum nmrp_code { NMRP_C_NONE = 0, NMRP_C_ADVERTISE = 1, @@ -66,7 +59,7 @@ struct nmrp_msg { uint8_t code; uint8_t id; uint16_t len; - struct nmrp_opt opts[6]; + struct nmrp_opt opts[2]; uint32_t num_opts; } PACKED; @@ -84,6 +77,37 @@ static void msg_update_len(struct nmrp_msg *msg) } } +static void msg_dump(struct nmrp_msg *msg, int dump_opts) +{ + struct nmrp_opt *opt; + int remain_len, len, i; + + fprintf(stderr, "res=0x%04x, code=0x%02x, id=0x%02x, len=%u", + msg->reserved, msg->code, msg->id, msg->len); + + remain_len = msg->len - NMRP_HDR_LEN; + fprintf(stderr, "%s\n", remain_len ? "" : " (no opts)"); + + if (dump_opts) { + opt = msg->opts; + + while (remain_len > 0) { + len = opt->len; + fprintf(stderr, " opt type=%u, len=%u", opt->type, len); + for (i = 0; i != len - NMRP_OPT_LEN; ++i) { + if (!(i % 16)) { + fprintf(stderr, "\n "); + } + + fprintf(stderr, "%02x ", ((char*)&opt->val)[i] & 0xff); + } + fprintf(stderr, "\n"); + remain_len -= len; + opt = (struct nmrp_opt*)(((char*)opt) + len); + } + } +} + static void msg_hton(struct nmrp_msg *msg) { uint32_t i = 0; @@ -113,7 +137,8 @@ static int msg_ntoh(struct nmrp_msg *msg) while (remaining > 0) { if (remaining < NMRP_OPT_LEN) { - fprintf(stderr, "malformed message (rem=%d)\n", remaining); + fprintf(stderr, "Malformed message.\n"); + msg_dump(msg, 0); return 1; } @@ -121,41 +146,19 @@ static int msg_ntoh(struct nmrp_msg *msg) opt->len = ntohs(opt->len); remaining -= opt->len; + ++opt; } - return 0; -} - -static void msg_dump(struct nmrp_msg *msg) -{ - struct nmrp_opt *opt; - int remain_len, len, i; - - printf("res=0x%04x, code=0x%02x, 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); + if (remaining) { + fprintf(stderr, "Trailing data in message.\n"); + msg_dump(msg, 0); + return 1; } + + return 0; } -static int intf_get_index_and_addr(int fd, const char *name, int *index, +static int intf_get_info(int sock, const char *name, int *index, uint8_t *hwaddr) { struct ifreq ifr; @@ -163,13 +166,13 @@ static int intf_get_index_and_addr(int fd, const char *name, int *index, memset(&ifr, 0, sizeof(ifr)); strncpy(ifr.ifr_name, name, IFNAMSIZ - 1); - if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { + if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) { perror("ioctl(SIOCGIFINDEX)"); return -1; } *index = ifr.ifr_ifindex; - if (ioctl(fd, SIOCGIFHWADDR, &ifr) < 0) { + if (ioctl(sock, SIOCGIFHWADDR, &ifr) < 0) { perror("ioctl(SIOCGIFHWADDR)"); return -1; } @@ -178,20 +181,20 @@ static int intf_get_index_and_addr(int fd, const char *name, int *index, return 0; } -static int pkt_send(int fd, struct sockaddr_ll *addr, struct nmrp_pkt *pkt) +static int pkt_send(int sock, struct sockaddr_ll *addr, struct nmrp_pkt *pkt) { size_t len = ntohs(pkt->msg.len) + sizeof(pkt->eh); - return sendto(fd, pkt, len, 0, (struct sockaddr*)addr, sizeof(*addr)); + return sendto(sock, pkt, len, 0, (struct sockaddr*)addr, sizeof(*addr)); } -static int pkt_recv(int fd, struct nmrp_pkt *pkt) +static int pkt_recv(int sock, struct nmrp_pkt *pkt) { struct sockaddr_ll from; socklen_t addrlen; ssize_t bytes, len; memset(pkt, 0, sizeof(*pkt)); - bytes = recvfrom(fd, pkt, NMRP_MIN_PKT_LEN, MSG_PEEK, + bytes = recvfrom(sock, pkt, NMRP_MIN_PKT_LEN, MSG_PEEK, (struct sockaddr*)&from, &addrlen); if (bytes < 0) { @@ -203,38 +206,36 @@ static int pkt_recv(int fd, struct nmrp_pkt *pkt) } 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); + fprintf(stderr, "Short packet (%zi bytes)\n", bytes); return 1; } msg_hdr_ntoh(&pkt->msg); len = pkt->msg.len + sizeof(pkt->eh); - bytes = recvfrom(fd, pkt, len, MSG_DONTWAIT, NULL, NULL); + bytes = recvfrom(sock, 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); + fprintf(stderr, "Unexpected message length (%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_bind_to_intf(int fd, const char *name) +static int sock_bind_to_intf(int sock, const char *name) { struct ifreq ifr; strncpy(ifr.ifr_name, name, IFNAMSIZ - 1); - if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr)) < 0) { + if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr)) < 0) { perror("setsockopt(SO_BINDTODEVICE)"); return 1; } @@ -242,58 +243,93 @@ static int sock_bind_to_intf(int fd, const char *name) return 0; } -//static const char *arg_filename = "EX2700-V1.0.1.8.img"; -static unsigned arg_rx_timeout = 250; -static unsigned arg_ul_timeout = 60000; -static const char *arg_filename = "bad.img"; -static const char *arg_ipaddr = "192.168.2.2"; -static const char *arg_ipmask = "255.255.255.0"; -static const char *arg_intf = "enp4s0"; -static uint16_t arg_port = 69; -#if 0 -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 int mac_parse(const char *str, uint8_t *hwaddr) +{ + unsigned i, data[6]; + + i = sscanf(str, "%02x:%02x:%02x:%02x:%02x:%02x", + data, data + 1, data + 2, data + 3, data + 4, data + 5); + + if (i == 6) { + for (i = 0; i != 6; ++i) { + if (data[i] > 255) { + break; + } + + hwaddr[i] = data[i] & 0xff; + } + + if (i == 6) { + return 1; + } + } + + fprintf(stderr, "Invalid MAC address.\n"); + return 0; +} static const char *spinner = "\\|/-"; -int main(int argc, char **argv) +int nmrp_do(struct nmrpd_args *args) { struct nmrp_pkt tx, rx; struct sockaddr_ll addr; - uint8_t hwaddr[ETH_ALEN]; - int i, fd, err, ulreqs, expect; + uint8_t src[ETH_ALEN], dest[ETH_ALEN]; + struct in_addr ipaddr, ipmask; + int i, sock, err, ulreqs, expect; + + if (args->op != NMRP_UPLOAD_FW) { + fprintf(stderr, "Operation not implemented.\n"); + return 1; + } + + if (!mac_parse(args->mac, dest)) { + fprintf(stderr, "Invalid MAC address %s.\n", args->mac); + return 1; + } + + if (!inet_aton(args->ipaddr, &ipaddr)) { + fprintf(stderr, "Invalid IP address %s.\n", args->ipaddr); + return 1; + } + + if (!inet_aton(args->ipmask, &ipmask)) { + fprintf(stderr, "Invalid subnet mask %s.\n", args->ipmask); + return 1; + } + + if (access(args->filename, R_OK) == -1) { + fprintf(stderr, "Error accessing file %s.\n", args->filename); + return 1; + } err = 1; - fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_NMRP)); - if (fd == -1) { + sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_NMRP)); + if (sock == -1) { perror("socket"); return 1; } - if (intf_get_index_and_addr(fd, arg_intf, &addr.sll_ifindex, hwaddr)) { + if (intf_get_info(sock, args->intf, &addr.sll_ifindex, src)) { return 1; } - if (sock_bind_to_intf(fd, arg_intf)) { + if (sock_bind_to_intf(sock, args->intf)) { return 1; } - if (sock_set_rx_timeout(fd, arg_rx_timeout)) { + if (sock_set_rx_timeout(sock, args->rx_timeout)) { return 1; } - addr.sll_family = PF_PACKET; - //addr.sll_hatype = ARPHRD_ETHER; - //addr.sll_pkttype = PACKET_OTHERHOST; + addr.sll_family = AF_PACKET; addr.sll_protocol = htons(ETH_P_NMRP); addr.sll_halen = ETH_ALEN; - memcpy(addr.sll_addr, target, ETH_ALEN); + memcpy(addr.sll_addr, dest, ETH_ALEN); - memcpy(tx.eh.ether_shost, hwaddr, ETH_ALEN); - memcpy(tx.eh.ether_dhost, target, ETH_ALEN); + memcpy(tx.eh.ether_shost, src, ETH_ALEN); + memcpy(tx.eh.ether_dhost, dest, ETH_ALEN); tx.eh.ether_type = htons(ETH_P_NMRP); tx.msg.reserved = 0; @@ -313,16 +349,17 @@ int main(int argc, char **argv) i = 0; while (1) { - printf("\rAdvertising NMRP server on %s ... %c", arg_intf, spinner[i]); + printf("\rAdvertising NMRP server on %s ... %c", args->intf, + spinner[i]); fflush(stdout); i = (i + 1) & 3; - if (pkt_send(fd, &addr, &tx) < 0) { + if (pkt_send(sock, &addr, &tx) < 0) { perror("sendto"); goto out; } - err = pkt_recv(fd, &rx); + err = pkt_recv(sock, &rx); if (err == 0) { break; } else if (err == 1) { @@ -358,10 +395,8 @@ int main(int argc, char **argv) tx.msg.opts[0].type = NMRP_O_DEV_IP; tx.msg.opts[0].len = NMRP_OPT_LEN + 2 * IP_LEN; - inet_aton(arg_ipaddr, - (struct in_addr*)tx.msg.opts[0].val.ip.addr); - inet_aton(arg_ipmask, - (struct in_addr*)tx.msg.opts[0].val.ip.mask); + memcpy(tx.msg.opts[0].val.ip.addr, &ipaddr, IP_LEN); + memcpy(tx.msg.opts[0].val.ip.mask, &ipmask, IP_LEN); tx.msg.opts[1].type = NMRP_O_FW_UP; tx.msg.opts[1].len = NMRP_OPT_LEN; @@ -373,8 +408,12 @@ int main(int argc, char **argv) rx.eh.ether_shost[0], rx.eh.ether_shost[1], rx.eh.ether_shost[2], rx.eh.ether_shost[3], rx.eh.ether_shost[4], rx.eh.ether_shost[5]); - printf("Sending configuration: ip %s, mask %s.\n", arg_ipaddr, - arg_ipmask); + + memcpy(tx.eh.ether_dhost, rx.eh.ether_shost, ETH_ALEN); + memcpy(addr.sll_addr, rx.eh.ether_shost, ETH_ALEN); + + printf("Sending configuration: ip %s, mask %s.\n", + args->ipaddr, args->ipmask); break; case NMRP_C_TFTP_UL_REQ: @@ -384,16 +423,26 @@ int main(int argc, char **argv) tx.msg.code = NMRP_C_CLOSE_REQ; break; } - printf("Uploading %s ... ", arg_filename); - fflush(stdout); - err = tftp_put(arg_filename, arg_ipaddr, arg_port); + + if (!args->tftpcmd) { + printf("Uploading %s ... ", args->filename); + fflush(stdout); + err = tftp_put(args); + } else { + printf("Running %s ... ", args->tftpcmd); + fflush(stdout); + err = system(args->tftpcmd); + } + if (!err) { printf("OK\nWaiting for router to respond.\n"); - sock_set_rx_timeout(fd, arg_ul_timeout); + sock_set_rx_timeout(sock, args->ul_timeout); expect = NMRP_C_CLOSE_REQ; - } else if (err != -3) { + } else { + printf("\n"); goto out; } + break; case NMRP_C_KEEP_ALIVE_REQ: tx.msg.code = NMRP_C_KEEP_ALIVE_ACK; @@ -405,15 +454,16 @@ int main(int argc, char **argv) err = 0; goto out; default: - fprintf(stderr, "Unhandled message code 0x%02x!\n", + fprintf(stderr, "Unknown message code 0x%02x!\n", rx.msg.code); + msg_dump(&rx.msg, 0); } if (tx.msg.code != NMRP_C_NONE) { msg_update_len(&tx.msg); msg_hton(&tx.msg); - if (pkt_send(fd, &addr, &tx) < 0) { + if (pkt_send(sock, &addr, &tx) < 0) { perror("sendto"); goto out; } @@ -424,7 +474,7 @@ int main(int argc, char **argv) break; } - err = pkt_recv(fd, &rx); + err = pkt_recv(sock, &rx); if (err) { if (err == 2) { fprintf(stderr, "Timeout while waiting for 0x%02x.\n", expect); @@ -432,14 +482,14 @@ int main(int argc, char **argv) goto out; } - sock_set_rx_timeout(fd, arg_rx_timeout); + sock_set_rx_timeout(sock, args->rx_timeout); } while (1); err = 0; out: - close(fd); + close(sock); return err; } diff --git a/tftp.c b/tftp.c index 4342bc8..551c139 100644 --- a/tftp.c +++ b/tftp.c @@ -7,6 +7,7 @@ #include #include #include +#include "nmrpd.h" #define TFTP_PKT_SIZE 516 @@ -142,7 +143,7 @@ int sock_set_rx_timeout(int fd, unsigned msec) return 0; } -int tftp_put(const char *filename, const char *ipaddr, uint16_t port) +int tftp_put(struct nmrpd_args *args) { struct sockaddr_in addr; uint16_t block; @@ -150,7 +151,7 @@ int tftp_put(const char *filename, const char *ipaddr, uint16_t port) int fd, sock, err, timeout, last_len; char rx[TFTP_PKT_SIZE], tx[TFTP_PKT_SIZE]; - fd = open(filename, O_RDONLY); + fd = open(args->filename, O_RDONLY); if (fd < 0) { perror("open"); err = fd; @@ -164,21 +165,21 @@ int tftp_put(const char *filename, const char *ipaddr, uint16_t port) goto cleanup; } - err = sock_set_rx_timeout(sock, 999); + err = sock_set_rx_timeout(sock, args->rx_timeout); if (err) { goto cleanup; } - err = !inet_aton(ipaddr, &addr.sin_addr); + err = !inet_aton(args->ipaddr, &addr.sin_addr); if (err) { perror("inet_aton"); goto cleanup; } addr.sin_family = AF_INET; - addr.sin_port = htons(port); + addr.sin_port = htons(args->port); - pkt_mkwrq(tx, filename, "octet"); + pkt_mkwrq(tx, args->filename, "octet"); len = tftp_sendto(sock, tx, 0, &addr); if (len < 0) { @@ -237,6 +238,7 @@ int tftp_put(const char *filename, const char *ipaddr, uint16_t port) goto cleanup; } else { timeout = 0; + err = 0; } } while(1); -- 2.25.1