diff options
Diffstat (limited to '')
| -rw-r--r-- | lib/mnl_utils.c | 254 |
1 files changed, 254 insertions, 0 deletions
diff --git a/lib/mnl_utils.c b/lib/mnl_utils.c new file mode 100644 index 0000000..f8e07d2 --- /dev/null +++ b/lib/mnl_utils.c @@ -0,0 +1,254 @@ +// SPDX-License-Identifier: GPL-2.0+ +/* + * mnl_utils.c Helpers for working with libmnl. + */ + +#include <errno.h> +#include <string.h> +#include <time.h> +#include <libmnl/libmnl.h> +#include <linux/genetlink.h> + +#include "libnetlink.h" +#include "mnl_utils.h" +#include "utils.h" + +struct mnl_socket *mnlu_socket_open(int bus) +{ + struct mnl_socket *nl; + int one = 1; + + nl = mnl_socket_open(bus); + if (nl == NULL) + return NULL; + + mnl_socket_setsockopt(nl, NETLINK_CAP_ACK, &one, sizeof(one)); + mnl_socket_setsockopt(nl, NETLINK_EXT_ACK, &one, sizeof(one)); + + if (mnl_socket_bind(nl, 0, MNL_SOCKET_AUTOPID) < 0) + goto err_bind; + + return nl; + +err_bind: + mnl_socket_close(nl); + return NULL; +} + +struct nlmsghdr *mnlu_msg_prepare(void *buf, uint32_t nlmsg_type, uint16_t flags, + void *extra_header, size_t extra_header_size) +{ + struct nlmsghdr *nlh; + void *eh; + + nlh = mnl_nlmsg_put_header(buf); + nlh->nlmsg_type = nlmsg_type; + nlh->nlmsg_flags = flags; + nlh->nlmsg_seq = time(NULL); + + eh = mnl_nlmsg_put_extra_header(nlh, extra_header_size); + memcpy(eh, extra_header, extra_header_size); + + return nlh; +} + +static int mnlu_cb_noop(const struct nlmsghdr *nlh, void *data) +{ + return MNL_CB_OK; +} + +static int mnlu_cb_error(const struct nlmsghdr *nlh, void *data) +{ + const struct nlmsgerr *err = mnl_nlmsg_get_payload(nlh); + + /* Netlink subsystems returns the errno value with different signess */ + if (err->error < 0) + errno = -err->error; + else + errno = err->error; + + if (nl_dump_ext_ack(nlh, NULL)) + return MNL_CB_ERROR; + + return err->error == 0 ? MNL_CB_STOP : MNL_CB_ERROR; +} + +static int mnlu_cb_stop(const struct nlmsghdr *nlh, void *data) +{ + int len = *(int *)NLMSG_DATA(nlh); + + if (len < 0) { + errno = -len; + nl_dump_ext_ack_done(nlh, sizeof(int), len); + return MNL_CB_ERROR; + } + return MNL_CB_STOP; +} + +static mnl_cb_t mnlu_cb_array[NLMSG_MIN_TYPE] = { + [NLMSG_NOOP] = mnlu_cb_noop, + [NLMSG_ERROR] = mnlu_cb_error, + [NLMSG_DONE] = mnlu_cb_stop, + [NLMSG_OVERRUN] = mnlu_cb_noop, +}; + +int mnlu_socket_recv_run(struct mnl_socket *nl, unsigned int seq, void *buf, size_t buf_size, + mnl_cb_t cb, void *data) +{ + unsigned int portid = mnl_socket_get_portid(nl); + int err; + + do { + err = mnl_socket_recvfrom(nl, buf, buf_size); + if (err <= 0) + break; + err = mnl_cb_run2(buf, err, seq, portid, + cb, data, mnlu_cb_array, + ARRAY_SIZE(mnlu_cb_array)); + } while (err > 0); + + return err; +} + +static int get_family_attrs_cb(const struct nlattr *attr, void *data) +{ + int type = mnl_attr_get_type(attr); + const struct nlattr **tb = data; + + if (mnl_attr_type_valid(attr, CTRL_ATTR_MAX) < 0) + return MNL_CB_ERROR; + + if (type == CTRL_ATTR_FAMILY_ID && + mnl_attr_validate(attr, MNL_TYPE_U16) < 0) + return MNL_CB_ERROR; + if (type == CTRL_ATTR_MAXATTR && + mnl_attr_validate(attr, MNL_TYPE_U32) < 0) + return MNL_CB_ERROR; + tb[type] = attr; + return MNL_CB_OK; +} + +static int get_family_cb(const struct nlmsghdr *nlh, void *data) +{ + struct genlmsghdr *genl = mnl_nlmsg_get_payload(nlh); + struct nlattr *tb[CTRL_ATTR_MAX + 1] = {}; + struct mnlu_gen_socket *nlg = data; + + mnl_attr_parse(nlh, sizeof(*genl), get_family_attrs_cb, tb); + if (!tb[CTRL_ATTR_FAMILY_ID]) + return MNL_CB_ERROR; + if (!tb[CTRL_ATTR_MAXATTR]) + return MNL_CB_ERROR; + nlg->family = mnl_attr_get_u16(tb[CTRL_ATTR_FAMILY_ID]); + nlg->maxattr = mnl_attr_get_u32(tb[CTRL_ATTR_MAXATTR]); + return MNL_CB_OK; +} + +static int family_get(struct mnlu_gen_socket *nlg, const char *family_name) +{ + struct genlmsghdr hdr = {}; + struct nlmsghdr *nlh; + int err; + + hdr.cmd = CTRL_CMD_GETFAMILY; + hdr.version = 0x1; + + nlh = mnlu_msg_prepare(nlg->buf, GENL_ID_CTRL, + NLM_F_REQUEST | NLM_F_ACK, + &hdr, sizeof(hdr)); + + mnl_attr_put_strz(nlh, CTRL_ATTR_FAMILY_NAME, family_name); + + err = mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len); + if (err < 0) + return err; + + err = mnlu_socket_recv_run(nlg->nl, nlh->nlmsg_seq, nlg->buf, + MNL_SOCKET_BUFFER_SIZE, + get_family_cb, nlg); + return err; +} + +int mnlu_gen_socket_open(struct mnlu_gen_socket *nlg, const char *family_name, + uint8_t version) +{ + int err; + + nlg->buf = malloc(MNL_SOCKET_BUFFER_SIZE); + if (!nlg->buf) + goto err_buf_alloc; + + nlg->nl = mnlu_socket_open(NETLINK_GENERIC); + if (!nlg->nl) + goto err_socket_open; + + err = family_get(nlg, family_name); + if (err) + goto err_socket; + + return 0; + +err_socket: + mnl_socket_close(nlg->nl); +err_socket_open: + free(nlg->buf); +err_buf_alloc: + return -1; +} + +void mnlu_gen_socket_close(struct mnlu_gen_socket *nlg) +{ + mnl_socket_close(nlg->nl); + free(nlg->buf); +} + +struct nlmsghdr * +_mnlu_gen_socket_cmd_prepare(struct mnlu_gen_socket *nlg, + uint8_t cmd, uint16_t flags, + uint32_t id, uint8_t version) +{ + struct genlmsghdr hdr = {}; + struct nlmsghdr *nlh; + + hdr.cmd = cmd; + hdr.version = version; + nlh = mnlu_msg_prepare(nlg->buf, id, flags, &hdr, sizeof(hdr)); + nlg->seq = nlh->nlmsg_seq; + return nlh; +} + +struct nlmsghdr *mnlu_gen_socket_cmd_prepare(struct mnlu_gen_socket *nlg, + uint8_t cmd, uint16_t flags) +{ + return _mnlu_gen_socket_cmd_prepare(nlg, cmd, flags, nlg->family, + nlg->version); +} + +int mnlu_gen_socket_sndrcv(struct mnlu_gen_socket *nlg, const struct nlmsghdr *nlh, + mnl_cb_t data_cb, void *data) +{ + int err; + + err = mnl_socket_sendto(nlg->nl, nlh, nlh->nlmsg_len); + if (err < 0) { + perror("Failed to send data"); + return -errno; + } + + err = mnlu_socket_recv_run(nlg->nl, nlh->nlmsg_seq, nlg->buf, + MNL_SOCKET_BUFFER_SIZE, + data_cb, data); + if (err < 0) { + fprintf(stderr, "kernel answers: %s\n", strerror(errno)); + return -errno; + } + return 0; +} + +int mnlu_gen_socket_recv_run(struct mnlu_gen_socket *nlg, mnl_cb_t cb, + void *data) +{ + return mnlu_socket_recv_run(nlg->nl, nlg->seq, nlg->buf, + MNL_SOCKET_BUFFER_SIZE, + cb, data); +} |