b51bf223f42d43ca6b1b33c95124edcfa5a871a4nd

b66914021bd429f41311d2909a7e9289866da7fdnd

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include <netinet/in.h>

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include <stdbool.h>

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include <unistd.h>

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "util.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "socket-util.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "formats-util.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "refcnt.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "missing.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "sd-netlink.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "netlink-util.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "netlink-internal.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd#include "netlink-types.h"

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72ndint socket_open(int family) {

eed2a23d9b5986937f1e2b1c120be97744508a72nd int fd;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd fd = socket(PF_NETLINK, SOCK_RAW|SOCK_CLOEXEC|SOCK_NONBLOCK, family);

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (fd < 0)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return -errno;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd return fd;

eed2a23d9b5986937f1e2b1c120be97744508a72nd}

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72ndstatic int broadcast_groups_get(sd_netlink *nl) {

eed2a23d9b5986937f1e2b1c120be97744508a72nd _cleanup_free_ uint32_t *groups = NULL;

eed2a23d9b5986937f1e2b1c120be97744508a72nd socklen_t len = 0, old_len;

eed2a23d9b5986937f1e2b1c120be97744508a72nd unsigned i, j;

eed2a23d9b5986937f1e2b1c120be97744508a72nd int r;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd assert(nl);

eed2a23d9b5986937f1e2b1c120be97744508a72nd assert(nl->fd > 0);

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd r = getsockopt(nl->fd, SOL_NETLINK, NETLINK_LIST_MEMBERSHIPS, NULL, &len);

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (r < 0) {

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (errno == ENOPROTOOPT) {

eed2a23d9b5986937f1e2b1c120be97744508a72nd nl->broadcast_group_dont_leave = true;

eed2a23d9b5986937f1e2b1c120be97744508a72nd return 0;

eed2a23d9b5986937f1e2b1c120be97744508a72nd } else

eed2a23d9b5986937f1e2b1c120be97744508a72nd return -errno;

eed2a23d9b5986937f1e2b1c120be97744508a72nd }

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (len == 0)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return 0;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

f5e4573f2a3ca4b7d7d10bfb50950fa7eff27efbnilgun groups = new0(uint32_t, len);

f5e4573f2a3ca4b7d7d10bfb50950fa7eff27efbnilgun if (!groups)

f5e4573f2a3ca4b7d7d10bfb50950fa7eff27efbnilgun return -ENOMEM;

f5e4573f2a3ca4b7d7d10bfb50950fa7eff27efbnilgun

eed2a23d9b5986937f1e2b1c120be97744508a72nd old_len = len;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd r = getsockopt(nl->fd, SOL_NETLINK, NETLINK_LIST_MEMBERSHIPS, groups, &len);

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (r < 0)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return -errno;

bcf004854091600aa279525d6772e1827114d39dnd

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (old_len != len)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return -EIO;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd r = hashmap_ensure_allocated(&nl->broadcast_group_refs, NULL);

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd if (r < 0)

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd return r;

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd for (i = 0; i < len; i++) {

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd for (j = 0; j < sizeof(uint32_t) * 8; j ++) {

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd uint32_t offset;

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd unsigned group;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd offset = 1U << j;

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd if (!(groups[i] & offset))

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd continue;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd group = i * sizeof(uint32_t) * 8 + j + 1;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd r = hashmap_put(nl->broadcast_group_refs, UINT_TO_PTR(group), UINT_TO_PTR(1));

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (r < 0)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return r;

eed2a23d9b5986937f1e2b1c120be97744508a72nd }

9ead22080b72dcfbf031f25179e89a7fab346f8and }

9ead22080b72dcfbf031f25179e89a7fab346f8and

9ead22080b72dcfbf031f25179e89a7fab346f8and return 0;

eed2a23d9b5986937f1e2b1c120be97744508a72nd}

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72ndint socket_bind(sd_netlink *nl) {

eed2a23d9b5986937f1e2b1c120be97744508a72nd socklen_t addrlen;

eed2a23d9b5986937f1e2b1c120be97744508a72nd int r, one = 1;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd r = setsockopt(nl->fd, SOL_NETLINK, NETLINK_PKTINFO, &one, sizeof(one));

9963f91528694fb21e93da8584c31f226c6de97akess if (r < 0)

bcf004854091600aa279525d6772e1827114d39dnd return -errno;

9963f91528694fb21e93da8584c31f226c6de97akess

eed2a23d9b5986937f1e2b1c120be97744508a72nd addrlen = sizeof(nl->sockaddr);

eed2a23d9b5986937f1e2b1c120be97744508a72nd

b51bf223f42d43ca6b1b33c95124edcfa5a871a4nd r = bind(nl->fd, &nl->sockaddr.sa, addrlen);

b51bf223f42d43ca6b1b33c95124edcfa5a871a4nd /* ignore EINVAL to allow opening an already bound socket */

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (r < 0 && errno != EINVAL)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return -errno;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd r = getsockname(nl->fd, &nl->sockaddr.sa, &addrlen);

8daf1ddf6285e4ded262f7a7a8e2e983a84a3cbdhumbedooh if (r < 0)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return -errno;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd r = broadcast_groups_get(nl);

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (r < 0)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return r;

b51bf223f42d43ca6b1b33c95124edcfa5a871a4nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd return 0;

eed2a23d9b5986937f1e2b1c120be97744508a72nd}

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72ndstatic unsigned broadcast_group_get_ref(sd_netlink *nl, unsigned group) {

eed2a23d9b5986937f1e2b1c120be97744508a72nd assert(nl);

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd return PTR_TO_UINT(hashmap_get(nl->broadcast_group_refs, UINT_TO_PTR(group)));

eed2a23d9b5986937f1e2b1c120be97744508a72nd}

