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