nhrpd/znl.c

   1 /* Netlink helpers for zbuf
   2  * Copyright (c) 2014-2015 Timo Teräs
   3  *
   4  * This file is free software: you may copy, redistribute and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  */
   9
  10 #include <fcntl.h>
  11 #include <errno.h>
  12 #include <string.h>
  13 #include <unistd.h>
  14 #include <sys/types.h>
  15 #include <sys/socket.h>
  16 #include <linux/netlink.h>
  17 #include <linux/rtnetlink.h>
  18
  19 #include "znl.h"
  20
  21 #define ZNL_ALIGN(len)          (((len)+3) & ~3)
  22
  23 void *znl_push(struct zbuf *zb, size_t n)
  24 {
  25         return zbuf_pushn(zb, ZNL_ALIGN(n));
  26 }
  27
  28 void *znl_pull(struct zbuf *zb, size_t n)
  29 {
  30         return zbuf_pulln(zb, ZNL_ALIGN(n));
  31 }
  32
  33 struct nlmsghdr *znl_nlmsg_push(struct zbuf *zb, uint16_t type, uint16_t flags)
  34 {
  35         struct nlmsghdr *n;
  36
  37         n = znl_push(zb, sizeof(*n));
  38         if (!n) return NULL;
  39
  40         *n = (struct nlmsghdr) {
  41                 .nlmsg_type = type,
  42                 .nlmsg_flags = flags,
  43         };
  44         return n;
  45 }
  46
  47 void znl_nlmsg_complete(struct zbuf *zb, struct nlmsghdr *n)
  48 {
  49         n->nlmsg_len = zb->tail - (uint8_t*)n;
  50 }
  51
  52 struct nlmsghdr *znl_nlmsg_pull(struct zbuf *zb, struct zbuf *payload)
  53 {
  54         struct nlmsghdr *n;
  55         size_t plen;
  56
  57         n = znl_pull(zb, sizeof(*n));
  58         if (!n) return NULL;
  59
  60         plen = n->nlmsg_len - sizeof(*n);
  61         zbuf_init(payload, znl_pull(zb, plen), plen, plen);
  62         zbuf_may_pulln(zb, ZNL_ALIGN(plen) - plen);
  63
  64         return n;
  65 }
  66
  67 struct rtattr *znl_rta_push(struct zbuf *zb, uint16_t type, const void *val, size_t len)
  68 {
  69         struct rtattr *rta;
  70         uint8_t *dst;
  71
  72         rta = znl_push(zb, ZNL_ALIGN(sizeof(*rta)) + ZNL_ALIGN(len));
  73         if (!rta) return NULL;
  74
  75         *rta = (struct rtattr) {
  76                 .rta_type = type,
  77                 .rta_len  = ZNL_ALIGN(sizeof(*rta)) + len,
  78         };
  79
  80         dst = (uint8_t *)(rta+1);
  81         memcpy(dst, val, len);
  82         memset(dst+len, 0, ZNL_ALIGN(len) - len);
  83
  84         return rta;
  85 }
  86
  87 struct rtattr *znl_rta_push_u32(struct zbuf *zb, uint16_t type, uint32_t val)
  88 {
  89         return znl_rta_push(zb, type, &val, sizeof(val));
  90 }
  91
  92 struct rtattr *znl_rta_nested_push(struct zbuf *zb, uint16_t type)
  93 {
  94         struct rtattr *rta;
  95
  96         rta = znl_push(zb, sizeof(*rta));
  97         if (!rta) return NULL;
  98
  99         *rta = (struct rtattr) {
 100                 .rta_type = type,
 101         };
 102         return rta;
 103 }
 104
 105 void znl_rta_nested_complete(struct zbuf *zb, struct rtattr *rta)
 106 {
 107         size_t len = zb->tail - (uint8_t*) rta;
 108         size_t align = ZNL_ALIGN(len) - len;
 109
 110         if (align) {
 111                 void *dst = zbuf_pushn(zb, align);
 112                 if (dst) memset(dst, 0, align);
 113         }
 114         rta->rta_len = len;
 115 }
 116
 117 struct rtattr *znl_rta_pull(struct zbuf *zb, struct zbuf *payload)
 118 {
 119         struct rtattr *rta;
 120         size_t plen;
 121
 122         rta = znl_pull(zb, sizeof(*rta));
 123         if (!rta) return NULL;
 124
 125         if (rta->rta_len > sizeof(*rta)) {
 126                 plen = rta->rta_len - sizeof(*rta);
 127                 zbuf_init(payload, znl_pull(zb, plen), plen, plen);
 128         } else {
 129                 zbuf_init(payload, NULL, 0, 0);
 130         }
 131
 132         return rta;
 133 }
 134
 135 int znl_open(int protocol, int groups)
 136 {
 137         struct sockaddr_nl addr;
 138         int fd, buf = 128 * 1024;
 139
 140         fd = socket(AF_NETLINK, SOCK_RAW, protocol);
 141         if (fd < 0)
 142                 return -1;
 143
 144         if (fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK) < 0)
 145                 goto error;
 146         if (fcntl(fd, F_SETFD, FD_CLOEXEC) < 0)
 147                 goto error;
 148         if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &buf, sizeof(buf)) < 0)
 149                 goto error;
 150
 151         memset(&addr, 0, sizeof(addr));
 152         addr.nl_family = AF_NETLINK;
 153         addr.nl_groups = groups;
 154         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0)
 155                 goto error;
 156
 157         return fd;
 158 error:
 159         close(fd);
 160         return -1;
 161 }
 162