4930be147adf9e3f6d3ca9313a6524f9bf654b2dnd

4930be147adf9e3f6d3ca9313a6524f9bf654b2dndstatic int broadcast_group_set_ref(sd_netlink *nl, unsigned group, unsigned n_ref) {

eed2a23d9b5986937f1e2b1c120be97744508a72nd int r;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd assert(nl);

eed2a23d9b5986937f1e2b1c120be97744508a72nd

eed2a23d9b5986937f1e2b1c120be97744508a72nd r = hashmap_replace(nl->broadcast_group_refs, UINT_TO_PTR(group), UINT_TO_PTR(n_ref));

eed2a23d9b5986937f1e2b1c120be97744508a72nd if (r < 0)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return r;

b3027c7641b1104ee50404cca6868e5101a7ca45nd

b3ab5b870526aebe7da94a01350677b5087fc26and return 0;

b3ab5b870526aebe7da94a01350677b5087fc26and}

b3027c7641b1104ee50404cca6868e5101a7ca45nd

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3ndstatic int broadcast_group_join(sd_netlink *nl, unsigned group) {

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3nd int r;

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3nd

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3nd assert(nl);

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3nd assert(nl->fd >= 0);

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3nd assert(group > 0);

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3nd

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3nd r = setsockopt(nl->fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &group, sizeof(group));

f1110149b5b6c28ecc39d1958d98ad9cfa8e41f3nd if (r < 0)

eed2a23d9b5986937f1e2b1c120be97744508a72nd return -errno;

eed2a23d9b5986937f1e2b1c120be97744508a72nd

return 0;

}

int socket_broadcast_group_ref(sd_netlink *nl, unsigned group) {

unsigned n_ref;

int r;

assert(nl);

n_ref = broadcast_group_get_ref(nl, group);

n_ref ++;

r = hashmap_ensure_allocated(&nl->broadcast_group_refs, NULL);

if (r < 0)

return r;

r = broadcast_group_set_ref(nl, group, n_ref);

if (r < 0)

return r;

if (n_ref > 1)

/* not yet in the group */

return 0;

r = broadcast_group_join(nl, group);

if (r < 0)

return r;

return 0;

}

