wifi: Add SSIDs and frequencies to wpa_supplicant scan for fast scan

Lot of fixes by Jukka Rissanen <jukka.rissanen@linux.intel.com>

3 files changed, 341 insertions, 9 deletions

diff --git a/gsupplicant/gsupplicant.h b/gsupplicant/gsupplicant.h
index a68bba0a..49815e15 100644
--- a/gsupplicant/gsupplicant.h
+++ b/gsupplicant/gsupplicant.h
@@ -73,6 +73,8 @@ extern "C" {
 #define G_SUPPLICANT_PAIRWISE_TKIP	(1 << 1)
 #define G_SUPPLICANT_PAIRWISE_CCMP	(1 << 2)
 
+#define G_SUPPLICANT_MAX_FAST_SCAN	4
+
 typedef enum {
 	G_SUPPLICANT_MODE_UNKNOWN,
 	G_SUPPLICANT_MODE_INFRA,
@@ -131,6 +133,19 @@ struct _GSupplicantSSID {
 
 typedef struct _GSupplicantSSID GSupplicantSSID;
 
+struct _GSupplicantScanParams {
+	struct scan_ssid {
+		unsigned char ssid[32];
+		uint8_t ssid_len;
+	} ssids[G_SUPPLICANT_MAX_FAST_SCAN];
+
+	uint8_t num_ssids;
+
+	uint16_t freqs[G_SUPPLICANT_MAX_FAST_SCAN];
+};
+
+typedef struct _GSupplicantScanParams GSupplicantScanParams;
+
 /* global API */
 typedef void (*GSupplicantCountryCallback) (void *user_data);
 
@@ -155,6 +170,7 @@ int g_supplicant_interface_remove(GSupplicantInterface *interface,
 					GSupplicantInterfaceCallback callback,
 							void *user_data);
 int g_supplicant_interface_scan(GSupplicantInterface *interface,
+					GSupplicantScanParams *scan_data,
 					GSupplicantInterfaceCallback callback,
 							void *user_data);
 
diff --git a/gsupplicant/supplicant.c b/gsupplicant/supplicant.c
index 599abd23..183e341c 100644
--- a/gsupplicant/supplicant.c
+++ b/gsupplicant/supplicant.c
@@ -2181,6 +2181,13 @@ struct interface_connect_data {
 	void *user_data;
 };
 
+struct interface_scan_data {
+	GSupplicantInterface *interface;
+	GSupplicantInterfaceCallback callback;
+	GSupplicantScanParams *scan_params;
+	void *user_data;
+};
+
 static void interface_create_property(const char *key, DBusMessageIter *iter,
 							void *user_data)
 {
@@ -2436,9 +2443,11 @@ int g_supplicant_interface_remove(GSupplicantInterface *interface,
 static void interface_scan_result(const char *error,
 				DBusMessageIter *iter, void *user_data)
 {
-	struct interface_data *data = user_data;
+	struct interface_scan_data *data = user_data;
 
 	if (error != NULL) {
+		SUPPLICANT_DBG("error %s", error);
+
 		if (data->callback != NULL)
 			data->callback(-EIO, data->interface, data->user_data);
 	} else {
@@ -2446,27 +2455,137 @@ static void interface_scan_result(const char *error,
 		data->interface->scan_data = data->user_data;
 	}
 
+	if (data != NULL && data->scan_params != NULL)
+		g_free(data->scan_params);
+
 	dbus_free(data);
 }
 
+static void add_scan_frequency(DBusMessageIter *iter, unsigned int freq)
+{
+	DBusMessageIter data;
+	unsigned int width = 0; /* Not used by wpa_supplicant atm */
+
+	dbus_message_iter_open_container(iter, DBUS_TYPE_STRUCT, NULL, &data);
+
+	dbus_message_iter_append_basic(&data, DBUS_TYPE_UINT32, &freq);
+	dbus_message_iter_append_basic(&data, DBUS_TYPE_UINT32, &width);
+
+	dbus_message_iter_close_container(iter, &data);
+}
+
+static void add_scan_frequencies(DBusMessageIter *iter,
+						void *user_data)
+{
+	GSupplicantScanParams *scan_data = user_data;
+	unsigned int freq;
+	int i;
+
+	for (i = 0; i < G_SUPPLICANT_MAX_FAST_SCAN; i++) {
+		freq = scan_data->freqs[i];
+		if (!freq)
+			break;
+
+		add_scan_frequency(iter, freq);
+	}
+}
+
+static void append_ssid(DBusMessageIter *iter,
+			const void *ssid, unsigned int len)
+{
+	DBusMessageIter array;
+
+	dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY,
+	DBUS_TYPE_BYTE_AS_STRING, &array);
+
+	dbus_message_iter_append_fixed_array(&array, DBUS_TYPE_BYTE,
+								&ssid, len);
+	dbus_message_iter_close_container(iter, &array);
+}
+
+static void append_ssids(DBusMessageIter *iter, void *user_data)
+{
+	GSupplicantScanParams *scan_data = user_data;
+	int i;
+
+	for (i = 0; i < scan_data->num_ssids; i++)
+		append_ssid(iter, scan_data->ssids[i].ssid,
+					scan_data->ssids[i].ssid_len);
+}
+
+static void supplicant_add_scan_frequency(DBusMessageIter *dict,
+		supplicant_dbus_array_function function,
+					void *user_data)
+{
+	GSupplicantScanParams *scan_params = user_data;
+	DBusMessageIter entry, value, array;
+	const char *key = "Channels";
+
+	if (scan_params->freqs[0] != 0) {
+		dbus_message_iter_open_container(dict, DBUS_TYPE_DICT_ENTRY,
+						NULL, &entry);
+
+		dbus_message_iter_append_basic(&entry, DBUS_TYPE_STRING, &key);
+
+		dbus_message_iter_open_container(&entry, DBUS_TYPE_VARIANT,
+					DBUS_TYPE_ARRAY_AS_STRING
+					DBUS_STRUCT_BEGIN_CHAR_AS_STRING
+					DBUS_TYPE_UINT32_AS_STRING
+					DBUS_TYPE_UINT32_AS_STRING
+					DBUS_STRUCT_END_CHAR_AS_STRING,
+					&value);
+
+		dbus_message_iter_open_container(&value, DBUS_TYPE_ARRAY,
+					DBUS_STRUCT_BEGIN_CHAR_AS_STRING
+					DBUS_TYPE_UINT32_AS_STRING
+					DBUS_TYPE_UINT32_AS_STRING
+					DBUS_STRUCT_END_CHAR_AS_STRING,
+					&array);
+
+		if (function)
+			function(&array, user_data);
+
+		dbus_message_iter_close_container(&value, &array);
+		dbus_message_iter_close_container(&entry, &value);
+		dbus_message_iter_close_container(dict, &entry);
+	}
+}
+
 static void interface_scan_params(DBusMessageIter *iter, void *user_data)
 {
 	DBusMessageIter dict;
 	const char *type = "passive";
+	struct interface_scan_data *data = user_data;
 
 	supplicant_dbus_dict_open(iter, &dict);
 
-	supplicant_dbus_dict_append_basic(&dict, "Type",
-						DBUS_TYPE_STRING, &type);
+	if (data && data->scan_params) {
+		type = "active";
+
+		supplicant_dbus_dict_append_basic(&dict, "Type",
+					DBUS_TYPE_STRING, &type);
+
+		supplicant_dbus_dict_append_array(&dict, "SSIDs",
+						DBUS_TYPE_STRING,
+						append_ssids,
+						data->scan_params);
+
+		supplicant_add_scan_frequency(&dict, add_scan_frequencies,
+						data->scan_params);
+	} else
+		supplicant_dbus_dict_append_basic(&dict, "Type",
+					DBUS_TYPE_STRING, &type);
 
 	supplicant_dbus_dict_close(iter, &dict);
 }
 
 int g_supplicant_interface_scan(GSupplicantInterface *interface,
+				GSupplicantScanParams *scan_data,
 				GSupplicantInterfaceCallback callback,
 							void *user_data)
 {
-	struct interface_data *data;
+	struct interface_scan_data *data;
+	int ret;
 
 	if (interface == NULL)
 		return -EINVAL;
@@ -2499,10 +2618,16 @@ int g_supplicant_interface_scan(GSupplicantInterface *interface,
 	data->interface = interface;
 	data->callback = callback;
 	data->user_data = user_data;
+	data->scan_params = scan_data;
 
-	return supplicant_dbus_method_call(interface->path,
+	ret = supplicant_dbus_method_call(interface->path,
 			SUPPLICANT_INTERFACE ".Interface", "Scan",
 			interface_scan_params, interface_scan_result, data);
+
+	if (ret < 0)
+		dbus_free(data);
+
+	return ret;
 }
 
 static int parse_supplicant_error(DBusMessageIter *iter)
diff --git a/plugins/wifi.c b/plugins/wifi.c
index c392991e..a9d78710 100644
--- a/plugins/wifi.c
+++ b/plugins/wifi.c
@@ -26,6 +26,7 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <errno.h>
+#include <stdio.h>
 #include <string.h>
 #include <sys/ioctl.h>
 #include <sys/socket.h>
@@ -48,6 +49,7 @@
 #include <connman/technology.h>
 #include <connman/log.h>
 #include <connman/option.h>
+#include <connman/storage.h>
 
 #include <gsupplicant/gsupplicant.h>
 
@@ -282,6 +284,172 @@ static void scan_callback(int result, GSupplicantInterface *interface,
 	connman_device_unref(device);
 }
 
+static int add_scan_param(gchar *hex_ssid, int freq,
+			GSupplicantScanParams *scan_data,
+			int driver_max_scan_ssids)
+{
+	unsigned int i;
+
+	if (driver_max_scan_ssids > scan_data->num_ssids && hex_ssid != NULL) {
+		gchar *ssid;
+		unsigned int j = 0, hex;
+		size_t hex_ssid_len = strlen(hex_ssid);
+
+		ssid = g_try_malloc0(hex_ssid_len / 2);
+		if (ssid == NULL)
+			return -ENOMEM;
+
+		for (i = 0; i < hex_ssid_len; i += 2) {
+			sscanf(hex_ssid + i, "%02x", &hex);
+			ssid[j++] = hex;
+		}
+
+		memcpy(scan_data->ssids[scan_data->num_ssids].ssid, ssid, j);
+		scan_data->ssids[scan_data->num_ssids].ssid_len = j;
+		scan_data->num_ssids++;
+
+		g_free(ssid);
+	}
+
+	/* Don't add duplicate entries */
+	for (i = 0; i < G_SUPPLICANT_MAX_FAST_SCAN; i++) {
+		if (scan_data->freqs[i] == 0) {
+			scan_data->freqs[i] = freq;
+			break;
+		} else if (scan_data->freqs[i] == freq)
+			break;
+	}
+
+	return 0;
+}
+
+struct last_connected {
+	GTimeVal modified;
+	gchar *ssid;
+	int freq;
+};
+
+static gint sort_entry(gconstpointer a, gconstpointer b, gpointer user_data)
+{
+	GTimeVal *aval = (GTimeVal *)a;
+	GTimeVal *bval = (GTimeVal *)b;
+
+	/* Note that the sort order is descending */
+	if (aval->tv_sec < bval->tv_sec)
+		return 1;
+
+	if (aval->tv_sec > bval->tv_sec)
+		return -1;
+
+	return 0;
+}
+
+static void free_entry(gpointer data)
+{
+	struct last_connected *entry = data;
+
+	g_free(entry->ssid);
+	g_free(entry);
+}
+
+static int get_latest_connections(int max_ssids,
+				GSupplicantScanParams *scan_data)
+{
+	GSequenceIter *iter;
+	GSequence *latest_list;
+	struct last_connected *entry;
+	GKeyFile *keyfile;
+	GTimeVal modified;
+	gchar **services;
+	gchar *str;
+	char *ssid;
+	int i, freq;
+	int num_ssids = 0;
+
+	latest_list = g_sequence_new(free_entry);
+	if (latest_list == NULL)
+		return -ENOMEM;
+
+	services = connman_storage_get_services();
+	for (i = 0; services && services[i]; i++) {
+		keyfile = connman_storage_load_service(services[i]);
+
+		str = g_key_file_get_string(keyfile,
+					services[i], "Favorite", NULL);
+		if (str == NULL || g_strcmp0(str, "true")) {
+			if (str)
+				g_free(str);
+			g_key_file_free(keyfile);
+			continue;
+		}
+		g_free(str);
+
+		str = g_key_file_get_string(keyfile,
+					services[i], "AutoConnect", NULL);
+		if (str == NULL || g_strcmp0(str, "true")) {
+			if (str)
+				g_free(str);
+			g_key_file_free(keyfile);
+			continue;
+		}
+		g_free(str);
+
+		str = g_key_file_get_string(keyfile,
+					services[i], "Modified", NULL);
+		if (str != NULL) {
+			g_time_val_from_iso8601(str, &modified);
+			g_free(str);
+		}
+
+		ssid = g_key_file_get_string(keyfile,
+					services[i], "SSID", NULL);
+
+		freq = g_key_file_get_integer(keyfile, services[i],
+					"Frequency", NULL);
+		if (freq) {
+			entry = g_try_new(struct last_connected, 1);
+			if (entry == NULL) {
+				g_sequence_free(latest_list);
+				g_key_file_free(keyfile);
+				g_free(ssid);
+				return -ENOMEM;
+			}
+
+			entry->ssid = ssid;
+			entry->modified = modified;
+			entry->freq = freq;
+
+			g_sequence_insert_sorted(latest_list, entry,
+						sort_entry, NULL);
+			num_ssids++;
+		} else
+			g_free(ssid);
+
+		g_key_file_free(keyfile);
+	}
+
+	g_strfreev(services);
+
+	num_ssids = num_ssids > G_SUPPLICANT_MAX_FAST_SCAN ?
+		G_SUPPLICANT_MAX_FAST_SCAN : num_ssids;
+
+	iter = g_sequence_get_begin_iter(latest_list);
+
+	for (i = 0; i < num_ssids; i++) {
+		entry = g_sequence_get(iter);
+
+		DBG("ssid %s freq %d modified %lu", entry->ssid, entry->freq,
+						entry->modified.tv_sec);
+
+		add_scan_param(entry->ssid, entry->freq, scan_data, max_ssids);
+
+		iter = g_sequence_iter_next(iter);
+	}
+
+	g_sequence_free(latest_list);
+	return num_ssids;
+}
+
 static int wifi_scan(struct connman_device *device)
 {
 	struct wifi_data *wifi = connman_device_get_data(device);
@@ -293,8 +461,8 @@ static int wifi_scan(struct connman_device *device)
 		return 0;
 
 	connman_device_ref(device);
-	ret = g_supplicant_interface_scan(wifi->interface, scan_callback,
-								device);
+	ret = g_supplicant_interface_scan(wifi->interface, NULL,
+					scan_callback, device);
 	if (ret == 0)
 		connman_device_set_scanning(device, TRUE);
 	else
@@ -306,17 +474,40 @@ static int wifi_scan(struct connman_device *device)
 static int wifi_scan_fast(struct connman_device *device)
 {
 	struct wifi_data *wifi = connman_device_get_data(device);
+	GSupplicantScanParams *scan_params = NULL;
 	int ret;
+	int driver_max_ssids = 0;
 
 	DBG("device %p %p", device, wifi->interface);
 
 	if (wifi->tethering == TRUE)
 		return 0;
 
-	ret = g_supplicant_interface_scan(wifi->interface, scan_callback,
-									device);
+	driver_max_ssids = g_supplicant_interface_get_max_scan_ssids(
+							wifi->interface);
+	DBG("max ssids %d", driver_max_ssids);
+	if (driver_max_ssids == 0)
+		return wifi_scan(device);
+
+	scan_params = g_try_malloc0(sizeof(GSupplicantScanParams));
+	if (scan_params == NULL)
+		return -ENOMEM;
+
+	ret = get_latest_connections(driver_max_ssids, scan_params);
+	if (ret <= 0) {
+		g_free(scan_params);
+		return wifi_scan(device);
+	}
+
+	connman_device_ref(device);
+	ret = g_supplicant_interface_scan(wifi->interface, scan_params,
+						scan_callback, device);
 	if (ret == 0)
 		connman_device_set_scanning(device, TRUE);
+	else {
+		g_free(scan_params);
+		connman_device_unref(device);
+	}
 
 	return ret;
 }