eilduscd/src/netlink_watch.c

#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <net/if.h>

#include <linux/socket.h>
#include <linux/types.h>
#include <linux/netlink.h>
#include <linux/rtnetlink.h>

#define NL_BUFSIZE 64 * 1024

#include "netlink_watch.h"
#include "nwstatus.h"
#include "common.h"

typedef struct {
  uint8_t buf[NL_BUFSIZE];
  int fd;
  struct nlmsghdr *msg; 
  int len;
} netlink_t ;

netlink_t *netlink = NULL;

static void
do_dump(int type, int seq) {
  struct {
    struct nlmsghdr nh;
    struct rtgenmsg gen;
  } req;
  req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtgenmsg));
  req.nh.nlmsg_flags =  NLM_F_DUMP|NLM_F_REQUEST;
  req.nh.nlmsg_type = type;
  req.nh.nlmsg_pid = 0;
  req.nh.nlmsg_seq = seq;
  req.gen.rtgen_family = AF_UNSPEC;

  send(netlink->fd, &req, req.nh.nlmsg_len, 0);
}

static void
handle_address(struct nlmsghdr *p) {
  if (p->nlmsg_type == RTM_NEWADDR || p->nlmsg_type == RTM_DELADDR) {
    struct ifaddrmsg *msg;
    int l;
    struct rtattr *r;
    int added = p->nlmsg_type == RTM_NEWADDR;
    msg = NLMSG_DATA(p);
    l = IFA_PAYLOAD(p);
    r = IFA_RTA(NLMSG_DATA(p));
    for (;RTA_OK(r, l); r = RTA_NEXT(r,l)) {
      switch (r->rta_type) {
        case IFA_ADDRESS:
          if (added) {
            add_address(msg->ifa_index, msg->ifa_scope,
                        msg->ifa_family, RTA_DATA(r));
          } else {
            del_address(msg->ifa_index, msg->ifa_family, RTA_DATA(r));
          }
          break;
#if 0
        case IFA_LOCAL:
          printf("Local: %s\n", 
              inet_ntop(msg->ifa_family,RTA_DATA(r), 
                        namebuf, INET6_ADDRSTRLEN)
          );
          break;
        case IFA_BROADCAST:
          printf("Broadcast: %s\n", 
              inet_ntop(msg->ifa_family,RTA_DATA(r), 
                        namebuf, INET6_ADDRSTRLEN)
          );
          break;
        case IFA_LABEL:
          printf("Label: %s\n", (char *)RTA_DATA(r));
          break;
        case IFA_CACHEINFO: {
          struct ifa_cacheinfo *c  = RTA_DATA(r);
          printf("Cache info: %d\n", c->ifa_prefered);
                            }
          break;
        default:
          printf("-->%d\n", r->rta_type);
#endif
      }
    }
  } 
}

static void
handle_link(struct nlmsghdr *p) {
  struct ifinfomsg *msg;
  int l;
  struct rtattr *r;
  msg = NLMSG_DATA(p);
  l = IFLA_PAYLOAD(p);
  r = IFLA_RTA(NLMSG_DATA(p));

  if (p->nlmsg_type == RTM_DELLINK) {
    del_interface(msg->ifi_index);
    return;
  }

  interface_set_link(msg->ifi_index, msg->ifi_flags & IFF_UP);
  interface_set_flags(msg->ifi_index, msg->ifi_flags);
  for (;RTA_OK(r, l); r = RTA_NEXT(r,l)) {
    switch (r->rta_type) {
      case IFLA_IFNAME:
        interface_set_name(msg->ifi_index, (char *)RTA_DATA(r));
        break;
    }
  }
} 

static int
netlink_next_message(void) {
  if (netlink->len <= 0) {
    return FALSE;
  }
  if (netlink->msg == NULL) {
    netlink->msg = (struct nlmsghdr *)netlink->buf;
    return TRUE;
  }
  netlink->msg = NLMSG_NEXT(netlink->msg, netlink->len);
  if (!NLMSG_OK(netlink->msg, netlink->len)) {
    /* No more messages in the queue */
    netlink->len = 0;
    netlink->msg = NULL;
    return FALSE;
  }
  return TRUE;
}

int
netlink_recv(void) {
  netlink->len = recv(netlink->fd, netlink->buf, NL_BUFSIZE, 0);
  if (netlink->len < 0) {
    fprintf(stderr, "Netlink receive failed\n");
    exit(1);
  }
  return TRUE;
}


static void
netlink_handle_message(void) {
  switch (netlink->msg->nlmsg_type) {
    case RTM_NEWADDR: 
    case RTM_DELADDR: 
      handle_address(netlink->msg);
      break;
    case RTM_NEWLINK:
    case RTM_DELLINK:
      handle_link(netlink->msg);
      break;
  }
}

void
netlink_flush(void) {
 while (netlink_next_message()) {
   netlink_handle_message();
 }
}

void 
watch_dump(int seq) {
  for (;;) {
    while (netlink_next_message()) {
      switch (netlink->msg->nlmsg_type) {
        case NLMSG_ERROR:
          if (seq == netlink->msg->nlmsg_seq) {
            struct nlmsgerr *e = NLMSG_DATA(netlink->msg);
            fprintf(stderr, "error seq: %d: %s\n", netlink->msg->nlmsg_seq, 
                   strerror(-e->error));
            return;
          } 
          break;
        case NLMSG_DONE: 
          if (seq == netlink->msg->nlmsg_seq) {
            return;
          }
          break;
        default:
          netlink_handle_message();
          break;
      }
    }
    netlink_recv();
  }
}

void
netlink_init(void) {
  struct sockaddr_nl addr;
  netlink = malloc(sizeof(netlink_t));
  netlink->msg = NULL;
  netlink->len = 0;

  if ((netlink->fd = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE)) < 0) {
    fprintf(stderr, "netlink socket failed\n");
    exit(1);
  }

  memset(&addr, 0, sizeof(addr));
  addr.nl_family = AF_NETLINK;
  addr.nl_groups = 
    RTMGRP_LINK|RTMGRP_IPV4_IFADDR|RTMGRP_IPV6_IFADDR|RTMGRP_IPV6_IFADDR_BUG;
  addr.nl_pid = getpid();

  if (bind(netlink->fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
    fprintf(stderr, "bind failed\n");
    exit(1);
  }

  /* Dump all interfaces */
  do_dump(RTM_GETLINK, 0xdeadbeef);
  watch_dump(0xdeadbeef);
  /* Dump all addresses */
  do_dump(RTM_GETADDR, 0xdeadbeef);
  watch_dump(0xdeadbeef);
}

int netlink_get_fd(void) {
  return netlink->fd;
}