1 files changed, 139 insertions, 0 deletions
diff --git a/src/nlrequest.c b/src/nlrequest.c
new file mode 100644
index 0000000..1f72357
--- /dev/null
+++ b/src/nlrequest.c
@@ -0,0 +1,139 @@
+#include <assert.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include "nlrequest.h"
+
+int netlink_open(void) {
+    struct sockaddr_nl addr;
+    int s;
+    
+    if ((s = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE)) < 0) {
+        fprintf(stderr, "socket(PF_NETLINK): %s\n", strerror(errno));
+        return -1;
+    }
+
+    memset(&addr, 0, sizeof(addr));
+    addr.nl_family = AF_NETLINK;
+    addr.nl_groups = 0;
+    addr.nl_pid = getpid();
+
+    if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+        fprintf(stderr, "bind(): %s\n", strerror(errno));
+        return -1;
+    }
+
+    return s;
+}
+
+
+int netlink_request(int s, struct nlmsghdr *n, int (*callback) (struct nlmsghdr *n, void *u), void *u) {
+    static int seq = 0;
+    pid_t pid = getpid();
+    assert(s >= 0 && n);
+
+    n->nlmsg_seq = seq++;
+    n->nlmsg_flags |= NLM_F_ACK;
+    
+    if (send(s, n, n->nlmsg_len, 0) < 0) {
+        fprintf(stderr, "NETLINK: send(): %s\n", strerror(errno));
+        return -1;
+    }
+
+    for (;;) {
+        int bytes;
+        char replybuf[1024];
+        struct nlmsghdr *p = (struct nlmsghdr *) replybuf;
+        
+        if ((bytes = recv(s, &replybuf, sizeof(replybuf), 0)) < 0) {
+            fprintf(stderr, "recv(): %s\n", strerror(errno));
+            return -1;
+        }
+
+        for (; bytes > 0; p = NLMSG_NEXT(p, bytes)) {
+            int ret;
+            
+             if (!NLMSG_OK(p, bytes) || bytes < sizeof(struct nlmsghdr) || bytes < p->nlmsg_len) {
+                fprintf(stderr, "NETLINK: Packet too small or truncated!\n");
+                return -1;
+            }
+
+            if (p->nlmsg_type == NLMSG_DONE)
+                return 0;
+
+            if (p->nlmsg_type == NLMSG_ERROR) {
+                struct nlmsgerr *e = (struct nlmsgerr *) NLMSG_DATA (p);
+
+                if (e->error) {
+                    fprintf(stderr, "NETLINK: Error: %s\n", strerror(-e->error));
+                    return -1;
+                } else
+                    return 0;
+            }
+
+            if (p->nlmsg_pid != pid)
+                continue;
+
+            if (p->nlmsg_seq != n->nlmsg_seq) {
+                fprintf(stderr, "NETLINK: Received message with bogus sequence number!\n");
+                continue;
+            }
+
+            if (callback)
+                if ((ret = callback(p, u)) < 0)
+                    return ret;
+        }
+    }
+        
+    return 0;
+}
+
+/*
+ * Utility function comes from iproute2.
+ * Author: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
+ */
+
+int addattr_l(struct nlmsghdr *n, int maxlen, int type, void *data, int alen) {
+    int len;
+    struct rtattr *rta; 
+    
+    len = RTA_LENGTH(alen);
+    
+    if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen)
+        return -1;
+    
+    rta = (struct rtattr*) (((char*)n) + NLMSG_ALIGN (n->nlmsg_len));
+    rta->rta_type = type;
+    rta->rta_len = len;
+    memcpy (RTA_DATA(rta), data, alen);
+    n->nlmsg_len = NLMSG_ALIGN (n->nlmsg_len) + len;
+    
+    return 0;
+}
+
+/*
+ * Utility function comes from iproute2.
+ * Author: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
+ */
+
+
+int addattr32(struct nlmsghdr *n, int maxlen, int type, int data) {
+    int len;
+    struct rtattr *rta;
+    
+    len = RTA_LENGTH(4);
+    
+    if (NLMSG_ALIGN (n->nlmsg_len) + len > maxlen)
+        return -1;
+    
+    rta = (struct rtattr*) (((char*)n) + NLMSG_ALIGN (n->nlmsg_len));
+    rta->rta_type = type;
+    rta->rta_len = len;
+    memcpy (RTA_DATA(rta), &data, 4);
+    n->nlmsg_len = NLMSG_ALIGN (n->nlmsg_len) + len;
+    
+    return 0;
+}
+