diff options
Diffstat (limited to '')
-rw-r--r-- | nhrpd/znl.c | 170 |
1 files changed, 170 insertions, 0 deletions
diff --git a/nhrpd/znl.c b/nhrpd/znl.c new file mode 100644 index 0000000..6030987 --- /dev/null +++ b/nhrpd/znl.c @@ -0,0 +1,170 @@ +/* Netlink helpers for zbuf + * Copyright (c) 2014-2015 Timo Teräs + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <fcntl.h> +#include <errno.h> +#include <string.h> +#include <unistd.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <linux/netlink.h> +#include <linux/rtnetlink.h> + +#include "znl.h" + +#define ZNL_ALIGN(len) (((len)+3) & ~3) + +void *znl_push(struct zbuf *zb, size_t n) +{ + return zbuf_pushn(zb, ZNL_ALIGN(n)); +} + +void *znl_pull(struct zbuf *zb, size_t n) +{ + return zbuf_pulln(zb, ZNL_ALIGN(n)); +} + +struct nlmsghdr *znl_nlmsg_push(struct zbuf *zb, uint16_t type, uint16_t flags) +{ + struct nlmsghdr *n; + + n = znl_push(zb, sizeof(*n)); + if (!n) + return NULL; + + *n = (struct nlmsghdr){ + .nlmsg_type = type, .nlmsg_flags = flags, + }; + return n; +} + +void znl_nlmsg_complete(struct zbuf *zb, struct nlmsghdr *n) +{ + n->nlmsg_len = zb->tail - (uint8_t *)n; +} + +struct nlmsghdr *znl_nlmsg_pull(struct zbuf *zb, struct zbuf *payload) +{ + struct nlmsghdr *n; + size_t plen; + + n = znl_pull(zb, sizeof(*n)); + if (!n) + return NULL; + + plen = n->nlmsg_len - sizeof(*n); + zbuf_init(payload, znl_pull(zb, plen), plen, plen); + zbuf_may_pulln(zb, ZNL_ALIGN(plen) - plen); + + return n; +} + +struct rtattr *znl_rta_push(struct zbuf *zb, uint16_t type, const void *val, + size_t len) +{ + struct rtattr *rta; + uint8_t *dst; + + rta = znl_push(zb, ZNL_ALIGN(sizeof(*rta)) + ZNL_ALIGN(len)); + if (!rta) + return NULL; + + *rta = (struct rtattr){ + .rta_type = type, .rta_len = ZNL_ALIGN(sizeof(*rta)) + len, + }; + + dst = (uint8_t *)(rta + 1); + memcpy(dst, val, len); + memset(dst + len, 0, ZNL_ALIGN(len) - len); + + return rta; +} + +struct rtattr *znl_rta_push_u32(struct zbuf *zb, uint16_t type, uint32_t val) +{ + return znl_rta_push(zb, type, &val, sizeof(val)); +} + +struct rtattr *znl_rta_nested_push(struct zbuf *zb, uint16_t type) +{ + struct rtattr *rta; + + rta = znl_push(zb, sizeof(*rta)); + if (!rta) + return NULL; + + *rta = (struct rtattr){ + .rta_type = type, + }; + return rta; +} + +void znl_rta_nested_complete(struct zbuf *zb, struct rtattr *rta) +{ + size_t len = zb->tail - (uint8_t *)rta; + size_t align = ZNL_ALIGN(len) - len; + + if (align) { + void *dst = zbuf_pushn(zb, align); + if (dst) + memset(dst, 0, align); + } + rta->rta_len = len; +} + +struct rtattr *znl_rta_pull(struct zbuf *zb, struct zbuf *payload) +{ + struct rtattr *rta; + size_t plen; + + rta = znl_pull(zb, sizeof(*rta)); + if (!rta) + return NULL; + + if (rta->rta_len > sizeof(*rta)) { + plen = rta->rta_len - sizeof(*rta); + zbuf_init(payload, znl_pull(zb, plen), plen, plen); + } else { + zbuf_init(payload, NULL, 0, 0); + } + + return rta; +} + +int znl_open(int protocol, int groups) +{ + struct sockaddr_nl addr; + int fd, buf = 128 * 1024; + + fd = socket(AF_NETLINK, SOCK_RAW, protocol); + if (fd < 0) + return -1; + + if (fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK) < 0) + goto error; + if (fcntl(fd, F_SETFD, FD_CLOEXEC) < 0) + goto error; + if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &buf, sizeof(buf)) < 0) + goto error; + + memset(&addr, 0, sizeof(addr)); + addr.nl_family = AF_NETLINK; + addr.nl_groups = groups; + if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) + goto error; + + return fd; +error: + close(fd); + return -1; +} |