This patch adds start and stop advertising commands for btp client.
---
tools/btpclient.c | 661 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
1 file changed, 660 insertions(+), 1 deletion(-)
diff --git a/tools/btpclient.c b/tools/btpclient.c
index 806403f6a..472e9e4c2 100644
--- a/tools/btpclient.c
+++ b/tools/btpclient.c
@@ -34,6 +34,19 @@
#include "src/shared/btp.h"
+#define AD_PATH "/org/bluez/advertising"
+#define AD_IFACE "org.bluez.LEAdvertisement1"
+
+/* List of assigned numbers for advetising data and scan response */
+#define AD_TYPE_FLAGS 0x01
+#define AD_TYPE_INCOMPLETE_UUID16_SERVICE_LIST 0x02
+#define AD_TYPE_SHORT_NAME 0x08
+#define AD_TYPE_SERVICE_DATA_UUID16 0x16
+#define AD_TYPE_APPEARANCE 0x19
+#define AD_TYPE_MANUFACTURER_DATA 0xff
+
+struct l_dbus *dbus;
+
struct btp_adapter {
struct l_dbus_proxy *proxy;
struct l_dbus_proxy *ad_proxy;
@@ -53,12 +66,64 @@ static struct btp *btp;
static bool gap_service_registered;
+struct ad_data {
+ uint8_t data[25];
+ uint8_t len;
+};
+
+struct service_data {
+ char *uuid;
+ struct ad_data data;
+};
+
+struct manufacturer_data {
+ uint16_t id;
+ struct ad_data data;
+};
+
+static struct ad {
+ bool registered;
+ char *type;
+ char *local_name;
+ uint16_t local_appearance;
+ uint16_t duration;
+ uint16_t timeout;
+ char **uuids;
+ size_t uuids_cnt;
+ struct service_data *services;
+ size_t services_data_len;
+ struct manufacturer_data *manufacturer;
+ size_t manufacturer_data_len;
+ bool tx_power;
+ bool name;
+ bool appearance;
+} ad = {
+ .local_appearance = UINT16_MAX,
+};
+
static bool str2addr(const char *str, uint8_t *addr)
{
return sscanf(str, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", &addr[5], &addr[4],
&addr[3], &addr[2], &addr[1], &addr[0]) == 6;
}
+static char *dupuuid2str(const uint8_t *uuid, uint8_t len)
+{
+ switch (len) {
+ case 16:
+ return l_strdup_printf("%hhx%hhx", uuid[0], uuid[1]);
+ case 128:
+ return l_strdup_printf("%hhx%hhx%hhx%hhx%hhx%hhx%hhx%hhx%hhx"
+ "%hhx%hhx%hhx%hhx%hhx%hhx%hhx", uuid[0],
+ uuid[1], uuid[2], uuid[3], uuid[4],
+ uuid[5], uuid[6], uuid[6], uuid[8],
+ uuid[7], uuid[10], uuid[11], uuid[12],
+ uuid[13], uuid[14], uuid[15]);
+ default:
+ return NULL;
+ }
+}
+
static struct btp_adapter *find_adapter_by_proxy(struct l_dbus_proxy *proxy)
{
const struct l_queue_entry *entry;
@@ -123,6 +188,8 @@ static void btp_gap_read_commands(uint8_t index, const void *param,
commands |= (1 << BTP_OP_GAP_SET_CONNECTABLE);
commands |= (1 << BTP_OP_GAP_SET_DISCOVERABLE);
commands |= (1 << BTP_OP_GAP_SET_BONDABLE);
+ commands |= (1 << BTP_OP_GAP_START_ADVERTISING);
+ commands |= (1 << BTP_OP_GAP_STOP_ADVERTISING);
commands |= (1 << BTP_OP_GAP_START_DISCOVERY);
commands |= (1 << BTP_OP_GAP_STOP_DISCOVERY);
@@ -234,6 +301,46 @@ static void remove_device_reply(struct l_dbus_proxy *proxy,
l_queue_remove(adapter->devices, device);
}
+static void unreg_advertising_setup(struct l_dbus_message *message,
+ void *user_data)
+{
+ struct l_dbus_message_builder *builder;
+
+ builder = l_dbus_message_builder_new(message);
+ l_dbus_message_builder_append_basic(builder, 'o', AD_PATH);
+ l_dbus_message_builder_finalize(builder);
+ l_dbus_message_builder_destroy(builder);
+}
+
+static void unreg_advertising_reply(struct l_dbus_proxy *proxy,
+ struct l_dbus_message *result,
+ void *user_data)
+{
+ const char *path = l_dbus_proxy_get_path(proxy);
+ struct btp_adapter *adapter = find_adapter_by_path(path);
+
+ if (!adapter)
+ return;
+
+ if (l_dbus_message_is_error(result)) {
+ const char *name;
+
+ l_dbus_message_get_error(result, &name, NULL);
+
+ l_error("Failed to stop advertising %s (%s)",
+ l_dbus_proxy_get_path(proxy), name);
+ return;
+ }
+
+ if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
+ l_info("Unable to remove ad instance");
+ if (!l_dbus_object_remove_interface(dbus, AD_PATH,
+ L_DBUS_INTERFACE_PROPERTIES))
+ l_info("Unable to remove propety instance");
+ if (!l_dbus_unregister_interface(dbus, AD_IFACE))
+ l_info("Unable to unregister ad interface");
+}
+
static void btp_gap_reset(uint8_t index, const void *param, uint16_t length,
void *user_data)
{
@@ -264,6 +371,16 @@ static void btp_gap_reset(uint8_t index, const void *param, uint16_t length,
NULL);
}
+ if (adapter->ad_proxy)
+ if (!l_dbus_proxy_method_call(adapter->ad_proxy,
+ "UnregisterAdvertisement",
+ unreg_advertising_setup,
+ unreg_advertising_reply,
+ NULL, NULL)) {
+ status = BTP_ERROR_FAIL;
+ goto failed;
+ }
+
/* TODO for we assume all went well */
btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_RESET, index, 0, NULL);
return;
@@ -449,6 +566,536 @@ failed:
btp_send_error(btp, BTP_GAP_SERVICE, index, status);
}
+static struct l_dbus_message *ad_release_call(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ void *user_data)
+{
+ struct l_dbus_message *reply;
+ uint8_t i;
+
+ l_dbus_unregister_object(dbus, AD_PATH);
+ l_dbus_unregister_interface(dbus, AD_IFACE);
+
+ reply = l_dbus_message_new_method_return(message);
+ l_dbus_message_set_arguments(reply, "");
+
+ for (i = 0; i < ad.uuids_cnt; i++)
+ l_free(ad.uuids[i]);
+ l_free(ad.uuids);
+ ad.uuids_cnt = 0;
+
+ l_free(ad.local_name);
+
+ for (i = 0; i < ad.services_data_len; i++)
+ l_free(ad.services[i].uuid);
+ l_free(ad.services);
+ ad.services_data_len = 0;
+
+ l_free(ad.manufacturer);
+ ad.manufacturer_data_len = 0;
+
+ return reply;
+}
+
+static bool ad_type_getter(struct l_dbus *dbus, struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ l_dbus_message_builder_append_basic(builder, 's', ad.type);
+
+ return true;
+}
+
+static bool ad_serviceuuids_getter(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ size_t i;
+
+ if (!ad.uuids_cnt)
+ return false;
+
+ l_dbus_message_builder_enter_array(builder, "s");
+
+ for (i = 0; i < ad.uuids_cnt; i++)
+ l_dbus_message_builder_append_basic(builder, 's', ad.uuids[i]);
+
+ l_dbus_message_builder_leave_array(builder);
+
+ return true;
+}
+
+static bool ad_servicedata_getter(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ const uint8_t *array;
+ size_t i, j;
+
+ if (!ad.services_data_len)
+ return false;
+
+ l_dbus_message_builder_enter_array(builder, "{sv}");
+
+ for (j = 0; j < ad.services_data_len; j++) {
+ array = ad.services[j].data.data;
+
+ l_dbus_message_builder_enter_dict(builder, "sv");
+ l_dbus_message_builder_append_basic(builder, 's',
+ ad.services[j].uuid);
+ l_dbus_message_builder_enter_variant(builder, "ay");
+ l_dbus_message_builder_enter_array(builder, "y");
+
+ for (i = 0; i < ad.services[j].data.len; i++)
+ l_dbus_message_builder_append_basic(builder, 'y',
+ &(array[i]));
+
+ l_dbus_message_builder_leave_array(builder);
+ l_dbus_message_builder_leave_variant(builder);
+ l_dbus_message_builder_leave_dict(builder);
+ }
+ l_dbus_message_builder_leave_array(builder);
+
+ return true;
+}
+
+static bool ad_manufacturerdata_getter(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ const uint8_t *array;
+ size_t i, j;
+
+ if (!ad.manufacturer_data_len)
+ return false;
+
+ l_dbus_message_builder_enter_array(builder, "{qv}");
+
+ for (j = 0; j < ad.manufacturer_data_len; j++) {
+ array = ad.manufacturer[j].data.data;
+
+ l_dbus_message_builder_enter_dict(builder, "qv");
+ l_dbus_message_builder_append_basic(builder, 'q',
+ &ad.manufacturer[j].id);
+ l_dbus_message_builder_enter_variant(builder, "ay");
+ l_dbus_message_builder_enter_array(builder, "y");
+
+ for (i = 0; i < ad.manufacturer[j].data.len; i++)
+ l_dbus_message_builder_append_basic(builder, 'y',
+ &(array[i]));
+
+ l_dbus_message_builder_leave_array(builder);
+ l_dbus_message_builder_leave_variant(builder);
+ l_dbus_message_builder_leave_dict(builder);
+ }
+ l_dbus_message_builder_leave_array(builder);
+
+ return true;
+}
+
+static bool ad_includes_getter(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ l_dbus_message_builder_enter_array(builder, "s");
+
+ if (!(ad.tx_power || ad.name || ad.appearance))
+ return false;
+
+ if (ad.tx_power) {
+ const char *str = "tx-power";
+
+ l_dbus_message_builder_append_basic(builder, 's', &str);
+ }
+
+ if (ad.name) {
+ const char *str = "local-name";
+
+ l_dbus_message_builder_append_basic(builder, 's', &str);
+ }
+
+ if (ad.appearance) {
+ const char *str = "appearance";
+
+ l_dbus_message_builder_append_basic(builder, 's', &str);
+ }
+
+ l_dbus_message_builder_leave_array(builder);
+
+ return true;
+}
+
+static bool ad_localname_getter(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ if (!ad.local_name)
+ return false;
+
+ l_dbus_message_builder_append_basic(builder, 's', ad.local_name);
+
+ return true;
+}
+
+static bool ad_appearance_getter(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ if (!ad.local_appearance)
+ return false;
+
+ l_dbus_message_builder_append_basic(builder, 'q', &ad.local_appearance);
+
+ return true;
+}
+
+static bool ad_duration_getter(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ if (!ad.duration)
+ return false;
+
+ l_dbus_message_builder_append_basic(builder, 'q', &ad.duration);
+
+ return true;
+}
+
+static bool ad_timeout_getter(struct l_dbus *dbus,
+ struct l_dbus_message *message,
+ struct l_dbus_message_builder *builder,
+ void *user_data)
+{
+ if (!ad.timeout)
+ return false;
+
+ l_dbus_message_builder_append_basic(builder, 'q', &ad.timeout);
+
+ return true;
+}
+
+static void setup_ad_interface(struct l_dbus_interface *interface)
+{
+ l_dbus_interface_method(interface, "Release", 0, ad_release_call, "",
+ "");
+ l_dbus_interface_property(interface, "Type", 0, "s", ad_type_getter,
+ NULL);
+ l_dbus_interface_property(interface, "ServiceUUIDs", 0, "as",
+ ad_serviceuuids_getter, NULL);
+ l_dbus_interface_property(interface, "ServiceData", 0, "a{sv}",
+ ad_servicedata_getter, NULL);
+ l_dbus_interface_property(interface, "ManufacturerServiceData", 0,
+ "a{qv}", ad_manufacturerdata_getter,
+ NULL);
+ l_dbus_interface_property(interface, "Includes", 0, "as",
+ ad_includes_getter, NULL);
+ l_dbus_interface_property(interface, "LocalName", 0, "s",
+ ad_localname_getter, NULL);
+ l_dbus_interface_property(interface, "Appearance", 0, "q",
+ ad_appearance_getter, NULL);
+ l_dbus_interface_property(interface, "Duration", 0, "q",
+ ad_duration_getter, NULL);
+ l_dbus_interface_property(interface, "Timeout", 0, "q",
+ ad_timeout_getter, NULL);
+ return;
+}
+
+static void start_advertising_reply(struct l_dbus_proxy *proxy,
+ struct l_dbus_message *result,
+ void *user_data)
+{
+ const char *path = l_dbus_proxy_get_path(proxy);
+ struct btp_adapter *adapter = find_adapter_by_path(path);
+ uint32_t settings;
+
+ if (!adapter) {
+ btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
+ BTP_ERROR_FAIL);
+ return;
+ }
+
+ if (l_dbus_message_is_error(result)) {
+ const char *name, *desc;
+
+ l_dbus_message_get_error(result, &name, &desc);
+ l_error("Failed to start advertising (%s), %s", name, desc);
+
+ btp_send_error(btp, BTP_GAP_SERVICE, adapter->index,
+ BTP_ERROR_FAIL);
+ return;
+ }
+
+ settings = L_CPU_TO_LE32(adapter->current_settings);
+
+ btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_ADVERTISING,
+ adapter->index, sizeof(settings), &settings);
+}
+
+static void create_advertising_data(uint8_t adv_data_len, const uint8_t *data)
+{
+ const uint8_t *ad_data;
+ uint8_t ad_type, ad_len;
+ uint8_t remaining_data_len = adv_data_len;
+
+ while (remaining_data_len) {
+ ad_type = data[adv_data_len - remaining_data_len];
+ ad_len = data[adv_data_len - remaining_data_len + 1];
+ ad_data = &data[adv_data_len - remaining_data_len + 2];
+
+ switch (ad_type) {
+ case AD_TYPE_INCOMPLETE_UUID16_SERVICE_LIST:
+ ad.uuids_cnt += 1;
+ ad.uuids = realloc(ad.uuids,
+ ad.uuids_cnt * sizeof(ad.uuids));
+ ad.uuids[ad.uuids_cnt - 1] = dupuuid2str(ad_data, 16);
+
+ break;
+ case AD_TYPE_SHORT_NAME:
+ ad.local_name = malloc(ad_len + 1);
+ memcpy(ad.local_name, ad_data, ad_len);
+ ad.local_name[ad_len] = '\0';
+
+ break;
+ case AD_TYPE_SERVICE_DATA_UUID16:
+ {
+ uint8_t svc_id;
+
+ ad.services_data_len += 1;
+ svc_id = ad.services_data_len - 1;
+ ad.services = l_realloc(ad.services,
+ sizeof(struct service_data) *
+ ad.services_data_len);
+ /* The first 2 octets contain the 16 bit Service UUID
+ * followed by additional service data
+ */
+ ad.services[svc_id].uuid = dupuuid2str(ad_data, 16);
+ ad.services[svc_id].data.len = ad_len - 2;
+ memcpy(ad.services[svc_id].data.data, ad_data + 2,
+ ad.services[svc_id].data.len);
+
+ break;
+ }
+ case AD_TYPE_APPEARANCE:
+ memcpy(&ad.local_appearance, ad_data, ad_len);
+
+ break;
+ case AD_TYPE_MANUFACTURER_DATA:
+ {
+ uint8_t md_id;
+
+ ad.manufacturer_data_len += 1;
+ md_id = ad.manufacturer_data_len - 1;
+ ad.manufacturer = l_realloc(ad.manufacturer,
+ sizeof(struct manufacturer_data) *
+ ad.manufacturer_data_len);
+ /* The first 2 octets contain the Company Identifier
+ * Code followed by additional manufacturer specific
+ * data.
+ */
+ memcpy(&ad.manufacturer[md_id].id, ad_data, 2);
+ ad.manufacturer[md_id].data.len = ad_len - 2;
+ memcpy(&ad.manufacturer[md_id].data.data, ad_data + 2,
+ ad.manufacturer[md_id].data.len);
+
+ break;
+ }
+ }
+ /* Advertising entity data len + advertising entity header
+ * (type, len)
+ */
+ remaining_data_len -= ad_len + 2;
+ }
+}
+
+struct start_advertising_data {
+ uint32_t settings;
+ struct btp_gap_start_adv_cp cp;
+};
+
+static void create_scan_response(uint8_t scan_rsp_len, const uint8_t *data)
+{
+ /* TODO */
+}
+
+static void start_advertising_setup(struct l_dbus_message *message,
+ void *user_data)
+{
+ const struct start_advertising_data *sad = user_data;
+ const struct btp_gap_start_adv_cp *cp = &sad->cp;
+ struct l_dbus_message_builder *builder;
+
+ if ((sad->settings >> BTP_GAP_SETTING_CONNECTABLE) & 1U)
+ ad.type = l_strdup("peripheral");
+ else
+ ad.type = l_strdup("broadcast");
+
+ if (cp->adv_data_len != 0)
+ create_advertising_data(cp->adv_data_len, cp->data);
+ if (cp->scan_rsp_len != 0)
+ create_scan_response(cp->scan_rsp_len,
+ cp->data + cp->scan_rsp_len);
+
+ free(user_data);
+
+ builder = l_dbus_message_builder_new(message);
+ l_dbus_message_builder_append_basic(builder, 'o', AD_PATH);
+ l_dbus_message_builder_enter_array(builder, "{sv}");
+ l_dbus_message_builder_enter_dict(builder, "sv");
+ l_dbus_message_builder_leave_dict(builder);
+ l_dbus_message_builder_leave_array(builder);
+ l_dbus_message_builder_finalize(builder);
+ l_dbus_message_builder_destroy(builder);
+}
+
+static void btp_gap_start_advertising(uint8_t index, const void *param,
+ uint16_t length, void *user_data)
+{
+ struct btp_adapter *adapter = find_adapter_by_index(index);
+ const struct btp_gap_start_adv_cp *cp = param;
+ struct start_advertising_data *data;
+ uint8_t status = BTP_ERROR_FAIL;
+ bool prop;
+ void *buff;
+
+ if (!adapter) {
+ status = BTP_ERROR_INVALID_INDEX;
+ goto failed;
+ }
+
+ /* Adapter needs to be powered to be able to remove devices */
+ if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
+ !prop)
+ goto failed;
+
+ if (!l_dbus_register_interface(dbus, AD_IFACE, setup_ad_interface, NULL,
+ false)) {
+ l_info("Unable to register ad interface");
+ goto failed;
+ }
+
+ if (!l_dbus_object_add_interface(dbus, AD_PATH, AD_IFACE, NULL)) {
+ l_info("Unable to instantiate ad interface");
+
+ if (!l_dbus_unregister_interface(dbus, AD_IFACE))
+ l_info("Unable to unregister ad interface");
+
+ goto failed;
+ }
+
+ if (!l_dbus_object_add_interface(dbus, AD_PATH,
+ L_DBUS_INTERFACE_PROPERTIES,
+ NULL)) {
+ l_info("Unable to instantiate the properties interface");
+
+ if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
+ l_info("Unable to remove ad instance");
+ if (!l_dbus_unregister_interface(dbus, AD_IFACE))
+ l_info("Unable to unregister ad interface");
+
+ goto failed;
+ }
+
+ buff = l_malloc(sizeof(struct start_advertising_data) +
+ cp->adv_data_len + cp->scan_rsp_len);
+ data = buff;
+ data->settings = adapter->current_settings;
+ data->cp.adv_data_len = cp->adv_data_len;
+ data->cp.scan_rsp_len = cp->scan_rsp_len;
+ memcpy(data->cp.data, cp->data, cp->adv_data_len + cp->scan_rsp_len);
+
+ if (!l_dbus_proxy_method_call(adapter->ad_proxy,
+ "RegisterAdvertisement",
+ start_advertising_setup,
+ start_advertising_reply,
+ data, NULL)) {
+ if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
+ l_info("Unable to remove ad instance");
+ if (!l_dbus_unregister_interface(dbus, AD_IFACE))
+ l_info("Unable to unregister ad interface");
+
+ free(buff);
+ goto failed;
+ }
+
+ return;
+
+failed:
+ btp_send_error(btp, BTP_GAP_SERVICE, index, status);
+}
+
+static void stop_advertising_reply(struct l_dbus_proxy *proxy,
+ struct l_dbus_message *result,
+ void *user_data)
+{
+ const char *path = l_dbus_proxy_get_path(proxy);
+ struct btp_adapter *adapter = find_adapter_by_path(path);
+ uint32_t settings;
+
+ if (!adapter)
+ return;
+
+ if (l_dbus_message_is_error(result)) {
+ const char *name;
+
+ l_dbus_message_get_error(result, &name, NULL);
+
+ l_error("Failed to stop advertising %s (%s)",
+ l_dbus_proxy_get_path(proxy), name);
+ return;
+ }
+
+ if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
+ l_info("Unable to remove ad instance");
+ if (!l_dbus_object_remove_interface(dbus, AD_PATH,
+ L_DBUS_INTERFACE_PROPERTIES))
+ l_info("Unable to remove propety instance");
+ if (!l_dbus_unregister_interface(dbus, AD_IFACE))
+ l_info("Unable to unregister ad interface");
+
+ settings = L_CPU_TO_LE32(adapter->current_settings);
+
+ btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_ADVERTISING,
+ adapter->index, sizeof(settings), &settings);
+}
+
+static void btp_gap_stop_advertising(uint8_t index, const void *param,
+ uint16_t length, void *user_data)
+{
+ struct btp_adapter *adapter = find_adapter_by_index(index);
+ uint8_t status = BTP_ERROR_FAIL;
+ bool prop;
+
+ if (!adapter) {
+ status = BTP_ERROR_INVALID_INDEX;
+ goto failed;
+ }
+
+ if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
+ !prop)
+ goto failed;
+
+ if (adapter->ad_proxy) {
+ if (!l_dbus_proxy_method_call(adapter->ad_proxy,
+ "UnregisterAdvertisement",
+ unreg_advertising_setup,
+ stop_advertising_reply,
+ NULL, NULL)) {
+ status = BTP_ERROR_FAIL;
+ goto failed;
+ }
+ }
+
+failed:
+ btp_send_error(btp, BTP_GAP_SERVICE, index, status);
+}
+
static void start_discovery_reply(struct l_dbus_proxy *proxy,
struct l_dbus_message *result,
void *user_data)
@@ -728,6 +1375,12 @@ static void register_gap_service(void)
btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_SET_BONDABLE,
btp_gap_set_bondable, NULL, NULL);
+ btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_ADVERTISING,
+ btp_gap_start_advertising, NULL, NULL);
+
+ btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_ADVERTISING,
+ btp_gap_stop_advertising, NULL, NULL);
+
btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_DISCOVERY,
btp_gap_start_discovery, NULL, NULL);
@@ -1124,6 +1777,12 @@ static void client_ready(struct l_dbus_client *client, void *user_data)
BTP_INDEX_NON_CONTROLLER, 0, NULL);
}
+static void ready_callback(void *user_data)
+{
+ if (!l_dbus_object_manager_enable(dbus))
+ l_info("Unable to register the ObjectManager");
+}
+
static void usage(void)
{
l_info("btpclient - Bluetooth tester");
@@ -1148,7 +1807,6 @@ int main(int argc, char *argv[])
{
struct l_dbus_client *client;
struct l_signal *signal;
- struct l_dbus *dbus;
sigset_t mask;
int opt;
@@ -1192,6 +1850,7 @@ int main(int argc, char *argv[])
signal = l_signal_create(&mask, signal_handler, NULL, NULL);
dbus = l_dbus_new_default(L_DBUS_SYSTEM_BUS);
+ l_dbus_set_ready_handler(dbus, ready_callback, NULL, NULL);
client = l_dbus_client_new(dbus, "org.bluez", "/org/bluez");
l_dbus_client_set_connect_handler(client, client_connected, NULL, NULL);
--
2.13.6
Hi Johan,
On Wednesday, 10 January 2018 13:25:27 CET Johan Hedberg wrote:
> Hi Szymon,
>
> On Wed, Jan 10, 2018, Szymon Janc wrote:
> > On Wednesday, 10 January 2018 12:24:56 CET Johan Hedberg wrote:
> > > On Wed, Jan 10, 2018, Szymon Janc wrote:
> > > > On Tuesday, 9 January 2018 16:45:20 CET Grzegorz Kolodziejczyk wrote:
> > > > > +static bool addr2str(const uint8_t *addr, char *str)
> > > > > +{
> > > > > + return sprintf(str, "%02hhX:%02hhX:%02hhX:%02hhX:%02hhX:%02hhX",
> > > > > + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5])
> > > > > + == 17;
> > > > > +}
> > > >
> > > > Use snprintf.
> > >
> > > Why isn't this tool using the address conversion helpers from
> > > lib/bluetooth.c?
> >
> > This is BTP address and has different byte order.
>
> Hmm... I'm a bit confused now. I thought BTP is little endian like HCI
> and most Bluetooth protocols. At least that's what it says here:
>
> https://github.com/zephyrproject-rtos/zephyr/blob/master/tests/bluetooth/tes
> ter/btp_spec.txt#L58
>
> What am I missing? :)
Yes, you're correct. This needs fixing, although we would have to link against
libbluetooth only for those sprintf wrappers.. (especially that those use
insecure variant of printf).
--
pozdrawiam
Szymon Janc
Hi Szymon,
On Wed, Jan 10, 2018, Szymon Janc wrote:
> On Wednesday, 10 January 2018 12:24:56 CET Johan Hedberg wrote:
> > On Wed, Jan 10, 2018, Szymon Janc wrote:
> > > On Tuesday, 9 January 2018 16:45:20 CET Grzegorz Kolodziejczyk wrote:
> > > > +static bool addr2str(const uint8_t *addr, char *str)
> > > > +{
> > > > + return sprintf(str, "%02hhX:%02hhX:%02hhX:%02hhX:%02hhX:%02hhX",
> > > > + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5])
> > > > + == 17;
> > > > +}
> > >
> > > Use snprintf.
> >
> > Why isn't this tool using the address conversion helpers from
> > lib/bluetooth.c?
>
> This is BTP address and has different byte order.
Hmm... I'm a bit confused now. I thought BTP is little endian like HCI
and most Bluetooth protocols. At least that's what it says here:
https://github.com/zephyrproject-rtos/zephyr/blob/master/tests/bluetooth/tester/btp_spec.txt#L58
What am I missing? :)
Johan
Hi Johan,
On Wednesday, 10 January 2018 12:24:56 CET Johan Hedberg wrote:
> Hi,
>
> On Wed, Jan 10, 2018, Szymon Janc wrote:
> > On Tuesday, 9 January 2018 16:45:20 CET Grzegorz Kolodziejczyk wrote:
> > > +static bool addr2str(const uint8_t *addr, char *str)
> > > +{
> > > + return sprintf(str, "%02hhX:%02hhX:%02hhX:%02hhX:%02hhX:%02hhX",
> > > + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5])
> > > + == 17;
> > > +}
> >
> > Use snprintf.
>
> Why isn't this tool using the address conversion helpers from
> lib/bluetooth.c?
This is BTP address and has different byte order.
--
pozdrawiam
Szymon Janc
Hi Szymon,
2018-01-10 11:47 GMT+01:00 Szymon Janc <[email protected]>:
> Hi Grzegorz,
>
> On Tuesday, 9 January 2018 16:45:19 CET Grzegorz Kolodziejczyk wrote:
>> This patch adds start and stop advertising commands for btp client.
>> ---
>> tools/btpclient.c | 661
>> +++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 6=
60
>> insertions(+), 1 deletion(-)
>>
>> diff --git a/tools/btpclient.c b/tools/btpclient.c
>> index 806403f6a..472e9e4c2 100644
>> --- a/tools/btpclient.c
>> +++ b/tools/btpclient.c
>> @@ -34,6 +34,19 @@
>>
>> #include "src/shared/btp.h"
>>
>> +#define AD_PATH "/org/bluez/advertising"
>> +#define AD_IFACE "org.bluez.LEAdvertisement1"
>> +
>> +/* List of assigned numbers for advetising data and scan response */
>> +#define AD_TYPE_FLAGS 0x01
>> +#define AD_TYPE_INCOMPLETE_UUID16_SERVICE_LIST 0x02
>> +#define AD_TYPE_SHORT_NAME 0x08
>> +#define AD_TYPE_SERVICE_DATA_UUID16 0x16
>> +#define AD_TYPE_APPEARANCE 0x19
>> +#define AD_TYPE_MANUFACTURER_DATA 0xff
>> +
>> +struct l_dbus *dbus;
>
> Should be static.
Right.
>
>> +
>> struct btp_adapter {
>> struct l_dbus_proxy *proxy;
>> struct l_dbus_proxy *ad_proxy;
>> @@ -53,12 +66,64 @@ static struct btp *btp;
>>
>> static bool gap_service_registered;
>>
>> +struct ad_data {
>> + uint8_t data[25];
>> + uint8_t len;
>> +};
>> +
>> +struct service_data {
>> + char *uuid;
>> + struct ad_data data;
>> +};
>> +
>> +struct manufacturer_data {
>> + uint16_t id;
>> + struct ad_data data;
>> +};
>> +
>> +static struct ad {
>> + bool registered;
>> + char *type;
>> + char *local_name;
>> + uint16_t local_appearance;
>> + uint16_t duration;
>> + uint16_t timeout;
>> + char **uuids;
>> + size_t uuids_cnt;
>
> Lets use l_queue for storing uuids.
Yes it will simplify code. It was based on client/advertising.
>
>> + struct service_data *services;
>> + size_t services_data_len;
>> + struct manufacturer_data *manufacturer;
>> + size_t manufacturer_data_len;
>
> Just make those arrays (this is on heap anyway), will make code simpler.
>
>> + bool tx_power;
>> + bool name;
>> + bool appearance;
>> +} ad =3D {
>> + .local_appearance =3D UINT16_MAX,
>> +};
>> +
>> static bool str2addr(const char *str, uint8_t *addr)
>> {
>> return sscanf(str, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", &addr[5], &add=
r[4],
>> &addr[3], &addr[2], &addr[1], &addr[0]) =
=3D=3D 6;
>> }
>>
>> +static char *dupuuid2str(const uint8_t *uuid, uint8_t len)
>> +{
>> + switch (len) {
>> + case 16:
>> + return l_strdup_printf("%hhx%hhx", uuid[0], uuid[1]);
>> + case 128:
>> + return l_strdup_printf("%hhx%hhx%hhx%hhx%hhx%hhx%hhx%hhx%h=
hx"
>> + "%hhx%hhx%hhx%hhx%hhx%hhx%hhx", uu=
id[0],
>> + uuid[1], uuid[2], uuid[3], uuid[4]=
,
>> + uuid[5], uuid[6], uuid[6], uuid[8]=
,
>> + uuid[7], uuid[10], uuid[11], uuid[=
12],
>> + uuid[13], uuid[14], uuid[15]);
>> + default:
>> + return NULL;
>> + }
>> +}
>> +
>> static struct btp_adapter *find_adapter_by_proxy(struct l_dbus_proxy
>> *proxy) {
>> const struct l_queue_entry *entry;
>> @@ -123,6 +188,8 @@ static void btp_gap_read_commands(uint8_t index, con=
st
>> void *param, commands |=3D (1 << BTP_OP_GAP_SET_CONNECTABLE);
>> commands |=3D (1 << BTP_OP_GAP_SET_DISCOVERABLE);
>> commands |=3D (1 << BTP_OP_GAP_SET_BONDABLE);
>> + commands |=3D (1 << BTP_OP_GAP_START_ADVERTISING);
>> + commands |=3D (1 << BTP_OP_GAP_STOP_ADVERTISING);
>> commands |=3D (1 << BTP_OP_GAP_START_DISCOVERY);
>> commands |=3D (1 << BTP_OP_GAP_STOP_DISCOVERY);
>>
>> @@ -234,6 +301,46 @@ static void remove_device_reply(struct l_dbus_proxy
>> *proxy, l_queue_remove(adapter->devices, device);
>> }
>>
>> +static void unreg_advertising_setup(struct l_dbus_message *message,
>> + void *user=
_data)
>> +{
>> + struct l_dbus_message_builder *builder;
>> +
>> + builder =3D l_dbus_message_builder_new(message);
>> + l_dbus_message_builder_append_basic(builder, 'o', AD_PATH);
>> + l_dbus_message_builder_finalize(builder);
>> + l_dbus_message_builder_destroy(builder);
>> +}
>> +
>> +static void unreg_advertising_reply(struct l_dbus_proxy *proxy,
>> + struct l_dbus_message *res=
ult,
>> + void *user_data)
>> +{
>> + const char *path =3D l_dbus_proxy_get_path(proxy);
>> + struct btp_adapter *adapter =3D find_adapter_by_path(path);
>> +
>> + if (!adapter)
>> + return;
>> +
>> + if (l_dbus_message_is_error(result)) {
>> + const char *name;
>> +
>> + l_dbus_message_get_error(result, &name, NULL);
>> +
>> + l_error("Failed to stop advertising %s (%s)",
>> + l_dbus_proxy_get_path(proxy), name=
);
>> + return;
>> + }
>> +
>> + if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
>> + l_info("Unable to remove ad instance");
>> + if (!l_dbus_object_remove_interface(dbus, AD_PATH,
>> + L_DBUS_INTERFACE_PROPERTIE=
S))
>> + l_info("Unable to remove propety instance");
>> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
>> + l_info("Unable to unregister ad interface");
>> +}
>> +
>> static void btp_gap_reset(uint8_t index, const void *param, uint16_t
>> length, void *user_data)
>> {
>> @@ -264,6 +371,16 @@ static void btp_gap_reset(uint8_t index, const void
>> *param, uint16_t length, NULL);
>> }
>>
>> + if (adapter->ad_proxy)
>> + if (!l_dbus_proxy_method_call(adapter->ad_proxy,
>> + "UnregisterAdvertisement",
>> + unreg_advertising_setup,
>> + unreg_advertising_reply,
>> + NULL, NULL)) {
>> + status =3D BTP_ERROR_FAIL;
>> + goto failed;
>> + }
>> +
>> /* TODO for we assume all went well */
>> btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_RESET, index, 0, NULL);
>> return;
>> @@ -449,6 +566,536 @@ failed:
>> btp_send_error(btp, BTP_GAP_SERVICE, index, status);
>> }
>>
>> +static struct l_dbus_message *ad_release_call(struct l_dbus *dbus,
>> + struct l_dbus_message *mes=
sage,
>> + void *user_data)
>> +{
>> + struct l_dbus_message *reply;
>> + uint8_t i;
>> +
>> + l_dbus_unregister_object(dbus, AD_PATH);
>> + l_dbus_unregister_interface(dbus, AD_IFACE);
>> +
>> + reply =3D l_dbus_message_new_method_return(message);
>> + l_dbus_message_set_arguments(reply, "");
>> +
>> + for (i =3D 0; i < ad.uuids_cnt; i++)
>> + l_free(ad.uuids[i]);
>> + l_free(ad.uuids);
>> + ad.uuids_cnt =3D 0;
>> +
>> + l_free(ad.local_name);
>> +
>> + for (i =3D 0; i < ad.services_data_len; i++)
>> + l_free(ad.services[i].uuid);
>> + l_free(ad.services);
>> + ad.services_data_len =3D 0;
>> +
>> + l_free(ad.manufacturer);
>> + ad.manufacturer_data_len =3D 0;
>
> You are leaving some dangling pointers in ad here.
Ok. Changing those allocated data to list should get rid of this issue.
>
>> +
>> + return reply;
>> +}
>> +
>> +static bool ad_type_getter(struct l_dbus *dbus, struct l_dbus_message
>> *message, + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + l_dbus_message_builder_append_basic(builder, 's', ad.type);
>> +
>> + return true;
>> +}
>> +
>> +static bool ad_serviceuuids_getter(struct l_dbus *dbus,
>> + struct l_dbus_message *message,
>> + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + size_t i;
>> +
>> + if (!ad.uuids_cnt)
>> + return false;
>> +
>> + l_dbus_message_builder_enter_array(builder, "s");
>> +
>> + for (i =3D 0; i < ad.uuids_cnt; i++)
>> + l_dbus_message_builder_append_basic(builder, 's', ad.uuids=
[i]);
>> +
>> + l_dbus_message_builder_leave_array(builder);
>> +
>> + return true;
>> +}
>> +
>> +static bool ad_servicedata_getter(struct l_dbus *dbus,
>> + struct l_dbus_message *message,
>> + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + const uint8_t *array;
>> + size_t i, j;
>> +
>> + if (!ad.services_data_len)
>> + return false;
>> +
>> + l_dbus_message_builder_enter_array(builder, "{sv}");
>> +
>> + for (j =3D 0; j < ad.services_data_len; j++) {
>> + array =3D ad.services[j].data.data;
>> +
>> + l_dbus_message_builder_enter_dict(builder, "sv");
>> + l_dbus_message_builder_append_basic(builder, 's',
>> + ad.services[j].uui=
d);
>> + l_dbus_message_builder_enter_variant(builder, "ay");
>> + l_dbus_message_builder_enter_array(builder, "y");
>> +
>> + for (i =3D 0; i < ad.services[j].data.len; i++)
>> + l_dbus_message_builder_append_basic(builder, 'y',
>> + &(array[i]=
));
>> +
>> + l_dbus_message_builder_leave_array(builder);
>> + l_dbus_message_builder_leave_variant(builder);
>> + l_dbus_message_builder_leave_dict(builder);
>> + }
>> + l_dbus_message_builder_leave_array(builder);
>> +
>> + return true;
>> +}
>> +
>> +static bool ad_manufacturerdata_getter(struct l_dbus *dbus,
>> + struct l_dbus_message *message,
>> + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + const uint8_t *array;
>> + size_t i, j;
>> +
>> + if (!ad.manufacturer_data_len)
>> + return false;
>> +
>> + l_dbus_message_builder_enter_array(builder, "{qv}");
>> +
>> + for (j =3D 0; j < ad.manufacturer_data_len; j++) {
>> + array =3D ad.manufacturer[j].data.data;
>> +
>> + l_dbus_message_builder_enter_dict(builder, "qv");
>> + l_dbus_message_builder_append_basic(builder, 'q',
>> + &ad.manufacturer[j=
].id);
>> + l_dbus_message_builder_enter_variant(builder, "ay");
>> + l_dbus_message_builder_enter_array(builder, "y");
>> +
>> + for (i =3D 0; i < ad.manufacturer[j].data.len; i++)
>> + l_dbus_message_builder_append_basic(builder, 'y',
>> + &(array[i]=
));
>> +
>> + l_dbus_message_builder_leave_array(builder);
>> + l_dbus_message_builder_leave_variant(builder);
>> + l_dbus_message_builder_leave_dict(builder);
>> + }
>> + l_dbus_message_builder_leave_array(builder);
>> +
>> + return true;
>> +}
>> +
>> +static bool ad_includes_getter(struct l_dbus *dbus,
>> + struct l_dbus_message *message,
>> + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + l_dbus_message_builder_enter_array(builder, "s");
>> +
>> + if (!(ad.tx_power || ad.name || ad.appearance))
>> + return false;
>> +
>> + if (ad.tx_power) {
>> + const char *str =3D "tx-power";
>> +
>> + l_dbus_message_builder_append_basic(builder, 's', &str);
>> + }
>> +
>> + if (ad.name) {
>> + const char *str =3D "local-name";
>> +
>> + l_dbus_message_builder_append_basic(builder, 's', &str);
>> + }
>> +
>> + if (ad.appearance) {
>> + const char *str =3D "appearance";
>> +
>> + l_dbus_message_builder_append_basic(builder, 's', &str);
>> + }
>> +
>> + l_dbus_message_builder_leave_array(builder);
>> +
>> + return true;
>> +}
>> +
>> +static bool ad_localname_getter(struct l_dbus *dbus,
>> + struct l_dbus_message *message,
>> + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + if (!ad.local_name)
>> + return false;
>> +
>> + l_dbus_message_builder_append_basic(builder, 's', ad.local_name);
>> +
>> + return true;
>> +}
>> +
>> +static bool ad_appearance_getter(struct l_dbus *dbus,
>> + struct l_dbus_message *message,
>> + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + if (!ad.local_appearance)
>> + return false;
>> +
>> + l_dbus_message_builder_append_basic(builder, 'q', &ad.local_appear=
ance);
>> +
>> + return true;
>> +}
>> +
>> +static bool ad_duration_getter(struct l_dbus *dbus,
>> + struct l_dbus_message *message,
>> + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + if (!ad.duration)
>> + return false;
>> +
>> + l_dbus_message_builder_append_basic(builder, 'q', &ad.duration);
>> +
>> + return true;
>> +}
>> +
>> +static bool ad_timeout_getter(struct l_dbus *dbus,
>> + struct l_dbus_message *message,
>> + struct l_dbus_message_builder *bui=
lder,
>> + void *user_data)
>> +{
>> + if (!ad.timeout)
>> + return false;
>> +
>> + l_dbus_message_builder_append_basic(builder, 'q', &ad.timeout);
>> +
>> + return true;
>> +}
>> +
>> +static void setup_ad_interface(struct l_dbus_interface *interface)
>> +{
>> + l_dbus_interface_method(interface, "Release", 0, ad_release_call, =
"",
>> + ""=
);
>> + l_dbus_interface_property(interface, "Type", 0, "s", ad_type_gette=
r,
>> + NU=
LL);
>> + l_dbus_interface_property(interface, "ServiceUUIDs", 0, "as",
>> + ad_serviceuuids_getter, NU=
LL);
>> + l_dbus_interface_property(interface, "ServiceData", 0, "a{sv}",
>> + ad_servicedata_getter, NUL=
L);
>> + l_dbus_interface_property(interface, "ManufacturerServiceData", 0,
>> + "a{qv}", ad_manufacturerdata_gette=
r,
>> + NULL);
>> + l_dbus_interface_property(interface, "Includes", 0, "as",
>> + ad_includes_getter, NULL);
>> + l_dbus_interface_property(interface, "LocalName", 0, "s",
>> + ad_localname_getter, NULL)=
;
>> + l_dbus_interface_property(interface, "Appearance", 0, "q",
>> + ad_appearance_getter, NULL=
);
>> + l_dbus_interface_property(interface, "Duration", 0, "q",
>> + ad_duration_getter, NULL);
>> + l_dbus_interface_property(interface, "Timeout", 0, "q",
>> + ad_timeout_getter, NULL);
>> + return;
>
> Redundant return.
Will be fixed.
>
>> +}
>> +
>> +static void start_advertising_reply(struct l_dbus_proxy *proxy,
>> + struct l_dbus_message *res=
ult,
>> + void *user_data)
>> +{
>> + const char *path =3D l_dbus_proxy_get_path(proxy);
>> + struct btp_adapter *adapter =3D find_adapter_by_path(path);
>> + uint32_t settings;
>> +
>> + if (!adapter) {
>> + btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROL=
LER,
>> + BTP_ERROR_=
FAIL);
>> + return;
>> + }
>> +
>> + if (l_dbus_message_is_error(result)) {
>> + const char *name, *desc;
>> +
>> + l_dbus_message_get_error(result, &name, &desc);
>> + l_error("Failed to start advertising (%s), %s", name, desc=
);
>> +
>> + btp_send_error(btp, BTP_GAP_SERVICE, adapter->index,
>> + BTP_ERROR_=
FAIL);
>> + return;
>> + }
>> +
>> + settings =3D L_CPU_TO_LE32(adapter->current_settings);
>
> Shouldn't we set advertising setting here?
>
>> +
>> + btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_ADVERTISING,
>> + adapter->index, sizeof(settings), &setting=
s);
>> +}
>> +
>> +static void create_advertising_data(uint8_t adv_data_len, const uint8_t
>> *data) +{
>> + const uint8_t *ad_data;
>> + uint8_t ad_type, ad_len;
>> + uint8_t remaining_data_len =3D adv_data_len;
>> +
>> + while (remaining_data_len) {
>> + ad_type =3D data[adv_data_len - remaining_data_len];
>> + ad_len =3D data[adv_data_len - remaining_data_len + 1];
>> + ad_data =3D &data[adv_data_len - remaining_data_len + 2];
>> +
>> + switch (ad_type) {
>> + case AD_TYPE_INCOMPLETE_UUID16_SERVICE_LIST:
>> + ad.uuids_cnt +=3D 1;
>> + ad.uuids =3D realloc(ad.uuids,
>> + ad.uuids_cnt * sizeof(ad.uuids));
>> + ad.uuids[ad.uuids_cnt - 1] =3D dupuuid2str(ad_data=
, 16);
>> +
>> + break;
>> + case AD_TYPE_SHORT_NAME:
>> + ad.local_name =3D malloc(ad_len + 1);
>> + memcpy(ad.local_name, ad_data, ad_len);
>> + ad.local_name[ad_len] =3D '\0';
>> +
>> + break;
>> + case AD_TYPE_SERVICE_DATA_UUID16:
>> + {
>> + uint8_t svc_id;
>> +
>> + ad.services_data_len +=3D 1;
>> + svc_id =3D ad.services_data_len - 1;
>> + ad.services =3D l_realloc(ad.services,
>> + sizeof(struct service_data=
) *
>> + ad.services_data_len);
>> + /* The first 2 octets contain the 16 bit Service U=
UID
>> + * followed by additional service data
>> + */
>> + ad.services[svc_id].uuid =3D dupuuid2str(ad_data, =
16);
>> + ad.services[svc_id].data.len =3D ad_len - 2;
>> + memcpy(ad.services[svc_id].data.data, ad_data + 2,
>> + ad.services[svc_id].data.l=
en);
>> +
>> + break;
>> + }
>> + case AD_TYPE_APPEARANCE:
>> + memcpy(&ad.local_appearance, ad_data, ad_len);
>> +
>> + break;
>> + case AD_TYPE_MANUFACTURER_DATA:
>> + {
>> + uint8_t md_id;
>> +
>> + ad.manufacturer_data_len +=3D 1;
>> + md_id =3D ad.manufacturer_data_len - 1;
>> + ad.manufacturer =3D l_realloc(ad.manufacturer,
>> + sizeof(struct manufacturer_data) *
>> + ad.manufacturer_data_len);
>> + /* The first 2 octets contain the Company Identifi=
er
>> + * Code followed by additional manufacturer specif=
ic
>> + * data.
>> + */
>> + memcpy(&ad.manufacturer[md_id].id, ad_data, 2);
>> + ad.manufacturer[md_id].data.len =3D ad_len - 2;
>> + memcpy(&ad.manufacturer[md_id].data.data, ad_data =
+ 2,
>> + ad.manufacturer[md_id].data.len);
>> +
>> + break;
>> + }
>
> Missing default.
Will be fixed.
>
>> + }
>> + /* Advertising entity data len + advertising entity header
>> + * (type, len)
>> + */
>> + remaining_data_len -=3D ad_len + 2;
>> + }
>> +}
>> +
>> +struct start_advertising_data {
>> + uint32_t settings;
>> + struct btp_gap_start_adv_cp cp;
>> +};
>> +
>> +static void create_scan_response(uint8_t scan_rsp_len, const uint8_t *d=
ata)
>> +{
>> + /* TODO */
>> +}
>> +
>> +static void start_advertising_setup(struct l_dbus_message *message,
>> + void *user_data)
>> +{
>> + const struct start_advertising_data *sad =3D user_data;
>> + const struct btp_gap_start_adv_cp *cp =3D &sad->cp;
>> + struct l_dbus_message_builder *builder;
>> +
>> + if ((sad->settings >> BTP_GAP_SETTING_CONNECTABLE) & 1U)
>
> Settings are already bit masks.
> (I think we have same bug in btp_gap_set_connectable())
I agree, here we simply can "&" settings with connectable mask. But in
set connectable I don't see bug.
>
>> + ad.type =3D l_strdup("peripheral");
>> + else
>> + ad.type =3D l_strdup("broadcast");
>> +
>> + if (cp->adv_data_len !=3D 0)
>> + create_advertising_data(cp->adv_data_len, cp->data);
>> + if (cp->scan_rsp_len !=3D 0)
>> + create_scan_response(cp->scan_rsp_len,
>> + cp->data + cp->scan_rsp_le=
n);
>> +
>> + free(user_data);
>> +
>> + builder =3D l_dbus_message_builder_new(message);
>> + l_dbus_message_builder_append_basic(builder, 'o', AD_PATH);
>> + l_dbus_message_builder_enter_array(builder, "{sv}");
>> + l_dbus_message_builder_enter_dict(builder, "sv");
>> + l_dbus_message_builder_leave_dict(builder);
>> + l_dbus_message_builder_leave_array(builder);
>> + l_dbus_message_builder_finalize(builder);
>> + l_dbus_message_builder_destroy(builder);
>> +}
>> +
>> +static void btp_gap_start_advertising(uint8_t index, const void *param,
>> + uint16_t length, void *user_data)
>> +{
>> + struct btp_adapter *adapter =3D find_adapter_by_index(index);
>> + const struct btp_gap_start_adv_cp *cp =3D param;
>> + struct start_advertising_data *data;
>> + uint8_t status =3D BTP_ERROR_FAIL;
>> + bool prop;
>> + void *buff;
>> +
>> + if (!adapter) {
>> + status =3D BTP_ERROR_INVALID_INDEX;
>> + goto failed;
>> + }
>> +
>> + /* Adapter needs to be powered to be able to remove devices */
>> + if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &pr=
op) ||
>> + !p=
rop)
>> + goto failed;
>> +
>> + if (!l_dbus_register_interface(dbus, AD_IFACE, setup_ad_interface,=
NULL,
>> + fa=
lse)) {
>> + l_info("Unable to register ad interface");
>> + goto failed;
>> + }
>> +
>> + if (!l_dbus_object_add_interface(dbus, AD_PATH, AD_IFACE, NULL)) {
>> + l_info("Unable to instantiate ad interface");
>> +
>> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
>> + l_info("Unable to unregister ad interface");
>> +
>> + goto failed;
>> + }
>> +
>> + if (!l_dbus_object_add_interface(dbus, AD_PATH,
>> + L_DBUS_INTERFACE_PROPERTIE=
S,
>> + NULL)) {
>> + l_info("Unable to instantiate the properties interface");
>> +
>> + if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFAC=
E))
>> + l_info("Unable to remove ad instance");
>> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
>> + l_info("Unable to unregister ad interface");
>> +
>> + goto failed;
>> + }
>> +
>> + buff =3D l_malloc(sizeof(struct start_advertising_data) +
>> + cp->adv_data_len + cp->scan_rsp_le=
n);
>> + data =3D buff;
>
> buff is not needed. Just assing to data directly.
>
> Also do paring of those buffers to ad here, instead of setup callback. Th=
is
> will allow for early error check (as setup cb returns void).
Agree.
>
>> + data->settings =3D adapter->current_settings;
>> + data->cp.adv_data_len =3D cp->adv_data_len;
>> + data->cp.scan_rsp_len =3D cp->scan_rsp_len;
>> + memcpy(data->cp.data, cp->data, cp->adv_data_len + cp->scan_rsp_le=
n);
>> +
>> + if (!l_dbus_proxy_method_call(adapter->ad_proxy,
>> + "RegisterAdvertise=
ment",
>> + start_advertising_=
setup,
>> + start_advertising_=
reply,
>> + data, NULL)) {
>> + if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFAC=
E))
>> + l_info("Unable to remove ad instance");
>> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
>> + l_info("Unable to unregister ad interface");
>> +
>> + free(buff);
>> + goto failed;
>> + }
>> +
>> + return;
>> +
>> +failed:
>> + btp_send_error(btp, BTP_GAP_SERVICE, index, status);
>> +}
>> +
>> +static void stop_advertising_reply(struct l_dbus_proxy *proxy,
>> + struct l_dbus_message *res=
ult,
>> + void *user_data)
>> +{
>> + const char *path =3D l_dbus_proxy_get_path(proxy);
>> + struct btp_adapter *adapter =3D find_adapter_by_path(path);
>> + uint32_t settings;
>> +
>> + if (!adapter)
>> + return;
>> +
>> + if (l_dbus_message_is_error(result)) {
>> + const char *name;
>> +
>> + l_dbus_message_get_error(result, &name, NULL);
>> +
>> + l_error("Failed to stop advertising %s (%s)",
>> + l_dbus_proxy_get_path(proxy), name=
);
>> + return;
>> + }
>> +
>> + if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
>> + l_info("Unable to remove ad instance");
>> + if (!l_dbus_object_remove_interface(dbus, AD_PATH,
>> + L_DBUS_INTERFACE_PROPERTIE=
S))
>> + l_info("Unable to remove propety instance");
>> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
>> + l_info("Unable to unregister ad interface");
>> +
>> + settings =3D L_CPU_TO_LE32(adapter->current_settings);
>> +
>> + btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_ADVERTISING,
>> + adapter->index, sizeof(settings), &setting=
s);
>> +}
>> +
>> +static void btp_gap_stop_advertising(uint8_t index, const void *param,
>> + uint16_t length, void *user_data)
>> +{
>> + struct btp_adapter *adapter =3D find_adapter_by_index(index);
>> + uint8_t status =3D BTP_ERROR_FAIL;
>> + bool prop;
>> +
>> + if (!adapter) {
>> + status =3D BTP_ERROR_INVALID_INDEX;
>> + goto failed;
>> + }
>> +
>> + if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &pr=
op) ||
>> + !p=
rop)
>> + goto failed;
>> +
>> + if (adapter->ad_proxy) {
>> + if (!l_dbus_proxy_method_call(adapter->ad_proxy,
>> + "UnregisterAdvertisement",
>> + unreg_advertising_setup,
>> + stop_advertising_reply,
>> + NULL, NULL)) {
>> + status =3D BTP_ERROR_FAIL;
>> + goto failed;
>> + }
>> + }
>> +
>> +failed:
>> + btp_send_error(btp, BTP_GAP_SERVICE, index, status);
>> +}
>> +
>> static void start_discovery_reply(struct l_dbus_proxy *proxy,
>> struct l_dbus_message *res=
ult,
>> void *user_data)
>> @@ -728,6 +1375,12 @@ static void register_gap_service(void)
>> btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_SET_BONDABLE,
>> btp_gap_set_bondable, NULL, NULL);
>>
>> + btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_ADVERTISING,
>> + btp_gap_start_advertising, NULL, N=
ULL);
>> +
>> + btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_ADVERTISING,
>> + btp_gap_stop_advertising, NULL, NU=
LL);
>> +
>> btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_DISCOVERY,
>> btp_gap_start_discovery, NULL, NUL=
L);
>>
>> @@ -1124,6 +1777,12 @@ static void client_ready(struct l_dbus_client
>> *client, void *user_data) BTP_INDEX_NON_CONTROLLER, 0, NULL);
>> }
>>
>> +static void ready_callback(void *user_data)
>> +{
>> + if (!l_dbus_object_manager_enable(dbus))
>> + l_info("Unable to register the ObjectManager");
>> +}
>> +
>> static void usage(void)
>> {
>> l_info("btpclient - Bluetooth tester");
>> @@ -1148,7 +1807,6 @@ int main(int argc, char *argv[])
>> {
>> struct l_dbus_client *client;
>> struct l_signal *signal;
>> - struct l_dbus *dbus;
>> sigset_t mask;
>> int opt;
>>
>> @@ -1192,6 +1850,7 @@ int main(int argc, char *argv[])
>> signal =3D l_signal_create(&mask, signal_handler, NULL, NULL);
>>
>> dbus =3D l_dbus_new_default(L_DBUS_SYSTEM_BUS);
>> + l_dbus_set_ready_handler(dbus, ready_callback, NULL, NULL);
>> client =3D l_dbus_client_new(dbus, "org.bluez", "/org/bluez");
>>
>> l_dbus_client_set_connect_handler(client, client_connected, NULL, =
NULL);
>
>
> --
> pozdrawiam
> Szymon Janc
>
>
Pozdrawiam,
Grzegorz Ko=C5=82odziejczyk
Hi,
On Wed, Jan 10, 2018, Szymon Janc wrote:
> On Tuesday, 9 January 2018 16:45:20 CET Grzegorz Kolodziejczyk wrote:
> > +static bool addr2str(const uint8_t *addr, char *str)
> > +{
> > + return sprintf(str, "%02hhX:%02hhX:%02hhX:%02hhX:%02hhX:%02hhX",
> > + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5])
> > + == 17;
> > +}
>
> Use snprintf.
Why isn't this tool using the address conversion helpers from
lib/bluetooth.c?
Johan
Hi Grzegorz,
On Tuesday, 9 January 2018 16:45:20 CET Grzegorz Kolodziejczyk wrote:
> This patch adds start and stop connect, disconnect commands for btp
> client.
> ---
> tools/btpclient.c | 175
> ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 175
> insertions(+)
>
> diff --git a/tools/btpclient.c b/tools/btpclient.c
> index 472e9e4c2..053ae07a2 100644
> --- a/tools/btpclient.c
> +++ b/tools/btpclient.c
> @@ -107,6 +107,13 @@ static bool str2addr(const char *str, uint8_t *addr)
> &addr[3], &addr[2], &addr[1], &addr[0]) == 6;
> }
>
> +static bool addr2str(const uint8_t *addr, char *str)
> +{
> + return sprintf(str, "%02hhX:%02hhX:%02hhX:%02hhX:%02hhX:%02hhX",
> + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5])
> + == 17;
> +}
Use snprintf.
> +
> static char *dupuuid2str(const uint8_t *uuid, uint8_t len)
> {
> switch (len) {
> @@ -124,6 +131,17 @@ static char *dupuuid2str(const uint8_t *uuid, uint8_t
> len) }
> }
>
> +static bool match_dev_addr_type(const char *addr_type_str, uint8_t
> addr_type) +{
> + if (((addr_type == BTP_GAP_ADDR_PUBLIC) &&
> + (strcmp(addr_type_str, "public"))) ||
> + ((addr_type == BTP_GAP_ADDR_RANDOM) &&
> + strcmp(addr_type_str, "random")))
> + return false;
Split those check for code readiblity.
> +
> + return true;
> +}
> +
> static struct btp_adapter *find_adapter_by_proxy(struct l_dbus_proxy
> *proxy) {
> const struct l_queue_entry *entry;
> @@ -169,6 +187,34 @@ static struct btp_adapter *find_adapter_by_path(const
> char *path) return NULL;
> }
>
> +static struct btp_device *find_device_by_address(struct btp_adapter
> *adapter, + const uint8_t *addr,
> + uint8_t addr_type)
> +{
> + const struct l_queue_entry *entry;
> + const char *str;
> + char addr_str[18];
> +
> + if (!addr2str(addr, addr_str))
> + return NULL;
> +
> + for (entry = l_queue_get_entries(adapter->devices); entry;
> + entry = entry->next) {
> + struct btp_device *device = entry->data;
> +
> + l_dbus_proxy_get_property(device->proxy, "Address", "s", &str);
> + if (strcmp(str, addr_str))
> + continue;
> +
> + l_dbus_proxy_get_property(device->proxy, "AddressType", "s",
> + &str);
> + if (match_dev_addr_type(str, addr_type))
> + return device;
> + }
> +
> + return NULL;
> +}
> +
> static void btp_gap_read_commands(uint8_t index, const void *param,
> uint16_t length, void *user_data)
> {
> @@ -192,6 +238,8 @@ static void btp_gap_read_commands(uint8_t index, const
> void *param, commands |= (1 << BTP_OP_GAP_STOP_ADVERTISING);
> commands |= (1 << BTP_OP_GAP_START_DISCOVERY);
> commands |= (1 << BTP_OP_GAP_STOP_DISCOVERY);
> + commands |= (1 << BTP_OP_GAP_CONNECT);
> + commands |= (1 << BTP_OP_GAP_DISCONNECT);
>
> commands = L_CPU_TO_LE16(commands);
>
> @@ -1314,6 +1362,127 @@ static void btp_gap_stop_discovery(uint8_t index,
> const void *param, stop_discovery_reply, NULL, NULL);
> }
>
> +static void connect_reply(struct l_dbus_proxy *proxy,
> + struct l_dbus_message *result, void *user_data)
> +{
> + uint8_t adapter_index = L_PTR_TO_UINT(user_data);
> + struct btp_adapter *adapter = find_adapter_by_index(adapter_index);
> +
> + if (!adapter) {
> + btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
> + BTP_ERROR_FAIL);
> + return;
> + }
> +
> + if (l_dbus_message_is_error(result)) {
> + const char *name, *desc;
> +
> + l_dbus_message_get_error(result, &name, &desc);
> + l_error("Failed to connect (%s), %s", name, desc);
> +
> + btp_send_error(btp, BTP_GAP_SERVICE, adapter_index,
> + BTP_ERROR_FAIL);
> + return;
> + }
> +
> + btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_CONNECT, adapter_index, 0,
> + NULL);
> +}
> +
> +static void btp_gap_connect(uint8_t index, const void *param, uint16_t
> length, + void *user_data)
> +{
> + struct btp_adapter *adapter = find_adapter_by_index(index);
> + const struct btp_gap_connect_cp *cp = param;
> + struct btp_device *device;
> + bool prop;
> + uint8_t status = BTP_ERROR_FAIL;
> +
> + if (!adapter) {
> + status = BTP_ERROR_INVALID_INDEX;
> + goto failed;
> + }
> +
> + /* Adapter needs to be powered to be able to connect */
> + if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
> + !prop)
> + goto failed;
> +
> + device = find_device_by_address(adapter, cp->address, cp->address_type);
> +
> + if (!device)
> + goto failed;
> +
> + l_dbus_proxy_method_call(device->proxy, "Connect", NULL, connect_reply,
> + L_UINT_TO_PTR(adapter->index), NULL);
> +
> + return;
> +
> +failed:
> + btp_send_error(btp, BTP_GAP_SERVICE, index, status);
> +}
> +
> +static void disconnect_reply(struct l_dbus_proxy *proxy,
> + struct l_dbus_message *result, void *user_data)
> +{
> + uint8_t adapter_index = L_PTR_TO_UINT(user_data);
> + struct btp_adapter *adapter = find_adapter_by_index(adapter_index);
> +
> + if (!adapter) {
> + btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
> + BTP_ERROR_FAIL);
> + return;
> + }
> +
> + if (l_dbus_message_is_error(result)) {
> + const char *name, *desc;
> +
> + l_dbus_message_get_error(result, &name, &desc);
> + l_error("Failed to disconnect (%s), %s", name, desc);
> +
> + btp_send_error(btp, BTP_GAP_SERVICE, adapter_index,
> + BTP_ERROR_FAIL);
> + return;
> + }
> +
> + btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_DISCONNECT, adapter_index, 0,
> + NULL);
> +}
> +
> +static void btp_gap_disconnect(uint8_t index, const void *param,
> + uint16_t length, void *user_data)
> +{
> + struct btp_adapter *adapter = find_adapter_by_index(index);
> + const struct btp_gap_disconnect_cp *cp = param;
> + uint8_t status = BTP_ERROR_FAIL;
> + struct btp_device *device;
> + bool prop;
> +
> + if (!adapter) {
> + status = BTP_ERROR_INVALID_INDEX;
> + goto failed;
> + }
> +
> + /* Adapter needs to be powered to be able to connect */
> + if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
> + !prop)
> + goto failed;
> +
> + device = find_device_by_address(adapter, cp->address, cp->address_type);
> +
> + if (!device)
> + goto failed;
> +
> + l_dbus_proxy_method_call(device->proxy, "Disconnect", NULL,
> + disconnect_reply,
> + L_UINT_TO_PTR(adapter->index), NULL);
> +
> + return;
> +
> +failed:
> + btp_send_error(btp, BTP_GAP_SERVICE, index, status);
> +}
> +
> static void btp_gap_device_found_ev(struct l_dbus_proxy *proxy)
> {
> struct btp_device_found_ev ev;
> @@ -1386,6 +1555,12 @@ static void register_gap_service(void)
>
> btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_DISCOVERY,
> btp_gap_stop_discovery, NULL, NULL);
> +
> + btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_CONNECT, btp_gap_connect,
> + NULL, NULL);
> +
> + btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_CONNECT,
> + btp_gap_disconnect, NULL, NULL);
> }
>
> static void btp_core_read_commands(uint8_t index, const void *param,
--
pozdrawiam
Szymon Janc
Hi Grzegorz,
On Tuesday, 9 January 2018 16:45:19 CET Grzegorz Kolodziejczyk wrote:
> This patch adds start and stop advertising commands for btp client.
> ---
> tools/btpclient.c | 661
> +++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 660
> insertions(+), 1 deletion(-)
>
> diff --git a/tools/btpclient.c b/tools/btpclient.c
> index 806403f6a..472e9e4c2 100644
> --- a/tools/btpclient.c
> +++ b/tools/btpclient.c
> @@ -34,6 +34,19 @@
>
> #include "src/shared/btp.h"
>
> +#define AD_PATH "/org/bluez/advertising"
> +#define AD_IFACE "org.bluez.LEAdvertisement1"
> +
> +/* List of assigned numbers for advetising data and scan response */
> +#define AD_TYPE_FLAGS 0x01
> +#define AD_TYPE_INCOMPLETE_UUID16_SERVICE_LIST 0x02
> +#define AD_TYPE_SHORT_NAME 0x08
> +#define AD_TYPE_SERVICE_DATA_UUID16 0x16
> +#define AD_TYPE_APPEARANCE 0x19
> +#define AD_TYPE_MANUFACTURER_DATA 0xff
> +
> +struct l_dbus *dbus;
Should be static.
> +
> struct btp_adapter {
> struct l_dbus_proxy *proxy;
> struct l_dbus_proxy *ad_proxy;
> @@ -53,12 +66,64 @@ static struct btp *btp;
>
> static bool gap_service_registered;
>
> +struct ad_data {
> + uint8_t data[25];
> + uint8_t len;
> +};
> +
> +struct service_data {
> + char *uuid;
> + struct ad_data data;
> +};
> +
> +struct manufacturer_data {
> + uint16_t id;
> + struct ad_data data;
> +};
> +
> +static struct ad {
> + bool registered;
> + char *type;
> + char *local_name;
> + uint16_t local_appearance;
> + uint16_t duration;
> + uint16_t timeout;
> + char **uuids;
> + size_t uuids_cnt;
Lets use l_queue for storing uuids.
> + struct service_data *services;
> + size_t services_data_len;
> + struct manufacturer_data *manufacturer;
> + size_t manufacturer_data_len;
Just make those arrays (this is on heap anyway), will make code simpler.
> + bool tx_power;
> + bool name;
> + bool appearance;
> +} ad = {
> + .local_appearance = UINT16_MAX,
> +};
> +
> static bool str2addr(const char *str, uint8_t *addr)
> {
> return sscanf(str, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", &addr[5], &addr[4],
> &addr[3], &addr[2], &addr[1], &addr[0]) == 6;
> }
>
> +static char *dupuuid2str(const uint8_t *uuid, uint8_t len)
> +{
> + switch (len) {
> + case 16:
> + return l_strdup_printf("%hhx%hhx", uuid[0], uuid[1]);
> + case 128:
> + return l_strdup_printf("%hhx%hhx%hhx%hhx%hhx%hhx%hhx%hhx%hhx"
> + "%hhx%hhx%hhx%hhx%hhx%hhx%hhx", uuid[0],
> + uuid[1], uuid[2], uuid[3], uuid[4],
> + uuid[5], uuid[6], uuid[6], uuid[8],
> + uuid[7], uuid[10], uuid[11], uuid[12],
> + uuid[13], uuid[14], uuid[15]);
> + default:
> + return NULL;
> + }
> +}
> +
> static struct btp_adapter *find_adapter_by_proxy(struct l_dbus_proxy
> *proxy) {
> const struct l_queue_entry *entry;
> @@ -123,6 +188,8 @@ static void btp_gap_read_commands(uint8_t index, const
> void *param, commands |= (1 << BTP_OP_GAP_SET_CONNECTABLE);
> commands |= (1 << BTP_OP_GAP_SET_DISCOVERABLE);
> commands |= (1 << BTP_OP_GAP_SET_BONDABLE);
> + commands |= (1 << BTP_OP_GAP_START_ADVERTISING);
> + commands |= (1 << BTP_OP_GAP_STOP_ADVERTISING);
> commands |= (1 << BTP_OP_GAP_START_DISCOVERY);
> commands |= (1 << BTP_OP_GAP_STOP_DISCOVERY);
>
> @@ -234,6 +301,46 @@ static void remove_device_reply(struct l_dbus_proxy
> *proxy, l_queue_remove(adapter->devices, device);
> }
>
> +static void unreg_advertising_setup(struct l_dbus_message *message,
> + void *user_data)
> +{
> + struct l_dbus_message_builder *builder;
> +
> + builder = l_dbus_message_builder_new(message);
> + l_dbus_message_builder_append_basic(builder, 'o', AD_PATH);
> + l_dbus_message_builder_finalize(builder);
> + l_dbus_message_builder_destroy(builder);
> +}
> +
> +static void unreg_advertising_reply(struct l_dbus_proxy *proxy,
> + struct l_dbus_message *result,
> + void *user_data)
> +{
> + const char *path = l_dbus_proxy_get_path(proxy);
> + struct btp_adapter *adapter = find_adapter_by_path(path);
> +
> + if (!adapter)
> + return;
> +
> + if (l_dbus_message_is_error(result)) {
> + const char *name;
> +
> + l_dbus_message_get_error(result, &name, NULL);
> +
> + l_error("Failed to stop advertising %s (%s)",
> + l_dbus_proxy_get_path(proxy), name);
> + return;
> + }
> +
> + if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
> + l_info("Unable to remove ad instance");
> + if (!l_dbus_object_remove_interface(dbus, AD_PATH,
> + L_DBUS_INTERFACE_PROPERTIES))
> + l_info("Unable to remove propety instance");
> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
> + l_info("Unable to unregister ad interface");
> +}
> +
> static void btp_gap_reset(uint8_t index, const void *param, uint16_t
> length, void *user_data)
> {
> @@ -264,6 +371,16 @@ static void btp_gap_reset(uint8_t index, const void
> *param, uint16_t length, NULL);
> }
>
> + if (adapter->ad_proxy)
> + if (!l_dbus_proxy_method_call(adapter->ad_proxy,
> + "UnregisterAdvertisement",
> + unreg_advertising_setup,
> + unreg_advertising_reply,
> + NULL, NULL)) {
> + status = BTP_ERROR_FAIL;
> + goto failed;
> + }
> +
> /* TODO for we assume all went well */
> btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_RESET, index, 0, NULL);
> return;
> @@ -449,6 +566,536 @@ failed:
> btp_send_error(btp, BTP_GAP_SERVICE, index, status);
> }
>
> +static struct l_dbus_message *ad_release_call(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + void *user_data)
> +{
> + struct l_dbus_message *reply;
> + uint8_t i;
> +
> + l_dbus_unregister_object(dbus, AD_PATH);
> + l_dbus_unregister_interface(dbus, AD_IFACE);
> +
> + reply = l_dbus_message_new_method_return(message);
> + l_dbus_message_set_arguments(reply, "");
> +
> + for (i = 0; i < ad.uuids_cnt; i++)
> + l_free(ad.uuids[i]);
> + l_free(ad.uuids);
> + ad.uuids_cnt = 0;
> +
> + l_free(ad.local_name);
> +
> + for (i = 0; i < ad.services_data_len; i++)
> + l_free(ad.services[i].uuid);
> + l_free(ad.services);
> + ad.services_data_len = 0;
> +
> + l_free(ad.manufacturer);
> + ad.manufacturer_data_len = 0;
You are leaving some dangling pointers in ad here.
> +
> + return reply;
> +}
> +
> +static bool ad_type_getter(struct l_dbus *dbus, struct l_dbus_message
> *message, + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + l_dbus_message_builder_append_basic(builder, 's', ad.type);
> +
> + return true;
> +}
> +
> +static bool ad_serviceuuids_getter(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + size_t i;
> +
> + if (!ad.uuids_cnt)
> + return false;
> +
> + l_dbus_message_builder_enter_array(builder, "s");
> +
> + for (i = 0; i < ad.uuids_cnt; i++)
> + l_dbus_message_builder_append_basic(builder, 's', ad.uuids[i]);
> +
> + l_dbus_message_builder_leave_array(builder);
> +
> + return true;
> +}
> +
> +static bool ad_servicedata_getter(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + const uint8_t *array;
> + size_t i, j;
> +
> + if (!ad.services_data_len)
> + return false;
> +
> + l_dbus_message_builder_enter_array(builder, "{sv}");
> +
> + for (j = 0; j < ad.services_data_len; j++) {
> + array = ad.services[j].data.data;
> +
> + l_dbus_message_builder_enter_dict(builder, "sv");
> + l_dbus_message_builder_append_basic(builder, 's',
> + ad.services[j].uuid);
> + l_dbus_message_builder_enter_variant(builder, "ay");
> + l_dbus_message_builder_enter_array(builder, "y");
> +
> + for (i = 0; i < ad.services[j].data.len; i++)
> + l_dbus_message_builder_append_basic(builder, 'y',
> + &(array[i]));
> +
> + l_dbus_message_builder_leave_array(builder);
> + l_dbus_message_builder_leave_variant(builder);
> + l_dbus_message_builder_leave_dict(builder);
> + }
> + l_dbus_message_builder_leave_array(builder);
> +
> + return true;
> +}
> +
> +static bool ad_manufacturerdata_getter(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + const uint8_t *array;
> + size_t i, j;
> +
> + if (!ad.manufacturer_data_len)
> + return false;
> +
> + l_dbus_message_builder_enter_array(builder, "{qv}");
> +
> + for (j = 0; j < ad.manufacturer_data_len; j++) {
> + array = ad.manufacturer[j].data.data;
> +
> + l_dbus_message_builder_enter_dict(builder, "qv");
> + l_dbus_message_builder_append_basic(builder, 'q',
> + &ad.manufacturer[j].id);
> + l_dbus_message_builder_enter_variant(builder, "ay");
> + l_dbus_message_builder_enter_array(builder, "y");
> +
> + for (i = 0; i < ad.manufacturer[j].data.len; i++)
> + l_dbus_message_builder_append_basic(builder, 'y',
> + &(array[i]));
> +
> + l_dbus_message_builder_leave_array(builder);
> + l_dbus_message_builder_leave_variant(builder);
> + l_dbus_message_builder_leave_dict(builder);
> + }
> + l_dbus_message_builder_leave_array(builder);
> +
> + return true;
> +}
> +
> +static bool ad_includes_getter(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + l_dbus_message_builder_enter_array(builder, "s");
> +
> + if (!(ad.tx_power || ad.name || ad.appearance))
> + return false;
> +
> + if (ad.tx_power) {
> + const char *str = "tx-power";
> +
> + l_dbus_message_builder_append_basic(builder, 's', &str);
> + }
> +
> + if (ad.name) {
> + const char *str = "local-name";
> +
> + l_dbus_message_builder_append_basic(builder, 's', &str);
> + }
> +
> + if (ad.appearance) {
> + const char *str = "appearance";
> +
> + l_dbus_message_builder_append_basic(builder, 's', &str);
> + }
> +
> + l_dbus_message_builder_leave_array(builder);
> +
> + return true;
> +}
> +
> +static bool ad_localname_getter(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + if (!ad.local_name)
> + return false;
> +
> + l_dbus_message_builder_append_basic(builder, 's', ad.local_name);
> +
> + return true;
> +}
> +
> +static bool ad_appearance_getter(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + if (!ad.local_appearance)
> + return false;
> +
> + l_dbus_message_builder_append_basic(builder, 'q', &ad.local_appearance);
> +
> + return true;
> +}
> +
> +static bool ad_duration_getter(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + if (!ad.duration)
> + return false;
> +
> + l_dbus_message_builder_append_basic(builder, 'q', &ad.duration);
> +
> + return true;
> +}
> +
> +static bool ad_timeout_getter(struct l_dbus *dbus,
> + struct l_dbus_message *message,
> + struct l_dbus_message_builder *builder,
> + void *user_data)
> +{
> + if (!ad.timeout)
> + return false;
> +
> + l_dbus_message_builder_append_basic(builder, 'q', &ad.timeout);
> +
> + return true;
> +}
> +
> +static void setup_ad_interface(struct l_dbus_interface *interface)
> +{
> + l_dbus_interface_method(interface, "Release", 0, ad_release_call, "",
> + "");
> + l_dbus_interface_property(interface, "Type", 0, "s", ad_type_getter,
> + NULL);
> + l_dbus_interface_property(interface, "ServiceUUIDs", 0, "as",
> + ad_serviceuuids_getter, NULL);
> + l_dbus_interface_property(interface, "ServiceData", 0, "a{sv}",
> + ad_servicedata_getter, NULL);
> + l_dbus_interface_property(interface, "ManufacturerServiceData", 0,
> + "a{qv}", ad_manufacturerdata_getter,
> + NULL);
> + l_dbus_interface_property(interface, "Includes", 0, "as",
> + ad_includes_getter, NULL);
> + l_dbus_interface_property(interface, "LocalName", 0, "s",
> + ad_localname_getter, NULL);
> + l_dbus_interface_property(interface, "Appearance", 0, "q",
> + ad_appearance_getter, NULL);
> + l_dbus_interface_property(interface, "Duration", 0, "q",
> + ad_duration_getter, NULL);
> + l_dbus_interface_property(interface, "Timeout", 0, "q",
> + ad_timeout_getter, NULL);
> + return;
Redundant return.
> +}
> +
> +static void start_advertising_reply(struct l_dbus_proxy *proxy,
> + struct l_dbus_message *result,
> + void *user_data)
> +{
> + const char *path = l_dbus_proxy_get_path(proxy);
> + struct btp_adapter *adapter = find_adapter_by_path(path);
> + uint32_t settings;
> +
> + if (!adapter) {
> + btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
> + BTP_ERROR_FAIL);
> + return;
> + }
> +
> + if (l_dbus_message_is_error(result)) {
> + const char *name, *desc;
> +
> + l_dbus_message_get_error(result, &name, &desc);
> + l_error("Failed to start advertising (%s), %s", name, desc);
> +
> + btp_send_error(btp, BTP_GAP_SERVICE, adapter->index,
> + BTP_ERROR_FAIL);
> + return;
> + }
> +
> + settings = L_CPU_TO_LE32(adapter->current_settings);
Shouldn't we set advertising setting here?
> +
> + btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_ADVERTISING,
> + adapter->index, sizeof(settings), &settings);
> +}
> +
> +static void create_advertising_data(uint8_t adv_data_len, const uint8_t
> *data) +{
> + const uint8_t *ad_data;
> + uint8_t ad_type, ad_len;
> + uint8_t remaining_data_len = adv_data_len;
> +
> + while (remaining_data_len) {
> + ad_type = data[adv_data_len - remaining_data_len];
> + ad_len = data[adv_data_len - remaining_data_len + 1];
> + ad_data = &data[adv_data_len - remaining_data_len + 2];
> +
> + switch (ad_type) {
> + case AD_TYPE_INCOMPLETE_UUID16_SERVICE_LIST:
> + ad.uuids_cnt += 1;
> + ad.uuids = realloc(ad.uuids,
> + ad.uuids_cnt * sizeof(ad.uuids));
> + ad.uuids[ad.uuids_cnt - 1] = dupuuid2str(ad_data, 16);
> +
> + break;
> + case AD_TYPE_SHORT_NAME:
> + ad.local_name = malloc(ad_len + 1);
> + memcpy(ad.local_name, ad_data, ad_len);
> + ad.local_name[ad_len] = '\0';
> +
> + break;
> + case AD_TYPE_SERVICE_DATA_UUID16:
> + {
> + uint8_t svc_id;
> +
> + ad.services_data_len += 1;
> + svc_id = ad.services_data_len - 1;
> + ad.services = l_realloc(ad.services,
> + sizeof(struct service_data) *
> + ad.services_data_len);
> + /* The first 2 octets contain the 16 bit Service UUID
> + * followed by additional service data
> + */
> + ad.services[svc_id].uuid = dupuuid2str(ad_data, 16);
> + ad.services[svc_id].data.len = ad_len - 2;
> + memcpy(ad.services[svc_id].data.data, ad_data + 2,
> + ad.services[svc_id].data.len);
> +
> + break;
> + }
> + case AD_TYPE_APPEARANCE:
> + memcpy(&ad.local_appearance, ad_data, ad_len);
> +
> + break;
> + case AD_TYPE_MANUFACTURER_DATA:
> + {
> + uint8_t md_id;
> +
> + ad.manufacturer_data_len += 1;
> + md_id = ad.manufacturer_data_len - 1;
> + ad.manufacturer = l_realloc(ad.manufacturer,
> + sizeof(struct manufacturer_data) *
> + ad.manufacturer_data_len);
> + /* The first 2 octets contain the Company Identifier
> + * Code followed by additional manufacturer specific
> + * data.
> + */
> + memcpy(&ad.manufacturer[md_id].id, ad_data, 2);
> + ad.manufacturer[md_id].data.len = ad_len - 2;
> + memcpy(&ad.manufacturer[md_id].data.data, ad_data + 2,
> + ad.manufacturer[md_id].data.len);
> +
> + break;
> + }
Missing default.
> + }
> + /* Advertising entity data len + advertising entity header
> + * (type, len)
> + */
> + remaining_data_len -= ad_len + 2;
> + }
> +}
> +
> +struct start_advertising_data {
> + uint32_t settings;
> + struct btp_gap_start_adv_cp cp;
> +};
> +
> +static void create_scan_response(uint8_t scan_rsp_len, const uint8_t *data)
> +{
> + /* TODO */
> +}
> +
> +static void start_advertising_setup(struct l_dbus_message *message,
> + void *user_data)
> +{
> + const struct start_advertising_data *sad = user_data;
> + const struct btp_gap_start_adv_cp *cp = &sad->cp;
> + struct l_dbus_message_builder *builder;
> +
> + if ((sad->settings >> BTP_GAP_SETTING_CONNECTABLE) & 1U)
Settings are already bit masks.
(I think we have same bug in btp_gap_set_connectable())
> + ad.type = l_strdup("peripheral");
> + else
> + ad.type = l_strdup("broadcast");
> +
> + if (cp->adv_data_len != 0)
> + create_advertising_data(cp->adv_data_len, cp->data);
> + if (cp->scan_rsp_len != 0)
> + create_scan_response(cp->scan_rsp_len,
> + cp->data + cp->scan_rsp_len);
> +
> + free(user_data);
> +
> + builder = l_dbus_message_builder_new(message);
> + l_dbus_message_builder_append_basic(builder, 'o', AD_PATH);
> + l_dbus_message_builder_enter_array(builder, "{sv}");
> + l_dbus_message_builder_enter_dict(builder, "sv");
> + l_dbus_message_builder_leave_dict(builder);
> + l_dbus_message_builder_leave_array(builder);
> + l_dbus_message_builder_finalize(builder);
> + l_dbus_message_builder_destroy(builder);
> +}
> +
> +static void btp_gap_start_advertising(uint8_t index, const void *param,
> + uint16_t length, void *user_data)
> +{
> + struct btp_adapter *adapter = find_adapter_by_index(index);
> + const struct btp_gap_start_adv_cp *cp = param;
> + struct start_advertising_data *data;
> + uint8_t status = BTP_ERROR_FAIL;
> + bool prop;
> + void *buff;
> +
> + if (!adapter) {
> + status = BTP_ERROR_INVALID_INDEX;
> + goto failed;
> + }
> +
> + /* Adapter needs to be powered to be able to remove devices */
> + if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
> + !prop)
> + goto failed;
> +
> + if (!l_dbus_register_interface(dbus, AD_IFACE, setup_ad_interface, NULL,
> + false)) {
> + l_info("Unable to register ad interface");
> + goto failed;
> + }
> +
> + if (!l_dbus_object_add_interface(dbus, AD_PATH, AD_IFACE, NULL)) {
> + l_info("Unable to instantiate ad interface");
> +
> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
> + l_info("Unable to unregister ad interface");
> +
> + goto failed;
> + }
> +
> + if (!l_dbus_object_add_interface(dbus, AD_PATH,
> + L_DBUS_INTERFACE_PROPERTIES,
> + NULL)) {
> + l_info("Unable to instantiate the properties interface");
> +
> + if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
> + l_info("Unable to remove ad instance");
> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
> + l_info("Unable to unregister ad interface");
> +
> + goto failed;
> + }
> +
> + buff = l_malloc(sizeof(struct start_advertising_data) +
> + cp->adv_data_len + cp->scan_rsp_len);
> + data = buff;
buff is not needed. Just assing to data directly.
Also do paring of those buffers to ad here, instead of setup callback. This
will allow for early error check (as setup cb returns void).
> + data->settings = adapter->current_settings;
> + data->cp.adv_data_len = cp->adv_data_len;
> + data->cp.scan_rsp_len = cp->scan_rsp_len;
> + memcpy(data->cp.data, cp->data, cp->adv_data_len + cp->scan_rsp_len);
> +
> + if (!l_dbus_proxy_method_call(adapter->ad_proxy,
> + "RegisterAdvertisement",
> + start_advertising_setup,
> + start_advertising_reply,
> + data, NULL)) {
> + if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
> + l_info("Unable to remove ad instance");
> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
> + l_info("Unable to unregister ad interface");
> +
> + free(buff);
> + goto failed;
> + }
> +
> + return;
> +
> +failed:
> + btp_send_error(btp, BTP_GAP_SERVICE, index, status);
> +}
> +
> +static void stop_advertising_reply(struct l_dbus_proxy *proxy,
> + struct l_dbus_message *result,
> + void *user_data)
> +{
> + const char *path = l_dbus_proxy_get_path(proxy);
> + struct btp_adapter *adapter = find_adapter_by_path(path);
> + uint32_t settings;
> +
> + if (!adapter)
> + return;
> +
> + if (l_dbus_message_is_error(result)) {
> + const char *name;
> +
> + l_dbus_message_get_error(result, &name, NULL);
> +
> + l_error("Failed to stop advertising %s (%s)",
> + l_dbus_proxy_get_path(proxy), name);
> + return;
> + }
> +
> + if (!l_dbus_object_remove_interface(dbus, AD_PATH, AD_IFACE))
> + l_info("Unable to remove ad instance");
> + if (!l_dbus_object_remove_interface(dbus, AD_PATH,
> + L_DBUS_INTERFACE_PROPERTIES))
> + l_info("Unable to remove propety instance");
> + if (!l_dbus_unregister_interface(dbus, AD_IFACE))
> + l_info("Unable to unregister ad interface");
> +
> + settings = L_CPU_TO_LE32(adapter->current_settings);
> +
> + btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_ADVERTISING,
> + adapter->index, sizeof(settings), &settings);
> +}
> +
> +static void btp_gap_stop_advertising(uint8_t index, const void *param,
> + uint16_t length, void *user_data)
> +{
> + struct btp_adapter *adapter = find_adapter_by_index(index);
> + uint8_t status = BTP_ERROR_FAIL;
> + bool prop;
> +
> + if (!adapter) {
> + status = BTP_ERROR_INVALID_INDEX;
> + goto failed;
> + }
> +
> + if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
> + !prop)
> + goto failed;
> +
> + if (adapter->ad_proxy) {
> + if (!l_dbus_proxy_method_call(adapter->ad_proxy,
> + "UnregisterAdvertisement",
> + unreg_advertising_setup,
> + stop_advertising_reply,
> + NULL, NULL)) {
> + status = BTP_ERROR_FAIL;
> + goto failed;
> + }
> + }
> +
> +failed:
> + btp_send_error(btp, BTP_GAP_SERVICE, index, status);
> +}
> +
> static void start_discovery_reply(struct l_dbus_proxy *proxy,
> struct l_dbus_message *result,
> void *user_data)
> @@ -728,6 +1375,12 @@ static void register_gap_service(void)
> btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_SET_BONDABLE,
> btp_gap_set_bondable, NULL, NULL);
>
> + btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_ADVERTISING,
> + btp_gap_start_advertising, NULL, NULL);
> +
> + btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_ADVERTISING,
> + btp_gap_stop_advertising, NULL, NULL);
> +
> btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_DISCOVERY,
> btp_gap_start_discovery, NULL, NULL);
>
> @@ -1124,6 +1777,12 @@ static void client_ready(struct l_dbus_client
> *client, void *user_data) BTP_INDEX_NON_CONTROLLER, 0, NULL);
> }
>
> +static void ready_callback(void *user_data)
> +{
> + if (!l_dbus_object_manager_enable(dbus))
> + l_info("Unable to register the ObjectManager");
> +}
> +
> static void usage(void)
> {
> l_info("btpclient - Bluetooth tester");
> @@ -1148,7 +1807,6 @@ int main(int argc, char *argv[])
> {
> struct l_dbus_client *client;
> struct l_signal *signal;
> - struct l_dbus *dbus;
> sigset_t mask;
> int opt;
>
> @@ -1192,6 +1850,7 @@ int main(int argc, char *argv[])
> signal = l_signal_create(&mask, signal_handler, NULL, NULL);
>
> dbus = l_dbus_new_default(L_DBUS_SYSTEM_BUS);
> + l_dbus_set_ready_handler(dbus, ready_callback, NULL, NULL);
> client = l_dbus_client_new(dbus, "org.bluez", "/org/bluez");
>
> l_dbus_client_set_connect_handler(client, client_connected, NULL, NULL);
--
pozdrawiam
Szymon Janc
This patch adds start and stop connect, disconnect commands for btp
client.
---
tools/btpclient.c | 175 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 175 insertions(+)
diff --git a/tools/btpclient.c b/tools/btpclient.c
index 472e9e4c2..053ae07a2 100644
--- a/tools/btpclient.c
+++ b/tools/btpclient.c
@@ -107,6 +107,13 @@ static bool str2addr(const char *str, uint8_t *addr)
&addr[3], &addr[2], &addr[1], &addr[0]) == 6;
}
+static bool addr2str(const uint8_t *addr, char *str)
+{
+ return sprintf(str, "%02hhX:%02hhX:%02hhX:%02hhX:%02hhX:%02hhX",
+ addr[0], addr[1], addr[2], addr[3], addr[4], addr[5])
+ == 17;
+}
+
static char *dupuuid2str(const uint8_t *uuid, uint8_t len)
{
switch (len) {
@@ -124,6 +131,17 @@ static char *dupuuid2str(const uint8_t *uuid, uint8_t len)
}
}
+static bool match_dev_addr_type(const char *addr_type_str, uint8_t addr_type)
+{
+ if (((addr_type == BTP_GAP_ADDR_PUBLIC) &&
+ (strcmp(addr_type_str, "public"))) ||
+ ((addr_type == BTP_GAP_ADDR_RANDOM) &&
+ strcmp(addr_type_str, "random")))
+ return false;
+
+ return true;
+}
+
static struct btp_adapter *find_adapter_by_proxy(struct l_dbus_proxy *proxy)
{
const struct l_queue_entry *entry;
@@ -169,6 +187,34 @@ static struct btp_adapter *find_adapter_by_path(const char *path)
return NULL;
}
+static struct btp_device *find_device_by_address(struct btp_adapter *adapter,
+ const uint8_t *addr,
+ uint8_t addr_type)
+{
+ const struct l_queue_entry *entry;
+ const char *str;
+ char addr_str[18];
+
+ if (!addr2str(addr, addr_str))
+ return NULL;
+
+ for (entry = l_queue_get_entries(adapter->devices); entry;
+ entry = entry->next) {
+ struct btp_device *device = entry->data;
+
+ l_dbus_proxy_get_property(device->proxy, "Address", "s", &str);
+ if (strcmp(str, addr_str))
+ continue;
+
+ l_dbus_proxy_get_property(device->proxy, "AddressType", "s",
+ &str);
+ if (match_dev_addr_type(str, addr_type))
+ return device;
+ }
+
+ return NULL;
+}
+
static void btp_gap_read_commands(uint8_t index, const void *param,
uint16_t length, void *user_data)
{
@@ -192,6 +238,8 @@ static void btp_gap_read_commands(uint8_t index, const void *param,
commands |= (1 << BTP_OP_GAP_STOP_ADVERTISING);
commands |= (1 << BTP_OP_GAP_START_DISCOVERY);
commands |= (1 << BTP_OP_GAP_STOP_DISCOVERY);
+ commands |= (1 << BTP_OP_GAP_CONNECT);
+ commands |= (1 << BTP_OP_GAP_DISCONNECT);
commands = L_CPU_TO_LE16(commands);
@@ -1314,6 +1362,127 @@ static void btp_gap_stop_discovery(uint8_t index, const void *param,
stop_discovery_reply, NULL, NULL);
}
+static void connect_reply(struct l_dbus_proxy *proxy,
+ struct l_dbus_message *result, void *user_data)
+{
+ uint8_t adapter_index = L_PTR_TO_UINT(user_data);
+ struct btp_adapter *adapter = find_adapter_by_index(adapter_index);
+
+ if (!adapter) {
+ btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
+ BTP_ERROR_FAIL);
+ return;
+ }
+
+ if (l_dbus_message_is_error(result)) {
+ const char *name, *desc;
+
+ l_dbus_message_get_error(result, &name, &desc);
+ l_error("Failed to connect (%s), %s", name, desc);
+
+ btp_send_error(btp, BTP_GAP_SERVICE, adapter_index,
+ BTP_ERROR_FAIL);
+ return;
+ }
+
+ btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_CONNECT, adapter_index, 0,
+ NULL);
+}
+
+static void btp_gap_connect(uint8_t index, const void *param, uint16_t length,
+ void *user_data)
+{
+ struct btp_adapter *adapter = find_adapter_by_index(index);
+ const struct btp_gap_connect_cp *cp = param;
+ struct btp_device *device;
+ bool prop;
+ uint8_t status = BTP_ERROR_FAIL;
+
+ if (!adapter) {
+ status = BTP_ERROR_INVALID_INDEX;
+ goto failed;
+ }
+
+ /* Adapter needs to be powered to be able to connect */
+ if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
+ !prop)
+ goto failed;
+
+ device = find_device_by_address(adapter, cp->address, cp->address_type);
+
+ if (!device)
+ goto failed;
+
+ l_dbus_proxy_method_call(device->proxy, "Connect", NULL, connect_reply,
+ L_UINT_TO_PTR(adapter->index), NULL);
+
+ return;
+
+failed:
+ btp_send_error(btp, BTP_GAP_SERVICE, index, status);
+}
+
+static void disconnect_reply(struct l_dbus_proxy *proxy,
+ struct l_dbus_message *result, void *user_data)
+{
+ uint8_t adapter_index = L_PTR_TO_UINT(user_data);
+ struct btp_adapter *adapter = find_adapter_by_index(adapter_index);
+
+ if (!adapter) {
+ btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
+ BTP_ERROR_FAIL);
+ return;
+ }
+
+ if (l_dbus_message_is_error(result)) {
+ const char *name, *desc;
+
+ l_dbus_message_get_error(result, &name, &desc);
+ l_error("Failed to disconnect (%s), %s", name, desc);
+
+ btp_send_error(btp, BTP_GAP_SERVICE, adapter_index,
+ BTP_ERROR_FAIL);
+ return;
+ }
+
+ btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_DISCONNECT, adapter_index, 0,
+ NULL);
+}
+
+static void btp_gap_disconnect(uint8_t index, const void *param,
+ uint16_t length, void *user_data)
+{
+ struct btp_adapter *adapter = find_adapter_by_index(index);
+ const struct btp_gap_disconnect_cp *cp = param;
+ uint8_t status = BTP_ERROR_FAIL;
+ struct btp_device *device;
+ bool prop;
+
+ if (!adapter) {
+ status = BTP_ERROR_INVALID_INDEX;
+ goto failed;
+ }
+
+ /* Adapter needs to be powered to be able to connect */
+ if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
+ !prop)
+ goto failed;
+
+ device = find_device_by_address(adapter, cp->address, cp->address_type);
+
+ if (!device)
+ goto failed;
+
+ l_dbus_proxy_method_call(device->proxy, "Disconnect", NULL,
+ disconnect_reply,
+ L_UINT_TO_PTR(adapter->index), NULL);
+
+ return;
+
+failed:
+ btp_send_error(btp, BTP_GAP_SERVICE, index, status);
+}
+
static void btp_gap_device_found_ev(struct l_dbus_proxy *proxy)
{
struct btp_device_found_ev ev;
@@ -1386,6 +1555,12 @@ static void register_gap_service(void)
btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_DISCOVERY,
btp_gap_stop_discovery, NULL, NULL);
+
+ btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_CONNECT, btp_gap_connect,
+ NULL, NULL);
+
+ btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_CONNECT,
+ btp_gap_disconnect, NULL, NULL);
}
static void btp_core_read_commands(uint8_t index, const void *param,
--
2.13.6
This patch adds conntected, disconnected events for btp client.
---
tools/btpclient.c | 70 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 70 insertions(+)
diff --git a/tools/btpclient.c b/tools/btpclient.c
index 053ae07a2..f4c408a7f 100644
--- a/tools/btpclient.c
+++ b/tools/btpclient.c
@@ -215,6 +215,26 @@ static struct btp_device *find_device_by_address(struct btp_adapter *adapter,
return NULL;
}
+static struct btp_adapter *find_adapter_by_dev_proxy(struct l_dbus_proxy *proxy)
+{
+ const struct l_queue_entry *entry, *dev_entry;
+
+ for (entry = l_queue_get_entries(adapters); entry;
+ entry = entry->next) {
+ struct btp_adapter *adapter = entry->data;
+
+ for (dev_entry = l_queue_get_entries(adapter->devices);
+ dev_entry; dev_entry = dev_entry->next) {
+ struct btp_device *device = dev_entry->data;
+
+ if (proxy == device->proxy)
+ return adapter;
+ }
+ }
+
+ return NULL;
+}
+
static void btp_gap_read_commands(uint8_t index, const void *param,
uint16_t length, void *user_data)
{
@@ -1517,6 +1537,49 @@ static void btp_gap_device_found_ev(struct l_dbus_proxy *proxy)
sizeof(ev) + ev.eir_len, &ev);
}
+static void btp_gap_device_connection_ev(struct l_dbus_proxy *proxy,
+ bool connected)
+{
+ struct btp_adapter *adapter;
+ const char *str_addr, *str_addr_type;
+ uint8_t address_type;
+
+
+ adapter = find_adapter_by_dev_proxy(proxy);
+
+ if (!adapter)
+ return;
+
+ if (!l_dbus_proxy_get_property(proxy, "Address", "s", &str_addr))
+ return;
+
+
+ if (!l_dbus_proxy_get_property(proxy, "AddressType", "s",
+ &str_addr_type))
+ return;
+
+ address_type = strcmp(str_addr_type, "public") ? BTP_GAP_ADDR_RANDOM :
+ BTP_GAP_ADDR_PUBLIC;
+
+ if (connected) {
+ struct btp_gap_device_connected_ev ev;
+
+ str2addr(str_addr, ev.address);
+ ev.address_type = address_type;
+
+ btp_send(btp, BTP_GAP_SERVICE, BTP_EV_GAP_DEVICE_CONNECTED,
+ adapter->index, sizeof(ev), &ev);
+ } else {
+ struct btp_gap_device_disconnected_ev ev;
+
+ str2addr(str_addr, ev.address);
+ ev.address_type = address_type;
+
+ btp_send(btp, BTP_GAP_SERVICE, BTP_EV_GAP_DEVICE_DISCONNECTED,
+ adapter->index, sizeof(ev), &ev);
+ }
+}
+
static void register_gap_service(void)
{
btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_READ_SUPPORTED_COMMANDS,
@@ -1920,6 +1983,13 @@ static void property_changed(struct l_dbus_proxy *proxy, const char *name,
return;
btp_gap_device_found_ev(proxy);
+ } else if (!strcmp(name, "Connected")) {
+ bool prop;
+
+ if (!l_dbus_message_get_arguments(msg, "b", &prop))
+ return;
+
+ btp_gap_device_connection_ev(proxy, prop);
}
}
}
--
2.13.6