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