[PATCH v6 1/8] core: device: add device_set_rssi_no_delta

[PATCH v6 1/8] core: device: add device_set_rssi_no_delta

[Jakub Pawlowski]

This patch adds new method that allow to update RSSI value without
taking delta into account. It will be used by StartFilteredDiscovery
method, in order to achieve more accurate proximity detection.
---
 src/device.c | 14 +++++++++++---
 src/device.h |  2 ++
 2 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/src/device.c b/src/device.c
index 8ad62d3..f0dded8 100644
--- a/src/device.c
+++ b/src/device.c
@@ -83,6 +83,8 @@
 #define MIN(a, b) ((a) < (b) ? (a) : (b))
 #endif
 
+#define RSSI_THRESHOLD		8
+
 static DBusConnection *dbus_conn = NULL;
 static unsigned service_state_cb_id;
 
@@ -4547,7 +4549,8 @@ void device_set_legacy(struct btd_device *device, bool legacy)
 					DEVICE_INTERFACE, "LegacyPairing");
 }
 
-void device_set_rssi(struct btd_device *device, int8_t rssi)
+void device_set_rssi_with_delta(struct btd_device *device, int8_t rssi,
+				int8_t delta_threshold)
 {
 	if (!device)
 		return;
@@ -4567,8 +4570,8 @@ void device_set_rssi(struct btd_device *device, int8_t rssi)
 		else
 			delta = rssi - device->rssi;
 
-		/* only report changes of 8 dBm or more */
-		if (delta < 8)
+		/* only report changes of delta_threshold dBm or more */
+		if (delta < delta_threshold)
 			return;
 
 		DBG("rssi %d delta %d", rssi, delta);
@@ -4580,6 +4583,11 @@ void device_set_rssi(struct btd_device *device, int8_t rssi)
 						DEVICE_INTERFACE, "RSSI");
 }
 
+void device_set_rssi(struct btd_device *device, int8_t rssi)
+{
+	device_set_rssi_with_delta(device, rssi, RSSI_THRESHOLD);
+}
+
 static gboolean start_discovery(gpointer user_data)
 {
 	struct btd_device *device = user_data;
diff --git a/src/device.h b/src/device.h
index b17f53a..d50fc84 100644
--- a/src/device.h
+++ b/src/device.h
@@ -90,6 +90,8 @@ void btd_device_set_temporary(struct btd_device *device, bool temporary);
 void btd_device_set_trusted(struct btd_device *device, gboolean trusted);
 void device_set_bonded(struct btd_device *device, uint8_t bdaddr_type);
 void device_set_legacy(struct btd_device *device, bool legacy);
+void device_set_rssi_with_delta(struct btd_device *device, int8_t rssi,
+				int8_t delta_threshold);
 void device_set_rssi(struct btd_device *device, int8_t rssi);
 bool btd_device_is_connected(struct btd_device *dev);
 uint8_t btd_device_get_bdaddr_type(struct btd_device *dev);
-- 
2.2.0.rc0.207.ga3a616c

[PATCH v6 2/8] core/adapter: Refactor of scan type

[Jakub Pawlowski]

This patch replaces scan type with defined constants, and creates
new method that might be used to get currently avaliable scan type.
---
 src/adapter.c | 28 ++++++++++++++++++++--------
 1 file changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/adapter.c b/src/adapter.c
index 6eeb2f9..23a9a81 100644
--- a/src/adapter.c
+++ b/src/adapter.c
@@ -88,6 +88,9 @@
 #define TEMP_DEV_TIMEOUT (3 * 60)
 #define BONDING_TIMEOUT (2 * 60)
 
+#define SCAN_TYPE_BREDR (1 << BDADDR_BREDR)
+#define SCAN_TYPE_LE ((1 << BDADDR_LE_PUBLIC) | (1 << BDADDR_LE_RANDOM))
+
 static DBusConnection *dbus_conn = NULL;
 
 static bool kernel_conn_control = false;
@@ -1185,7 +1188,7 @@ static gboolean passive_scanning_timeout(gpointer user_data)
 
 	adapter->passive_scan_timeout = 0;
 
-	cp.type = (1 << BDADDR_LE_PUBLIC) | (1 << BDADDR_LE_RANDOM);
+	cp.type = SCAN_TYPE_LE;
 
 	mgmt_send(adapter->mgmt, MGMT_OP_START_DISCOVERY,
 				adapter->dev_id, sizeof(cp), &cp,
@@ -1327,6 +1330,21 @@ static void cancel_passive_scanning(struct btd_adapter *adapter)
 	}
 }
 
