o Successfully decoding GATT services.

1 files changed, 82 insertions, 19 deletions

diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp
index b886dde..7be7c9a 100644
--- a/LinuxBluetooth.cpp
+++ b/LinuxBluetooth.cpp
@@ -1,4 +1,4 @@
-#include "Bluetooth.h"
+#include "BluetoothImpl.h"
 
 #include <string.h>
 #include <bluetooth/bluetooth.h>
@@ -35,9 +35,9 @@ public:
     BluetoothDevice &getDevice(Mac &mac) override;
 
 private:
-    void startScan();
+    void startScan() override;
 
-    void stopScan();
+    void stopScan() override;
 
     int hciDeviceId;
     int hciSocket;
@@ -47,7 +47,7 @@ private:
     map<Mac, LinuxBluetoothDevice *> devices;
 };
 
-class LinuxBluetoothDevice : public BluetoothDevice {
+class LinuxBluetoothDevice : public DefaultBluetoothDevice {
 public:
     LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac mac);
 
@@ -62,6 +62,8 @@ public:
     void discoverServices() override;
 
 private:
+    vector <AttributeData> discoverServices(uint16_t startHandle);
+
     LinuxBluetoothAdapter &_adapter;
     Mac _mac;
     int l2cap;
@@ -86,7 +88,7 @@ Mac parseMac(bdaddr_t &a) {
 // -----------------------------------------------------------------------
 
 LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac mac) :
-        _adapter(adapter), _mac(mac) {
+        DefaultBluetoothDevice(), _adapter(adapter), _mac(mac) {
 }
 
 Mac const &LinuxBluetoothDevice::mac() {
@@ -149,25 +151,87 @@ void LinuxBluetoothDevice::disconnect() {
 }
 
 void LinuxBluetoothDevice::discoverServices() {
+    uint16_t startHandle = 0x0001;
+
+    do {
+        vector<AttributeData> values = discoverServices(startHandle);
+
+        // Shouldn't happen, but you never know.
+        if (values.size() == 0) {
+            break;
+        }
+
+        for (auto &data: values) {
+            D << "handle: 0x" << hex << setw(4) << setfill('0') << data.handle <<
+                        ", groupEndHandle: 0x" << hex << setw(4) << setfill('0') << data.groupEndHandle <<
+                        ", value: " << data.value.toString();
+
+            boost::uuids::uuid u;
+
+            size_t s = data.value.getSize();
+
+            if (s == 2) {
+                uint8_t bs[16] = BLUETOOTH_UUID_INITIALIZER;
+                bs[2] = data.value.get8(1);
+                bs[3] = data.value.get8(0);
+                memcpy(&u, bs, 16);
+            } else if (s == 16) {
+                uint8_t bs[16];
+                bs[15] = data.value.get8(0);
+                bs[14] = data.value.get8(1);
+                bs[13] = data.value.get8(2);
+                bs[12] = data.value.get8(3);
+                bs[11] = data.value.get8(4);
+                bs[10] = data.value.get8(5);
+                bs[9] = data.value.get8(6);
+                bs[8] = data.value.get8(7);
+                bs[7] = data.value.get8(8);
+                bs[6] = data.value.get8(9);
+                bs[5] = data.value.get8(10);
+                bs[4] = data.value.get8(11);
+                bs[3] = data.value.get8(12);
+                bs[2] = data.value.get8(13);
+                bs[1] = data.value.get8(14);
+                bs[0] = data.value.get8(15);
+                memcpy(&u, bs, 16);
+            }
+            else {
+                throw BluetoothException(this, "Unexpected size: " + to_string(data.value.getSize()));
+            }
+
+            services.push_back(new DefaultBluetoothGattService(*this, u));
+        }
+
+        auto last = values.back();
+
+        startHandle = last.groupEndHandle;
+    } while (startHandle != 0xffff);
+
+    for(auto s : services) {
+        D << "service: " << to_string(s->getUuid());
+    }
+}
+
+vector<AttributeData> LinuxBluetoothDevice::discoverServices(uint16_t startHandle) {
     DF;
 
-    uint8_t buffer[MAX_MTU];
+    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
     ByteBuffer out = ByteBuffer(buffer, MAX_MTU);
 
-    AttPdu::makeReadByGroupType(out, 0x0001, 0xffff, UUID_PRIMARY_SERVICE);
+    AttPdu::makeReadByGroupType(out, startHandle, 0xffff, UUID_PRIMARY_SERVICE);
 
     D << "pdu size=" << out.getCursor();
-    ssize_t written = write(l2cap, buffer, out.getCursor());
+    ssize_t written = write(l2cap, buffer.get(), out.getCursor());
 
     D << "written=" << written;
 
-    ssize_t r = read(l2cap, buffer, MAX_MTU);
+    ssize_t r = read(l2cap, buffer.get(), MAX_MTU);
 
     if (r == -1) {
         throw BluetoothException(this, "read(): " + errnoAsString());
     }
 
-    ByteBuffer in = ByteBuffer(buffer, (size_t) r);
+    auto in = ByteBuffer(buffer, (size_t) r);
 
     D << "read: " << r << " bytes: " << in.toString();
 
@@ -175,11 +239,7 @@ void LinuxBluetoothDevice::discoverServices() {
 
     D << "READ_BY_GROUP_TYPE response has " + to_string(values.size()) + " values";
 
-    for (auto &data: values) {
-        D << "handle: 0x" << setw(4) << setfill('0') << hex << data.handle <<
-                    ", groupEndHandle: 0x" << hex << setw(4) << setfill('0')  << data.groupEndHandle <<
-                    ", value: " << data.value.toString();
-    }
+    return values;
 }
 
 // -----------------------------------------------------------------------
@@ -241,7 +301,7 @@ void LinuxBluetoothAdapter::startScan() {
     int up = hci_test_bit(HCI_UP, &di.flags);
 
     if (!up) {
-        throw BluetoothException(this, "HCI adapter is not up: " + hciDeviceId);
+        throw BluetoothException(this, "HCI adapter is not up: " + to_string(hciDeviceId));
     }
 
     if (hci_le_set_scan_parameters(hciSocket, 0x01, htobs(0x0010), htobs(0x0010), 0x00, 0, 1000) < 0) {
@@ -326,7 +386,9 @@ void LinuxBluetoothAdapter::runScan(void (*callback)(BluetoothDevice &device)) {
     }
 }
 
-}}}
+}
+}
+}
 
 // -----------------------------------------------------------------------
 // Implementation of platform-specific method.
@@ -338,7 +400,7 @@ using namespace trygvis::bluetooth::linux;
 
 map<int, LinuxBluetoothAdapter *> adapters;
 
-LinuxBluetoothAdapter &getAdapterImpl(int hciDevice) {
+BluetoothAdapter &getAdapterImpl(int hciDevice) {
     map<int, LinuxBluetoothAdapter *>::iterator it = adapters.find(hciDevice);
 
     if (it == adapters.end()) {
@@ -356,4 +418,5 @@ void shutdownImpl() {
     }
 }
 
-}}
+}
+}