core: Track wiphy devices and its network interfaces

src/wiphy.c

@@ -25,6 +25,9 @@
 #endif
 
 #include <stdlib.h>
+#include <sys/socket.h>
+#include <linux/if.h>
+#include <linux/if_ether.h>
 #include <ell/ell.h>
 
 #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;
 }