+static uint8_t get_scan_type(struct btd_adapter *adapter)
+{
+	uint8_t type;
+
+	if (adapter->current_settings & MGMT_SETTING_BREDR)
+		type = SCAN_TYPE_BREDR;
+	else
+		type = 0;
+
+	if (adapter->current_settings & MGMT_SETTING_LE)
+		type |= SCAN_TYPE_LE;
+
+	return type;
+}
+
 static void trigger_start_discovery(struct btd_adapter *adapter, guint delay);
 
 static void start_discovery_complete(uint8_t status, uint16_t length,
@@ -1372,13 +1390,7 @@ static gboolean start_discovery_timeout(gpointer user_data)
 
 	adapter->discovery_idle_timeout = 0;
 
-	if (adapter->current_settings & MGMT_SETTING_BREDR)
-		new_type = (1 << BDADDR_BREDR);
-	else
-		new_type = 0;
-
-	if (adapter->current_settings & MGMT_SETTING_LE)
-		new_type |= (1 << BDADDR_LE_PUBLIC) | (1 << BDADDR_LE_RANDOM);
+	new_type = get_scan_type(adapter);
 
 	if (adapter->discovery_enable == 0x01) {
 		/*
-- 
2.2.0.rc0.207.ga3a616c

[PATCH v6 3/8] core: adapter: Add SetDiscoveryFilter method

[Jakub Pawlowski]

This patch adds SetDiscoveryFilter method, as described in
doc/adapter-api.txt to DBus interface. Method content wil follow in
next patches.
---
 src/adapter.c | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/adapter.c b/src/adapter.c
index 23a9a81..8bf23d1 100644
--- a/src/adapter.c
+++ b/src/adapter.c
@@ -1759,6 +1759,12 @@ static DBusMessage *start_discovery(DBusConnection *conn,
 	return dbus_message_new_method_return(msg);
 }
 
+static DBusMessage *set_discovery_filter(DBusConnection *conn,
+					 DBusMessage *msg, void *user_data)
+{
+	return btd_error_failed(msg, "Not implemented yet");
+}
+
 static DBusMessage *stop_discovery(DBusConnection *conn,
 					DBusMessage *msg, void *user_data)
 {
@@ -2331,6 +2337,9 @@ static DBusMessage *remove_device(DBusConnection *conn,
 
 static const GDBusMethodTable adapter_methods[] = {
 	{ GDBUS_METHOD("StartDiscovery", NULL, NULL, start_discovery) },
+	{ GDBUS_METHOD("SetDiscoveryFilter",
+		GDBUS_ARGS({ "properties", "a{sv}" }), NULL,
+		set_discovery_filter) },
 	{ GDBUS_METHOD("StopDiscovery", NULL, NULL, stop_discovery) },
 	{ GDBUS_ASYNC_METHOD("RemoveDevice",
 			GDBUS_ARGS({ "device", "o" }), NULL, remove_device) },
-- 
2.2.0.rc0.207.ga3a616c

[PATCH v6 4/8] core: adapter: Add parameter parsing to SetDiscoveryFilter

[Jakub Pawlowski]

This patch adds parameter parsing, and basic internal logic checks to
SetDiscoveryFilter method.
---
 src/adapter.c | 202 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 202 insertions(+)

diff --git a/src/adapter.c b/src/adapter.c
index 8bf23d1..fd2cd58 100644
--- a/src/adapter.c
+++ b/src/adapter.c
@@ -90,6 +90,10 @@
 
 #define SCAN_TYPE_BREDR (1 << BDADDR_BREDR)
 #define SCAN_TYPE_LE ((1 << BDADDR_LE_PUBLIC) | (1 << BDADDR_LE_RANDOM))
+#define SCAN_TYPE_DUAL (SCAN_TYPE_BREDR | SCAN_TYPE_LE)
+
+#define	DISTANCE_VAL_INVALID	0x7FFF
+#define	PATHLOSS_MAX 137
 
 static DBusConnection *dbus_conn = NULL;
 
@@ -144,6 +148,13 @@ struct conn_param {
 	uint16_t timeout;
 };
 
+struct discovery_filter {
+	uint8_t type;
+	uint16_t pathloss;
+	int16_t rssi;
+	GSList *uuids;
+};
+
 struct watch_client {
 	struct btd_adapter *adapter;
 	char *owner;
@@ -1759,9 +1770,200 @@ static DBusMessage *start_discovery(DBusConnection *conn,
 	return dbus_message_new_method_return(msg);
 }
 
+static bool parse_uuids(DBusMessageIter *value,
+					 GSList **uuids)
+{
+	DBusMessageIter arriter;
+
+	if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_ARRAY)
+		return false;
+
+	dbus_message_iter_recurse(value, &arriter);
+	while (dbus_message_iter_get_arg_type(&arriter) != DBUS_TYPE_INVALID) {
+		bt_uuid_t uuid, u128;
+		char uuidstr[MAX_LEN_UUID_STR + 1];
+		char *uuid_param;
+
+		if (dbus_message_iter_get_arg_type(&arriter) !=
+						DBUS_TYPE_STRING)
+			return false;
+
+		dbus_message_iter_get_basic(&arriter, &uuid_param);
+
+		if (bt_string_to_uuid(&uuid, uuid_param))
+			return false;
+
+		bt_uuid_to_uuid128(&uuid, &u128);
+		bt_uuid_to_string(&u128, uuidstr, sizeof(uuidstr));
+
+		*uuids = g_slist_prepend(*uuids, strdup(uuidstr));
+
+		dbus_message_iter_next(&arriter);
+	}
+
+	return true;
+}
+
+static bool parse_rssi(DBusMessageIter *value, int16_t *rssi)
+{
+	if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_INT16)
+		return false;
+
+	dbus_message_iter_get_basic(value, rssi);
+	/* -127 <= RSSI <= +20 (spec V4.2 [Vol 2, Part E] 7.7.65.2) */
+	if (*rssi > 20 || *rssi < -127)
+		return false;
+
+	return true;
+}
+
+static bool parse_pathloss(DBusMessageIter *value,
+					    uint16_t *pathloss)
+{
+	if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_UINT16)
+		return false;
+
+	dbus_message_iter_get_basic(value, pathloss);
+	/* pathloss filter must be smaller that PATHLOSS_MAX */
+	if (*pathloss > PATHLOSS_MAX)
+		return false;
+
+	return true;
+}
+
+static bool parse_transport(DBusMessageIter *value,
+					     uint8_t *transport)
+{
+	char *transport_str;
+
+	if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_STRING)
+		return false;
+
+	dbus_message_iter_get_basic(value, &transport_str);
+
+	if (!strcmp(transport_str, "bredr"))
+		*transport = SCAN_TYPE_BREDR;
+	else if (!strcmp(transport_str, "le"))
+		*transport = SCAN_TYPE_LE;
+	else if (!strcmp(transport_str, "auto"))
+		*transport = SCAN_TYPE_DUAL;
+	else
+		return false;
+
+	return true;
+}
+
+static bool parse_discovery_filter_entry(char *key, DBusMessageIter *value,
+					struct discovery_filter *filter)
+{
+	if (!strcmp("UUIDs", key))
+		return parse_uuids(value, &filter->uuids);
+
+	if (!strcmp("RSSI", key))
+		return parse_rssi(value, &filter->rssi);
+
+	if (!strcmp("Pathloss", key))
+		return parse_pathloss(value, &filter->pathloss);
+
+	if (!strcmp("Transport", key))
+		return parse_transport(value, &filter->type);
+
+	DBG("Unknown key parameter: %s!\n", key);
+	return false;
+}
+
+/* This method is responsible for parsing parameters to SetDiscoveryFilter. If
+ * filter in msg was empty, sets *filter to NULL. If whole parsing was
+ * successful, sets *filter to proper value.
+ * Returns false on any error, and true on success.
+ */
+static bool parse_discovery_filter_dict(struct discovery_filter **filter,
+					DBusMessage *msg)
+{
+	DBusMessageIter iter, subiter, dictiter, variantiter;
+	bool is_empty = true;
+
+	*filter = g_try_malloc(sizeof(**filter));
+	if (!*filter)
+		return false;
+
+	(*filter)->uuids = NULL;
+	(*filter)->pathloss = DISTANCE_VAL_INVALID;
+	(*filter)->rssi = DISTANCE_VAL_INVALID;
+	(*filter)->type = SCAN_TYPE_DUAL;
+
+	dbus_message_iter_init(msg, &iter);
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_ARRAY ||
+	    dbus_message_iter_get_element_type(&iter) != DBUS_TYPE_DICT_ENTRY)
+		return false;
+
+	dbus_message_iter_recurse(&iter, &subiter);
+	do {
+		int type = dbus_message_iter_get_arg_type(&subiter);
+		char *key;
+
+		if (type == DBUS_TYPE_INVALID)
+			break;
+
+		is_empty = false;
+		dbus_message_iter_recurse(&subiter, &dictiter);
+
+		dbus_message_iter_get_basic(&dictiter, &key);
+		if (!dbus_message_iter_next(&dictiter))
+			goto invalid_args;
+
+		if (dbus_message_iter_get_arg_type(&dictiter) !=
+							     DBUS_TYPE_VARIANT)
+			goto invalid_args;
+
+		dbus_message_iter_recurse(&dictiter, &variantiter);
+
+		if (!parse_discovery_filter_entry(key, &variantiter, *filter))
+			goto invalid_args;
+
+		dbus_message_iter_next(&subiter);
+	} while (true);
+
+	if (is_empty) {
+		g_free(*filter);
+		*filter = NULL;
+		return true;
+	}
+
+	/* only pathlos or rssi can be set, never both */
+	if ((*filter)->pathloss != DISTANCE_VAL_INVALID &&
+	    (*filter)->rssi != DISTANCE_VAL_INVALID)
+		goto invalid_args;
+
+	DBG("filtered discovery params: transport: %d rssi: %d pathloss: %d",
+	    (*filter)->type, (*filter)->rssi, (*filter)->pathloss);
+
+	return true;
+
+invalid_args:
+	g_slist_free_full((*filter)->uuids, g_free);
+	g_free(*filter);
+	*filter = NULL;
+	return false;
+}
+
 static DBusMessage *set_discovery_filter(DBusConnection *conn,
 					 DBusMessage *msg, void *user_data)
 {
+	struct btd_adapter *adapter = user_data;
+	struct discovery_filter *discovery_filter;
+
+	const char *sender = dbus_message_get_sender(msg);
+
+	DBG("sender %s", sender);
+
+	if (!(adapter->current_settings & MGMT_SETTING_POWERED))
+		return btd_error_not_ready(msg);
+
+	/* parse parameters */
+	if (!parse_discovery_filter_dict(&discovery_filter, msg))
+		return btd_error_invalid_args(msg);
+
 	return btd_error_failed(msg, "Not implemented yet");
 }
 
-- 
2.2.0.rc0.207.ga3a616c

[PATCH v6 5/8] core: adapter: set the filter for each discovery client

[Jakub Pawlowski]

This patch adds logic for setting, updating and deleting discovery
filter for each client.

Note that the filter is not used yet, making use from filter will
be added in following patches.
---
 src/adapter.c | 120 ++++++++++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 113 insertions(+), 7 deletions(-)

diff --git a/src/adapter.c b/src/adapter.c
index fd2cd58..4abafe6 100644
--- a/src/adapter.c
+++ b/src/adapter.c
@@ -159,6 +159,7 @@ struct watch_client {
 	struct btd_adapter *adapter;
 	char *owner;
 	guint watch;
+	struct discovery_filter *discovery_filter;
 };
 
 struct service_auth {
@@ -208,6 +209,9 @@ struct btd_adapter {
 	uint8_t discovery_enable;	/* discovery enabled/disabled */
 	bool discovery_suspended;	/* discovery has been suspended */
 	GSList *discovery_list;		/* list of discovery clients */
+	GSList *set_filter_list;	/* list of clients that specified
+					 * filter, but don't scan yet
+					 */
 	GSList *discovery_found;	/* list of found devices */
 	guint discovery_idle_timeout;	/* timeout between discovery runs */
 	guint passive_scan_timeout;	/* timeout between passive scans */
@@ -1356,6 +1360,15 @@ static uint8_t get_scan_type(struct btd_adapter *adapter)
 	return type;
 }
 
+static void free_discovery_filter(struct discovery_filter *discovery_filter)
+{
+	if (!discovery_filter)
+		return;
+
+	g_slist_free_full(discovery_filter->uuids, g_free);
+	g_free(discovery_filter);
+}
+
 static void trigger_start_discovery(struct btd_adapter *adapter, guint delay);
 
 static void start_discovery_complete(uint8_t status, uint16_t length,
@@ -1655,9 +1668,17 @@ static void discovery_destroy(void *user_data)
 
 	DBG("owner %s", client->owner);
 
+	adapter->set_filter_list = g_slist_remove(adapter->set_filter_list,
+								client);
+
 	adapter->discovery_list = g_slist_remove(adapter->discovery_list,
 								client);
 
+	if (client->discovery_filter) {
+		free_discovery_filter(client->discovery_filter);
+		client->discovery_filter = NULL;
+	}
+
 	g_free(client->owner);
 	g_free(client);
 
@@ -1694,6 +1715,9 @@ static void discovery_disconnect(DBusConnection *conn, void *user_data)
 
 	DBG("owner %s", client->owner);
 
+	adapter->set_filter_list = g_slist_remove(adapter->set_filter_list,
+								client);
+
 	adapter->discovery_list = g_slist_remove(adapter->discovery_list,
 								client);
 
@@ -1727,36 +1751,74 @@ static void discovery_disconnect(DBusConnection *conn, void *user_data)
 				stop_discovery_complete, adapter, NULL);
 }
 
