359 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/* Taken & modified from iproute2's libnetlink.c3 * Authors: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>4 */5#include <stdio.h>6#include <stdlib.h>7#include <unistd.h>8#include <errno.h>9#include <time.h>10#include <sys/socket.h>11 12#include "netlink_helpers.h"13 14static int rcvbuf = 1024 * 1024;15 16void rtnl_close(struct rtnl_handle *rth)17{18 if (rth->fd >= 0) {19 close(rth->fd);20 rth->fd = -1;21 }22}23 24int rtnl_open_byproto(struct rtnl_handle *rth, unsigned int subscriptions,25 int protocol)26{27 socklen_t addr_len;28 int sndbuf = 32768;29 int one = 1;30 31 memset(rth, 0, sizeof(*rth));32 rth->proto = protocol;33 rth->fd = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC, protocol);34 if (rth->fd < 0) {35 perror("Cannot open netlink socket");36 return -1;37 }38 if (setsockopt(rth->fd, SOL_SOCKET, SO_SNDBUF,39 &sndbuf, sizeof(sndbuf)) < 0) {40 perror("SO_SNDBUF");41 goto err;42 }43 if (setsockopt(rth->fd, SOL_SOCKET, SO_RCVBUF,44 &rcvbuf, sizeof(rcvbuf)) < 0) {45 perror("SO_RCVBUF");46 goto err;47 }48 49 /* Older kernels may no support extended ACK reporting */50 setsockopt(rth->fd, SOL_NETLINK, NETLINK_EXT_ACK,51 &one, sizeof(one));52 53 memset(&rth->local, 0, sizeof(rth->local));54 rth->local.nl_family = AF_NETLINK;55 rth->local.nl_groups = subscriptions;56 57 if (bind(rth->fd, (struct sockaddr *)&rth->local,58 sizeof(rth->local)) < 0) {59 perror("Cannot bind netlink socket");60 goto err;61 }62 addr_len = sizeof(rth->local);63 if (getsockname(rth->fd, (struct sockaddr *)&rth->local,64 &addr_len) < 0) {65 perror("Cannot getsockname");66 goto err;67 }68 if (addr_len != sizeof(rth->local)) {69 fprintf(stderr, "Wrong address length %d\n", addr_len);70 goto err;71 }72 if (rth->local.nl_family != AF_NETLINK) {73 fprintf(stderr, "Wrong address family %d\n",74 rth->local.nl_family);75 goto err;76 }77 rth->seq = time(NULL);78 return 0;79err:80 rtnl_close(rth);81 return -1;82}83 84int rtnl_open(struct rtnl_handle *rth, unsigned int subscriptions)85{86 return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE);87}88 89static int __rtnl_recvmsg(int fd, struct msghdr *msg, int flags)90{91 int len;92 93 do {94 len = recvmsg(fd, msg, flags);95 } while (len < 0 && (errno == EINTR || errno == EAGAIN));96 if (len < 0) {97 fprintf(stderr, "netlink receive error %s (%d)\n",98 strerror(errno), errno);99 return -errno;100 }101 if (len == 0) {102 fprintf(stderr, "EOF on netlink\n");103 return -ENODATA;104 }105 return len;106}107 108static int rtnl_recvmsg(int fd, struct msghdr *msg, char **answer)109{110 struct iovec *iov = msg->msg_iov;111 char *buf;112 int len;113 114 iov->iov_base = NULL;115 iov->iov_len = 0;116 117 len = __rtnl_recvmsg(fd, msg, MSG_PEEK | MSG_TRUNC);118 if (len < 0)119 return len;120 if (len < 32768)121 len = 32768;122 buf = malloc(len);123 if (!buf) {124 fprintf(stderr, "malloc error: not enough buffer\n");125 return -ENOMEM;126 }127 iov->iov_base = buf;128 iov->iov_len = len;129 len = __rtnl_recvmsg(fd, msg, 0);130 if (len < 0) {131 free(buf);132 return len;133 }134 if (answer)135 *answer = buf;136 else137 free(buf);138 return len;139}140 141static void rtnl_talk_error(struct nlmsghdr *h, struct nlmsgerr *err,142 nl_ext_ack_fn_t errfn)143{144 fprintf(stderr, "RTNETLINK answers: %s\n",145 strerror(-err->error));146}147 148static int __rtnl_talk_iov(struct rtnl_handle *rtnl, struct iovec *iov,149 size_t iovlen, struct nlmsghdr **answer,150 bool show_rtnl_err, nl_ext_ack_fn_t errfn)151{152 struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK };153 struct iovec riov;154 struct msghdr msg = {155 .msg_name = &nladdr,156 .msg_namelen = sizeof(nladdr),157 .msg_iov = iov,158 .msg_iovlen = iovlen,159 };160 unsigned int seq = 0;161 struct nlmsghdr *h;162 int i, status;163 char *buf;164 165 for (i = 0; i < iovlen; i++) {166 h = iov[i].iov_base;167 h->nlmsg_seq = seq = ++rtnl->seq;168 if (answer == NULL)169 h->nlmsg_flags |= NLM_F_ACK;170 }171 status = sendmsg(rtnl->fd, &msg, 0);172 if (status < 0) {173 perror("Cannot talk to rtnetlink");174 return -1;175 }176 /* change msg to use the response iov */177 msg.msg_iov = &riov;178 msg.msg_iovlen = 1;179 i = 0;180 while (1) {181next:182 status = rtnl_recvmsg(rtnl->fd, &msg, &buf);183 ++i;184 if (status < 0)185 return status;186 if (msg.msg_namelen != sizeof(nladdr)) {187 fprintf(stderr,188 "Sender address length == %d!\n",189 msg.msg_namelen);190 exit(1);191 }192 for (h = (struct nlmsghdr *)buf; status >= sizeof(*h); ) {193 int len = h->nlmsg_len;194 int l = len - sizeof(*h);195 196 if (l < 0 || len > status) {197 if (msg.msg_flags & MSG_TRUNC) {198 fprintf(stderr, "Truncated message!\n");199 free(buf);200 return -1;201 }202 fprintf(stderr,203 "Malformed message: len=%d!\n",204 len);205 exit(1);206 }207 if (nladdr.nl_pid != 0 ||208 h->nlmsg_pid != rtnl->local.nl_pid ||209 h->nlmsg_seq > seq || h->nlmsg_seq < seq - iovlen) {210 /* Don't forget to skip that message. */211 status -= NLMSG_ALIGN(len);212 h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len));213 continue;214 }215 if (h->nlmsg_type == NLMSG_ERROR) {216 struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h);217 int error = err->error;218 219 if (l < sizeof(struct nlmsgerr)) {220 fprintf(stderr, "ERROR truncated\n");221 free(buf);222 return -1;223 }224 if (error) {225 errno = -error;226 if (rtnl->proto != NETLINK_SOCK_DIAG &&227 show_rtnl_err)228 rtnl_talk_error(h, err, errfn);229 }230 if (i < iovlen) {231 free(buf);232 goto next;233 }234 if (error) {235 free(buf);236 return -i;237 }238 if (answer)239 *answer = (struct nlmsghdr *)buf;240 else241 free(buf);242 return 0;243 }244 if (answer) {245 *answer = (struct nlmsghdr *)buf;246 return 0;247 }248 fprintf(stderr, "Unexpected reply!\n");249 status -= NLMSG_ALIGN(len);250 h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len));251 }252 free(buf);253 if (msg.msg_flags & MSG_TRUNC) {254 fprintf(stderr, "Message truncated!\n");255 continue;256 }257 if (status) {258 fprintf(stderr, "Remnant of size %d!\n", status);259 exit(1);260 }261 }262}263 264static int __rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n,265 struct nlmsghdr **answer, bool show_rtnl_err,266 nl_ext_ack_fn_t errfn)267{268 struct iovec iov = {269 .iov_base = n,270 .iov_len = n->nlmsg_len,271 };272 273 return __rtnl_talk_iov(rtnl, &iov, 1, answer, show_rtnl_err, errfn);274}275 276int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n,277 struct nlmsghdr **answer)278{279 return __rtnl_talk(rtnl, n, answer, true, NULL);280}281 282int addattr(struct nlmsghdr *n, int maxlen, int type)283{284 return addattr_l(n, maxlen, type, NULL, 0);285}286 287int addattr8(struct nlmsghdr *n, int maxlen, int type, __u8 data)288{289 return addattr_l(n, maxlen, type, &data, sizeof(__u8));290}291 292int addattr16(struct nlmsghdr *n, int maxlen, int type, __u16 data)293{294 return addattr_l(n, maxlen, type, &data, sizeof(__u16));295}296 297int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)298{299 return addattr_l(n, maxlen, type, &data, sizeof(__u32));300}301 302int addattr64(struct nlmsghdr *n, int maxlen, int type, __u64 data)303{304 return addattr_l(n, maxlen, type, &data, sizeof(__u64));305}306 307int addattrstrz(struct nlmsghdr *n, int maxlen, int type, const char *str)308{309 return addattr_l(n, maxlen, type, str, strlen(str)+1);310}311 312int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,313 int alen)314{315 int len = RTA_LENGTH(alen);316 struct rtattr *rta;317 318 if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {319 fprintf(stderr, "%s: Message exceeded bound of %d\n",320 __func__, maxlen);321 return -1;322 }323 rta = NLMSG_TAIL(n);324 rta->rta_type = type;325 rta->rta_len = len;326 if (alen)327 memcpy(RTA_DATA(rta), data, alen);328 n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);329 return 0;330}331 332int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len)333{334 if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) {335 fprintf(stderr, "%s: Message exceeded bound of %d\n",336 __func__, maxlen);337 return -1;338 }339 340 memcpy(NLMSG_TAIL(n), data, len);341 memset((void *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len);342 n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len);343 return 0;344}345 346struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type)347{348 struct rtattr *nest = NLMSG_TAIL(n);349 350 addattr_l(n, maxlen, type, NULL, 0);351 return nest;352}353 354int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest)355{356 nest->rta_len = (void *)NLMSG_TAIL(n) - (void *)nest;357 return n->nlmsg_len;358}359