diff --git a/src/wiphy.c b/src/wiphy.c index b2cae21e..cc3e0453 100644 --- a/src/wiphy.c +++ b/src/wiphy.c @@ -25,6 +25,9 @@ #endif #include +#include +#include +#include #include #include "linux/nl80211.h" @@ -33,6 +36,21 @@ static struct l_genl *genl = NULL; static struct l_genl_family *nl80211 = NULL; +struct netdev { + uint32_t index; + char name[IFNAMSIZ]; + uint32_t type; + uint8_t addr[ETH_ALEN]; +}; + +struct wiphy { + uint32_t id; + char name[20]; + struct l_queue *netdev_list; +}; + +static struct l_queue *wiphy_list = NULL; + static void do_debug(const char *str, void *user_data) { const char *prefix = user_data; @@ -40,20 +58,253 @@ static void do_debug(const char *str, void *user_data) l_info("%s%s", prefix, str); } +static void netdev_free(void *data) +{ + struct netdev *netdev = data; + + l_debug("Freeing interface %s", netdev->name); + + l_free(netdev); +} + +static bool netdev_match(const void *a, const void *b) +{ + const struct netdev *netdev = a; + uint32_t index = L_PTR_TO_UINT(b); + + return (netdev->index == index); +} + +static void wiphy_free(void *data) +{ + struct wiphy *wiphy = data; + + l_debug("Freeing wiphy %s", wiphy->name); + + l_queue_destroy(wiphy->netdev_list, netdev_free); + + l_free(wiphy); +} + +static bool wiphy_match(const void *a, const void *b) +{ + const struct wiphy *wiphy = a; + uint32_t id = L_PTR_TO_UINT(b); + + return (wiphy->id == id); +} + +static void wiphy_interface_dump_callback(struct l_genl_msg *msg, + void *user_data) +{ + struct wiphy *wiphy = user_data; + struct netdev *netdev; + struct l_genl_attr attr; + uint16_t type, len; + const void *data; + char ifname[IFNAMSIZ]; + uint8_t ifaddr[ETH_ALEN]; + uint32_t ifindex, iftype; + + if (!l_genl_attr_init(&attr, msg)) + return; + + memset(ifname, 0, sizeof(ifname)); + memset(ifaddr, 0, sizeof(ifaddr)); + iftype = NL80211_IFTYPE_UNSPECIFIED; + ifindex = 0; + + /* + * The interface index and interface name attributes are normally + * listed before the wiphy attribute. This handling assumes that + * all attributes are included in the same message. + * + * If any required attribute is missing, the whole message will + * be ignored. + */ + while (l_genl_attr_next(&attr, &type, &len, &data)) { + switch (type) { + case NL80211_ATTR_IFINDEX: + if (len != sizeof(uint32_t)) { + l_warn("Invalid interface index attribute"); + return; + } + + ifindex = *((uint32_t *) data); + break; + + case NL80211_ATTR_IFNAME: + if (len > sizeof(ifname)) { + l_warn("Invalid interface name attribute"); + return; + } + + memcpy(ifname, data, len); + break; + + case NL80211_ATTR_WIPHY: + if (len != sizeof(uint32_t)) { + l_warn("Invalid wiphy attribute"); + return; + } + + if (wiphy->id != *((uint32_t *) data)) { + l_warn("Mismatching wiphy attribute"); + return; + } + break; + + case NL80211_ATTR_IFTYPE: + if (len != sizeof(uint32_t)) { + l_warn("Invalid interface type attribute"); + return; + } + + iftype = *((uint32_t *) data); + break; + + case NL80211_ATTR_MAC: + if (len != sizeof(ifaddr)) { + l_warn("Invalid interface address attribute"); + return; + } + + memcpy(ifaddr, data, len); + break; + } + } + + if (!ifindex) { + l_warn("Missing interface index attribute"); + return; + } + + netdev = l_queue_find(wiphy->netdev_list, netdev_match, + L_UINT_TO_PTR(ifindex)); + if (!netdev) { + netdev = l_new(struct netdev, 1); + l_queue_push_tail(wiphy->netdev_list, netdev); + } + + memcpy(netdev->name, ifname, sizeof(netdev->name)); + memcpy(netdev->addr, ifaddr, sizeof(netdev->addr)); + netdev->index = ifindex; + netdev->type = iftype; + + l_debug("Found interface %s", netdev->name); +} + +static void wiphy_get_interfaces(struct wiphy *wiphy) +{ + struct l_genl_msg *msg; + + msg = l_genl_msg_new_sized(NL80211_CMD_GET_INTERFACE, 8); + + if (!l_genl_msg_append_attr(msg, NL80211_ATTR_WIPHY, + sizeof(uint32_t), &wiphy->id)) + return; + + l_genl_family_dump(nl80211, msg, wiphy_interface_dump_callback, + wiphy, NULL); +} + +static void wiphy_dump_callback(struct l_genl_msg *msg, void *user_data) +{ + struct wiphy *wiphy = NULL; + struct l_genl_attr attr; + uint16_t type, len; + const void *data; + uint32_t id; + bool created = false; + + if (!l_genl_attr_init(&attr, msg)) + return; + + /* + * The wiphy attribute is always the first attribute in the + * list. If not then error out with a warning and ignore the + * whole message. + * + * In most cases multiple of these message will be send + * since the information included can not fit into a single + * message. + */ + while (l_genl_attr_next(&attr, &type, &len, &data)) { + switch (type) { + case NL80211_ATTR_WIPHY: + if (wiphy) { + l_warn("Duplicate wiphy attribute"); + return; + } + + if (len != sizeof(uint32_t)) { + l_warn("Invalid wiphy attribute"); + return; + } + + id = *((uint32_t *) data); + + wiphy = l_queue_find(wiphy_list, wiphy_match, + L_UINT_TO_PTR(id)); + if (!wiphy) { + wiphy = l_new(struct wiphy, 1); + wiphy->id = id; + wiphy->netdev_list = l_queue_new(); + l_queue_push_tail(wiphy_list, wiphy); + created = true; + } + break; + + case NL80211_ATTR_WIPHY_NAME: + if (!wiphy) { + l_warn("No wiphy structure found"); + return; + } + + if (len > sizeof(wiphy->name)) { + l_warn("Invalid wiphy name attribute"); + return; + } + + memcpy(wiphy->name, data, len); + break; + } + } + + if (created) { + l_debug("Found wiphy %s", wiphy->name); + wiphy_get_interfaces(wiphy); + } +} + static void nl80211_appeared(void *user_data) { struct l_genl_msg *msg; l_debug("Found nl80211 interface"); + /* + * This is an extra sanity check so that no memory is leaked + * in case the generic netlink handling gets confused. + */ + if (wiphy_list) { + l_warn("Destroying existing list of wiphy devices"); + l_queue_destroy(wiphy_list, NULL); + } + + wiphy_list = l_queue_new(); + msg = l_genl_msg_new(NL80211_CMD_GET_WIPHY); - l_genl_family_dump(nl80211, msg, NULL, NULL, NULL); + l_genl_family_dump(nl80211, msg, wiphy_dump_callback, NULL, NULL); } static void nl80211_vanished(void *user_data) { l_debug("Lost nl80211 interface"); + + l_queue_destroy(wiphy_list, wiphy_free); + wiphy_list = NULL; } bool wiphy_init(void) @@ -104,5 +355,16 @@ bool wiphy_exit(void) l_genl_unref(genl); genl = NULL; + /* + * This is an extra sanity check so that no memory is leaked + * in case the generic netlink handling forgets to call the + * vanished callback. + */ + if (wiphy_list) { + l_warn("Found leftover list of wiphy devices"); + l_queue_destroy(wiphy_list, wiphy_free); + wiphy_list = NULL; + } + return true; }