Service discovery via Bluez5

At the same time we shift the ServiceClassUUID to service name mapping
to QBluetoothUuid. It was used by Android and now Bluez 5 uses it too.

Task-number: QTBUG-32085
Change-Id: I9f2d4dc4e2997683485f2ba7aaefb646cb72fb75
Reviewed-by: Aaron McCarthy <mccarthy.aaron@gmail.com>

1 files changed, 416 insertions, 31 deletions

diff --git a/src/bluetooth/qbluetoothservicediscoveryagent_bluez.cpp b/src/bluetooth/qbluetoothservicediscoveryagent_bluez.cpp
index 0c9addfb..a65b99f6 100644
--- a/src/bluetooth/qbluetoothservicediscoveryagent_bluez.cpp
+++ b/src/bluetooth/qbluetoothservicediscoveryagent_bluez.cpp
@@ -45,30 +45,58 @@
 #include "bluez/manager_p.h"
 #include "bluez/adapter_p.h"
 #include "bluez/device_p.h"
+#include "bluez/bluez5_helper_p.h"
+#include "bluez/objectmanager_p.h"
+#include "bluez/adapter1_bluez5_p.h"
+
+#include "bluetooth/bluetooth.h"
+#include "bluetooth/sdp.h"
+#include "bluetooth/sdp_lib.h"
 
 #include <QtCore/QLoggingCategory>
 #include <QtDBus/QDBusPendingCallWatcher>
+#include <QtConcurrent/QtConcurrentRun>
 
 QT_BEGIN_NAMESPACE
 
 Q_DECLARE_LOGGING_CATEGORY(QT_BT_BLUEZ)
 
+static inline void convertAddress(quint64 from, quint8 (&to)[6])
+{
+    to[0] = (from >> 0) & 0xff;
+    to[1] = (from >> 8) & 0xff;
+    to[2] = (from >> 16) & 0xff;
+    to[3] = (from >> 24) & 0xff;
+    to[4] = (from >> 32) & 0xff;
+    to[5] = (from >> 40) & 0xff;
+}
+
+Q_GLOBAL_STATIC_WITH_ARGS(QUuid, btBaseUuid, ("{00000000-0000-1000-8000-00805F9B34FB}"));
+
 QBluetoothServiceDiscoveryAgentPrivate::QBluetoothServiceDiscoveryAgentPrivate(const QBluetoothAddress &deviceAdapter)
 :   error(QBluetoothServiceDiscoveryAgent::NoError), m_deviceAdapterAddress(deviceAdapter), state(Inactive), deviceDiscoveryAgent(0),
     mode(QBluetoothServiceDiscoveryAgent::MinimalDiscovery), singleDevice(false),
-    manager(0), adapter(0), device(0)
+    manager(0), managerBluez5(0), adapter(0), device(0)
 {
-    qRegisterMetaType<ServiceMap>("ServiceMap");
-    qDBusRegisterMetaType<ServiceMap>();
+    if (isBluez5()) {
+        managerBluez5 = new OrgFreedesktopDBusObjectManagerInterface(
+                                    QStringLiteral("org.bluez"), QStringLiteral("/"),
+                                    QDBusConnection::systemBus());
+        qRegisterMetaType<QBluetoothServiceDiscoveryAgent::Error>("QBluetoothServiceDiscoveryAgent::Error");
+    } else {
+        qRegisterMetaType<ServiceMap>("ServiceMap");
+        qDBusRegisterMetaType<ServiceMap>();
 
-    manager = new OrgBluezManagerInterface(QStringLiteral("org.bluez"), QStringLiteral("/"),
-                                           QDBusConnection::systemBus());
+        manager = new OrgBluezManagerInterface(QStringLiteral("org.bluez"), QStringLiteral("/"),
+                                               QDBusConnection::systemBus());
+    }
 }
 
 QBluetoothServiceDiscoveryAgentPrivate::~QBluetoothServiceDiscoveryAgentPrivate()
 {
     delete device;
     delete manager;
+    delete managerBluez5;
     delete adapter;
 }
 
