nhrpd/znl.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /* Netlink helpers for zbuf
   3  * Copyright (c) 2014-2015 Timo Teräs
   4  */
   5
   6 #ifdef HAVE_CONFIG_H
   7 #include "config.h"
   8 #endif
   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)
  39                 return NULL;
  40
  41         *n = (struct nlmsghdr){
  42                 .nlmsg_type = type, .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)
  59                 return NULL;
  60
  61         plen = n->nlmsg_len - sizeof(*n);
  62         zbuf_init(payload, znl_pull(zb, plen), plen, plen);
  63         zbuf_may_pulln(zb, ZNL_ALIGN(plen) - plen);
  64
  65         return n;
  66 }
  67
  68 struct rtattr *znl_rta_push(struct zbuf *zb, uint16_t type, const void *val,
  69                             size_t len)
  70 {
  71         struct rtattr *rta;
  72         uint8_t *dst;
  73
  74         rta = znl_push(zb, ZNL_ALIGN(sizeof(*rta)) + ZNL_ALIGN(len));
  75         if (!rta)
  76                 return NULL;
  77
  78         *rta = (struct rtattr){
  79                 .rta_type = type, .rta_len = ZNL_ALIGN(sizeof(*rta)) + len,
  80         };
  81
  82         dst = (uint8_t *)(rta + 1);
  83         memcpy(dst, val, len);
  84         memset(dst + len, 0, ZNL_ALIGN(len) - len);
  85
  86         return rta;
  87 }
  88
  89 struct rtattr *znl_rta_push_u32(struct zbuf *zb, uint16_t type, uint32_t val)
  90 {
  91         return znl_rta_push(zb, type, &val, sizeof(val));
  92 }
  93
  94 struct rtattr *znl_rta_nested_push(struct zbuf *zb, uint16_t type)
  95 {
  96         struct rtattr *rta;
  97
  98         rta = znl_push(zb, sizeof(*rta));
  99         if (!rta)
 100                 return NULL;
 101
 102         *rta = (struct rtattr){
 103                 .rta_type = type,
 104         };
 105         return rta;
 106 }
 107
 108 void znl_rta_nested_complete(struct zbuf *zb, struct rtattr *rta)
 109 {
 110         size_t len = zb->tail - (uint8_t *)rta;
 111         size_t align = ZNL_ALIGN(len) - len;
 112
 113         if (align) {
 114                 void *dst = zbuf_pushn(zb, align);
 115                 if (dst)
 116                         memset(dst, 0, align);
 117         }
 118         rta->rta_len = len;
 119 }
 120
 121 struct rtattr *znl_rta_pull(struct zbuf *zb, struct zbuf *payload)
 122 {
 123         struct rtattr *rta;
 124         size_t plen;
 125
 126         rta = znl_pull(zb, sizeof(*rta));
 127         if (!rta)
 128                 return NULL;
 129
 130         if (rta->rta_len > sizeof(*rta)) {
 131                 plen = rta->rta_len - sizeof(*rta);
 132                 zbuf_init(payload, znl_pull(zb, plen), plen, plen);
 133         } else {
 134                 zbuf_init(payload, NULL, 0, 0);
 135         }
 136
 137         return rta;
 138 }
 139
 140 int znl_open(int protocol, int groups)
 141 {
 142         struct sockaddr_nl addr;
 143         int fd, buf = 128 * 1024;
 144
 145         fd = socket(AF_NETLINK, SOCK_RAW, protocol);
 146         if (fd < 0)
 147                 return -1;
 148
 149         if (fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK) < 0)
 150                 goto error;
 151         if (fcntl(fd, F_SETFD, FD_CLOEXEC) < 0)
 152                 goto error;
 153         if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &buf, sizeof(buf)) < 0)
 154                 goto error;
 155
 156         memset(&addr, 0, sizeof(addr));
 157         addr.nl_family = AF_NETLINK;
 158         addr.nl_groups = groups;
 159         if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
 160                 goto error;
 161
 162         return fd;
 163 error:
 164         close(fd);
 165         return -1;
 166 }