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

Use ethsock

Browse source
This commit is contained in:
Joseph C. Lehner 2016-01-29 19:21:33 +02:00
parent f99ee379bc
commit d0da9d8b14
1 changed files with 32 additions and 100 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

130
nmrp.c
View file

@ -18,8 +18,8 @@
*/ */
#define _BSD_SOURCE #define _BSD_SOURCE
#include <netinet/ether.h> #include <netinet/if_ether.h>
#include <linux/if_packet.h> //#include <linux/if_packet.h>
#include <arpa/inet.h> #include <arpa/inet.h>
#include <sys/socket.h> #include <sys/socket.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
@ -30,6 +30,7 @@
#include <errno.h> #include <errno.h>
#include <stdio.h> #include <stdio.h>
#include <time.h> #include <time.h>
#include "ethsock.h"
#include "nmrpd.h" #include "nmrpd.h"
#define NMRP_HDR_LEN 6 #define NMRP_HDR_LEN 6
@ -71,8 +72,8 @@ struct nmrp_opt {
union { union {
uint8_t magic[4]; uint8_t magic[4];
struct { struct {
uint8_t addr[IP_LEN]; uint8_t addr[4];
uint8_t mask[IP_LEN]; uint8_t mask[4];
} ip; } ip;
} val; } val;
} PACKED; } PACKED;
@ -187,53 +188,22 @@ static int msg_ntoh(struct nmrp_msg *msg)
return 1; return 1;
} }
static int intf_get_info(int sock, const char *name, int *index, static int pkt_send(struct ethsock *sock, struct nmrp_pkt *pkt)
uint8_t *hwaddr)
{
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
strncpy(ifr.ifr_name, name, IFNAMSIZ - 1);
if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) {
perror("ioctl(SIOCGIFINDEX)");
return -1;
}
*index = ifr.ifr_ifindex;
if (ioctl(sock, SIOCGIFHWADDR, &ifr) < 0) {
perror("ioctl(SIOCGIFHWADDR)");
return -1;
}
memcpy(hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
return 0;
}
static int pkt_send(int sock, struct sockaddr_ll *addr, struct nmrp_pkt *pkt)
{ {
size_t len = ntohs(pkt->msg.len) + sizeof(pkt->eh); size_t len = ntohs(pkt->msg.len) + sizeof(pkt->eh);
return sendto(sock, pkt, len, 0, (struct sockaddr*)addr, sizeof(*addr)); return ethsock_send(sock, pkt, len);
} }
static int pkt_recv(int sock, struct nmrp_pkt *pkt) static int pkt_recv(struct ethsock *sock, struct nmrp_pkt *pkt)
{ {
struct sockaddr_ll from;
socklen_t addrlen;
ssize_t bytes, len; ssize_t bytes, len;
memset(pkt, 0, sizeof(*pkt)); memset(pkt, 0, sizeof(*pkt));
bytes = recvfrom(sock, pkt, NMRP_MIN_PKT_LEN, MSG_PEEK, bytes = ethsock_recv(sock, pkt, sizeof(*pkt));
(struct sockaddr*)&from, &addrlen);
if (bytes < 0) { if (bytes < 0) {
if (errno == EAGAIN) {
return 2;
}
perror("recvfrom(pkt)");
return 1; return 1;
} else if (ntohs(pkt->eh.ether_type) != ETH_P_NMRP) { } else if (!bytes) {
return 3; return 2;
} else if (bytes < NMRP_MIN_PKT_LEN) { } 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; return 1;
@ -242,34 +212,12 @@ static int pkt_recv(int sock, struct nmrp_pkt *pkt)
msg_hdr_ntoh(&pkt->msg); msg_hdr_ntoh(&pkt->msg);
len = pkt->msg.len + sizeof(pkt->eh); len = pkt->msg.len + sizeof(pkt->eh);
bytes = recvfrom(sock, pkt, len, MSG_DONTWAIT, NULL, NULL); if (bytes != len) {
if (bytes < 0) {
perror("recvfrom(msg)");
return 1;
} else if (bytes != len) {
fprintf(stderr, "Unexpected message length (%zi bytes).\n", len); fprintf(stderr, "Unexpected message length (%zi bytes).\n", len);
return 1; return 1;
} else {
if (msg_ntoh(&pkt->msg) != 0) {
return 1;
}
return 0;
} }
return 1; return msg_ntoh(&pkt->msg);
}
static int sock_bind_to_intf(int sock, const char *name)
{
struct ifreq ifr;
strncpy(ifr.ifr_name, name, IFNAMSIZ - 1);
if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr)) < 0) {
perror("setsockopt(SO_BINDTODEVICE)");
return 1;
}
return 0;
} }
static int mac_parse(const char *str, uint8_t *hwaddr) static int mac_parse(const char *str, uint8_t *hwaddr)
@ -301,11 +249,11 @@ static const char *spinner = "\\|/-";
int nmrp_do(struct nmrpd_args *args) int nmrp_do(struct nmrpd_args *args)
{ {
struct nmrp_pkt tx, rx; struct nmrp_pkt tx, rx;
struct sockaddr_ll addr; uint8_t src[6], dest[6];
uint8_t src[ETH_ALEN], dest[ETH_ALEN];
struct in_addr ipaddr, ipmask; struct in_addr ipaddr, ipmask;
time_t beg; time_t beg;
int i, sock, err, ulreqs, expect; int i, err, ulreqs, expect;
struct ethsock *sock;
if (args->op != NMRP_UPLOAD_FW) { if (args->op != NMRP_UPLOAD_FW) {
fprintf(stderr, "Operation not implemented.\n"); fprintf(stderr, "Operation not implemented.\n");
@ -334,31 +282,17 @@ int nmrp_do(struct nmrpd_args *args)
err = 1; err = 1;
sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_NMRP)); sock = ethsock_create(args->intf, ETH_P_NMRP);
if (sock == -1) { if (!sock) {
perror("socket");
return 1; return 1;
} }
if (intf_get_info(sock, args->intf, &addr.sll_ifindex, src)) { if (ethsock_set_timeout(sock, args->rx_timeout)) {
return 1; return 1;
} }
if (sock_bind_to_intf(sock, args->intf)) { memcpy(tx.eh.ether_shost, src, 6);
return 1; memcpy(tx.eh.ether_dhost, dest, 6);
}
if (sock_set_rx_timeout(sock, args->rx_timeout)) {
return 1;
}
addr.sll_family = AF_PACKET;
addr.sll_protocol = htons(ETH_P_NMRP);
addr.sll_halen = ETH_ALEN;
memcpy(addr.sll_addr, dest, 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.eh.ether_type = htons(ETH_P_NMRP);
tx.msg.reserved = 0; tx.msg.reserved = 0;
@ -384,13 +318,13 @@ int nmrp_do(struct nmrpd_args *args)
fflush(stdout); fflush(stdout);
i = (i + 1) & 3; i = (i + 1) & 3;
if (pkt_send(sock, &addr, &tx) < 0) { if (pkt_send(sock, &tx) < 0) {
perror("sendto"); perror("sendto");
goto out; goto out;
} }
err = pkt_recv(sock, &rx); err = pkt_recv(sock, &rx);
if (err == 0 && memcmp(rx.eh.ether_dhost, src, ETH_ALEN) == 0) { if (err == 0 && memcmp(rx.eh.ether_dhost, src, 6) == 0) {
break; break;
} else if (err == 1) { } else if (err == 1) {
printf("ERR\n"); printf("ERR\n");
@ -436,10 +370,10 @@ int nmrp_do(struct nmrpd_args *args)
tx.msg.num_opts = 2; tx.msg.num_opts = 2;
tx.msg.opts[0].type = NMRP_O_DEV_IP; tx.msg.opts[0].type = NMRP_O_DEV_IP;
tx.msg.opts[0].len = NMRP_OPT_LEN + 2 * IP_LEN; tx.msg.opts[0].len = NMRP_OPT_LEN + 2 * 4;
memcpy(tx.msg.opts[0].val.ip.addr, &ipaddr, IP_LEN); memcpy(tx.msg.opts[0].val.ip.addr, &ipaddr, 4);
memcpy(tx.msg.opts[0].val.ip.mask, &ipmask, IP_LEN); memcpy(tx.msg.opts[0].val.ip.mask, &ipmask, 4);
tx.msg.opts[1].type = NMRP_O_FW_UP; tx.msg.opts[1].type = NMRP_O_FW_UP;
tx.msg.opts[1].len = NMRP_OPT_LEN; tx.msg.opts[1].len = NMRP_OPT_LEN;
@ -452,8 +386,7 @@ int nmrp_do(struct nmrpd_args *args)
rx.eh.ether_shost[2], rx.eh.ether_shost[3], rx.eh.ether_shost[2], rx.eh.ether_shost[3],
rx.eh.ether_shost[4], rx.eh.ether_shost[5]); rx.eh.ether_shost[4], rx.eh.ether_shost[5]);
memcpy(tx.eh.ether_dhost, rx.eh.ether_shost, ETH_ALEN); memcpy(tx.eh.ether_dhost, rx.eh.ether_shost, 6);
memcpy(addr.sll_addr, rx.eh.ether_shost, ETH_ALEN);
printf("Sending configuration: ip %s, mask %s.\n", printf("Sending configuration: ip %s, mask %s.\n",
args->ipaddr, args->ipmask); args->ipaddr, args->ipmask);
@ -479,7 +412,7 @@ int nmrp_do(struct nmrpd_args *args)
if (!err) { if (!err) {
printf("OK\nWaiting for remote to respond.\n"); printf("OK\nWaiting for remote to respond.\n");
sock_set_rx_timeout(sock, args->ul_timeout); ethsock_set_timeout(sock, args->ul_timeout);
expect = NMRP_C_CLOSE_REQ; expect = NMRP_C_CLOSE_REQ;
} else { } else {
printf("\n"); printf("\n");
@ -506,7 +439,7 @@ int nmrp_do(struct nmrpd_args *args)
msg_update_len(&tx.msg); msg_update_len(&tx.msg);
msg_hton(&tx.msg); msg_hton(&tx.msg);
if (pkt_send(sock, &addr, &tx) < 0) { if (pkt_send(sock, &tx) < 0) {
perror("sendto"); perror("sendto");
goto out; goto out;
} }
@ -525,14 +458,13 @@ int nmrp_do(struct nmrpd_args *args)
goto out; goto out;
} }
sock_set_rx_timeout(sock, args->rx_timeout); ethsock_set_timeout(sock, args->rx_timeout);
} while (1); } while (1);
err = 0; err = 0;
out: out:
close(sock); ethsock_close(sock);
return err; return err;
} }