hwsim: Add daemon mode with radio information tracking

Add a daemon mode that is entered when no action was specified on the
command line.  In this mode hwsim tracks information on radios through
the netlink events.  The interface to make use of the information is
added in the next patch.

Makefile.am

@@ -109,6 +109,7 @@ monitor_iwmon_LDADD = ell/libell-internal.la
 noinst_PROGRAMS = tools/hwsim
 noinst_PROGRAMS += tools/test-runner
 
+tools_hwsim_SOURCES = tools/hwsim.c src/util.h src/util.c src/mpdu.h
 tools_hwsim_LDADD = ell/libell-internal.la
 tools_test_runner_LDADD = ell/libell-internal.la
 

tools/hwsim.c

@@ -27,8 +27,14 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <getopt.h>
+#include <linux/if_ether.h>
+
 #include <ell/ell.h>
 
+#include "linux/nl80211.h"
+
+#include "src/util.h"
+
 enum {
 	HWSIM_CMD_UNSPEC,
 	HWSIM_CMD_REGISTER,
@@ -66,12 +72,15 @@ enum {
 };
 #define HWSIM_ATTR_MAX (__HWSIM_ATTR_MAX - 1)
 
+static struct l_genl *genl;
 static struct l_genl_family *hwsim;
+static struct l_genl_family *nl80211;
 
 static const char *options;
 static int exit_status;
 
 static enum action {
+	ACTION_NONE,
 	ACTION_CREATE,
 	ACTION_DESTROY,
 	ACTION_LIST,
@@ -148,24 +157,6 @@ done:
 	l_main_quit();
 }
 
-static void hwsim_config(struct l_genl_msg *msg, void *user_data)
-{
-	struct l_genl_attr attr;
-	uint16_t type, len;
-	const void *data;
-	uint8_t cmd;
-
-	cmd = l_genl_msg_get_command(msg);
-
-	l_debug("Config changed cmd %u", cmd);
-
-	if (!l_genl_attr_init(&attr, msg))
-		return;
-
-	while (l_genl_attr_next(&attr, &type, &len, &data))
-		l_debug("\tattr type %d len %d", type, len);
-}
-
 static void list_callback_done(void *user_data)
 {
 	l_main_quit();
@@ -248,6 +239,529 @@ static void list_callback(struct l_genl_msg *msg, void *user_data)
 		l_free(hwname);
 }
 
+struct radio_info_rec {
+	uint32_t id;
+	char alpha2[2];
+	bool p2p;
+	uint32_t regdom;
+	int channels;
+	bool ready;	/* Whether we have radio, wiphy and interface data */
+	char wiphy_name[0];
+};
+
+struct wiphy_info_rec {
+	uint32_t id;
+	char name[0];
+};
+
+struct interface_info_rec {
+	uint32_t id;
+	uint32_t wiphy_id;
+	uint8_t addr[ETH_ALEN];
+	char name[0];
+};
+
+static struct l_queue *radio_info;
+static struct l_queue *wiphy_info;
+static struct l_queue *interface_info;
+
+static void hwsim_radio_cache_cleanup(void)
+{
+	l_queue_destroy(radio_info, l_free);
+	l_queue_destroy(wiphy_info, l_free);
+	l_queue_destroy(interface_info, l_free);
+	radio_info = NULL;
+	wiphy_info = NULL;
+	interface_info = NULL;
+}
+
+static bool radio_info_match_id(const void *a, const void *b)
+{
+	const struct radio_info_rec *rec = a;
+	uint32_t id = L_PTR_TO_UINT(b);
+
+	return rec->id == id;
+}
+
+static bool radio_info_match_name(const void *a, const void *b)
+{
+	const struct radio_info_rec *rec = a;
+
+	return !strcmp(rec->wiphy_name, b);
+}
+
+static bool wiphy_info_match_id(const void *a, const void *b)
+{
+	const struct wiphy_info_rec *rec = a;
+	uint32_t id = L_PTR_TO_UINT(b);
+
+	return rec->id == id;
+}
+
+static bool wiphy_info_match_name(const void *a, const void *b)
+{
+	const struct wiphy_info_rec *rec = a;
+
+	return !strcmp(rec->name, b);
+}
+
+static bool interface_info_match_id(const void *a, const void *b)
+{
+	const struct interface_info_rec *rec = a;
+	uint32_t id = L_PTR_TO_UINT(b);
+
+	return rec->id == id;
+}
+
+static bool interface_info_match_wiphy_id(const void *a, const void *b)
+{
+	const struct interface_info_rec *rec = a;
+	uint32_t id = L_PTR_TO_UINT(b);
+
+	return rec->wiphy_id == id;
+}
+
+/*
+ * See if we have any radios that should become "ready", i.e. where matching
+ * wiphy or interface record was missing and is now available.
+ */
+static void process_new_radios(void)
+{
+	const struct l_queue_entry *radio_entry;
+
+	for (radio_entry = l_queue_get_entries(radio_info); radio_entry;
+			radio_entry = radio_entry->next) {
+		struct radio_info_rec *radio = radio_entry->data;
+		const struct wiphy_info_rec *wiphy;
+		const struct interface_info_rec *interface;
+
+		if (radio->ready)
+			continue;
+
+		wiphy = l_queue_find(wiphy_info, wiphy_info_match_name,
+					radio->wiphy_name);
+		if (!wiphy)
+			continue;
+
+		interface = l_queue_find(interface_info,
+						interface_info_match_wiphy_id,
+						L_UINT_TO_PTR(wiphy->id));
+		if (!interface)
+			continue;
+
+		radio->ready = true;
+
+		/* TODO: Create DBus object */
+		/* TODO: insert into address cache */
+	}
+}
+
+static void process_del_radio(struct radio_info_rec *radio)
+{
+	if (!radio->ready)
+		return;
+
+	radio->ready = false;
+
+	/* TODO: unregister DBus object */
+	/* TODO: remove from address cache */
+}
+
+static void get_radio_callback(struct l_genl_msg *msg, void *user_data)
+{
+	struct l_genl_attr attr;
+	uint16_t type, len;
+	const void *data;
+	const char *name = NULL;
+	const uint32_t *id = NULL;
+	size_t name_len = 0;
+	struct radio_info_rec *rec;
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	while (l_genl_attr_next(&attr, &type, &len, &data)) {
+		switch (type) {
+		case HWSIM_ATTR_RADIO_ID:
+			if (len != 4)
+				break;
+
+			id = data;
+			break;
+
+		case HWSIM_ATTR_RADIO_NAME:
+			name = data;
+			name_len = len;
+			break;
+		}
+	}
+
+	if (!id || !name)
+		return;
+
+	l_free(l_queue_remove_if(radio_info, radio_info_match_id,
+					L_UINT_TO_PTR(*id)));
+
+	rec = l_malloc(sizeof(struct radio_info_rec) + name_len + 1);
+	memset(rec, 0, sizeof(struct radio_info_rec) + name_len + 1);
+
+	rec->id = *id;
+	memcpy(rec->wiphy_name, name, name_len);
+
+	l_genl_attr_init(&attr, msg);
+
+	while (l_genl_attr_next(&attr, &type, &len, &data)) {
+		switch (type) {
+		case HWSIM_ATTR_CHANNELS:
+			if (len != 4)
+				break;
+
+			rec->channels = *(uint32_t *) data;
+			break;
+
+		case HWSIM_ATTR_REG_HINT_ALPHA2:
+			if (len != 2)
+				break;
+
+			memcpy(rec->alpha2, data, 2);
+
+			break;
+
+		case HWSIM_ATTR_SUPPORT_P2P_DEVICE:
+			rec->p2p = true;
+			break;
+
+		case HWSIM_ATTR_REG_CUSTOM_REG:
+			if (len != 4)
+				break;
+
+			rec->regdom = *(uint32_t *) data;
+			break;
+		}
+	}
+
+	if (!radio_info)
+		radio_info = l_queue_new();
+
+	l_queue_push_tail(radio_info, rec);
+
+	process_new_radios();
+}
+
+static void get_wiphy_callback(struct l_genl_msg *msg, void *user_data)
+{
+	struct l_genl_attr attr;
+	uint16_t type, len;
+	const void *data;
+	const char *name = NULL;
+	uint16_t name_len = 0;
+	const uint32_t *id = NULL;
+	struct wiphy_info_rec *rec;
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	while (l_genl_attr_next(&attr, &type, &len, &data)) {
+		switch (type) {
+		case NL80211_ATTR_WIPHY:
+			id = data;
+			break;
+		case NL80211_ATTR_WIPHY_NAME:
+			name = data;
+			name_len = len;
+			break;
+		}
+	}
+
+	if (!name || !id)
+		return;
+
+	l_free(l_queue_remove_if(wiphy_info, wiphy_info_match_id,
+					L_UINT_TO_PTR(*id)));
+
+	rec = l_malloc(sizeof(struct wiphy_info_rec) + name_len + 1);
+	memset(rec, 0, sizeof(struct wiphy_info_rec) + name_len + 1);
+
+	memcpy(rec->name, name, name_len);
+	rec->id = *id;
+
+	if (!wiphy_info)
+		wiphy_info = l_queue_new();
+
+	l_queue_push_tail(wiphy_info, rec);
+
+	process_new_radios();
+}
+
+static void get_interface_callback(struct l_genl_msg *msg, void *user_data)
+{
+	struct l_genl_attr attr;
+	uint16_t type, len;
+	const void *data;
+	const uint8_t *addr = NULL;
+	const uint32_t *wiphy_id = NULL;
+	const uint32_t *ifindex = NULL;
+	const char *ifname = NULL;
+	size_t ifname_len = 0;
+	struct interface_info_rec *rec;
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	while (l_genl_attr_next(&attr, &type, &len, &data)) {
+		switch (type) {
+		case NL80211_ATTR_MAC:
+			if (len != ETH_ALEN)
+				break;
+
+			addr = data;
+			break;
+
+		case NL80211_ATTR_WIPHY:
+			if (len != 4)
+				break;
+
+			wiphy_id = data;
+			break;
+
+		case NL80211_ATTR_IFINDEX:
+			if (len != 4)
+				break;
+
+			ifindex = data;
+			break;
+
+		case NL80211_ATTR_IFNAME:
+			ifname = data;
+			ifname_len = len;
+			break;
+		}
+	}
+
+	if (!addr || !wiphy_id || !ifindex || !ifname)
+		return;
+
+	l_free(l_queue_remove_if(interface_info, interface_info_match_id,
+					L_UINT_TO_PTR(*ifindex)));
+
+	rec = l_malloc(sizeof(struct interface_info_rec) + ifname_len + 1);
+	memset(rec, 0, sizeof(struct interface_info_rec) + ifname_len + 1);
+
+	rec->id = *ifindex;
+	rec->wiphy_id = *wiphy_id;
+	memcpy(rec->addr, addr, ETH_ALEN);
+	memcpy(rec->name, ifname, ifname_len);
+	rec->name[ifname_len] = '\0';
+
+	if (!interface_info)
+		interface_info = l_queue_new();
+
+	l_queue_push_tail(interface_info, rec);
+
+	process_new_radios();
+}
+
+static void del_radio_event(struct l_genl_msg *msg)
+{
+	struct radio_info_rec *radio;
+	struct l_genl_attr attr;
+	uint16_t type, len;
+	const void *data;
+	const uint32_t *id = NULL;
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	while (l_genl_attr_next(&attr, &type, &len, &data)) {
+		switch (type) {
+		case HWSIM_ATTR_RADIO_ID:
+			if (len != 4)
+				break;
+
+			id = data;
+
+			break;
+		}
+	}
+
+	if (!id)
+		return;
+
+	radio = l_queue_find(radio_info, radio_info_match_id,
+				L_UINT_TO_PTR(*id));
+	if (!radio)
+		return;
+
+	process_del_radio(radio);
+
+	l_free(radio);
+	l_queue_remove(radio_info, radio);
+}
+
+static void del_wiphy_event(struct l_genl_msg *msg)
+{
+	struct wiphy_info_rec *wiphy;
+	struct radio_info_rec *radio;
+	struct l_genl_attr attr;
+	uint16_t type, len;
+	const void *data;
+	uint32_t id;
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	if (!l_genl_attr_next(&attr, &type, &len, &data))
+		return;
+
+	if (type != NL80211_ATTR_WIPHY || len != 4)
+		return;
+
+	id = *((uint32_t *) data);
+
+	wiphy = l_queue_find(wiphy_info, wiphy_info_match_id,
+				L_UINT_TO_PTR(id));
+	if (!wiphy)
+		return;
+
+	radio = l_queue_find(radio_info, radio_info_match_name, wiphy->name);
+
+	if (radio)
+		process_del_radio(radio);
+
+	l_free(wiphy);
+	l_queue_remove(wiphy_info, wiphy);
+}
+
+static void del_interface_event(struct l_genl_msg *msg)
+{
+	struct interface_info_rec *interface;
+	struct wiphy_info_rec *wiphy;
+	struct radio_info_rec *radio;
+	struct l_genl_attr attr;
+	uint16_t type, len;
+	const void *data;
+	const uint32_t *ifindex = NULL;
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	while (l_genl_attr_next(&attr, &type, &len, &data)) {
+		switch (type) {
+		case NL80211_ATTR_IFINDEX:
+			if (len != 4)
+				break;
+
+			ifindex = data;
+			break;
+		}
+	}
+
+	if (!ifindex)
+		return;
+
+	interface = l_queue_find(interface_info, interface_info_match_id,
+					L_UINT_TO_PTR(*ifindex));
+	if (!interface)
+		return;
+
+	wiphy = l_queue_find(wiphy_info, wiphy_info_match_id,
+				L_UINT_TO_PTR(interface->wiphy_id));
+	if (wiphy)
+		radio = l_queue_find(radio_info, radio_info_match_name,
+					wiphy->name);
+	else
+		radio = NULL;
+
+	if (radio)
+		process_del_radio(radio);
+
+	l_free(interface);
+	l_queue_remove(interface_info, interface);
+}
+
+static void hwsim_config(struct l_genl_msg *msg, void *user_data)
+{
+	struct l_genl_attr attr;
+	uint16_t type, len;
+	const void *data;
+	uint8_t cmd;
+
+	cmd = l_genl_msg_get_command(msg);
+
+	l_debug("Config changed cmd %u", cmd);
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	while (l_genl_attr_next(&attr, &type, &len, &data))
+		l_debug("\tattr type %d len %d", type, len);
+
+	switch (cmd) {
+	case HWSIM_CMD_NEW_RADIO:
+		get_radio_callback(msg, NULL);
+		break;
+	case HWSIM_CMD_DEL_RADIO:
+		del_radio_event(msg);
+		break;
+	}
+}
+
+static void nl80211_config_notify(struct l_genl_msg *msg, void *user_data)
+{
+	uint8_t cmd;
+
+	cmd = l_genl_msg_get_command(msg);
+
+	l_debug("Notification of command %u", cmd);
+
+	switch (cmd) {
+	case NL80211_CMD_NEW_WIPHY:
+		get_wiphy_callback(msg, NULL);
+		break;
+	case NL80211_CMD_DEL_WIPHY:
+		del_wiphy_event(msg);
+		break;
+	case NL80211_CMD_NEW_INTERFACE:
+		get_interface_callback(msg, NULL);
+		break;
+	case NL80211_CMD_DEL_INTERFACE:
+		del_interface_event(msg);
+		break;
+	}
+}
+
+static void nl80211_ready(void *user_data)
+{
+	struct l_genl_msg *msg;
+
+	msg = l_genl_msg_new(NL80211_CMD_GET_WIPHY);
+	if (!l_genl_family_dump(nl80211, msg, get_wiphy_callback,
+				NULL, NULL)) {
+		l_error("Getting nl80211 wiphy information failed");
+		goto error;
+	}
+
+	msg = l_genl_msg_new(NL80211_CMD_GET_INTERFACE);
+	if (!l_genl_family_dump(nl80211, msg, get_interface_callback,
+				NULL, NULL)) {
+		l_error("Getting nl80211 interface information failed");
+		goto error;
+	}
+
+	if (!l_genl_family_register(nl80211, "config", nl80211_config_notify,
+					NULL, NULL)) {
+		l_error("Registering for nl80211 config notification "
+			"failed");
+		goto error;
+	}
+
+	return;
+
+error:
+	exit_status = EXIT_FAILURE;
+	l_main_quit();
+}
+
 static void hwsim_ready(void *user_data)
 {
 	struct l_genl_msg *msg;
@@ -331,11 +845,25 @@ static void hwsim_ready(void *user_data)
 
 		break;
 
-	default:
+	case ACTION_NONE:
-		l_main_quit();
+		msg = l_genl_msg_new(HWSIM_CMD_GET_RADIO);
+		if (!l_genl_family_dump(hwsim, msg, get_radio_callback,
+					NULL, NULL)) {
+			l_error("Getting hwsim radio information failed");
+			goto error;
+		}
+
+		l_genl_family_set_watches(nl80211, nl80211_ready, NULL,
+						NULL, NULL);
 
 		break;
 	}
+
+	return;
+
+error:
+	exit_status = EXIT_FAILURE;
+	l_main_quit();
 }
 
 static void hwsim_disappeared(void *user_data)
