aux/os: Add D-Bus based BLE code

CMakeLists.txt

@@ -56,6 +56,8 @@ if(CMAKE_SYSTEM_NAME STREQUAL "Linux")
 		pkg_search_module(WAYLAND_PROTOCOLS wayland-protocols)
 	endif()
 	find_package(OpenGL COMPONENTS GLX)
+	pkg_search_module(DBUS dbus-1)
+
 else()
 	find_package(OpenGL)
 endif()
@@ -65,6 +67,7 @@ cmake_dependent_option(BUILD_WITH_XLIB "Enable xlib support" ON "X11_FOUND" OFF)
 cmake_dependent_option(BUILD_WITH_XCB "Enable xcb support" ON "XCB_FOUND" OFF)
 cmake_dependent_option(BUILD_WITH_OPENGL "Enable OpenGL Graphics API support" ON "OPENGL_FOUND" OFF)
 cmake_dependent_option(BUILD_WITH_EGL "Enable OpenGL on EGL Graphics API support" ON "BUILD_WITH_OPENGL AND EGL_FOUND" OFF)
+cmake_dependent_option(BUILD_WITH_DBUS "Enable dbus support (for BLE support)" ON "DBUS_FOUND" OFF)
 cmake_dependent_option(BUILD_COMPOSITOR_MAIN "Build main compositor host" ON "BUILD_WITH_WAYLAND OR BUILD_WITH_XCB" OFF)
 cmake_dependent_option(BUILD_TARGET_OPENXR "Build OpenXR runtime target" ON "BUILD_COMPOSITOR_MAIN" OFF)
 
@@ -87,6 +90,7 @@ option(BUILD_WITH_DUMMY "Enable dummy driver" ON)
 cmake_dependent_option(BUILD_WITH_VIVE "Enable Vive driver" ON "ZLIB_FOUND" OFF)
 cmake_dependent_option(BUILD_WITH_OPENHMD "Enable OpenHMD driver" ON "OPENHMD_FOUND" OFF)
 cmake_dependent_option(BUILD_WITH_SDL2 "Enable SDL2 based test application" ON "SDL2_FOUND" OFF)
+cmake_dependent_option(BUILD_WITH_DAYDREAM "Enable Bluetooth LE via DBUS" ON "BUILD_WITH_DBUS" OFF)
 
 # These all use the Monado internal hid wrapper which is assumed to be available.
 option(BUILD_WITH_HDK "Enable HDK driver" ON)
@@ -105,6 +109,10 @@ if(BUILD_WITH_LIBUDEV)
 	set(XRT_HAVE_LIBUDEV TRUE)
 endif()
 
+if(BUILD_WITH_DBUS)
+	set(XRT_HAVE_DBUS TRUE)
+endif()
+
 if(BUILD_DRIVER_V4L2)
 	set(XRT_HAVE_V4L2 TRUE)
 endif()

src/xrt/auxiliary/CMakeLists.txt

@@ -21,11 +21,17 @@ set(OGL_SOURCE_FILES
 	)
 
 set(OS_SOURCE_FILES
+	os/os_ble.h
 	os/os_documentation.h
 	os/os_hid.h
 	os/os_hid_hidraw.c
 	os/os_threading.h
 	)