static int broadcast_group_leave(sd_netlink *nl, unsigned group) {

int r;

assert(nl);

assert(nl->fd >= 0);

assert(group > 0);

if (nl->broadcast_group_dont_leave)

return 0;

r = setsockopt(nl->fd, SOL_NETLINK, NETLINK_DROP_MEMBERSHIP, &group, sizeof(group));

if (r < 0)

return -errno;

return 0;

}

int socket_broadcast_group_unref(sd_netlink *nl, unsigned group) {

unsigned n_ref;

int r;

assert(nl);

n_ref = broadcast_group_get_ref(nl, group);

assert(n_ref > 0);

n_ref --;

r = broadcast_group_set_ref(nl, group, n_ref);

if (r < 0)

return r;

if (n_ref > 0)

/* still refs left */

return 0;

r = broadcast_group_leave(nl, group);

if (r < 0)

return r;

return 0;

}

int socket_write_message(sd_netlink *nl, sd_netlink_message *m) {

union {

struct sockaddr sa;

struct sockaddr_nl nl;

} addr = {

.nl.nl_family = AF_NETLINK,

};

ssize_t k;

assert(nl);

assert(m);

assert(m->hdr);

k = sendto(nl->fd, m->hdr, m->hdr->nlmsg_len,

0, &addr.sa, sizeof(addr));

if (k < 0)

return -errno;

return k;

}

static int socket_recv_message(int fd, struct iovec *iov, uint32_t *_group, bool peek) {

union sockaddr_union sender;

uint8_t cmsg_buffer[CMSG_SPACE(sizeof(struct nl_pktinfo))];

struct msghdr msg = {

.msg_iov = iov,

.msg_iovlen = 1,

.msg_name = &sender,

.msg_namelen = sizeof(sender),

.msg_control = cmsg_buffer,

.msg_controllen = sizeof(cmsg_buffer),

};

struct cmsghdr *cmsg;

uint32_t group = 0;

int r;

assert(fd >= 0);

assert(iov);

r = recvmsg(fd, &msg, MSG_TRUNC | (peek ? MSG_PEEK : 0));

if (r < 0) {

/* no data */

if (errno == ENOBUFS)

log_debug("rtnl: kernel receive buffer overrun");

else if (errno == EAGAIN)

log_debug("rtnl: no data in socket");

return (errno == EAGAIN || errno == EINTR) ? 0 : -errno;

}

if (sender.nl.nl_pid != 0) {

/* not from the kernel, ignore */

log_debug("rtnl: ignoring message from portid %"PRIu32, sender.nl.nl_pid);

if (peek) {

/* drop the message */

r = recvmsg(fd, &msg, 0);

if (r < 0)

return (errno == EAGAIN || errno == EINTR) ? 0 : -errno;

}

return 0;

}

CMSG_FOREACH(cmsg, &msg) {

if (cmsg->cmsg_level == SOL_NETLINK &&

cmsg->cmsg_type == NETLINK_PKTINFO &&

cmsg->cmsg_len == CMSG_LEN(sizeof(struct nl_pktinfo))) {

struct nl_pktinfo *pktinfo = (void *)CMSG_DATA(cmsg);

/* multi-cast group */

group = pktinfo->group;

}

}

if (_group)

*_group = group;

return r;

}