@@ -76,7 +104,12 @@ void QBluetoothServiceDiscoveryAgentPrivate::start(const QBluetoothAddress &addr
 {
     Q_Q(QBluetoothServiceDiscoveryAgent);
 
-    qCDebug(QT_BT_BLUEZ) << "Full discovery on: " << address.toString();
+    qCDebug(QT_BT_BLUEZ) << "Discovery on: " << address.toString() << "Mode:" << DiscoveryMode();
+
+    if (managerBluez5) {
+        startBluez5(address);
+        return;
+    }
 
     QDBusPendingReply<QDBusObjectPath> reply;
     if (m_deviceAdapterAddress.isNull())
@@ -105,6 +138,242 @@ void QBluetoothServiceDiscoveryAgentPrivate::start(const QBluetoothAddress &addr
 
 }
 
+void QBluetoothServiceDiscoveryAgentPrivate::startBluez5(const QBluetoothAddress &address)
+{
+    Q_Q(QBluetoothServiceDiscoveryAgent);
+
+    if (foundHostAdapterPath.isEmpty()) {
+        // check that we match adapter addresses or use first if it wasn't specified
+
+        QDBusPendingReply<ManagedObjectList> reply = managerBluez5->GetManagedObjects();
+        reply.waitForFinished();
+        if (reply.isError()) {
+            discoveredDevices.clear();
+            error = QBluetoothServiceDiscoveryAgent::InputOutputError;
+            errorString = reply.error().message();
+            emit q->error(error);
+            _q_serviceDiscoveryFinished();
+            return;
+        }
+
+        const QString desiredAdapter = m_deviceAdapterAddress.toString();
+        foreach (const QDBusObjectPath &path, reply.value().keys()) {
+            const InterfaceList ifaceList = reply.value().value(path);
+            foreach (const QString &iface, ifaceList.keys()) {
+                if (iface == QStringLiteral("org.bluez.Adapter1")) {
+                    if (m_deviceAdapterAddress.isNull()
+                        || desiredAdapter == ifaceList.value(iface).
+                                value(QStringLiteral("Address")).toString()) {
+                        // use first adapter or we just matched one
+                        foundHostAdapterPath = path.path();
+                    }
+
+                    if (!foundHostAdapterPath.isEmpty())
+                        break;
+                }
+            }
+
+            if (!foundHostAdapterPath.isEmpty())
+                break;
+        }
+
+        if (foundHostAdapterPath.isEmpty()) {
+            // Cannot find a local adapter
+            // Abort any outstanding discoveries
+            discoveredDevices.clear();
+
+            error = QBluetoothServiceDiscoveryAgent::InvalidBluetoothAdapterError;
+            errorString = QBluetoothServiceDiscoveryAgent::tr("Cannot find local Bluetooth adapter");
+            emit q->error(error);
+            _q_serviceDiscoveryFinished();
+
+            return;
+        }
+    }
+
+    // ensure we didn't go offline yet
+    OrgBluezAdapter1Interface adapter(QStringLiteral("org.bluez"),
+                                      foundHostAdapterPath, QDBusConnection::systemBus());
+    if (!adapter.powered()) {
+        discoveredDevices.clear();
+
+        error = QBluetoothServiceDiscoveryAgent::PoweredOffError;
+        errorString = QBluetoothServiceDiscoveryAgent::tr("Local device is powered off");
+        emit q->error(error);
+
+        _q_serviceDiscoveryFinished();
+        return;
+    }
+
+    if (DiscoveryMode() == QBluetoothServiceDiscoveryAgent::MinimalDiscovery) {
+        performMinimalServiceDiscovery(address);
+    } else {
+        // we need to run the discovery in a different thread
+        // as it involves blocking calls
+        QtConcurrent::run(this, &QBluetoothServiceDiscoveryAgentPrivate::runSdpScan,
+                          address, QBluetoothAddress(adapter.address()));
+    }
+}
+
+/*
+ * This function runs in a different thread. We need to be very careful what we
+ * access from here. That's why invokeMethod is used below.
+ */
+void QBluetoothServiceDiscoveryAgentPrivate::runSdpScan(
+        const QBluetoothAddress &remoteAddress, const QBluetoothAddress localAddress)
+{
+    Q_Q(QBluetoothServiceDiscoveryAgent);
+
+    // connect to SDP server
+    bdaddr_t local, remote;
+    convertAddress(localAddress.toUInt64(), local.b);
+    convertAddress(remoteAddress.toUInt64(), remote.b);
+
+    /* We use singleshot timer below because this function runs in a different
+     * thread than the rest of this class.
+     */
+
+    sdp_session_t *session = sdp_connect( &local, &remote, SDP_RETRY_IF_BUSY);
+    // try one more time if first attempt fails
+    if (!session)
+        session = sdp_connect( &local, &remote, SDP_RETRY_IF_BUSY);
+
+    qCDebug(QT_BT_BLUEZ) << "SDP for" << remoteAddress.toString() << session << qt_error_string(errno);
+    if (!session) {
+        if (singleDevice) {
+            // was sole device without result -> error
+            QMetaObject::invokeMethod(q, "_q_finishSdpScan", Qt::QueuedConnection,
+                                  Q_ARG(QBluetoothServiceDiscoveryAgent::Error,
+                                        QBluetoothServiceDiscoveryAgent::InputOutputError),
+                                  Q_ARG(QString,
+                                        QBluetoothServiceDiscoveryAgent::tr("Unable to access device")),
+                                  Q_ARG(QStringList, QStringList()));
+        } else {
+            // go to next device
+            QMetaObject::invokeMethod(q, "_q_finishSdpScan", Qt::QueuedConnection,
+                                      Q_ARG(QBluetoothServiceDiscoveryAgent::Error,
+                                            QBluetoothServiceDiscoveryAgent::NoError),
+                                      Q_ARG(QString, QString()),
+                                      Q_ARG(QStringList, QStringList()));
+        }
+
+        return;
+    }
+
+
+    // set the filter for service matches
+    uuid_t publicBrowseGroupUuid;
+    sdp_uuid16_create(&publicBrowseGroupUuid, QBluetoothUuid::PublicBrowseGroup);
+    sdp_list_t *serviceFilter;
+    serviceFilter = sdp_list_append(0, &publicBrowseGroupUuid);
+
+    uint32_t attributeRange = 0x0000ffff; //all attributes
+    sdp_list_t *attributes;
+    attributes = sdp_list_append(0, &attributeRange);
+
+    sdp_list_t* sdpResults;
+    int result = sdp_service_search_attr_req(session, serviceFilter, SDP_ATTR_REQ_RANGE,
+                                             attributes, &sdpResults);
+    sdp_list_free(attributes, 0);
+    sdp_list_free(serviceFilter, 0);
+
+    if (result != 0) {
+        qCDebug(QT_BT_BLUEZ) << "SDP search failed" << qt_error_string(errno);
+        sdp_close(session);
+        if (singleDevice) {
+            QMetaObject::invokeMethod(q, "_q_finishSdpScan", Qt::QueuedConnection,
+                                  Q_ARG(QBluetoothServiceDiscoveryAgent::Error,
+                                        QBluetoothServiceDiscoveryAgent::InputOutputError),
+                                  Q_ARG(QString,
+                                        QBluetoothServiceDiscoveryAgent::tr("Unable to access device")),
+                                  Q_ARG(QStringList, QStringList()));
+        } else {
+            QMetaObject::invokeMethod(q, "_q_finishSdpScan", Qt::QueuedConnection,
+                                      Q_ARG(QBluetoothServiceDiscoveryAgent::Error,
+                                            QBluetoothServiceDiscoveryAgent::NoError),
+                                      Q_ARG(QString, QString()),
+                                      Q_ARG(QStringList, QStringList()));
+        }
+        return;
+    }
+
+    qCDebug(QT_BT_BLUEZ) << "SDP search a success. Iterating results" << sdpResults;
+    QStringList xmlRecords;
+
+    // process the results
+    for ( ; sdpResults; sdpResults = sdpResults->next) {
+        sdp_record_t *record = (sdp_record_t *) sdpResults->data;
+
+        QByteArray xml = parseSdpRecord(record);
+        if (xml.isEmpty())
+            continue;
+
+        //qDebug() << xml;
+        xmlRecords.append(QString::fromUtf8(xml));
+    }
+
+    sdp_close(session);
+
+    QMetaObject::invokeMethod(q, "_q_finishSdpScan", Qt::QueuedConnection,
+                              Q_ARG(QBluetoothServiceDiscoveryAgent::Error,
+                                    QBluetoothServiceDiscoveryAgent::NoError),
+                              Q_ARG(QString, QString()),
+                              Q_ARG(QStringList, xmlRecords));
+}
+
+void QBluetoothServiceDiscoveryAgentPrivate::_q_finishSdpScan(QBluetoothServiceDiscoveryAgent::Error errorCode,
+                                                              const QString &errorDescription,
+                                                              const QStringList &xmlRecords)
+{
+    Q_Q(QBluetoothServiceDiscoveryAgent);
+
+    if (errorCode != QBluetoothServiceDiscoveryAgent::NoError) {
+        qCWarning(QT_BT_BLUEZ) << "SDP search failed for"
+                              << discoveredDevices.at(0).address().toString();
+        // We have an error which we need to indicate and stop further processing
+        discoveredDevices.clear();
+        error = errorCode;
+        errorString = errorDescription;
+        emit q->error(error);
+    } else if (!xmlRecords.isEmpty()) {
+        foreach (const QString &record, xmlRecords) {
+            const QBluetoothServiceInfo serviceInfo = parseServiceXml(record);
+
+            //apply uuidFilter
+            if (!uuidFilter.isEmpty()) {
+                bool serviceNameMatched = uuidFilter.contains(serviceInfo.serviceUuid());
+                bool serviceClassMatched = false;
+                foreach (const QBluetoothUuid &id, serviceInfo.serviceClassUuids()) {
+                    if (uuidFilter.contains(id)) {
+                        serviceClassMatched = true;
+                        break;
+                    }
+                }
+
+                if (!serviceNameMatched && !serviceClassMatched)
+                    continue;
+            }
+
+            if (!serviceInfo.isValid())
+                continue;
+
+            discoveredServices.append(serviceInfo);
+            qCDebug(QT_BT_BLUEZ) << "Discovered services" << discoveredDevices.at(0).address().toString()
+                                 << serviceInfo.serviceName() << serviceInfo.serviceUuid()
+                                 << ">>>" << serviceInfo.serviceClassUuids();
+
+            // TODO check for duplicates (wait until stable has merged due to related change)
+            emit q->serviceDiscovered(serviceInfo);
+
+            // could stop discovery, check for state
+            if (discoveryState() == Inactive)
+                qCDebug(QT_BT_BLUEZ) << "Exit discovery after stop";
+        }
+    }
+
+    _q_serviceDiscoveryFinished();
+}
+
 void QBluetoothServiceDiscoveryAgentPrivate::stop()
 {
     qCDebug(QT_BT_BLUEZ) << Q_FUNC_INFO << "Stop called";
@@ -186,6 +455,7 @@ void QBluetoothServiceDiscoveryAgentPrivate::_q_createdDevice(QDBusPendingCallWa
                      q, SLOT(_q_discoveredServices(QDBusPendingCallWatcher*)));
 }
 
+// Bluez 4
 void QBluetoothServiceDiscoveryAgentPrivate::_q_discoveredServices(QDBusPendingCallWatcher *watcher)
 {
     if (!device)
@@ -214,29 +484,13 @@ void QBluetoothServiceDiscoveryAgentPrivate::_q_discoveredServices(QDBusPendingC
     qCDebug(QT_BT_BLUEZ) << "Parsing xml" << discoveredDevices.at(0).address().toString() << discoveredDevices.count() << map.count();
 
     foreach (const QString &record, reply.value()) {
-        QXmlStreamReader xml(record);
-
-        QBluetoothServiceInfo serviceInfo;
-        serviceInfo.setDevice(discoveredDevices.at(0));
-
-        while (!xml.atEnd()) {
-            xml.readNext();
-
-            if (xml.tokenType() == QXmlStreamReader::StartElement &&
-                xml.name() == QLatin1String("attribute")) {
-                quint16 attributeId =
-                    xml.attributes().value(QStringLiteral("id")).toString().toUShort(0, 0);
-
-                if (xml.readNextStartElement()) {
-                    QVariant value = readAttributeValue(xml);
-
-                    serviceInfo.setAttribute(attributeId, value);
-                }
-            }
-        }
+        const QBluetoothServiceInfo serviceInfo = parseServiceXml(record);
 
         if (!serviceInfo.isValid())
-            continue;
+            return;
+
+        // Don't need to apply uuidFilter because Bluez 4 applies
+        // search pattern during DiscoverServices() call
 
         Q_Q(QBluetoothServiceDiscoveryAgent);
 
@@ -254,15 +508,14 @@ void QBluetoothServiceDiscoveryAgentPrivate::_q_discoveredServices(QDBusPendingC
 
         if (!alreadyDiscovered) {
             discoveredServices.append(serviceInfo);
-            qCDebug(QT_BT_BLUEZ) << "Discovered services" << discoveredDevices.at(0).address().toString();
+            qCDebug(QT_BT_BLUEZ) << "Discovered services" << discoveredDevices.at(0).address().toString()
+                                 << serviceInfo.serviceName();
             emit q->serviceDiscovered(serviceInfo);
         }
 
         // could stop discovery, check for state
-        if(discoveryState() == Inactive){
+        if (discoveryState() == Inactive)
             qCDebug(QT_BT_BLUEZ) << "Exit discovery after stop";
-            break;
-        }
     }
 
     watcher->deleteLater();
@@ -272,6 +525,138 @@ void QBluetoothServiceDiscoveryAgentPrivate::_q_discoveredServices(QDBusPendingC
     _q_serviceDiscoveryFinished();
 }
 
+QBluetoothServiceInfo QBluetoothServiceDiscoveryAgentPrivate::parseServiceXml(const QString& xmlRecord)
+{
+    QXmlStreamReader xml(xmlRecord);
+
+    QBluetoothServiceInfo serviceInfo;
+    serviceInfo.setDevice(discoveredDevices.at(0));
+
+    while (!xml.atEnd()) {
+        xml.readNext();
+
+        if (xml.tokenType() == QXmlStreamReader::StartElement &&
+            xml.name() == QLatin1String("attribute")) {
+            quint16 attributeId =
+                xml.attributes().value(QLatin1String("id")).toString().toUShort(0, 0);
+
+            if (xml.readNextStartElement()) {
+                QVariant value = readAttributeValue(xml);
+
+                serviceInfo.setAttribute(attributeId, value);
+            }
+        }
+    }
+
+    return serviceInfo;
+}
+
+void QBluetoothServiceDiscoveryAgentPrivate::performMinimalServiceDiscovery(const QBluetoothAddress &deviceAddress)
+{
+    if (foundHostAdapterPath.isEmpty()) {
+        _q_serviceDiscoveryFinished();
+        return;
+    }
+
+    Q_Q(QBluetoothServiceDiscoveryAgent);
+
+    QDBusPendingReply<ManagedObjectList> reply = managerBluez5->GetManagedObjects();
+    reply.waitForFinished();
+    if (reply.isError()) {
+        if (singleDevice) {
+            error = QBluetoothServiceDiscoveryAgent::InputOutputError;
+            errorString = reply.error().message();
+            emit q->error(error);
+
+        }
+        _q_serviceDiscoveryFinished();
+        return;
+    }
+
+    QStringList uuidStrings;
+    foreach (const QDBusObjectPath &path, reply.value().keys()) {
+        const InterfaceList ifaceList = reply.value().value(path);
+        foreach (const QString &iface, ifaceList.keys()) {
+            if (iface == QStringLiteral("org.bluez.Device1")) {
+                const QVariantMap details = ifaceList.value(iface);
+                if (deviceAddress.toString()
+                        == details.value(QStringLiteral("Address")).toString()) {
+                    uuidStrings = details.value(QStringLiteral("UUIDs")).toStringList();
+                    break;
+
+                }
+            }
+        }
+        if (!uuidStrings.isEmpty())
+            break;
+    }
+
+    if (uuidStrings.isEmpty() || discoveredDevices.isEmpty()) {
+         // nothing found -> go to next uuid
+        _q_serviceDiscoveryFinished();
+        return;
+    }
+
+    qCDebug(QT_BT_BLUEZ) << "Minimal uuid list for" << deviceAddress.toString() << uuidStrings;
+
+    QBluetoothUuid uuid;
+    for (int i = 0; i < uuidStrings.count(); i++) {
+        uuid = QBluetoothUuid(uuidStrings.at(i));
+        if (uuid.isNull())
+            continue;
+
+        //apply uuidFilter
+        if (!uuidFilter.isEmpty() && !uuidFilter.contains(uuid))
+            continue;
+
+        bool isBaseUuid = false;
+        if (btBaseUuid()->data2 == uuid.data2 && btBaseUuid()->data3 == uuid.data3
+                   && btBaseUuid()->data4[0] == uuid.data4[0] && btBaseUuid()->data4[1] == uuid.data4[1]
+                   && btBaseUuid()->data4[2] == uuid.data4[2] && btBaseUuid()->data4[3] == uuid.data4[3]
+                   && btBaseUuid()->data4[4] == uuid.data4[4] && btBaseUuid()->data4[5] == uuid.data4[5]
+                   && btBaseUuid()->data4[6] == uuid.data4[6] && btBaseUuid()->data4[7] == uuid.data4[7])
+        {
+            isBaseUuid = true;
+        }
+
+        QBluetoothServiceInfo serviceInfo;
+        serviceInfo.setDevice(discoveredDevices.at(0));
+
+        if (!isBaseUuid) {
+            serviceInfo.setServiceUuid(uuid);
+            serviceInfo.setServiceName(QBluetoothServiceDiscoveryAgent::tr("Custom Service"));
+        } else {
+            // set uuid as service class id
+            QBluetoothServiceInfo::Sequence classId;
+            classId << QVariant::fromValue(uuid);
+            serviceInfo.setAttribute(QBluetoothServiceInfo::ServiceClassIds, classId);
+            QBluetoothUuid::ServiceClassUuid clsId
+                    = static_cast<QBluetoothUuid::ServiceClassUuid>(uuid.data1 & 0xffff);
+            serviceInfo.setServiceName(QBluetoothUuid::serviceClassToString(clsId));
+        }
+
+        //don't include the service if we already discovered it before
+        bool alreadyDiscovered = false;
+        for (int j = 0; j < discoveredServices.count(); j++) {
+            const QBluetoothServiceInfo &info = discoveredServices.at(j);
+            if (info.device() == serviceInfo.device()
+                    && info.serviceClassUuids() == serviceInfo.serviceClassUuids()
+                    && info.serviceUuid() == serviceInfo.serviceUuid()) {
+                alreadyDiscovered = true;
+                break;
+            }
+        }
+
+        if (!alreadyDiscovered) {
+            discoveredServices << serviceInfo;
+            //qCDebug(QT_BT_ANDROID) << serviceInfo;
+            emit q->serviceDiscovered(serviceInfo);
+        }
+    }
+
+    _q_serviceDiscoveryFinished();
+}
+
 QVariant QBluetoothServiceDiscoveryAgentPrivate::readAttributeValue(QXmlStreamReader &xml)
 {
     if (xml.name() == QLatin1String("boolean")) {