@@ -387,7 +915,6 @@ static const struct option main_options[] = {
 int main(int argc, char *argv[])
 {
 	struct l_signal *signal;
-	struct l_genl *genl;
 	sigset_t mask;
 	int actions = 0;
 
@@ -459,11 +986,6 @@ int main(int argc, char *argv[])
 		return EXIT_FAILURE;
 	}
 
-	if (!actions) {
-		fprintf(stderr, "No action has been specified\n");
-		return EXIT_FAILURE;
-	}
-
 	if (!l_main_init())
 		return EXIT_FAILURE;
 
@@ -495,15 +1017,27 @@ int main(int argc, char *argv[])
 		goto done;
 	}
 
+	nl80211 = l_genl_family_new(genl, NL80211_GENL_NAME);
+	if (!nl80211) {
+		fprintf(stderr, "Failed to create nl80211 genl family\n");
+		l_genl_family_unref(hwsim);
+		l_genl_unref(genl);
+		exit_status = EXIT_FAILURE;
+		goto done;
+	}
+
 	l_genl_family_set_watches(hwsim, hwsim_ready, hwsim_disappeared,
 					NULL, NULL);
 
+	exit_status = EXIT_SUCCESS;
+
 	l_main_run();
 
 	l_genl_family_unref(hwsim);
+	l_genl_family_unref(nl80211);
 	l_genl_unref(genl);
 
-	exit_status = EXIT_SUCCESS;
+	hwsim_radio_cache_cleanup();
 
 done:
 	l_signal_remove(signal);