Use ethsock
This commit is contained in:
Joseph C. Lehner
parent
f99ee379bc
commit
d0da9d8b14
1 changed files with 32 additions and 100 deletions
132
nmrp.c
132
nmrp.c
|
|
@ -18,8 +18,8 @@
|
|||
*/
|
||||
|
||||
#define _BSD_SOURCE
|
||||
#include <netinet/ether.h>
|
||||
#include <linux/if_packet.h>
|
||||
#include <netinet/if_ether.h>
|
||||
//#include <linux/if_packet.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
|
@ -30,6 +30,7 @@
|
|||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
#include "ethsock.h"
|
||||
#include "nmrpd.h"
|
||||
|
||||
#define NMRP_HDR_LEN 6
|
||||
|
|
@ -71,8 +72,8 @@ struct nmrp_opt {
|
|||
union {
|
||||
uint8_t magic[4];
|
||||
struct {
|
||||
uint8_t addr[IP_LEN];
|
||||
uint8_t mask[IP_LEN];
|
||||
uint8_t addr[4];
|
||||
uint8_t mask[4];
|
||||
} ip;
|
||||
} val;
|
||||
} PACKED;
|
||||
|
|
@ -187,53 +188,22 @@ static int msg_ntoh(struct nmrp_msg *msg)
|
|||
return 1;
|
||||
}
|
||||
|
||||
static int intf_get_info(int sock, const char *name, int *index,
|
||||
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)
|
||||
static int pkt_send(struct ethsock *sock, struct nmrp_pkt *pkt)
|
||||
{
|
||||
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;
|
||||
|
||||
memset(pkt, 0, sizeof(*pkt));
|
||||
bytes = recvfrom(sock, pkt, NMRP_MIN_PKT_LEN, MSG_PEEK,
|
||||
(struct sockaddr*)&from, &addrlen);
|
||||
|
||||
bytes = ethsock_recv(sock, pkt, sizeof(*pkt));
|
||||
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) {
|
||||
return 2;
|
||||
} else if (bytes < NMRP_MIN_PKT_LEN) {
|
||||
fprintf(stderr, "Short packet (%zi bytes)\n", bytes);
|
||||
return 1;
|
||||
|
|
@ -242,34 +212,12 @@ static int pkt_recv(int sock, struct nmrp_pkt *pkt)
|
|||
msg_hdr_ntoh(&pkt->msg);
|
||||
len = pkt->msg.len + sizeof(pkt->eh);
|
||||
|
||||
bytes = recvfrom(sock, pkt, len, MSG_DONTWAIT, NULL, NULL);
|
||||
if (bytes < 0) {
|
||||
perror("recvfrom(msg)");
|
||||
return 1;
|
||||
} else if (bytes != len) {
|
||||
if (bytes != len) {
|
||||
fprintf(stderr, "Unexpected message length (%zi bytes).\n", len);
|
||||
return 1;
|
||||
} else {
|
||||
if (msg_ntoh(&pkt->msg) != 0) {
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
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;
|
||||
return msg_ntoh(&pkt->msg);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
struct nmrp_pkt tx, rx;
|
||||
struct sockaddr_ll addr;
|
||||
uint8_t src[ETH_ALEN], dest[ETH_ALEN];
|
||||
uint8_t src[6], dest[6];
|
||||
struct in_addr ipaddr, ipmask;
|
||||
time_t beg;
|
||||
int i, sock, err, ulreqs, expect;
|
||||
int i, err, ulreqs, expect;
|
||||
struct ethsock *sock;
|
||||
|
||||
if (args->op != NMRP_UPLOAD_FW) {
|
||||
fprintf(stderr, "Operation not implemented.\n");
|
||||
|
|
@ -334,31 +282,17 @@ int nmrp_do(struct nmrpd_args *args)
|
|||
|
||||
err = 1;
|
||||
|
||||
sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_NMRP));
|
||||
if (sock == -1) {
|
||||
perror("socket");
|
||||
sock = ethsock_create(args->intf, ETH_P_NMRP);
|
||||
if (!sock) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (intf_get_info(sock, args->intf, &addr.sll_ifindex, src)) {
|
||||
if (ethsock_set_timeout(sock, args->rx_timeout)) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (sock_bind_to_intf(sock, args->intf)) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
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);
|
||||
memcpy(tx.eh.ether_shost, src, 6);
|
||||
memcpy(tx.eh.ether_dhost, dest, 6);
|
||||
tx.eh.ether_type = htons(ETH_P_NMRP);
|
||||
|
||||
tx.msg.reserved = 0;
|
||||
|
|
@ -379,18 +313,18 @@ int nmrp_do(struct nmrpd_args *args)
|
|||
beg = time(NULL);
|
||||
|
||||
while (1) {
|
||||
printf("\rAdvertising NMRP server on %s ... %c", args->intf,
|
||||
printf("\rAdvertising NMRP server on %s ... %c", args->intf,
|
||||
spinner[i]);
|
||||
fflush(stdout);
|
||||
i = (i + 1) & 3;
|
||||
|
||||
if (pkt_send(sock, &addr, &tx) < 0) {
|
||||
if (pkt_send(sock, &tx) < 0) {
|
||||
perror("sendto");
|
||||
goto out;
|
||||
}
|
||||
|
||||
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;
|
||||
} else if (err == 1) {
|
||||
printf("ERR\n");
|
||||
|
|
@ -436,10 +370,10 @@ int nmrp_do(struct nmrpd_args *args)
|
|||
tx.msg.num_opts = 2;
|
||||
|
||||
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.mask, &ipmask, IP_LEN);
|
||||
memcpy(tx.msg.opts[0].val.ip.addr, &ipaddr, 4);
|
||||
memcpy(tx.msg.opts[0].val.ip.mask, &ipmask, 4);
|
||||
|
||||
tx.msg.opts[1].type = NMRP_O_FW_UP;
|
||||
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[4], rx.eh.ether_shost[5]);
|
||||
|
||||
memcpy(tx.eh.ether_dhost, rx.eh.ether_shost, ETH_ALEN);
|
||||
memcpy(addr.sll_addr, rx.eh.ether_shost, ETH_ALEN);
|
||||
memcpy(tx.eh.ether_dhost, rx.eh.ether_shost, 6);
|
||||
|
||||
printf("Sending configuration: ip %s, mask %s.\n",
|
||||
args->ipaddr, args->ipmask);
|
||||
|
|
@ -479,7 +412,7 @@ int nmrp_do(struct nmrpd_args *args)
|
|||
|
||||
if (!err) {
|
||||
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;
|
||||
} else {
|
||||
printf("\n");
|
||||
|
|
@ -506,7 +439,7 @@ int nmrp_do(struct nmrpd_args *args)
|
|||
msg_update_len(&tx.msg);
|
||||
msg_hton(&tx.msg);
|
||||
|
||||
if (pkt_send(sock, &addr, &tx) < 0) {
|
||||
if (pkt_send(sock, &tx) < 0) {
|
||||
perror("sendto");
|
||||
goto out;
|
||||
}
|
||||
|
|
@ -525,14 +458,13 @@ int nmrp_do(struct nmrpd_args *args)
|
|||
goto out;
|
||||
}
|
||||
|
||||
sock_set_rx_timeout(sock, args->rx_timeout);
|
||||
ethsock_set_timeout(sock, args->rx_timeout);
|
||||
|
||||
} while (1);
|
||||
|
||||
err = 0;
|
||||
|
||||
out:
|
||||
close(sock);
|
||||
|
||||
ethsock_close(sock);
|
||||
return err;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue