From 69fb243fe753c828fa2cfb079f44b30bb20759f6 Mon Sep 17 00:00:00 2001 From: Olivier Martin Date: Sun, 14 Jul 2019 23:26:47 +0200 Subject: [PATCH] gattlib: Implement `gattlib_get_advertisement_data()` --- bluez/gattlib_connect.c | 6 - dbus/CMakeLists.txt | 1 + dbus/dbus-bluez-v5.40/org.bluez.Device1.xml | 5 +- dbus/gattlib.c | 60 ++++---- dbus/gattlib_advertisement.c | 128 ++++++++++++++++++ dbus/gattlib_internal.h | 1 + .../examples/find_eddystone/find_eddystone.py | 23 ++++ gattlib-py/gattlib/__init__.py | 17 +++ gattlib-py/gattlib/adapter.py | 43 ++++++ include/gattlib.h | 30 +++- 10 files changed, 275 insertions(+), 39 deletions(-) create mode 100644 dbus/gattlib_advertisement.c diff --git a/bluez/gattlib_connect.c b/bluez/gattlib_connect.c index 44a572b..84848e0 100644 --- a/bluez/gattlib_connect.c +++ b/bluez/gattlib_connect.c @@ -560,9 +560,3 @@ int gattlib_get_rssi_from_mac(void *adapter, const char *mac_address, int16_t *r { return GATTLIB_NOT_SUPPORTED; } - -int gattlib_get_advertisement_data(gatt_connection_t *connection, gattlib_advertisement_data_t **advertisement_data, - uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size) -{ - return GATTLIB_NOT_SUPPORTED; -} diff --git a/dbus/CMakeLists.txt b/dbus/CMakeLists.txt index 76a7b4a..8c23e4f 100644 --- a/dbus/CMakeLists.txt +++ b/dbus/CMakeLists.txt @@ -82,6 +82,7 @@ include_directories(. ${CMAKE_SOURCE_DIR}/common ${CMAKE_CURRENT_BINARY_DIR} ${G set(gattlib_SRCS gattlib.c gattlib_adapter.c + gattlib_advertisement.c gattlib_char.c gattlib_stream.c bluez5/lib/uuid.c diff --git a/dbus/dbus-bluez-v5.40/org.bluez.Device1.xml b/dbus/dbus-bluez-v5.40/org.bluez.Device1.xml index b9b1c62..3c3b506 100644 --- a/dbus/dbus-bluez-v5.40/org.bluez.Device1.xml +++ b/dbus/dbus-bluez-v5.40/org.bluez.Device1.xml @@ -30,8 +30,9 @@ - - + + + diff --git a/dbus/gattlib.c b/dbus/gattlib.c index 3fb6b6b..351d69c 100644 --- a/dbus/gattlib.c +++ b/dbus/gattlib.c @@ -940,6 +940,33 @@ int gattlib_notification_stop(gatt_connection_t* connection, const uuid_t* uuid) } } +int get_bluez_device_from_mac(void *adapter, const char *mac_address, OrgBluezDevice1 **bluez_device1) +{ + GError *error = NULL; + char object_path[100]; + + if (adapter != NULL) { + get_device_path_from_mac_with_adapter((OrgBluezAdapter1*)adapter, mac_address, object_path, sizeof(object_path)); + } else { + get_device_path_from_mac(NULL, mac_address, object_path, sizeof(object_path)); + } + + *bluez_device1 = org_bluez_device1_proxy_new_for_bus_sync( + G_BUS_TYPE_SYSTEM, + G_DBUS_OBJECT_MANAGER_CLIENT_FLAGS_NONE, + "org.bluez", + object_path, + NULL, + &error); + if (error) { + fprintf(stderr, "Failed to connection to new DBus Bluez Device: %s\n", error->message); + g_error_free(error); + return GATTLIB_ERROR_DBUS; + } + + return GATTLIB_SUCCESS; +} + #if 0 // Disable until https://github.com/labapart/gattlib/issues/75 is resolved int gattlib_get_rssi(gatt_connection_t *connection, int16_t *rssi) { @@ -957,41 +984,18 @@ int gattlib_get_rssi(gatt_connection_t *connection, int16_t *rssi) int gattlib_get_rssi_from_mac(void *adapter, const char *mac_address, int16_t *rssi) { - GError *error = NULL; - char object_path[100]; + OrgBluezDevice1 *bluez_device1; + int ret; if (rssi == NULL) { return GATTLIB_INVALID_PARAMETER; } - if (adapter != NULL) { - get_device_path_from_mac_with_adapter((OrgBluezAdapter1*)adapter, mac_address, object_path, sizeof(object_path)); - } else { - get_device_path_from_mac(NULL, mac_address, object_path, sizeof(object_path)); - } - - OrgBluezDevice1* bluez_device1 = org_bluez_device1_proxy_new_for_bus_sync( - G_BUS_TYPE_SYSTEM, - G_DBUS_OBJECT_MANAGER_CLIENT_FLAGS_NONE, - "org.bluez", - object_path, - NULL, - &error); - if (error) { - fprintf(stderr, "Failed to connection to new DBus Bluez Device: %s\n", - error->message); - g_error_free(error); - return GATTLIB_ERROR_DBUS; + ret = get_bluez_device_from_mac(adapter, mac_address, &bluez_device1); + if (ret != GATTLIB_SUCCESS) { + return ret; } *rssi = org_bluez_device1_get_rssi(bluez_device1); return GATTLIB_SUCCESS; } - -int gattlib_get_advertisement_data(gatt_connection_t *connection, gattlib_advertisement_data_t **advertisement_data, - uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size) -{ - //gattlib_context_t* conn_context = connection->context; - - return GATTLIB_NOT_SUPPORTED; -} diff --git a/dbus/gattlib_advertisement.c b/dbus/gattlib_advertisement.c new file mode 100644 index 0000000..3fefbff --- /dev/null +++ b/dbus/gattlib_advertisement.c @@ -0,0 +1,128 @@ +/* + * + * GattLib - GATT Library + * + * Copyright (C) 2016-2019 Olivier Martin + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include "gattlib_internal.h" + +#if BLUEZ_VERSION < BLUEZ_VERSIONS(5, 40) + +int gattlib_get_advertisement_data(gatt_connection_t *connection, + gattlib_advertisement_data_t **advertisement_data, size_t *advertisement_data_count, + uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size) +{ + return GATTLIB_NOT_SUPPORTED; +} + +int gattlib_get_advertisement_data_from_mac(void *adapter, const char *mac_address, + gattlib_advertisement_data_t **advertisement_data, size_t *advertisement_data_count, + uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size) +{ + return GATTLIB_NOT_SUPPORTED; +} + +#else + +int get_advertisement_data_from_device(OrgBluezDevice1 *bluez_device1, + gattlib_advertisement_data_t **advertisement_data, size_t *advertisement_data_count, + uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size) +{ + GVariant *manufacturer_data_variant; + GVariant *service_data_variant; + + if (advertisement_data == NULL) { + return GATTLIB_INVALID_PARAMETER; + } + + manufacturer_data_variant = org_bluez_device1_get_manufacturer_data(bluez_device1); + if (manufacturer_data_variant != NULL) { + fprintf(stderr, "Warning: Manufacturer Data not supported: %s\n", + g_variant_print(manufacturer_data_variant, TRUE)); + } + + service_data_variant = org_bluez_device1_get_service_data(bluez_device1); + if (service_data_variant != NULL) { + GVariantIter *iter; + const gchar *key; + GVariant *value; + size_t index = 0; + + *advertisement_data_count = g_variant_n_children(service_data_variant); + + *advertisement_data = calloc(sizeof(gattlib_advertisement_data_t), *advertisement_data_count); + if (*advertisement_data == NULL) { + return GATTLIB_OUT_OF_MEMORY; + } + + g_variant_get(service_data_variant, "a{sv}", &iter); + while (g_variant_iter_loop(iter, "{&sv}", &key, &value)) { + gattlib_string_to_uuid(key, strlen(key), &advertisement_data[index]->uuid); + + gsize n_elements = 0; + gconstpointer const_buffer = g_variant_get_fixed_array(value, &n_elements, sizeof(guchar)); + if (const_buffer) { + advertisement_data[index]->data = malloc(n_elements); + if (advertisement_data[index]->data == NULL) { + return GATTLIB_OUT_OF_MEMORY; + } + + advertisement_data[index]->data_length = n_elements; + memcpy(advertisement_data[index]->data, const_buffer, n_elements); + } else { + advertisement_data[index]->data_length = 0; + } + + index++; + } + } + + return GATTLIB_SUCCESS; +} + +int gattlib_get_advertisement_data(gatt_connection_t *connection, + gattlib_advertisement_data_t **advertisement_data, size_t *advertisement_data_count, + uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size) +{ + gattlib_context_t* conn_context = connection->context; + + return get_advertisement_data_from_device(conn_context->device, + advertisement_data, advertisement_data_count, + manufacturer_id, manufacturer_data, manufacturer_data_size); +} + +int gattlib_get_advertisement_data_from_mac(void *adapter, const char *mac_address, + gattlib_advertisement_data_t **advertisement_data, size_t *advertisement_data_count, + uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size) +{ + OrgBluezDevice1 *bluez_device1; + int ret; + + ret = get_bluez_device_from_mac(adapter, mac_address, &bluez_device1); + if (ret != GATTLIB_SUCCESS) { + return ret; + } + + return get_advertisement_data_from_device(bluez_device1, + advertisement_data, advertisement_data_count, + manufacturer_id, manufacturer_data, manufacturer_data_size); +} + +#endif /* #if BLUEZ_VERSION < BLUEZ_VERSIONS(5, 40) */ diff --git a/dbus/gattlib_internal.h b/dbus/gattlib_internal.h index 575dcfc..0806705 100644 --- a/dbus/gattlib_internal.h +++ b/dbus/gattlib_internal.h @@ -75,6 +75,7 @@ gboolean stop_scan_func(gpointer data); void get_device_path_from_mac_with_adapter(OrgBluezAdapter1* adapter, const char *mac_address, char *object_path, size_t object_path_len); void get_device_path_from_mac(const char *adapter_name, const char *mac_address, char *object_path, size_t object_path_len); +int get_bluez_device_from_mac(void *adapter, const char *mac_address, OrgBluezDevice1 **bluez_device1); struct dbus_characteristic get_characteristic_from_uuid(gatt_connection_t* connection, const uuid_t* uuid); diff --git a/gattlib-py/examples/find_eddystone/find_eddystone.py b/gattlib-py/examples/find_eddystone/find_eddystone.py index 3490bd8..414fd88 100755 --- a/gattlib-py/examples/find_eddystone/find_eddystone.py +++ b/gattlib-py/examples/find_eddystone/find_eddystone.py @@ -10,6 +10,13 @@ args = parser.parse_args() EDDYSTONE_COMMON_DATA_UUID = 'FEAA' +EDDYSTONE_URL_SCHEME_PREFIX = { + 0x00: "http://www.", + 0x01: "https://www.", + 0x02: "http://", + 0x03: "https://", +} + # Use default adapter default_adapter = adapter.Adapter() @@ -18,6 +25,22 @@ def on_discovered_ble_device(device, user_data): rssi = default_adapter.get_rssi_from_mac(device.id) print("Find Eddystone device %s (RSSI:%d)" % (device.id, rssi)) + # Retrieve Advertisement Data + advertisement_data, manufacturer_id, manufacturer_data = default_adapter.gattlib_get_advertisement_data_from_mac(device.id) + + # Service Data + service_data = advertisement_data[0xFEAA] + if service_data[0] == 0x00: + print("Eddystone UID: TX Power:0x%x NID:%s BID:%s" % (service_data[1], service_data[2:12], service_data[12:18])) + elif service_data[0] == 0x10: + print("Eddystone URL: TX Power:0x%x URL:%s%s" % (service_data[1], EDDYSTONE_URL_SCHEME_PREFIX[service_data[2]], service_data[3:].decode("utf-8"))) + elif service_data[0] == 0x20: + print("Eddystone TLM") + elif service_data[0] == 0x30: + print("Eddystone EID") + else: + print("Eddystone frame not supported: 0x%x" % service_data[0]) + # Scan for 30 seconds default_adapter.open() diff --git a/gattlib-py/gattlib/__init__.py b/gattlib-py/gattlib/__init__.py index 9d5331d..337ce1e 100644 --- a/gattlib-py/gattlib/__init__.py +++ b/gattlib-py/gattlib/__init__.py @@ -50,6 +50,17 @@ class GattlibCharacteristic(Structure): ("uuid", GattlibUuid)] +# typedef struct { +# uuid_t uuid; +# uint8_t* data; +# size_t data_length; +# } gattlib_advertisement_data_t; +class GattlibAdvertisementData(Structure): + _fields_ = [("uuid", GattlibUuid), + ("data", c_void_p), + ("data_length", c_size_t)] + + # int gattlib_adapter_open(const char* adapter_name, void** adapter); gattlib_adapter_open = gattlib.gattlib_adapter_open gattlib_adapter_open.argtypes = [c_char_p, POINTER(c_void_p)] @@ -110,3 +121,9 @@ gattlib_register_on_disconnect.argtypes = [c_void_p, py_object, py_object] # int gattlib_get_rssi_from_mac(void *adapter, const char *mac_address, int16_t *rssi) gattlib_get_rssi_from_mac = gattlib.gattlib_get_rssi_from_mac gattlib_get_rssi_from_mac.argtypes = [c_void_p, c_char_p, POINTER(c_int16)] + +# int gattlib_get_advertisement_data_from_mac(void *adapter, const char *mac_address, +# gattlib_advertisement_data_t **advertisement_data, size_t *advertisement_data_length, +# uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size) +gattlib_get_advertisement_data_from_mac = gattlib.gattlib_get_advertisement_data_from_mac +gattlib_get_advertisement_data_from_mac.argtypes = [c_void_p, c_char_p, POINTER(POINTER(GattlibAdvertisementData)), POINTER(c_size_t), POINTER(c_uint16), POINTER(c_void_p), POINTER(c_size_t)] diff --git a/gattlib-py/gattlib/adapter.py b/gattlib-py/gattlib/adapter.py index bd1c6ee..937cd7f 100644 --- a/gattlib-py/gattlib/adapter.py +++ b/gattlib-py/gattlib/adapter.py @@ -1,6 +1,7 @@ from gattlib import * from .device import Device from .exception import handle_return +from .uuid import gattlib_uuid_to_int GATTLIB_DISCOVER_FILTER_USE_UUID = (1 << 0) GATTLIB_DISCOVER_FILTER_USE_RSSI = (1 << 1) @@ -76,3 +77,45 @@ class Adapter: rssi = c_int16(0) gattlib_get_rssi_from_mac(self._adapter, mac_address, byref(rssi)) return rssi.value + + def gattlib_get_advertisement_data_from_mac(self, mac_address): + if isinstance(mac_address, str): + mac_address = mac_address.encode("utf-8") + + _advertisement_data = POINTER(GattlibAdvertisementData)() + _advertisement_data_count = c_size_t(0) + _manufacturer_id = c_uint16(0) + _manufacturer_data = c_void_p(None) + _manufacturer_data_len = c_size_t(0) + + ret = gattlib_get_advertisement_data_from_mac(self._adapter, mac_address, + byref(_advertisement_data), byref(_advertisement_data_count), + byref(_manufacturer_id), + byref(_manufacturer_data), byref(_manufacturer_data_len)) + handle_return(ret) + + advertisement_data = {} + manufacturer_data = None + + for i in range(0, _advertisement_data_count.value): + service_data = _advertisement_data[i] + uuid = gattlib_uuid_to_int(service_data.uuid) + + pointer_type = POINTER(c_byte * service_data.data_length) + c_bytearray = cast(service_data.data, pointer_type) + + data = bytearray(service_data.data_length) + for i in range(service_data.data_length): + data[i] = c_bytearray.contents[i] & 0xFF + + advertisement_data[uuid] = data + + if _manufacturer_data_len.value > 0: + pointer_type = POINTER(c_byte * _manufacturer_data_len.value) + c_bytearray = cast(_manufacturer_data, pointer_type) + + manufacturer_data = bytearray(_manufacturer_data_len.value) + for i in range(_manufacturer_data_len.value): + manufacturer_data[i] = c_bytearray.contents[i] & 0xFF + + return advertisement_data, _manufacturer_id.value, manufacturer_data diff --git a/include/gattlib.h b/include/gattlib.h index bf3c30e..8752c2c 100644 --- a/include/gattlib.h +++ b/include/gattlib.h @@ -389,13 +389,37 @@ int gattlib_get_rssi(gatt_connection_t *connection, int16_t *rssi); int gattlib_get_rssi_from_mac(void *adapter, const char *mac_address, int16_t *rssi); /** - * @brief Function to retrieve Advertisement Data of the GATT connection + * @brief Function to retrieve Advertisement Data from a MAC Address * - * @param connection Active GATT connection + * @param adapter is the adapter the new device has been seen + * @param mac_address is the MAC address of the device to get the RSSI + * @param advertisement_data is an array of Service UUID and their respective data + * @param advertisement_data_count is the number of elements in the advertisement_data array + * @param manufacturer_id is the ID of the Manufacturer ID + * @param manufacturer_data is the data following Manufacturer ID + * @param manufacturer_data_size is the size of manufacturer_data * * @return GATTLIB_SUCCESS on success or GATTLIB_* error code */ -int gattlib_get_advertisement_data(gatt_connection_t *connection, gattlib_advertisement_data_t **advertisement_data, +int gattlib_get_advertisement_data(gatt_connection_t *connection, + gattlib_advertisement_data_t **advertisement_data, size_t *advertisement_data_count, + uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size); + +/** + * @brief Function to retrieve Advertisement Data from a MAC Address + * + * @param adapter is the adapter the new device has been seen + * @param mac_address is the MAC address of the device to get the RSSI + * @param advertisement_data is an array of Service UUID and their respective data + * @param advertisement_data_count is the number of elements in the advertisement_data array + * @param manufacturer_id is the ID of the Manufacturer ID + * @param manufacturer_data is the data following Manufacturer ID + * @param manufacturer_data_size is the size of manufacturer_data + * + * @return GATTLIB_SUCCESS on success or GATTLIB_* error code + */ +int gattlib_get_advertisement_data_from_mac(void *adapter, const char *mac_address, + gattlib_advertisement_data_t **advertisement_data, size_t *advertisement_data_count, uint16_t *manufacturer_id, uint8_t **manufacturer_data, size_t *manufacturer_data_size); int gattlib_uuid_to_string(const uuid_t *uuid, char *str, size_t n);