git.meshlink.io Git - catta/blob - netlink.c

   1 #include <unistd.h>
   2 #include <errno.h>
   3 #include <string.h>
   4
   5 #include "netlink.h"
   6
   7 struct _flxNetlink {
   8     GMainContext *context;
   9     gint fd;
  10     guint seq;
  11     GPollFD poll_fd;
  12     GSource *source;
  13     void (*callback) (flxNetlink *nl, struct nlmsghdr *n, gpointer userdata);
  14     gpointer userdata;
  15 };
  16 static gboolean work(flxNetlink *nl) {
  17     g_assert(nl);
  18
  19     for (;;) {
  20         guint8 replybuf[64*1024];
  21         ssize_t bytes;
  22         struct nlmsghdr *p = (struct nlmsghdr *) replybuf;
  23
  24         if ((bytes = recv(nl->fd, replybuf, sizeof(replybuf), MSG_DONTWAIT)) < 0) {
  25
  26             if (errno == EAGAIN || errno == EINTR)
  27                 break;
  28
  29             g_warning("NETLINK: recv() failed");
  30             return FALSE;
  31         }
  32
  33         if (nl->callback) {
  34             for (; bytes > 0; p = NLMSG_NEXT(p, bytes)) {
  35                 if (!NLMSG_OK(p, bytes)) {
  36                     g_warning("NETLINK: packet truncated");
  37                     return FALSE;
  38                 }
  39
  40                 nl->callback(nl, p, nl->userdata);
  41             }
  42         }
  43     }
  44
  45     return TRUE;
  46 }
  47
  48 static gboolean prepare_func(GSource *source, gint *timeout) {
  49     g_assert(source);
  50     g_assert(timeout);
  51
  52     *timeout = -1;
  53     return FALSE;
  54 }
  55
  56 static gboolean check_func(GSource *source) {
  57     flxNetlink* nl;
  58     g_assert(source);
  59
  60     nl = *((flxNetlink**) (((guint8*) source) + sizeof(GSource)));
  61     g_assert(nl);
  62
  63     return nl->poll_fd.revents & (G_IO_IN|G_IO_HUP|G_IO_ERR);
  64 }
  65
  66 static gboolean dispatch_func(GSource *source, GSourceFunc callback, gpointer user_data) {
  67     flxNetlink* nl;
  68     g_assert(source);
  69
  70     nl = *((flxNetlink**) (((guint8*) source) + sizeof(GSource)));
  71     g_assert(nl);
  72
  73     return work(nl);
  74 }
  75
  76 flxNetlink *flx_netlink_new(GMainContext *context, guint32 groups, void (*cb) (flxNetlink *nl, struct nlmsghdr *n, gpointer userdata), gpointer userdata) {
  77     int fd;
  78     struct sockaddr_nl addr;
  79     flxNetlink *nl;
  80
  81     static GSourceFuncs source_funcs = {
  82         prepare_func,
  83         check_func,
  84         dispatch_func,
  85         NULL,
  86         NULL,
  87         NULL
  88     };
  89
  90     g_assert(context);
  91     g_assert(cb);
  92
  93     if ((fd = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE)) < 0) {
  94         g_critical("NETLINK: socket(PF_NETLINK): %s", strerror(errno));
  95         return NULL;
  96     }
  97
  98     memset(&addr, 0, sizeof(addr));
  99     addr.nl_family = AF_NETLINK;
 100     addr.nl_groups = groups;
 101     addr.nl_pid = getpid();
 102
 103     if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 104         close(fd);
 105         g_critical("bind(): %s", strerror(errno));
 106         return NULL;
 107     }
 108
 109     nl = g_new(flxNetlink, 1);
 110     nl->context = context;
 111     g_main_context_ref(context);
 112     nl->fd = fd;
 113     nl->seq = 0;
 114     nl->callback = cb;
 115     nl->userdata = userdata;
 116
 117     nl->source = g_source_new(&source_funcs, sizeof(GSource) + sizeof(flxNetlink*));
 118     *((flxNetlink**) (((guint8*) nl->source) + sizeof(GSource))) = nl;
 119
 120     memset(&nl->poll_fd, 0, sizeof(GPollFD));
 121     nl->poll_fd.fd = fd;
 122     nl->poll_fd.events = G_IO_IN|G_IO_ERR|G_IO_HUP;
 123     g_source_add_poll(nl->source, &nl->poll_fd);
 124
 125     g_source_attach(nl->source, nl->context);
 126
 127     return nl;
 128 }
 129
 130 void flx_netlink_free(flxNetlink *nl) {
 131     g_assert(nl);
 132
 133     g_source_destroy(nl->source);
 134     g_source_unref(nl->source);
 135     g_main_context_unref(nl->context);
 136     close(nl->fd);
 137     g_free(nl);
 138 }
 139
 140 int flx_netlink_send(flxNetlink *nl, struct nlmsghdr *m, guint *ret_seq) {
 141     g_assert(nl);
 142     g_assert(m);
 143
 144     m->nlmsg_seq = nl->seq++;
 145     m->nlmsg_flags |= NLM_F_ACK;
 146
 147     if (send(nl->fd, m, m->nlmsg_len, 0) < 0) {
 148         g_warning("NETLINK: send(): %s\n", strerror(errno));
 149         return -1;
 150     }
 151
 152     if (ret_seq)
 153         *ret_seq = m->nlmsg_seq;
 154
 155     return 0;
 156 }