diff options
Diffstat (limited to 'src/core/platform/wpan/nm-wpan-utils.c')
| -rw-r--r-- | src/core/platform/wpan/nm-wpan-utils.c | 286 |
1 files changed, 286 insertions, 0 deletions
diff --git a/src/core/platform/wpan/nm-wpan-utils.c b/src/core/platform/wpan/nm-wpan-utils.c new file mode 100644 index 00000000..efb19979 --- /dev/null +++ b/src/core/platform/wpan/nm-wpan-utils.c @@ -0,0 +1,286 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2018 Red Hat, Inc. + */ + +#include "src/core/nm-default-daemon.h" + +#include "nm-wpan-utils.h" + +#include <linux/if.h> + +#include "nm-platform/nm-netlink.h" +#include "platform/linux/nl802154.h" +#include "nm-platform/nm-platform-utils.h" + +#define _NMLOG_PREFIX_NAME "wpan-nl802154" +#define _NMLOG(level, domain, ...) \ + G_STMT_START \ + { \ + char _ifname_buf[IFNAMSIZ]; \ + const char *_ifname = self ? nmp_utils_if_indextoname(self->ifindex, _ifname_buf) : NULL; \ + \ + nm_log((level), \ + (domain), \ + _ifname ?: NULL, \ + NULL, \ + "%s%s%s%s: " _NM_UTILS_MACRO_FIRST(__VA_ARGS__), \ + _NMLOG_PREFIX_NAME, \ + NM_PRINT_FMT_QUOTED(_ifname, " (", _ifname, ")", "") \ + _NM_UTILS_MACRO_REST(__VA_ARGS__)); \ + } \ + G_STMT_END + +/*****************************************************************************/ + +struct NMWpanUtils { + GObject parent; + int ifindex; + struct nl_sock *nl_sock; + int id; +}; + +typedef struct { + GObjectClass parent; +} NMWpanUtilsClass; + +G_DEFINE_TYPE(NMWpanUtils, nm_wpan_utils, G_TYPE_OBJECT) + +/*****************************************************************************/ + +static int +ack_handler(struct nl_msg *msg, void *arg) +{ + int *done = arg; + *done = 1; + return NL_STOP; +} + +static int +finish_handler(struct nl_msg *msg, void *arg) +{ + int *done = arg; + *done = 1; + return NL_SKIP; +} + +static int +error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg) +{ + int *done = arg; + *done = err->error; + return NL_SKIP; +} + +static struct nl_msg * +_nl802154_alloc_msg(int id, int ifindex, guint32 cmd, guint32 flags) +{ + nm_auto_nlmsg struct nl_msg *msg = NULL; + + msg = nlmsg_alloc(); + genlmsg_put(msg, 0, 0, id, 0, flags, cmd, 0); + NLA_PUT_U32(msg, NL802154_ATTR_IFINDEX, ifindex); + return g_steal_pointer(&msg); + +nla_put_failure: + g_return_val_if_reached(NULL); +} + +static struct nl_msg * +nl802154_alloc_msg(NMWpanUtils *self, guint32 cmd, guint32 flags) +{ + return _nl802154_alloc_msg(self->id, self->ifindex, cmd, flags); +} + +static int +nl802154_send_and_recv(NMWpanUtils * self, + struct nl_msg *msg, + int (*valid_handler)(struct nl_msg *, void *), + void *valid_data) +{ + int err; + int done = 0; + const struct nl_cb cb = { + .err_cb = error_handler, + .err_arg = &done, + .finish_cb = finish_handler, + .finish_arg = &done, + .ack_cb = ack_handler, + .ack_arg = &done, + .valid_cb = valid_handler, + .valid_arg = valid_data, + }; + + g_return_val_if_fail(msg != NULL, -ENOMEM); + + err = nl_send_auto(self->nl_sock, msg); + if (err < 0) + return err; + + /* Loop until one of our NL callbacks says we're done; on success + * done will be 1, on error it will be < 0. + */ + while (!done) { + err = nl_recvmsgs(self->nl_sock, &cb); + if (err < 0 && err != -EAGAIN) { + _LOGW(LOGD_PLATFORM, "nl_recvmsgs() error: (%d) %s", err, nm_strerror(err)); + break; + } + } + + if (err >= 0 && done < 0) + err = done; + return err; +} + +struct nl802154_interface { + guint16 pan_id; + guint16 short_addr; + + gboolean valid; +}; + +static int +nl802154_get_interface_handler(struct nl_msg *msg, void *arg) +{ + static const struct nla_policy nl802154_policy[] = { + [NL802154_ATTR_PAN_ID] = {.type = NLA_U16}, + [NL802154_ATTR_SHORT_ADDR] = {.type = NLA_U16}, + }; + struct nlattr * tb[G_N_ELEMENTS(nl802154_policy)]; + struct nl802154_interface *info = arg; + struct genlmsghdr * gnlh = nlmsg_data(nlmsg_hdr(msg)); + + if (nla_parse_arr(tb, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), nl802154_policy) < 0) + return NL_SKIP; + + if (tb[NL802154_ATTR_PAN_ID]) + info->pan_id = le16toh(nla_get_u16(tb[NL802154_ATTR_PAN_ID])); + + if (tb[NL802154_ATTR_SHORT_ADDR]) + info->short_addr = le16toh(nla_get_u16(tb[NL802154_ATTR_SHORT_ADDR])); + + info->valid = TRUE; + + return NL_SKIP; +} + +static void +nl802154_get_interface(NMWpanUtils *self, struct nl802154_interface *interface) +{ + nm_auto_nlmsg struct nl_msg *msg = NULL; + + memset(interface, 0, sizeof(*interface)); + + msg = nl802154_alloc_msg(self, NL802154_CMD_GET_INTERFACE, 0); + + nl802154_send_and_recv(self, msg, nl802154_get_interface_handler, interface); +} + +/*****************************************************************************/ + +guint16 +nm_wpan_utils_get_pan_id(NMWpanUtils *self) +{ + struct nl802154_interface interface; + + nl802154_get_interface(self, &interface); + + return interface.pan_id; +} + +gboolean +nm_wpan_utils_set_pan_id(NMWpanUtils *self, guint16 pan_id) +{ + nm_auto_nlmsg struct nl_msg *msg = NULL; + int err; + + g_return_val_if_fail(self != NULL, FALSE); + + msg = nl802154_alloc_msg(self, NL802154_CMD_SET_PAN_ID, 0); + NLA_PUT_U16(msg, NL802154_ATTR_PAN_ID, htole16(pan_id)); + err = nl802154_send_and_recv(self, msg, NULL, NULL); + return err >= 0; + +nla_put_failure: + g_return_val_if_reached(FALSE); +} + +guint16 +nm_wpan_utils_get_short_addr(NMWpanUtils *self) +{ + struct nl802154_interface interface; + + nl802154_get_interface(self, &interface); + + return interface.short_addr; +} + +gboolean +nm_wpan_utils_set_short_addr(NMWpanUtils *self, guint16 short_addr) +{ + nm_auto_nlmsg struct nl_msg *msg = NULL; + int err; + + g_return_val_if_fail(self != NULL, FALSE); + + msg = nl802154_alloc_msg(self, NL802154_CMD_SET_SHORT_ADDR, 0); + NLA_PUT_U16(msg, NL802154_ATTR_SHORT_ADDR, htole16(short_addr)); + err = nl802154_send_and_recv(self, msg, NULL, NULL); + return err >= 0; + +nla_put_failure: + g_return_val_if_reached(FALSE); +} + +gboolean +nm_wpan_utils_set_channel(NMWpanUtils *self, guint8 page, guint8 channel) +{ + nm_auto_nlmsg struct nl_msg *msg = NULL; + int err; + + g_return_val_if_fail(self != NULL, FALSE); + + msg = nl802154_alloc_msg(self, NL802154_CMD_SET_CHANNEL, 0); + NLA_PUT_U8(msg, NL802154_ATTR_PAGE, page); + NLA_PUT_U8(msg, NL802154_ATTR_CHANNEL, channel); + err = nl802154_send_and_recv(self, msg, NULL, NULL); + return err >= 0; + +nla_put_failure: + g_return_val_if_reached(FALSE); +} + +/*****************************************************************************/ + +static void +nm_wpan_utils_init(NMWpanUtils *self) +{} + +static void +nm_wpan_utils_class_init(NMWpanUtilsClass *klass) +{} + +NMWpanUtils * +nm_wpan_utils_new(int ifindex, struct nl_sock *genl, gboolean check_scan) +{ + NMWpanUtils *self; + + g_return_val_if_fail(ifindex > 0, NULL); + + if (!genl) + return NULL; + + self = g_object_new(NM_TYPE_WPAN_UTILS, NULL); + self->ifindex = ifindex; + self->nl_sock = genl; + self->id = genl_ctrl_resolve(genl, "nl802154"); + + if (self->id < 0) { + _LOGD(LOGD_PLATFORM, "genl_ctrl_resolve: failed to resolve \"nl802154\""); + g_object_unref(self); + return NULL; + } + + return self; +} |