Discover all Bluetooth services and their handles

Change-Id: I83d88412319c34cf6c8cf0c6e841458226c9d073
Reviewed-by: Fabian Bumberger <fbumberger@rim.com>

7 files changed, 183 insertions, 3 deletions

diff --git a/src/bluetooth/qbluetoothsocket_bluez.cpp b/src/bluetooth/qbluetoothsocket_bluez.cpp
index 16977f74..cb963b4f 100644
--- a/src/bluetooth/qbluetoothsocket_bluez.cpp
+++ b/src/bluetooth/qbluetoothsocket_bluez.cpp
@@ -164,7 +164,9 @@ void QBluetoothSocketPrivate::connectToService(const QBluetoothAddress &address,
         addr.l2_family = AF_BLUETOOTH;
         // This is an ugly hack but the socket class does what's needed already.
         // For L2CP GATT we need a channel rather than a socket and the LE address type
-        // We don't want to make this public API offering
+        // We don't want to make this public API offering for now especially since
+        // only Linux (of all platforms supported by this library) supports this type
+        // of socket.
         if (isLowEnergySocket) {
             addr.l2_cid = port;
             addr.l2_bdaddr_type = BDADDR_LE_PUBLIC;
diff --git a/src/bluetooth/qlowenergycontrollernew.cpp b/src/bluetooth/qlowenergycontrollernew.cpp
index a30d18b7..3b6cb5eb 100644
--- a/src/bluetooth/qlowenergycontrollernew.cpp
+++ b/src/bluetooth/qlowenergycontrollernew.cpp
@@ -173,6 +173,22 @@ void QLowEnergyControllerNew::disconnectFromDevice()
     d->disconnectFromDevice();
 }
 
+void QLowEnergyControllerNew::discoverServices()
+{
+    Q_D(QLowEnergyControllerNew);
+
+    if (d->state != QLowEnergyControllerNew::ConnectedState)
+        return;
+
+    d->serviceList.clear();
+    d->discoverServices();
+}
+
+QList<QBluetoothUuid> QLowEnergyControllerNew::services() const
+{
+    return d_ptr->serviceList.keys();
+}
+
 QLowEnergyControllerNew::Error QLowEnergyControllerNew::error() const
 {
     return d_ptr->error;
diff --git a/src/bluetooth/qlowenergycontrollernew.h b/src/bluetooth/qlowenergycontrollernew.h
index 16882f58..529781ee 100644
--- a/src/bluetooth/qlowenergycontrollernew.h
+++ b/src/bluetooth/qlowenergycontrollernew.h
@@ -44,6 +44,7 @@
 
 #include <QObject>
 #include <QtBluetooth/QBluetoothAddress>
+#include <QtBluetooth/QBluetoothUuid>
 
 QT_BEGIN_NAMESPACE
 
@@ -82,6 +83,9 @@ public:
     void connectToDevice();
     void disconnectFromDevice();
 
+    void discoverServices();
+    QList<QBluetoothUuid> services() const;
+
 
     Error error() const;
     QString errorString() const;
@@ -92,6 +96,9 @@ Q_SIGNALS:
     void stateChanged(QLowEnergyControllerNew::ControllerState state);
     void error(QLowEnergyControllerNew::Error newError);
 
+    void serviceDiscovered(const QBluetoothUuid &newService);
+    void discoveryFinished();
+
 private:
     Q_DECLARE_PRIVATE(QLowEnergyControllerNew)
     QLowEnergyControllerNewPrivate *d_ptr;
diff --git a/src/bluetooth/qlowenergycontrollernew_bluez.cpp b/src/bluetooth/qlowenergycontrollernew_bluez.cpp
index 9386fe33..ad0ee3eb 100644
--- a/src/bluetooth/qlowenergycontrollernew_bluez.cpp
+++ b/src/bluetooth/qlowenergycontrollernew_bluez.cpp
@@ -45,12 +45,38 @@
 #include <QtCore/QLoggingCategory>
 #include <QtBluetooth/QBluetoothSocket>
 
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/uuid.h>
+
 #define ATTRIBUTE_CHANNEL_ID 4
 
+#define GATT_PRIMARY_SERVICE    0x2800
+
+#define ATT_OP_READ_BY_GROUP_REQUEST    0x10
+#define ATT_OP_READ_BY_GROUP_RESPONSE   0x11
+
 QT_BEGIN_NAMESPACE
 
 Q_DECLARE_LOGGING_CATEGORY(QT_BT_BLUEZ)
 
+static inline QBluetoothUuid convert_uuid128(const uint128_t *p)
+{
+    uint128_t dst;
+    bt_uuid_t uuid;
+    btoh128(p, &dst);
+    bt_uuid128_create(&uuid, dst);
+//    //TODO don't use string conversion but raw ints once QBluetoothUuid ctor is fixed
+//    quint128 qtdst;
+//    memcpy(&qtdst, &dst, sizeof(uint128_t));
+
+    char buffer[48];
+    bt_uuid_to_string(&uuid, buffer, 48);
+
+//    qDebug() << buffer << QBluetoothUuid(qtdst);
+
+    return QBluetoothUuid(QString::fromLocal8Bit(buffer));
+}
+
 void QLowEnergyControllerNewPrivate::connectToDevice()
 {
     setState(QLowEnergyControllerNew::ConnectingState);
@@ -62,6 +88,7 @@ void QLowEnergyControllerNewPrivate::connectToDevice()
     connect(l2cpSocket, SIGNAL(disconnected()), this, SLOT(l2cpDisconnected()));
     connect(l2cpSocket, SIGNAL(error(QBluetoothSocket::SocketError)),
             this, SLOT(l2cpErrorChanged(QBluetoothSocket::SocketError)));
+    connect(l2cpSocket, SIGNAL(readyRead()), this, SLOT(l2cpReadyRead()));
 
     l2cpSocket->d_ptr->isLowEnergySocket = true;
     l2cpSocket->connectToService(remoteDevice, ATTRIBUTE_CHANNEL_ID);
@@ -115,4 +142,89 @@ void QLowEnergyControllerNewPrivate::l2cpErrorChanged(QBluetoothSocket::SocketEr
     setState(QLowEnergyControllerNew::UnconnectedState);
 }
 
+void QLowEnergyControllerNewPrivate::l2cpReadyRead()
+{
+    Q_Q(QLowEnergyControllerNew);
+
+    const QByteArray response = l2cpSocket->readAll();
+    //qDebug() << response.size() << "data:" << response.toHex();
+
+    if (response.isEmpty())
+        return;
+
+    const quint8 command = response.constData()[0];
+    QLowEnergyHandle start = 0, end = 0;
+
+    switch (command) {
+    case ATT_OP_READ_BY_GROUP_RESPONSE:
+    {
+        const quint16 elementLength = response.constData()[1];
+        const quint16 numElements = (response.size() - 2) / elementLength;
+        //qDebug() << numElements << elementLength << response.size() - 2;
+        quint16 offset = 2;
+        const char *data = response.constData();
+        for (int i = 0; i < numElements; i++) {
+            start = bt_get_le16(&data[offset]);
+            end = bt_get_le16(&data[offset+2]);
+
+            QBluetoothUuid uuid;
+            if (elementLength == 6) //16 bit uuid
+                uuid = QBluetoothUuid(bt_get_le16(&data[offset+4]));
+            else if (elementLength == 20) //128 bit uuid
+                uuid = convert_uuid128((uint128_t *)&data[offset+4]);
+            //else -> do nothing
+
+            offset += elementLength;
+
+
+            //qDebug() << "Found uuid:" << uuid << "start handle:" << hex
+            //         << start << "end handle:" << end;
+            HandlePair pair(start, end);
+            serviceList.insert(uuid, pair);
+            emit q->serviceDiscovered(uuid);
+        }
+
+        break;
+    }
+    default:
+        qCDebug(QT_BT_BLUEZ) << "Unknown packet: " << response.toHex();
+    }
+
+    if (end != 0xFFFF)
+        sendReadByGroupRequest(end+1, 0xFFFF);
+    else
+        emit q->discoveryFinished();
+}
+
+void QLowEnergyControllerNewPrivate::discoverServices()
+{
+    sendReadByGroupRequest(0x0001, 0xFFFF);
+}
+
+void QLowEnergyControllerNewPrivate::sendReadByGroupRequest(
+                    QLowEnergyHandle start, QLowEnergyHandle end)
+{
+    //call for primary services
+    bt_uuid_t primary;
+    bt_uuid16_create(&primary, GATT_PRIMARY_SERVICE);
+
+#define GRP_TYPE_REQ_SIZE 7
+    quint8 packet[GRP_TYPE_REQ_SIZE];
+
+    packet[0] = ATT_OP_READ_BY_GROUP_REQUEST;
+    bt_put_unaligned(htobs(start), (quint16 *) &packet[1]);
+    bt_put_unaligned(htobs(end), (quint16 *) &packet[3]);
+    bt_put_unaligned(htobs(primary.value.u16), (quint16 *) &packet[5]);
+
+    QByteArray data(GRP_TYPE_REQ_SIZE, Qt::Uninitialized);
+    memcpy(data.data(), packet,  GRP_TYPE_REQ_SIZE);
+
+    //qDebug() << "Sending grp request" << hex << start << end;
+    qint64 result = l2cpSocket->write(data.constData(), GRP_TYPE_REQ_SIZE);
+    if (result == -1) {
+        qCDebug(QT_BT_BLUEZ) << "Cannot write ReadByGroupRequest";
+        setError(QLowEnergyControllerNew::NetworkError);
+    }
+}
+
 QT_END_NAMESPACE
diff --git a/src/bluetooth/qlowenergycontrollernew_p.cpp b/src/bluetooth/qlowenergycontrollernew_p.cpp
index fd761dc9..25c12895 100644
--- a/src/bluetooth/qlowenergycontrollernew_p.cpp
+++ b/src/bluetooth/qlowenergycontrollernew_p.cpp
@@ -53,4 +53,9 @@ void QLowEnergyControllerNewPrivate::disconnectFromDevice()
 
 }
 
+void QLowEnergyControllerNewPrivate::discoverServices()
+{
+
+}
+
 QT_END_NAMESPACE
diff --git a/src/bluetooth/qlowenergycontrollernew_p.h b/src/bluetooth/qlowenergycontrollernew_p.h
index 737a8dfd..ff6eec66 100644
--- a/src/bluetooth/qlowenergycontrollernew_p.h
+++ b/src/bluetooth/qlowenergycontrollernew_p.h
@@ -47,8 +47,10 @@
 
 #ifdef QT_BLUEZ_BLUETOOTH
 #include <QtBluetooth/QBluetoothSocket>
+#include <QtBluetooth/qbluetooth.h>
 #endif
 
+typedef QPair<QLowEnergyHandle,QLowEnergyHandle> HandlePair;
 
 QT_BEGIN_NAMESPACE
 
@@ -73,6 +75,8 @@ public:
     void connectToDevice();
     void disconnectFromDevice();
 
+    void discoverServices();
+
     QBluetoothAddress remoteDevice;
     QBluetoothAddress localAdapter;
 
@@ -82,13 +86,19 @@ public:
 
 
 private:
+    // list of all found service uuids
+    QMap<QBluetoothUuid, HandlePair> serviceList;
+
 #ifdef QT_BLUEZ_BLUETOOTH
     QBluetoothSocket *l2cpSocket;
 
+    void sendReadByGroupRequest(QLowEnergyHandle start, QLowEnergyHandle end);
+
 private slots:
     void l2cpConnected();
     void l2cpDisconnected();
     void l2cpErrorChanged(QBluetoothSocket::SocketError);
+    void l2cpReadyRead();
 #endif
 private:
     QLowEnergyControllerNew *q_ptr;
diff --git a/tests/auto/qlowenergycontroller/tst_qlowenergycontroller.cpp b/tests/auto/qlowenergycontroller/tst_qlowenergycontroller.cpp
index 35ffe1d6..2c26ffe8 100644
--- a/tests/auto/qlowenergycontroller/tst_qlowenergycontroller.cpp
+++ b/tests/auto/qlowenergycontroller/tst_qlowenergycontroller.cpp
@@ -764,11 +764,13 @@ void tst_QLowEnergyController::tst_connectNew()
     QVERIFY(control.errorString().isEmpty());
     QCOMPARE(disconnectedSpy.count(), 0);
     QCOMPARE(connectedSpy.count(), 0);
+    QVERIFY(control.services().isEmpty());
 
     bool wasError = false;
     control.connectToDevice();
     QTRY_IMPL(control.state() != QLowEnergyControllerNew::ConnectingState,
               10000);
+
     QCOMPARE(disconnectedSpy.count(), 0);
     if (control.error() != QLowEnergyControllerNew::NoError) {
         //error during connect
@@ -780,6 +782,7 @@ void tst_QLowEnergyController::tst_connectNew()
         QCOMPARE(connectedSpy.count(), 0);
         QVERIFY(control.errorString().isEmpty());
         QCOMPARE(control.error(), QLowEnergyControllerNew::NoError);
+        QVERIFY(control.services().isEmpty());
         QSKIP("Connection to LE device cannot be established. Skipping test.");
         return;
     } else {
@@ -789,9 +792,34 @@ void tst_QLowEnergyController::tst_connectNew()
         QVERIFY(control.errorString().isEmpty());
     }
 
+    QVERIFY(control.services().isEmpty());
+
+    QSignalSpy discoveryFinishedSpy(&control, SIGNAL(discoveryFinished()));
+    QSignalSpy serviceFoundSpy(&control, SIGNAL(serviceDiscovered(QBluetoothUuid)));
+    control.discoverServices();
+    QTRY_VERIFY_WITH_TIMEOUT(discoveryFinishedSpy.count() == 1, 10000);
+
+    QCOMPARE(serviceFoundSpy.count(), foundServices.count());
+    QList<QBluetoothUuid> listing;
+    for (int i = 0; i < serviceFoundSpy.count(); i++) {
+        const QVariant v = serviceFoundSpy[i].at(0);
+        listing.append(v.value<QBluetoothUuid>());
+    }
+
+    foreach (const QLowEnergyServiceInfo &info, foundServices) {
+        QVERIFY2(listing.contains(info.serviceUuid()),
+                 info.serviceUuid().toString().toLatin1());
+    }
+
+    foreach (const QBluetoothUuid &uuid, control.services())
+        QVERIFY2(listing.contains(uuid), uuid.toString().toLatin1());
+
+    // Finish off
     control.disconnectFromDevice();
-    QTRY_VERIFY_WITH_TIMEOUT(control.state() == QLowEnergyControllerNew::UnconnectedState,
-                             10000);
+    QTRY_VERIFY_WITH_TIMEOUT(
+                control.state() == QLowEnergyControllerNew::UnconnectedState,
+                10000);
+
     if (wasError)
         QCOMPARE(disconnectedSpy.count(), 0);
     else