static void nl_parse_link_msg(struct nlmsghdr *nlp, struct nl_cb *cb)
{
nl_link_cb link_cb;
struct ifinfomsg *ifinfo;
struct rtattr *rtap;
struct nl_link link;
size_t len;
memset(&link, 0, sizeof(struct nl_link));
ifinfo = NLMSG_DATA(nlp);
rtap = IFLA_RTA(ifinfo);
len = IFLA_PAYLOAD(nlp);
for (; RTA_OK(rtap, len); rtap = RTA_NEXT(rtap, len)) {
switch (rtap->rta_type) {
case IFLA_IFNAME:
link.ifname = (char *)RTA_DATA(rtap);
break;
case IFLA_ADDRESS:
if (RTA_PAYLOAD(rtap) != sizeof(struct ether_addr)) {
XLOG_ERR("invalid ll address for %u", ifinfo->ifi_index);
return;
}
link.ifaddr = (struct ether_addr *)RTA_DATA(rtap);
break;
default:
/* XLOG_DEBUG("attr: %u", rtap->rta_type); */
break;
}
}
if (!link.ifname) {
XLOG_ERR("could not get name for link %u", ifinfo->ifi_index);
return;
}
if (!link.ifaddr) {
XLOG_ERR("could not get ll addr for link %u", ifinfo->ifi_index);
return;
}
link.ifindex = ifinfo->ifi_index;
link.iftype = ifinfo->ifi_type;
link.ifflags = ifinfo->ifi_flags;
link_cb = (nl_link_cb) cb->parse_cb;
link_cb(&link, cb->aux);
}
int daemonize()
{
pid_t pid = fork();
if (pid < 0) {
exit(EXIT_FAILURE);
}
if (pid > 0) {
exit(EXIT_SUCCESS);
}
if (write_pidfile() != 0) {
return -1;
}
int ret = setsid();
if (ret == -1) {
XLOG_ERR("failed to set session id: %s", strerror(errno));
unlink(PIDFILE);
return -1;
}
for (int fd = getdtablesize(); fd >= 0; --fd) {
close(fd);
}
chdir("/");
int fd = open("/dev/null", O_RDWR);
dup(fd);
dup(fd);
return 0;
}
bool setup_link(struct link *link, void *aux)
{
int fd = open_socket();
if (fd < 0) {
goto err;
}
if (do_bind(fd, link->ifindex) != 0) {
goto err;
}
if (set_promisc(fd, link->ifindex) != 0) {
goto err;
}
struct dispatcher *dispatcher = (struct dispatcher *) aux;
link->handler = dispatcher_watch(
dispatcher, fd, rarp_handler, link);
dispatcher_flags(&link->handler, POLLIN);
return true;
err:
close(fd);
XLOG_ERR("error setting up %s", link->name);
return false;
}
static bool nl_parse(char *buf, ssize_t len, struct nl_cb *cb)
{
struct nlmsghdr *nlp;
struct nlmsgerr *err;
nlp = (struct nlmsghdr *) buf;
for(;NLMSG_OK(nlp, len);nlp=NLMSG_NEXT(nlp, len)) {
switch (nlp->nlmsg_type) {
case NLMSG_ERROR:
err = (struct nlmsgerr *) NLMSG_DATA(nlp);
XLOG_ERR("received netlink error %s", strerror(-err->error));
case NLMSG_DONE:
return false;
default:
if (nlp->nlmsg_type == cb->msg_type) {
cb->parse_msg(nlp, cb);
break;
}
XLOG_DEBUG("unhandled netlink message of type %u", nlp->nlmsg_type);
break;
}
if (!(nlp->nlmsg_flags & NLM_F_MULTI)) {
return false;
}
}
return true;
}
static void nl_parse_addr_msg(struct nlmsghdr *nlp, struct nl_cb *cb)
{
struct ifaddrmsg *ifaddr;
struct rtattr *rtap;
nl_addr_cb addr_cb;
size_t len;
in_addr_t *addr;
struct nl_addr nl_addr;
addr = NULL;
ifaddr = NLMSG_DATA(nlp);
rtap = IFLA_RTA(ifaddr);
len = IFLA_PAYLOAD(nlp);
for (; RTA_OK(rtap, len); rtap = RTA_NEXT(rtap, len)) {
switch (rtap->rta_type) {
case IFA_LOCAL:
addr = (in_addr_t*)RTA_DATA(rtap);
break;
default:
break;
}
}
if (!addr) {
XLOG_ERR("could not get addr for link %u", ifaddr->ifa_index);
return;
}
addr_cb = (nl_addr_cb) cb->parse_cb;
nl_addr.ifindex = ifaddr->ifa_index;
nl_addr.ifaddr = *addr;
addr_cb(&nl_addr , cb->aux);
}
int open_socket()
{
int fd = socket(AF_PACKET, SOCK_DGRAM, htons(ETH_P_RARP));
if (fd < 0) {
XLOG_ERR("error opening socket %s", strerror(errno));
return -1;
}
if (fcntl(fd, F_SETFL, O_NONBLOCK) != 0) {
XLOG_ERR("error setting socket nonblocking %s",
strerror(errno));
close(fd);
return -1;
}
return fd;
}
int write_pidfile()
{
int fd = open(PIDFILE, O_WRONLY|O_CLOEXEC|O_CREAT|O_EXCL, 0644);
if (fd < 0) {
XLOG_ERR("could not open pidfile: %s", strerror(errno));
return -1;
}
FILE *pidfile = fdopen(fd, "w");
if (pidfile == NULL) {
XLOG_ERR("could not fdopen pidfile: %s", strerror(errno));
return -1;
}
fprintf(pidfile, "%u", getpid());
fclose(pidfile);
return 0;
}
int nl_open(struct nl_ctx *nl_ctx)
{
int ret;
nl_ctx->fd = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
if (nl_ctx->fd < 0) {
XLOG_ERR("error opening netlink socket: %s", strerror(errno));
return -1;
}
memset(&nl_ctx->sa, 0, sizeof(struct sockaddr_nl));
nl_ctx->sa.nl_family = AF_NETLINK;
nl_ctx->sa.nl_pid = 0;
nl_ctx->sa.nl_groups = 0;
ret = bind(nl_ctx->fd, (struct sockaddr *) &nl_ctx->sa, sizeof(struct sockaddr_nl));
if (ret < 0) {
XLOG_ERR("error binding netlink socket: %s", strerror(errno));
return -1;
}
return 0;
}
static enum dispatch_action
dispatch(struct poll_handler *handler, struct pollfd *pollfd)
{
if (pollfd->revents == 0) {
return DISPATCH_CONTINUE;
}
if (pollfd->revents & POLLERR) {
XLOG_ERR("poll error on fd %i", pollfd->fd);
return DISPATCH_ABORT;
}
return handler->handler(pollfd->fd, pollfd->events, handler->aux);
}
ssize_t read_request(int fd, struct sockaddr_ll *addr, char *buf, size_t size)
{
socklen_t addrlen = sizeof(struct sockaddr_ll);
ssize_t ret = recvfrom(fd, buf, size, 0, (struct sockaddr *)addr, &addrlen);
if (ret < 0) {
if (errno == EAGAIN || errno == EWOULDBLOCK) {
return 0;
}
XLOG_ERR("read error on fd %i: %s", fd, strerror(errno));
return -1;
}
XLOG_DEBUG("read %zi octets", ret);
return ret;
}
int nl_list_links(struct nl_ctx *nl_ctx)
{
int ret;
size_t size;
char buf[1024];
size = nl_create_msg(buf, sizeof(buf), RTM_GETLINK, NLM_F_REQUEST|NLM_F_DUMP, PF_PACKET);
ret = send(nl_ctx->fd, buf, size, 0);
if (ret != size) {
XLOG_ERR("error sending netlink message: %s", strerror(errno));
return -1;
}
return 0;
}
int send_reply(int fd, struct link *link)
{
ssize_t ret = sendto(fd, link->buf, sizeof(struct ether_arp), 0,
(struct sockaddr *)&link->src, sizeof(struct sockaddr_ll));
if (ret < 0) {
if (errno == EAGAIN || errno == EWOULDBLOCK) {
return 0;
}
XLOG_ERR("write error on fd %i: %s", fd, strerror(errno));
return -1;
}
XLOG_DEBUG("send %zi octets on %s", ret, link->name);
dispatcher_flags(&link->handler, POLLIN);
return 0;
}
int do_bind(int fd, size_t ifindex)
{
struct sockaddr_ll addr;
memset(&addr, 0, sizeof(struct sockaddr_ll));
addr.sll_family = PF_PACKET;
addr.sll_ifindex = ifindex;
int ret = bind(fd, (struct sockaddr *)&addr, sizeof(struct sockaddr_ll));
if (ret == -1) {
XLOG_ERR("error binding socket on %zu: %s",
ifindex, strerror(errno));
return -1;
}
return 0;
}
int nl_receive(struct nl_ctx *nl_ctx, struct nl_cb *cb)
{
char buf[32768];
ssize_t len;
do {
memset(buf, 0, sizeof(buf));
len = recv(nl_ctx->fd, buf, sizeof(buf), 0);
if (len < 0) {
XLOG_ERR("error receiving netlink message: %s", strerror(errno));
return -1;
}
} while (nl_parse(buf, len, cb));
return 0;
}
int set_promisc(int fd, size_t ifindex)
{
struct packet_mreq mreq;
memset(&mreq, 0, sizeof(struct packet_mreq));
mreq.mr_ifindex = ifindex;
mreq.mr_type = PACKET_MR_PROMISC;
int ret = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
&mreq, sizeof(struct packet_mreq));
if (ret != 0) {
XLOG_ERR("error setting promisc mode on link %zu: %s",
ifindex, strerror(errno));
return -1;
}
return 0;
}
int rarpd(int argc, char *argv[])
{
struct rarpd rarpd;
rarpd_init(&rarpd);
if (parse_options(&rarpd, argc, argv) != 0) {
return EXIT_FAILURE;
}
if (parse_args(&rarpd, argv) != 0) {
return EXIT_FAILURE;
}
if (!(rarpd.opts & FOREGROUND) && daemonize() != 0) {
return EXIT_FAILURE;
}
init_syslog(&rarpd);
if (rarpd_init_signals(&rarpd) != 0) {
return EXIT_FAILURE;
}
if (find_interfaces(&rarpd) != 0) {
return EXIT_FAILURE;
}
link_array_filter(&rarpd.links, has_addr, NULL);
if (rarpd.links.count == 0) {
XLOG_ERR("no usable links found");
return EXIT_FAILURE;
}
if (!link_array_foreach(&rarpd.links, setup_link, &rarpd.dispatcher)) {
cleanup_rarpd(&rarpd);
return EXIT_FAILURE;
}
dispatcher_run(&rarpd.dispatcher);
cleanup_rarpd(&rarpd);
return EXIT_SUCCESS;
}
int dispatcher_run(struct dispatcher *dispatcher)
{
int ret;
for(;;) {
foreach_handler(dispatcher, set_events);
do {
ret = poll(dispatcher->fds, dispatcher->nfds, -1);
} while (ret < 0 && errno == EINTR);
if (ret < 0) {
XLOG_ERR("poll error: %s", strerror(errno));
return -1;
}
if (foreach_handler(dispatcher, dispatch) == DISPATCH_ABORT) {
return 0;
}
}
}
int nl_set_neigh(struct nl_ctx *nl_ctx, size_t index, struct ether_addr *ether_addr, in_addr_t *in_addr)
{
int ret;
size_t size;
char buf[1024];
size = nl_create_msg(buf, sizeof(buf), RTM_NEWNEIGH,
NLM_F_REQUEST|NLM_F_REPLACE|NLM_F_CREATE, PF_INET);
// nl_add_attr((struct nlmsghdr *) data, NDA_DST, sizeof(in_addr_r), in_addr);
// nl_add_attr((struct nlmsghdr *) data, NDA_LLADDR, sizeof(struct ether_addr), ether_addr);
// fill in ndm_ifindex from link
// set ndm_state NUD_PERMANENT
ret = send(nl_ctx->fd, buf, size, 0);
if (ret != size) {
XLOG_ERR("error sending netlink message: %s", strerror(errno));
return -1;
}
return 0;
}