int socket_read_message(sd_netlink *rtnl) {

_cleanup_netlink_message_unref_ sd_netlink_message *first = NULL;

struct iovec iov = {};

uint32_t group = 0;

bool multi_part = false, done = false;

struct nlmsghdr *new_msg;

size_t len;

int r;

unsigned i = 0;

assert(rtnl);

assert(rtnl->rbuffer);

assert(rtnl->rbuffer_allocated >= sizeof(struct nlmsghdr));

/* read nothing, just get the pending message size */

r = socket_recv_message(rtnl->fd, &iov, NULL, true);

if (r <= 0)

return r;

else

len = (size_t)r;

/* make room for the pending message */

if (!greedy_realloc((void **)&rtnl->rbuffer,

&rtnl->rbuffer_allocated,

len, sizeof(uint8_t)))

return -ENOMEM;

iov.iov_base = rtnl->rbuffer;

iov.iov_len = rtnl->rbuffer_allocated;

/* read the pending message */

r = socket_recv_message(rtnl->fd, &iov, &group, false);

if (r <= 0)

return r;

else

len = (size_t)r;

if (len > rtnl->rbuffer_allocated)

/* message did not fit in read buffer */

return -EIO;

if (NLMSG_OK(rtnl->rbuffer, len) && rtnl->rbuffer->nlmsg_flags & NLM_F_MULTI) {

multi_part = true;

for (i = 0; i < rtnl->rqueue_partial_size; i++) {

if (rtnl_message_get_serial(rtnl->rqueue_partial[i]) ==

rtnl->rbuffer->nlmsg_seq) {

first = rtnl->rqueue_partial[i];

break;

}

}

}

for (new_msg = rtnl->rbuffer; NLMSG_OK(new_msg, len) && !done; new_msg = NLMSG_NEXT(new_msg, len)) {

_cleanup_netlink_message_unref_ sd_netlink_message *m = NULL;

const NLType *nl_type;

if (!group && new_msg->nlmsg_pid != rtnl->sockaddr.nl.nl_pid)

/* not broadcast and not for us */

continue;

if (new_msg->nlmsg_type == NLMSG_NOOP)

/* silently drop noop messages */

continue;

if (new_msg->nlmsg_type == NLMSG_DONE) {

/* finished reading multi-part message */

done = true;

/* if first is not defined, put NLMSG_DONE into the receive queue. */

if (first)

continue;

}

/* check that we support this message type */

r = type_system_get_type(&type_system_root, &nl_type, new_msg->nlmsg_type);

if (r < 0) {

if (r == -EOPNOTSUPP)

log_debug("sd-netlink: ignored message with unknown type: %i",

new_msg->nlmsg_type);

continue;

}

/* check that the size matches the message type */

if (new_msg->nlmsg_len < NLMSG_LENGTH(type_get_size(nl_type))) {

log_debug("sd-netlink: message larger than expected, dropping");

continue;

}

r = message_new_empty(rtnl, &m);

if (r < 0)

return r;

m->broadcast = !!group;

m->hdr = memdup(new_msg, new_msg->nlmsg_len);

if (!m->hdr)

return -ENOMEM;

/* seal and parse the top-level message */

r = sd_netlink_message_rewind(m);

if (r < 0)

return r;

/* push the message onto the multi-part message stack */

if (first)

m->next = first;

first = m;

m = NULL;

}

if (len)

log_debug("sd-netlink: discarding %zu bytes of incoming message", len);

if (!first)

return 0;

if (!multi_part || done) {

/* we got a complete message, push it on the read queue */

r = rtnl_rqueue_make_room(rtnl);

if (r < 0)

return r;

rtnl->rqueue[rtnl->rqueue_size ++] = first;

first = NULL;

if (multi_part && (i < rtnl->rqueue_partial_size)) {

/* remove the message form the partial read queue */

memmove(rtnl->rqueue_partial + i,rtnl->rqueue_partial + i + 1,

sizeof(sd_netlink_message*) * (rtnl->rqueue_partial_size - i - 1));

rtnl->rqueue_partial_size --;

}

return 1;

} else {

/* we only got a partial multi-part message, push it on the

if (i < rtnl->rqueue_partial_size) {

rtnl->rqueue_partial[i] = first;

} else {

r = rtnl_rqueue_partial_make_room(rtnl);

if (r < 0)

return r;

rtnl->rqueue_partial[rtnl->rqueue_partial_size ++] = first;

}

first = NULL;

return 0;

}

}