+if(BUILD_WITH_DBUS)
+	list(APPEND OS_SOURCE_FILES
+		os/os_ble_dbus.c
+		)
+endif()
 
 set(TRACKING_SOURCE_FILES
 	tracking/t_imu_fusion.hpp
@@ -108,6 +114,13 @@ target_link_libraries(aux_ogl PRIVATE xrt-external-headers)
 # OS library.
 add_library(aux_os STATIC ${OS_SOURCE_FILES})
 target_link_libraries(aux_os PUBLIC aux-includes PRIVATE Threads::Threads)
+if(BUILD_WITH_DBUS)
+	target_link_libraries(aux_os PRIVATE ${DBUS_LIBRARIES})
+	target_include_directories(aux_os SYSTEM
+		PRIVATE
+		${DBUS_INCLUDE_DIRS}
+		)
+endif()
 
 # Math library.
 add_library(aux_math STATIC ${MATH_SOURCE_FILES})

src/xrt/auxiliary/os/os_ble.h

+// Copyright 2019-2020, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Wrapper around OS native BLE functions.
+ * @author Pete Black <pete.black@collabora.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ *
+ * @ingroup aux_os
+ */
+
+#pragma once
+
+#include "xrt/xrt_config_os.h"
+#include "xrt/xrt_compiler.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/*!
+ * Representing a single ble notify attribute on a device.
+ *
+ * @ingroup aux_os
+ */
+struct os_ble_device
+{
+	int (*read)(struct os_ble_device *ble_dev,
+	            uint8_t *data,
+	            size_t size,
+	            int milliseconds);
+
+	void (*destroy)(struct os_ble_device *ble_dev);
+};
+
+/*!
+ * Read data from the ble file descriptor, if any, from the given bledevice.
+ *
+ * If milliseconds are negative, this call blocks indefinitely, 0 polls,
+ * and positive will block for that amount of milliseconds.
+ *
+ * @ingroup aux_os
+ */
+XRT_MAYBE_UNUSED static inline int
+os_ble_read(struct os_ble_device *ble_dev,
+            uint8_t *data,
+            size_t size,
+            int milliseconds)
+{
+	return ble_dev->read(ble_dev, data, size, milliseconds);
+}
+
+/*!
+ * Close and free the given device, does null checking and zeroing.
+ *
+ * @ingroup aux_os
+ */
+XRT_MAYBE_UNUSED static inline void
+os_ble_destroy(struct os_ble_device **ble_dev_ptr)
+{
+	struct os_ble_device *ble_dev = *ble_dev_ptr;
+	if (ble_dev == NULL) {
+		return;
+	}
+
+	ble_dev->destroy(ble_dev);
+	*ble_dev_ptr = NULL;
+}
+
+#ifdef XRT_OS_LINUX
+/*!
+ * Open the given mac and path to device endpoint (Currently Linux/BlueZ
+ * specific).
+ *
+ * @ingroup aux_os
+ */
+int
+os_ble_notify_open(const char *dev_uuid,
+                   const char *char_uuid,
+                   struct os_ble_device **out_ble);
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/auxiliary/os/os_ble_dbus.c

+// Copyright 2019-2020, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  BLE implementation based on Linux Bluez/dbus.
+ * @author Pete Black <pete.black@collabora.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ * @ingroup aux_os
+ */
+
+#include "os_ble.h"
+#include "util/u_misc.h"
+
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+#include <time.h>
+#include <inttypes.h>
+#include <dbus/dbus.h>
+
+
+/*
+ *
+ * Send helpers.
+ *
+ */
+
+static void
+add_empty_dict_sv(DBusMessage *msg)
+{
+	// Create an empty array of string variant dicts.
+	const char *container_signature = "{sv}"; // dbus type signature string
+	DBusMessageIter iter, options;
+
+	// attach it to our dbus message
+	dbus_message_iter_init_append(msg, &iter);
+	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
+	                                 container_signature, &options);
+	dbus_message_iter_close_container(&iter, &options);
+}
+
+
+static int
+send_message(DBusConnection *conn, DBusError *err, DBusMessage **msg_ptr)
+{
+	DBusPendingCall *pending;
+
+	// Take the message and null it.
+	DBusMessage *msg = *msg_ptr;
+	*msg_ptr = NULL;
+
+	if (msg == NULL) {
+		fprintf(stderr, "Message Null after construction\n");
+		return -1;
+	}
+
+	// send message and get a handle for a reply
+	if (!dbus_connection_send_with_reply(conn, msg, &pending, -1)) {
+		// -1 is default timeout
+		fprintf(stderr, "Out Of Memory!\n");
+		return -1;
+	}
+	if (pending == NULL) {
+		fprintf(stderr, "Pending Call Null\n");
+		return -1;
+	}
+	dbus_connection_flush(conn);
+
+	// Unref the message.
+	dbus_message_unref(msg);
+	msg = NULL;
+
+	// block until we receive a reply
+	dbus_pending_call_block(pending);
+
+	// get the reply message
+	msg = dbus_pending_call_steal_reply(pending);
+
+	// free the pending message handle
+	dbus_pending_call_unref(pending);
+	pending = NULL;
+
+	if (msg == NULL) {
+		fprintf(stderr, "Reply Null\n");
+		return -1;
+	}
+
+	*msg_ptr = msg;
+	return 0;
+}
+
+
+/*
+ *
+ * Dump functions
+ *
+ */
+
+static void
+dump_recurse(DBusMessageIter *parent, DBusMessageIter *sub, int level);
+
+static int
+dump_one_element(DBusMessageIter *element, int level)
+{
+	int type = dbus_message_iter_get_arg_type(element);
+	char *str;
+
+	for (int i = 0; i < level; i++) {
+		fprintf(stderr, " ");
+	}
+
+	switch (type) {
+	case DBUS_TYPE_INVALID: {
+		fprintf(stderr, "<>\n");
+		return -1;
+	}
+	case DBUS_TYPE_BOOLEAN: {
+		int val;
+		dbus_message_iter_get_basic(element, &val);
+		fprintf(stderr, "BOOLEAN: %s\n", val == 0 ? "false" : "true");
+		return 0;
+	}
+	case DBUS_TYPE_BYTE: {
+		int8_t val;
+		dbus_message_iter_get_basic(element, &val);
+		fprintf(stderr, "BYTE: %02x\n", val);
+		return 0;
+	}
+	case DBUS_TYPE_INT32: {
+		int32_t val;
+		dbus_message_iter_get_basic(element, &val);
+		fprintf(stderr, "INT32: %" PRIi32 "\n", val);
+		return 0;
+	}
+	case DBUS_TYPE_UINT32: {
+		uint32_t val;
+		dbus_message_iter_get_basic(element, &val);
+		fprintf(stderr, "UINT32: %" PRIu32 "\n", val);
+		return 0;
+	}
+	case DBUS_TYPE_INT64: {
+		int64_t val;
+		dbus_message_iter_get_basic(element, &val);
+		fprintf(stderr, "INT64: %" PRIi64 "\n", val);
+		return 0;
+	}
+	case DBUS_TYPE_UINT64: {
+		uint64_t val;
+		dbus_message_iter_get_basic(element, &val);
+		fprintf(stderr, "UINT32: %" PRIu64 "\n", val);
+		return 0;
+	}
+	case DBUS_TYPE_STRING: {
+		dbus_message_iter_get_basic(element, &str);
+		fprintf(stderr, "STRING: %s\n", str);
+		return 0;
+	}
+	case DBUS_TYPE_OBJECT_PATH: {
+		dbus_message_iter_get_basic(element, &str);
+		fprintf(stderr, "OBJECT_PATH: %s\n", str);
+		return 0;
+	}
+	case DBUS_TYPE_ARRAY: {
+		int elm_type = dbus_message_iter_get_element_type(element);
+		int elm_count = dbus_message_iter_get_element_count(element);
+		fprintf(stderr, "ARRAY: %c:%i\n", elm_type, elm_count);
+		DBusMessageIter sub;
+		dbus_message_iter_recurse(element, &sub);
+		dump_recurse(element, &sub, level + 2);
+		return 0;
+	}
+	case DBUS_TYPE_VARIANT: {
+		DBusMessageIter var;
+		dbus_message_iter_recurse(element, &var);
+		int var_type = dbus_message_iter_get_arg_type(&var);
+		fprintf(stderr, "VARIANT: %c\n", var_type);
+		dump_one_element(&var, level + 2);
+		return 0;
+	}
+	case DBUS_TYPE_DICT_ENTRY: {
+		fprintf(stderr, "DICT\n");
+		DBusMessageIter sub;
+		dbus_message_iter_recurse(element, &sub);
+		dump_recurse(element, &sub, level + 2);
+		return 0;
+	}
+	default:
+		fprintf(stderr, "Got! %c\n", type); // line break
+		return 0;
+	}
+}
+
+static void
+dump_recurse(DBusMessageIter *parent, DBusMessageIter *sub, int level)
+{
+	while (true) {
+		if (dump_one_element(sub, level) < 0) {
+			return;
+		}
+		dbus_message_iter_next(sub);
+	}
+}
+
+
+/*
+ *
+ * DBus iterator helper functions.
+ *
+ */
+
+/*!
+ * Checks if a string starts with, has extra slash and room for more.
+ */
+static bool
+starts_with_and_has_slash(const char *str, const char *beginning)
+{
+	size_t str_len = strlen(str);
+	size_t beginning_len = strlen(beginning);
+
+	if (str_len <= beginning_len + 1) {
+		return false;
+	}
+
+	size_t i = 0;
+	for (; i < beginning_len; i++) {
+		if (str[i] != beginning[i]) {
+			return false;
+		}
+	}
+
+	if (str[i] != '/') {
+		return false;
+	}
+
+	return true;
+}
+
+static int
+dict_get_string_and_varient_child(DBusMessageIter *dict,
+                                  const char **out_str,
+                                  DBusMessageIter *out_child)
+{
+	DBusMessageIter child;
+	int type = dbus_message_iter_get_arg_type(dict);
+	if (type != DBUS_TYPE_DICT_ENTRY) {
+		fprintf(stderr, "Expected dict got '%c'!\n", type);
+		return -1;
+	}
+
+	dbus_message_iter_recurse(dict, &child);
+	type = dbus_message_iter_get_arg_type(&child);
+	if (type != DBUS_TYPE_STRING && type != DBUS_TYPE_OBJECT_PATH) {
+		fprintf(stderr,
+		        "Expected dict first thing to be string or object "
+		        "path, got '%c'\n",
+		        type);
+		return -1;
+	}
+
+	dbus_message_iter_get_basic(&child, out_str);
+	dbus_message_iter_next(&child);
+
+	type = dbus_message_iter_get_arg_type(&child);
+	if (type != DBUS_TYPE_VARIANT) {
+		fprintf(stderr, "Expected variant got '%c'\n", type);
+		return -1;
+	}
+
+	dbus_message_iter_recurse(&child, out_child);
+
+	return 0;
+}
+
+static int
+dict_get_string_and_array_elm(const DBusMessageIter *in_dict,
+                              const char **out_str,
+                              DBusMessageIter *out_array_elm)
+{
+	DBusMessageIter dict = *in_dict;
+	DBusMessageIter child;
+	int type = dbus_message_iter_get_arg_type(&dict);
+	if (type != DBUS_TYPE_DICT_ENTRY) {
+		fprintf(stderr, "Expected dict got '%c'!\n", type);
+		return -1;
+	}
+
+	dbus_message_iter_recurse(&dict, &child);
+	type = dbus_message_iter_get_arg_type(&child);
+	if (type != DBUS_TYPE_STRING && type != DBUS_TYPE_OBJECT_PATH) {
+		fprintf(stderr,
+		        "Expected dict first thing to be string or object "
+		        "path, got '%c'\n",
+		        type);
+		return -1;
+	}
+
+	dbus_message_iter_get_basic(&child, out_str);
+	dbus_message_iter_next(&child);
+
+	type = dbus_message_iter_get_arg_type(&child);
+	if (type != DBUS_TYPE_ARRAY) {
+		fprintf(stderr, "Expected array got '%c'\n", type);
+		return -1;
+	}
+
+	dbus_message_iter_recurse(&child, out_array_elm);
+
+	return 0;
+}
+
+#define for_each(i, first)                                                     \
+	for (DBusMessageIter i = first;                                        \
+	     dbus_message_iter_get_arg_type(&i) != DBUS_TYPE_INVALID;          \
+	     dbus_message_iter_next(&i))
+
+/*!
+ * Ensures that the @p parent is a array and has a element type the given type,
+ * outputs the first element of the array on success.
+ */
+static int
+array_get_first_elem_of_type(const DBusMessageIter *in_parent,
+                             int of_type,
+                             DBusMessageIter *out_elm)
+{
+	DBusMessageIter parent = *in_parent;
+	int type = dbus_message_iter_get_arg_type(&parent);
+	if (type != DBUS_TYPE_ARRAY) {
+		fprintf(stderr, "Expected array got '%c'!\n", type);
+		return -1;
+	}
+
+	DBusMessageIter elm;
+	dbus_message_iter_recurse(&parent, &elm);
+
+	type = dbus_message_iter_get_arg_type(&elm);
+	if (type != of_type) {
+		fprintf(stderr, "Expected elem type of '%c' got '%c'!\n",
+		        of_type, type);
+		return -1;
+	}
+
+	*out_elm = elm;
+
+	return 1;
+}
+
+/*!
+ * Given a the first element in a array of dict, loop over them and check if
+ * the key matches it's string value. Returns positive if a match is found,
+ * zero if not found and negative on failure. The argument @p out_value holds
+ * the value of the dict pair.
+ */
+static int
+array_find_variant_value(const DBusMessageIter *first_elm,
+                         const char *key,
+                         DBusMessageIter *out_value)
+{
+	const char *str;
+
+	for_each(elm, *first_elm)
+	{
+		dict_get_string_and_varient_child(&elm, &str, out_value);
+
+		if (strcmp(key, str) == 0) {
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * Given a array which elements are of type string, loop over them and check if
+ * any of them matches the given @p key. Returns positive if a match is found,
+ * zero if not found and negative on failure.
+ */
+static int
+array_match_string_element(const DBusMessageIter *in_array, const char *key)
+{
+	DBusMessageIter array = *in_array;
+	int type = dbus_message_iter_get_arg_type(&array);
+	if (type != DBUS_TYPE_ARRAY) {
+		fprintf(stderr, "Expected array type ('%c')\n", type);
+		return -1;
+	}
+
+	int elm_type = dbus_message_iter_get_element_type(&array);
+	if (elm_type != DBUS_TYPE_STRING) {
+		fprintf(stderr, "Expected string element type ('%c')\n", type);
+		return -1;
+	}
+
+	DBusMessageIter first_elm;
+	dbus_message_iter_recurse(&array, &first_elm);
+
+	for_each(elm, first_elm)
+	{
+		const char *str = NULL;
+		dbus_message_iter_get_basic(&elm, &str);
+
+		if (strcmp(key, str) == 0) {
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+
+/*
+ *
+ * Bluez helpers.
+ *
+ */
+
+/*!
+ * On a gatt interface object get it's Flags property and check if notify is
+ * set, returns positive if it found that Flags property, zero on not finding it
+ * and negative on error.
+ */
+static int
+gatt_iface_get_flag_notifiable(const DBusMessageIter *iface_elm, bool *out_bool)
+{
+	DBusMessageIter value;
+	int ret = array_find_variant_value(iface_elm, "Flags", &value);
+	if (ret <= 0) {
+		return ret;
+	}
+
+	ret = array_match_string_element(&value, "notify");
+	if (ret < 0) {
+		// Error
+		return ret;
+	} else if (ret > 0) {
+		// Found the notify field!
+		*out_bool = true;
+	}
+
+	// We found the Flags field.
+	return 1;
+}
+
+/*!
+ * On a gatt interface object get it's UUID string property, returns positive
+ * if found, zero on not finding it and negative on error.
+ */
+static int
+gatt_iface_get_uuid(const DBusMessageIter *iface_elm, const char **out_str)
+{
+	DBusMessageIter value;
+	int ret = array_find_variant_value(iface_elm, "UUID", &value);
+	if (ret <= 0) {
+		return ret;
+	}
+
+	int type = dbus_message_iter_get_arg_type(&value);
+	if (type != DBUS_TYPE_STRING) {
+		fprintf(stderr, "Invalid UUID value type ('%c')\n", type);
+		return -1;
+	}
+
+	dbus_message_iter_get_basic(&value, out_str);
+	return 1;
+}
+
+/*!