+/* returns true if client was already discovering, false otherwise. *client
+ * will point to discovering client, or client that have pre-set his filter.
+ */
+static bool get_discovery_client(struct btd_adapter *adapter,
+				 const char *owner,
+				 struct watch_client **client)
+{
+	GSList *list = g_slist_find_custom(adapter->discovery_list, owner,
+				   compare_sender);
+	if (list) {
+		*client = list->data;
+		return true;
+	}
+
+	list = g_slist_find_custom(adapter->set_filter_list, owner,
+				   compare_sender);
+	if (list) {
+		*client = list->data;
+		return false;
+	}
+
+	*client = NULL;
+	return false;
+}
+
 static DBusMessage *start_discovery(DBusConnection *conn,
 					DBusMessage *msg, void *user_data)
 {
 	struct btd_adapter *adapter = user_data;
 	const char *sender = dbus_message_get_sender(msg);
 	struct watch_client *client;
-	GSList *list;
+	bool is_discovering;
 
 	DBG("sender %s", sender);
 
 	if (!(adapter->current_settings & MGMT_SETTING_POWERED))
 		return btd_error_not_ready(msg);
 
+	is_discovering = get_discovery_client(adapter, sender, &client);
+
 	/*
 	 * Every client can only start one discovery, if the client
 	 * already started a discovery then return an error.
 	 */
-	list = g_slist_find_custom(adapter->discovery_list, sender,
-						compare_sender);
-	if (list)
+	if (is_discovering)
 		return btd_error_busy(msg);
 
+	/*
+	 * If there was pre-set filter, just reconnect it to discovery_list,
+	 * and trigger scan.
+	 */
+	if (client) {
+		adapter->set_filter_list = g_slist_remove(
+					     adapter->set_filter_list, client);
+		adapter->discovery_list = g_slist_prepend(
+					     adapter->discovery_list, client);
+		trigger_start_discovery(adapter, 0);
+		return dbus_message_new_method_return(msg);
+	}
+
 	client = g_new0(struct watch_client, 1);
 
 	client->adapter = adapter;
 	client->owner = g_strdup(sender);
+	client->discovery_filter = NULL;
 	client->watch = g_dbus_add_disconnect_watch(dbus_conn, sender,
 						discovery_disconnect, client,
 						discovery_destroy);
-
 	adapter->discovery_list = g_slist_prepend(adapter->discovery_list,
 								client);
 
@@ -1951,9 +2013,10 @@ static DBusMessage *set_discovery_filter(DBusConnection *conn,
 					 DBusMessage *msg, void *user_data)
 {
 	struct btd_adapter *adapter = user_data;
+	struct watch_client *client;
 	struct discovery_filter *discovery_filter;
-
 	const char *sender = dbus_message_get_sender(msg);
+	bool is_discovering;
 
 	DBG("sender %s", sender);
 
@@ -1964,7 +2027,38 @@ static DBusMessage *set_discovery_filter(DBusConnection *conn,
 	if (!parse_discovery_filter_dict(&discovery_filter, msg))
 		return btd_error_invalid_args(msg);
 
-	return btd_error_failed(msg, "Not implemented yet");
+	is_discovering = get_discovery_client(adapter, sender, &client);
+
+	if (client) {
+		free_discovery_filter(client->discovery_filter);
+		client->discovery_filter = discovery_filter;
+
+		if (discovery_filter || is_discovering)
+			return dbus_message_new_method_return(msg);
+
+		/* Removing pre-set filter */
+		adapter->set_filter_list = g_slist_remove(
+					      adapter->set_filter_list,
+					      client);
+		g_free(client->owner);
+		g_free(client);
+		DBG("successfully cleared pre-set filter");
+	} else if (discovery_filter) {
+		/* Client pre-setting his filter for first time */
+		client = g_new0(struct watch_client, 1);
+		client->adapter = adapter;
+		client->owner = g_strdup(sender);
+		client->discovery_filter = discovery_filter;
+		client->watch = g_dbus_add_disconnect_watch(dbus_conn, sender,
+						discovery_disconnect, client,
+						discovery_destroy);
+		adapter->set_filter_list = g_slist_prepend(
+					     adapter->set_filter_list, client);
+
+		DBG("successfully pre-set filter");
+	}
+
+	return dbus_message_new_method_return(msg);
 }
 
 static DBusMessage *stop_discovery(DBusConnection *conn,
@@ -5189,6 +5283,18 @@ static void adapter_stop(struct btd_adapter *adapter)
 
 	cancel_passive_scanning(adapter);
 
+	while (adapter->set_filter_list) {
+		struct watch_client *client;
+
+		client = adapter->set_filter_list->data;
+
+		/* g_dbus_remove_watch will remove the client from the
+		 * adapter's list and free it using the discovery_destroy
+		 * function.
+		 */
+		g_dbus_remove_watch(dbus_conn, client->watch);
+	}
+
 	while (adapter->discovery_list) {
 		struct watch_client *client;
 
-- 
2.2.0.rc0.207.ga3a616c

[PATCH v6 6/8] core: adapter: start proper discovery depending on filter type

[Jakub Pawlowski]

This patch implement starting and updating filtered discovery,
depending on kind of filters used by each client. It also removes
iddle timeout for filtered scans, to make sure that this kind of
scan will run continuously.
---
 src/adapter.c | 291 +++++++++++++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 269 insertions(+), 22 deletions(-)

diff --git a/src/adapter.c b/src/adapter.c
index 4abafe6..fe9530c 100644
--- a/src/adapter.c
+++ b/src/adapter.c
@@ -92,6 +92,7 @@
 #define SCAN_TYPE_LE ((1 << BDADDR_LE_PUBLIC) | (1 << BDADDR_LE_RANDOM))
 #define SCAN_TYPE_DUAL (SCAN_TYPE_BREDR | SCAN_TYPE_LE)
 
+#define	HCI_RSSI_INVALID	127
 #define	DISTANCE_VAL_INVALID	0x7FFF
 #define	PATHLOSS_MAX 137
 
@@ -205,6 +206,9 @@ struct btd_adapter {
 	char *stored_alias;		/* stored adapter name alias */
 
 	bool discovering;		/* discovering property state */
+	bool filtered_discovery;	/* we are doing filtered discovery */
+	bool no_scan_restart_delay;	/* when this flag is set, restart scan
+					 * without delay */
 	uint8_t discovery_type;		/* current active discovery type */
 	uint8_t discovery_enable;	/* discovery enabled/disabled */
 	bool discovery_suspended;	/* discovery has been suspended */
@@ -212,6 +216,9 @@ struct btd_adapter {
 	GSList *set_filter_list;	/* list of clients that specified
 					 * filter, but don't scan yet
 					 */
+	/* current discovery filter, if any */
+	struct mgmt_cp_start_service_discovery *current_discovery_filter;
+
 	GSList *discovery_found;	/* list of found devices */
 	guint discovery_idle_timeout;	/* timeout between discovery runs */
 	guint passive_scan_timeout;	/* timeout between passive scans */
@@ -1388,6 +1395,11 @@ static void start_discovery_complete(uint8_t status, uint16_t length,
 		adapter->discovery_type = rp->type;
 		adapter->discovery_enable = 0x01;
 
+		if (adapter->current_discovery_filter)
+			adapter->filtered_discovery = true;
+		else
+			adapter->filtered_discovery = false;
+
 		if (adapter->discovering)
 			return;
 
@@ -1407,21 +1419,32 @@ static void start_discovery_complete(uint8_t status, uint16_t length,
 static gboolean start_discovery_timeout(gpointer user_data)
 {
 	struct btd_adapter *adapter = user_data;
-	struct mgmt_cp_start_discovery cp;
+	struct mgmt_cp_start_service_discovery *sd_cp;
 	uint8_t new_type;
 
 	DBG("");
 
 	adapter->discovery_idle_timeout = 0;
 
+	/* If we're doing filtered discovery, it must be quickly restarted */
+	adapter->no_scan_restart_delay = !!adapter->current_discovery_filter;
+
+	DBG("adapter->current_discovery_filter == %d",
+	    !!adapter->current_discovery_filter);
+
 	new_type = get_scan_type(adapter);
 
 	if (adapter->discovery_enable == 0x01) {
+		struct mgmt_cp_stop_discovery cp;
+
 		/*
-		 * If there is an already running discovery and it has the
-		 * same type, then just keep it.
+		 * If we're asked to start regular discovery, and there is an
+		 *  already running regular discovery and it has the same type,
+		 * then just keep it.
 		 */
-		if (adapter->discovery_type == new_type) {
+		if (!adapter->current_discovery_filter &&
+		    !adapter->filtered_discovery &&
+		    adapter->discovery_type == new_type) {
 			if (adapter->discovering)
 				return FALSE;
 
@@ -1436,20 +1459,43 @@ static gboolean start_discovery_timeout(gpointer user_data)
 		 * queue up a stop discovery command.
 		 *
 		 * This can happen if a passive scanning for Low Energy
-		 * devices is ongoing.
+		 * devices is ongoing, or scan type is changed between
+		 * regular and filtered, or filter was updated.
 		 */
 		cp.type = adapter->discovery_type;
-
 		mgmt_send(adapter->mgmt, MGMT_OP_STOP_DISCOVERY,
 					adapter->dev_id, sizeof(cp), &cp,
 					NULL, NULL, NULL);
+
+		/* Don't even bother to try to quickly start discovery
+		 * just after stopping it, it would fail with status
+		 * MGMT_BUSY. Instead discovering_callback will take
+		 * care of that.
+		 */
+		return FALSE;
+
 	}
 
-	cp.type = new_type;
+	/* Regular discovery is required */
+	if (!adapter->current_discovery_filter) {
+		struct mgmt_cp_start_discovery cp;
 
-	mgmt_send(adapter->mgmt, MGMT_OP_START_DISCOVERY,
+		cp.type = new_type;
+		mgmt_send(adapter->mgmt, MGMT_OP_START_DISCOVERY,
 				adapter->dev_id, sizeof(cp), &cp,
 				start_discovery_complete, adapter, NULL);
+		return FALSE;
+	}
+
+	/* Filtered discovery is required */
+	sd_cp = adapter->current_discovery_filter;
+
+	DBG("sending MGMT_OP_START_SERVICE_DISCOVERY %d, %d, %d",
+	    sd_cp->rssi, sd_cp->type, sd_cp->uuid_count);
+
+	mgmt_send(adapter->mgmt, MGMT_OP_START_SERVICE_DISCOVERY,
+		  adapter->dev_id, sizeof(*sd_cp) + sd_cp->uuid_count * 16,
+		  sd_cp, start_discovery_complete, adapter, NULL);
 
 	return FALSE;
 }
@@ -1561,8 +1607,8 @@ static void discovering_callback(uint16_t index, uint16_t length,
 		return;
 	}
 
-	DBG("hci%u type %u discovering %u", adapter->dev_id,
-					ev->type, ev->discovering);
+	DBG("hci%u type %u discovering %u method %d", adapter->dev_id, ev->type,
+				ev->discovering, adapter->filtered_discovery);
 
 	if (adapter->discovery_enable == ev->discovering)
 		return;
@@ -1588,7 +1634,10 @@ static void discovering_callback(uint16_t index, uint16_t length,
 
 	switch (adapter->discovery_enable) {
 	case 0x00:
-		trigger_start_discovery(adapter, IDLE_DISCOV_TIMEOUT);
+		if (adapter->no_scan_restart_delay)
+			trigger_start_discovery(adapter, 0);
+		else
+			trigger_start_discovery(adapter, IDLE_DISCOV_TIMEOUT);
 		break;
 
 	case 0x01:
@@ -1596,6 +1645,7 @@ static void discovering_callback(uint16_t index, uint16_t length,
 			g_source_remove(adapter->discovery_idle_timeout);
 			adapter->discovery_idle_timeout = 0;
 		}
+
 		break;
 	}
 }
@@ -1610,7 +1660,8 @@ static void stop_discovery_complete(uint8_t status, uint16_t length,
 	if (status == MGMT_STATUS_SUCCESS) {
 		adapter->discovery_type = 0x00;
 		adapter->discovery_enable = 0x00;
-
+		adapter->filtered_discovery = false;
+		adapter->no_scan_restart_delay = false;
 		adapter->discovering = false;
 		g_dbus_emit_property_changed(dbus_conn, adapter->path,
 					ADAPTER_INTERFACE, "Discovering");
@@ -1661,6 +1712,193 @@ static gboolean remove_temp_devices(gpointer user_data)
 	return FALSE;
 }
 
+static gint g_strcmp(gconstpointer a, gconstpointer b)
+{
+	return strcmp(a, b);
+}
+
+static void extract_unique_uuids(gpointer data, gpointer user_data)
+{
+	char *uuid_str = data;
+	GSList **uuids = user_data;
+
+	if (!g_slist_find_custom(*uuids, uuid_str, g_strcmp))
+		*uuids = g_slist_insert_sorted(*uuids, uuid_str, g_strcmp);
+}
+
+/*
+ * This method merges all adapter filters into one that will be send to kernel.
+ * cp_ptr is set to null when regular non-filtered discovery is needed,
+ * otherwise it's pointing to filter. Returns 0 on succes, -1 on error
+ */
+static int discovery_filter_to_mgmt_cp(struct btd_adapter *adapter,
+			       struct mgmt_cp_start_service_discovery **cp_ptr)
+{
+	GSList *l, *uuids = NULL;
+	struct mgmt_cp_start_service_discovery *cp;
+	int rssi = DISTANCE_VAL_INVALID;
+	int uuid_count;
+	uint8_t discovery_type = 0;
+	uint8_t (*current_uuid)[16];
+	bool empty_uuid = false;
+	bool has_regular_discovery = false;
+	bool has_filtered_discovery = false;
+
+	DBG("");
+
+	for (l = adapter->discovery_list; l != NULL; l = g_slist_next(l)) {
+		struct watch_client *client = l->data;
+		struct discovery_filter *item = client->discovery_filter;
+
+		if (!item) {
+			has_regular_discovery = true;
+			continue;
+		}
+
+		has_filtered_discovery = true;
+
+		discovery_type |= item->type;
+
+		/*
+		 * Rule for merging rssi and pathloss into rssi field of kernel
+		 * filter is as follow:
+		 * - if there's any client without proximity filter, then do no
+		 *   proximity filtering,
+		 * - if all clients specified RSSI, then use lowest value,
+		 * - if any client specified pathloss, then kernel filter should
+		 *   do no proximity, as kernel can't compute pathloss. We'll do
+		 *   filtering on our own.
+		 */
+		if (item->rssi == DISTANCE_VAL_INVALID)
+			rssi = HCI_RSSI_INVALID;
+		else if (rssi != HCI_RSSI_INVALID && rssi >= item->rssi)
+			rssi = item->rssi;
+		else if (item->pathloss != DISTANCE_VAL_INVALID)
+			rssi = HCI_RSSI_INVALID;
+
+		if (!g_slist_length(item->uuids))
+			empty_uuid = true;
+
+		g_slist_foreach(item->uuids, extract_unique_uuids, &uuids);
+	}
+
+	/* If no proximity filtering is set, disable it */
+	if (rssi == DISTANCE_VAL_INVALID)
+		rssi = HCI_RSSI_INVALID;
+
+	/*
+	 * Empty_uuid variable determines wether there was any filter with no
+	 * uuids. In this case someone might be looking for all devices in
+	 * certain proximity, and we need to have empty uuids in kernel filter.
+	 */
+	if (empty_uuid) {
+		g_slist_free(uuids);
+		uuids = NULL;
+	}
+
+	if (has_regular_discovery) {
+		/*
+		 * It there is both regular and filtered scan running, then
+		 * clear whole fitler to report all devices. If there are only
+		 * regular scans, run just regular scan.
+		 */
+		if (has_filtered_discovery) {
+			discovery_type = get_scan_type(adapter);
+			rssi = HCI_RSSI_INVALID;
+			g_slist_free(uuids);
+			uuids = NULL;
+		} else {
+			*cp_ptr = NULL;
+			return 0;
+		}
+	}
+
+	uuid_count = g_slist_length(uuids);
+
+	cp = g_try_malloc(sizeof(cp) + 16*uuid_count);
+	*cp_ptr = cp;
+	if (!cp)
+		return -1;
+
+	cp->type = discovery_type;
+	cp->rssi = rssi;
+	cp->uuid_count = uuid_count;
+
+	current_uuid = cp->uuids;
+
+	for (l = uuids; l != NULL; l = g_slist_next(l)) {
+		bt_uuid_t uuid, u128;
+		uint128_t uint128;
+
+		bt_string_to_uuid(&uuid, l->data);
+		bt_uuid_to_uuid128(&uuid, &u128);
+
+		ntoh128((uint128_t *) u128.value.u128.data, &uint128);
+		htob128(&uint128, (uint128_t *) current_uuid);
+
+		current_uuid++;
+	}
+
+	g_slist_free(uuids);
+	return 0;
+}
+
+static bool filters_equal(struct mgmt_cp_start_service_discovery *a,
+		   struct mgmt_cp_start_service_discovery *b) {
+	if (!a && !b)
+		return true;
+
+	if ((!a && b) || (a && !b))
+		return false;
+
+	if (a->type != b->type)
+		return false;
+
+	if (a->rssi != b->rssi)
+		return false;
+
+	/*
+	 * When we create mgmt_cp_start_service_discovery structure inside
+	 * discovery_filter_to_mgmt_cp, we always keep uuids sorted, and
+	 * unique, so we're safe to compare uuid_count, and uuids like that.
+	 */
+	if (a->uuid_count != b->uuid_count)
+		return false;
+
+	if (memcmp(a->uuids, b->uuids, 16 * a->uuid_count) != 0)
+		return false;
+
+	return true;
+}
+
+static void update_discovery_filter(struct btd_adapter *adapter)
+{
+	struct mgmt_cp_start_service_discovery *sd_cp;
+
+	DBG("");
+
+	if (discovery_filter_to_mgmt_cp(adapter, &sd_cp)) {
+		error("discovery_filter_to_mgmt_cp returned error");
+		return;
+	}
+
+	/*
+	 * If filters are equal, then don't update scan, except for when
+	 * starting discovery.
+	 */
+	if (filters_equal(adapter->current_discovery_filter, sd_cp) &&
+	    adapter->discovering != 0) {
+		DBG("filters were equal, deciding to not restart the scan.");
+		g_free(sd_cp);
+		return;
+	}
+
+	g_free(adapter->current_discovery_filter);
+	adapter->current_discovery_filter = sd_cp;
+
+	trigger_start_discovery(adapter, 0);
+}
+
 static void discovery_destroy(void *user_data)
 {
 	struct watch_client *client = user_data;
@@ -1728,8 +1966,10 @@ static void discovery_disconnect(DBusConnection *conn, void *user_data)
 	 * However in case this is the last client, the discovery in
 	 * the kernel needs to be disabled.
 	 */
-	if (adapter->discovery_list)
+	if (adapter->discovery_list) {
+		update_discovery_filter(adapter);
 		return;
+	}
 
 	/*
 	 * In the idle phase of a discovery, there is no need to stop it
@@ -1751,7 +1991,8 @@ static void discovery_disconnect(DBusConnection *conn, void *user_data)
 				stop_discovery_complete, adapter, NULL);
 }
 
-/* returns true if client was already discovering, false otherwise. *client
+/*
+ * Return true if client was already discovering, false otherwise. *client
  * will point to discovering client, or client that have pre-set his filter.
  */
 static bool get_discovery_client(struct btd_adapter *adapter,
@@ -1806,8 +2047,8 @@ static DBusMessage *start_discovery(DBusConnection *conn,
 		adapter->set_filter_list = g_slist_remove(
 					     adapter->set_filter_list, client);
 		adapter->discovery_list = g_slist_prepend(
-					     adapter->discovery_list, client);
-		trigger_start_discovery(adapter, 0);
+					      adapter->discovery_list, client);
+		update_discovery_filter(adapter);
 		return dbus_message_new_method_return(msg);
 	}
 
@@ -1827,7 +2068,7 @@ static DBusMessage *start_discovery(DBusConnection *conn,
 	 * discovery in idle phase exists, it will be restarted right
 	 * away.
 	 */
-	trigger_start_discovery(adapter, 0);
+	update_discovery_filter(adapter);
 
 	return dbus_message_new_method_return(msg);
 }
@@ -2033,6 +2274,9 @@ static DBusMessage *set_discovery_filter(DBusConnection *conn,
 		free_discovery_filter(client->discovery_filter);
 		client->discovery_filter = discovery_filter;
 
+		if (is_discovering)
+			update_discovery_filter(adapter);
+
 		if (discovery_filter || is_discovering)
 			return dbus_message_new_method_return(msg);
 
@@ -2090,12 +2334,10 @@ static DBusMessage *stop_discovery(DBusConnection *conn,
 	 */
 	g_dbus_remove_watch(dbus_conn, client->watch);
 
-	/*
-	 * As long as other discovery clients are still active, just
-	 * return success.
-	 */
-	if (adapter->discovery_list)
+	if (adapter->discovery_list) {
+		update_discovery_filter(adapter);
 		return dbus_message_new_method_return(msg);
+	}
 
 	/*
 	 * In the idle phase of a discovery, there is no need to stop it
@@ -5307,6 +5549,11 @@ static void adapter_stop(struct btd_adapter *adapter)
 		g_dbus_remove_watch(dbus_conn, client->watch);
 	}
 
+	adapter->filtered_discovery = false;
+	adapter->no_scan_restart_delay = false;
+	g_free(adapter->current_discovery_filter);
+	adapter->current_discovery_filter = NULL;
+
 	adapter->discovering = false;
 
 	while (adapter->connections) {
-- 
2.2.0.rc0.207.ga3a616c

[PATCH v6 7/8] core: adapter: filter discovery results when filered discovery is used

[Jakub Pawlowski]

This patch adds filtering to device found event, and enforces lack
of RSSI delta for filtered scan.

When filtering, we compare against each client filter. That
additionally removes some of reports.
---
 src/adapter.c | 69 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 68 insertions(+), 1 deletion(-)

diff --git a/src/adapter.c b/src/adapter.c
index fe9530c..ee07a9e 100644
--- a/src/adapter.c
+++ b/src/adapter.c
@@ -5292,6 +5292,63 @@ static void adapter_msd_notify(struct btd_adapter *adapter,
 	}
 }
 
+static bool is_filter_match(GSList *discovery_filter, struct eir_data *eir_data,
+								int8_t rssi)
+{
+	GSList *l, *m;
+	bool got_match = false;
+
+	for (l = discovery_filter; l != NULL && got_match != true;
+							l = g_slist_next(l)) {
+		struct watch_client *client = l->data;
+		struct discovery_filter *item = client->discovery_filter;
+
+		/*
+		 * If one of currently running scans is regular scan, then
+		 * return all devices as matches
+		 */
+		if (!item) {
+			got_match = true;
+			continue;
+		}
+
+		/* if someone started discovery with empty uuids, he wants all
+		 * devices in given proximity.
+		 */
+		if (!item->uuids)
+			got_match = true;
+		else
+			for (m = item->uuids; m != NULL && got_match != true;
+							   m = g_slist_next(m))
+				/* m->data contains string representation of
+				 * uuid.
+				 */
+				if (g_slist_find_custom(eir_data->services,
+							m->data,
+							g_strcmp) != NULL)
+					got_match = true;
+
+		if (got_match) {
+			/* we have service match, check proximity */
+			if (item->rssi != DISTANCE_VAL_INVALID &&
+			    item->rssi > rssi)
+				got_match = false;
+			if (item->pathloss != DISTANCE_VAL_INVALID &&
+			   (eir_data->tx_power == 127 ||
+			   eir_data->tx_power - rssi > item->pathloss))
+				got_match = false;
+
+			if (got_match)
+				return true;
+		}
+	}
+
+	if (!got_match)
+		return false;
+
+	return true;
+}
+
 static void update_found_devices(struct btd_adapter *adapter,
 					const bdaddr_t *bdaddr,
 					uint8_t bdaddr_type, int8_t rssi,
@@ -5358,8 +5415,18 @@ static void update_found_devices(struct btd_adapter *adapter,
 		return;
 	}
 
+	if (adapter->filtered_discovery &&
+	    !is_filter_match(adapter->discovery_list, &eir_data, rssi)) {
+		eir_data_free(&eir_data);
+		return;
+	}
+
 	device_set_legacy(dev, legacy);
-	device_set_rssi(dev, rssi);
+
+	if (adapter->filtered_discovery)
+		device_set_rssi_with_delta(dev, rssi, 0);
+	else
+		device_set_rssi(dev, rssi);
 
 	if (eir_data.appearance != 0)
 		device_set_appearance(dev, eir_data.appearance);
-- 
2.2.0.rc0.207.ga3a616c

[PATCH v6 8/8] client: main: add support for SetDiscoveryFilter

[Jakub Pawlowski]

This patch adds filtered-scan command to sample DBus client that might
be used to call SetDiscoveryFilter.
---
 client/main.c | 244 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 244 insertions(+)

diff --git a/client/main.c b/client/main.c
index 4360930..3a39a73 100644
--- a/client/main.c
+++ b/client/main.c
@@ -896,6 +896,248 @@ static void cmd_scan(const char *arg)
 	}
 }
 
+static void append_variant(DBusMessageIter *iter, int type, void *val)
+{
+	DBusMessageIter value;
+	char sig[2] = { type, '\0' };
+
+	dbus_message_iter_open_container(iter, DBUS_TYPE_VARIANT, sig, &value);
+
+	dbus_message_iter_append_basic(&value, type, val);
+
+	dbus_message_iter_close_container(iter, &value);
+}
+
+static void dict_append_entry(DBusMessageIter *dict, const char *key,
+							int type, void *val)
+{
+	DBusMessageIter entry;
+
+	if (type == DBUS_TYPE_STRING) {
+		const char *str = *((const char **) val);
+
+		if (str == NULL)
+			return;
+	}
+
+	dbus_message_iter_open_container(dict, DBUS_TYPE_DICT_ENTRY,
+							NULL, &entry);
+
+	dbus_message_iter_append_basic(&entry, DBUS_TYPE_STRING, &key);
+
+	append_variant(&entry, type, val);
+
+	dbus_message_iter_close_container(dict, &entry);
+}
+
+#define	DISTANCE_VAL_INVALID	0x7FFF
+
+struct set_discovery_filter_args {
+	char *transport;
+	dbus_uint16_t rssi;
+	dbus_int16_t pathloss;
+	GList *uuids;
+};
+
+static void set_discovery_filter_setup(DBusMessageIter *iter,
+					   void *user_data)
+{
+	struct set_discovery_filter_args *args = user_data;
+	DBusMessageIter dict;
+
+	dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY,
+			    DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
+			    DBUS_TYPE_STRING_AS_STRING
+			    DBUS_TYPE_VARIANT_AS_STRING
+			    DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);
+
+	if (args->uuids != NULL) {
+		DBusMessageIter entry, value, arrayIter;
+		char *uuids = "UUIDs";
+		GList *list;
+
+		dbus_message_iter_open_container(&dict, DBUS_TYPE_DICT_ENTRY,
+						 NULL, &entry);
+		/* dict key */
+		dbus_message_iter_append_basic(&entry, DBUS_TYPE_STRING,
+					       &uuids);
+
+		dbus_message_iter_open_container(&entry, DBUS_TYPE_VARIANT,
+						 "as", &value);
+
+		dbus_message_iter_open_container(&value, DBUS_TYPE_ARRAY, "s",
+						 &arrayIter);
+
+		for (list = g_list_first(args->uuids); list;
+						      list = g_list_next(list))
+			/* list->data contains string representation of uuid */
+			dbus_message_iter_append_basic(&arrayIter,
+						       DBUS_TYPE_STRING,
+						       &list->data);
+
+		dbus_message_iter_close_container(&value, &arrayIter);
+
+		/* close vararg*/
+		dbus_message_iter_close_container(&entry, &value);
+
+		/* close entry */
+		dbus_message_iter_close_container(&dict, &entry);
+	}
+
+	if (args->pathloss != DISTANCE_VAL_INVALID)
+		dict_append_entry(&dict, "Pathloss", DBUS_TYPE_UINT16,
+				  &args->pathloss);
+
+	if (args->rssi != DISTANCE_VAL_INVALID)
+		dict_append_entry(&dict, "RSSI", DBUS_TYPE_INT16, &args->rssi);
+
+	if (args->transport != NULL)
+		dict_append_entry(&dict, "Transport", DBUS_TYPE_STRING,
+				  &args->transport);
+
+	dbus_message_iter_close_container(iter, &dict);
+}
+
+
+static void set_discovery_filter_reply(DBusMessage *message,
+				       void *user_data)
+{
+	DBusError error;
+
+	dbus_error_init(&error);
+	if (dbus_set_error_from_message(&error, message) == TRUE) {
+		rl_printf("SetDiscoveryFilter failed: %s\n", error.name);
+		dbus_error_free(&error);
+		return;
+	}
+
+	rl_printf("SetDiscoveryFilter success\n");
+}
+
+static gint filtered_scan_rssi, filtered_scan_pathloss;
+static char **filtered_scan_uuids;
+static gboolean filtered_scan_help;
+static char *filtered_scan_transport;
+
+static GOptionEntry filtered_discovery_options[] = {
+	{ "rssi", 'r', 0, G_OPTION_ARG_INT, &filtered_scan_rssi,
+				"RSSI filter" },
+	{ "pathloss", 'p', 0, G_OPTION_ARG_INT, &filtered_scan_pathloss,
+				"pathloss filter" },
+	{ "transport", 't', 0, G_OPTION_ARG_STRING, &filtered_scan_transport,
+				"transport" },
+	{ "uuids", 'u', 0, G_OPTION_ARG_STRING_ARRAY, &filtered_scan_uuids,
+				"uuid to filter by" },
+	{ "help", 'h', 0, G_OPTION_ARG_NONE, &filtered_scan_help,
+				"show help" },
+	{ NULL },
+};
+
+static bool parse_set_discovery_filter_params(const char *arg,
+					struct set_discovery_filter_args *args)
+{
+	int argc, loop;
+	GOptionContext *context;
+	GError *error = NULL;
+
+	gchar **arguments = NULL, **argv, *cmdline_arg;
+
+	/* add fake program name at beginning for g_shell_parse_argv */
+	cmdline_arg = g_strconcat("set-discovery-filter ", arg, NULL);
+	if (g_shell_parse_argv(cmdline_arg, &argc, &arguments, &error)
+								    == FALSE) {
+		if (error != NULL) {
+			g_printerr("error when parsing arguments: %s\n",
+				   error->message);
+			g_error_free(error);
+		} else
+			g_printerr("An unknown error occurred\n");
+
+		g_strfreev(arguments);
+		g_free(cmdline_arg);
+		return false;
+	}
+	g_free(cmdline_arg);
+
+	argc = g_strv_length(arguments);
+
+	/* Rewrite arguments to argv, argv is not null-terminated and will be
+	 * passed to g_option_context_parse.
+	 */
+	argv = g_new(gchar *, argc);
+	for (loop = 0; loop < argc; loop++)
+		argv[loop] = arguments[loop];
+
+	context = g_option_context_new(NULL);
+	g_option_context_add_main_entries(context, filtered_discovery_options,
+									 NULL);
+	/* set default values for all options */
+	filtered_scan_rssi = DISTANCE_VAL_INVALID;
+	filtered_scan_pathloss = DISTANCE_VAL_INVALID;
+	filtered_scan_uuids = NULL;
+	filtered_scan_transport = NULL;
+	filtered_scan_help = FALSE;
+
+	g_option_context_set_help_enabled(context, FALSE);
+	if (g_option_context_parse(context, &argc, &argv, &error) == FALSE) {
+		if (error != NULL) {
+			g_printerr("error in g_option_context_parse: %s\n",
+							       error->message);
+			g_error_free(error);
+		} else
+			g_printerr("An unknown error occurred\n");
+
+		g_strfreev(arguments);
+		g_free(argv);
+		return false;
+	}
+
+	if (filtered_scan_help) {
+		printf("Set discovery filter. Usage:\n");
+		printf("	set-discovery-filter [-r rssi | -p pathlos] ");
+		printf("[-t transport] -u <uuid1> [-u <uuid2> ...]\n");
+		printf("\n");
+		printf("Example: set-discovery-filter -p 65 -u baba -u 1900\n");
+		return false;
+	}
+
+	args->uuids = NULL;
+	args->pathloss = filtered_scan_pathloss;
+	args->rssi = filtered_scan_rssi;
+	args->transport = filtered_scan_transport;
+
+	if (filtered_scan_uuids != NULL)
+		for (loop = 0; filtered_scan_uuids[loop] != NULL; loop++) {
+			args->uuids = g_list_append(args->uuids,
+					    strdup(filtered_scan_uuids[loop]));
+		}
+
+	g_strfreev(arguments);
+	g_free(argv);
+	g_strfreev(filtered_scan_uuids);
+
+	g_option_context_free(context);
+	return true;
+}
+
+static void cmd_set_discovery_filter(const char *arg)
+{
+	struct set_discovery_filter_args args;
+
+	if (!parse_set_discovery_filter_params(arg, &args))
+		return;
+
+	if (check_default_ctrl() == FALSE)
+		return;
+
+	if (g_dbus_proxy_method_call(default_ctrl, "SetDiscoveryFilter",
+		set_discovery_filter_setup, set_discovery_filter_reply,
+		&args, NULL /* TODO: proper freeing method here */) == FALSE) {
+		rl_printf("Failed to set discovery filter\n");
+		return;
+	}
+}
+
 static struct GDBusProxy *find_device(const char *arg)
 {
 	GDBusProxy *proxy;
@@ -1468,6 +1710,8 @@ static const struct {
 							capability_generator},
 	{ "default-agent",NULL,       cmd_default_agent,
 				"Set agent as the default one" },
+	{ "set-discovery-filter", "", cmd_set_discovery_filter,
+		"Set discovery filter. Run discovery-filter -h for help" },
 	{ "scan",         "<on/off>", cmd_scan, "Scan for devices" },
 	{ "info",         "[dev]",    cmd_info, "Device information",
 							dev_generator },
-- 
2.2.0.rc0.207.ga3a616c

[PATCH v6 4/8] core: adapter: Add parameter parsing to SetDiscoveryFilter

[Jakub Pawlowski]

This patch adds parameter parsing, and basic internal logic checks to
SetDiscoveryFilter method.
---
 src/adapter.c | 202 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 202 insertions(+)

diff --git a/src/adapter.c b/src/adapter.c
index b5f2a45..004e685 100644
--- a/src/adapter.c
+++ b/src/adapter.c
@@ -91,6 +91,10 @@
 
 #define SCAN_TYPE_BREDR (1 << BDADDR_BREDR)
 #define SCAN_TYPE_LE ((1 << BDADDR_LE_PUBLIC) | (1 << BDADDR_LE_RANDOM))
+#define SCAN_TYPE_DUAL (SCAN_TYPE_BREDR | SCAN_TYPE_LE)
+
+#define	DISTANCE_VAL_INVALID	0x7FFF
+#define	PATHLOSS_MAX 137
 
 static DBusConnection *dbus_conn = NULL;
 
@@ -145,6 +149,13 @@ struct conn_param {
 	uint16_t timeout;
 };
 
+struct discovery_filter {
+	uint8_t type;
+	uint16_t pathloss;
+	int16_t rssi;
+	GSList *uuids;
+};
+
 struct watch_client {
 	struct btd_adapter *adapter;
 	char *owner;
@@ -1761,9 +1772,200 @@ static DBusMessage *start_discovery(DBusConnection *conn,
 	return dbus_message_new_method_return(msg);
 }
 
+static bool parse_uuids(DBusMessageIter *value,
+					 GSList **uuids)
+{
+	DBusMessageIter arriter;
+
+	if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_ARRAY)
+		return false;
+
+	dbus_message_iter_recurse(value, &arriter);
+	while (dbus_message_iter_get_arg_type(&arriter) != DBUS_TYPE_INVALID) {
+		bt_uuid_t uuid, u128;
+		char uuidstr[MAX_LEN_UUID_STR + 1];
+		char *uuid_param;
+
+		if (dbus_message_iter_get_arg_type(&arriter) !=
+						DBUS_TYPE_STRING)
+			return false;
+
+		dbus_message_iter_get_basic(&arriter, &uuid_param);
+
+		if (bt_string_to_uuid(&uuid, uuid_param))
+			return false;
+
+		bt_uuid_to_uuid128(&uuid, &u128);
+		bt_uuid_to_string(&u128, uuidstr, sizeof(uuidstr));
+
+		*uuids = g_slist_prepend(*uuids, strdup(uuidstr));
+
+		dbus_message_iter_next(&arriter);
+	}
+
+	return true;
+}
+
+static bool parse_rssi(DBusMessageIter *value, int16_t *rssi)
+{
+	if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_INT16)
+		return false;
+
+	dbus_message_iter_get_basic(value, rssi);
+	/* -127 <= RSSI <= +20 (spec V4.2 [Vol 2, Part E] 7.7.65.2) */
+	if (*rssi > 20 || *rssi < -127)
+		return false;
+
+	return true;
+}
+
+static bool parse_pathloss(DBusMessageIter *value,
+					    uint16_t *pathloss)
+{
+	if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_UINT16)
+		return false;
+
+	dbus_message_iter_get_basic(value, pathloss);
+	/* pathloss filter must be smaller that PATHLOSS_MAX */
+	if (*pathloss > PATHLOSS_MAX)
+		return false;
+
+	return true;
+}
+
+static bool parse_transport(DBusMessageIter *value,
+					     uint8_t *transport)
+{
+	char *transport_str;
+
+	if (dbus_message_iter_get_arg_type(value) != DBUS_TYPE_STRING)
+		return false;
+
+	dbus_message_iter_get_basic(value, &transport_str);
+
+	if (!strcmp(transport_str, "bredr"))
+		*transport = SCAN_TYPE_BREDR;
+	else if (!strcmp(transport_str, "le"))
+		*transport = SCAN_TYPE_LE;
+	else if (!strcmp(transport_str, "auto"))
+		*transport = SCAN_TYPE_DUAL;
+	else
+		return false;
+
+	return true;
+}
+
+static bool parse_discovery_filter_entry(char *key, DBusMessageIter *value,
+					struct discovery_filter *filter)
+{
+	if (!strcmp("UUIDs", key))
+		return parse_uuids(value, &filter->uuids);
+
+	if (!strcmp("RSSI", key))
+		return parse_rssi(value, &filter->rssi);
+
+	if (!strcmp("Pathloss", key))
+		return parse_pathloss(value, &filter->pathloss);
+
+	if (!strcmp("Transport", key))
+		return parse_transport(value, &filter->type);
+
+	DBG("Unknown key parameter: %s!\n", key);
+	return false;
+}
+
+/* This method is responsible for parsing parameters to SetDiscoveryFilter. If
+ * filter in msg was empty, sets *filter to NULL. If whole parsing was
+ * successful, sets *filter to proper value.
+ * Returns false on any error, and true on success.
+ */
+static bool parse_discovery_filter_dict(struct discovery_filter **filter,
+					DBusMessage *msg)
+{
+	DBusMessageIter iter, subiter, dictiter, variantiter;
+	bool is_empty = true;
+
+	*filter = g_try_malloc(sizeof(**filter));
+	if (!*filter)
+		return false;
+
+	(*filter)->uuids = NULL;
+	(*filter)->pathloss = DISTANCE_VAL_INVALID;
+	(*filter)->rssi = DISTANCE_VAL_INVALID;
+	(*filter)->type = SCAN_TYPE_DUAL;
+
+	dbus_message_iter_init(msg, &iter);
+	if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_ARRAY ||
+	    dbus_message_iter_get_element_type(&iter) != DBUS_TYPE_DICT_ENTRY)
+		return false;
+
+	dbus_message_iter_recurse(&iter, &subiter);
+	do {
+		int type = dbus_message_iter_get_arg_type(&subiter);
+		char *key;
+
+		if (type == DBUS_TYPE_INVALID)
+			break;
+
+		is_empty = false;
+		dbus_message_iter_recurse(&subiter, &dictiter);
+
+		dbus_message_iter_get_basic(&dictiter, &key);
+		if (!dbus_message_iter_next(&dictiter))
+			goto invalid_args;
+
+		if (dbus_message_iter_get_arg_type(&dictiter) !=
+							     DBUS_TYPE_VARIANT)
+			goto invalid_args;
+
+		dbus_message_iter_recurse(&dictiter, &variantiter);
+
+		if (!parse_discovery_filter_entry(key, &variantiter, *filter))
+			goto invalid_args;
+
+		dbus_message_iter_next(&subiter);
+	} while (true);
+
+	if (is_empty) {
+		g_free(*filter);
+		*filter = NULL;
+		return true;
+	}
+
+	/* only pathlos or rssi can be set, never both */
+	if ((*filter)->pathloss != DISTANCE_VAL_INVALID &&
+	    (*filter)->rssi != DISTANCE_VAL_INVALID)
+		goto invalid_args;
+
+	DBG("filtered discovery params: transport: %d rssi: %d pathloss: %d",
+	    (*filter)->type, (*filter)->rssi, (*filter)->pathloss);
+
+	return true;
+
+invalid_args:
+	g_slist_free_full((*filter)->uuids, g_free);
+	g_free(*filter);
+	*filter = NULL;
+	return false;
+}
+
 static DBusMessage *set_discovery_filter(DBusConnection *conn,
 					 DBusMessage *msg, void *user_data)
 {
+	struct btd_adapter *adapter = user_data;
+	struct discovery_filter *discovery_filter;
+
+	const char *sender = dbus_message_get_sender(msg);
+
+	DBG("sender %s", sender);
+
+	if (!(adapter->current_settings & MGMT_SETTING_POWERED))
+		return btd_error_not_ready(msg);
+
+	/* parse parameters */
+	if (!parse_discovery_filter_dict(&discovery_filter, msg))
+		return btd_error_invalid_args(msg);
+
 	return btd_error_failed(msg, "Not implemented yet");
 }
 
-- 
2.2.0.rc0.207.ga3a616c