o Decoding GATT characteristics.

5 files changed, 178 insertions, 91 deletions

diff --git a/Bluetooth.cpp b/Bluetooth.cpp
index e634233..b135282 100644
--- a/Bluetooth.cpp
+++ b/Bluetooth.cpp
@@ -134,13 +134,12 @@ vector<AttributeData> AttPdu::parseReadByType(ByteBuffer &bytes) {
 
 AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) {
     uint16_t handle = bytes.read16le();
-    uint16_t groupEndHandle = bytes.read16le();
 
-    return AttributeData(handle, groupEndHandle, bytes.view());
+    return AttributeData(handle, bytes.view());
 }
 
-AttributeData::AttributeData(uint16_t handle, uint16_t groupEndHandle, ByteBuffer value) :
-        handle(handle), endGroupHandle(groupEndHandle), value(value) {
+AttributeData::AttributeData(uint16_t handle, ByteBuffer value) :
+        handle(handle), value(value) {
 }
 
 AttributeData::~AttributeData() {
diff --git a/Bluetooth.h b/Bluetooth.h
index e840f37..4314d8e 100644
--- a/Bluetooth.h
+++ b/Bluetooth.h
@@ -84,7 +84,13 @@ public:
 
     virtual BluetoothGattService &getService() const = 0;
 
+    virtual uint16_t getHandle() const = 0;
+
     virtual const boost::uuids::uuid getUuid() const = 0;
+
+    virtual uint8_t getProperties() const = 0;
+
+    virtual uint16_t getValueHandle() const = 0;
 };
 
 class BluetoothGattService {
@@ -100,7 +106,9 @@ public:
 
     virtual uint16_t getEndGroupHandle() const = 0;
 
-    virtual vector<BluetoothGattCharacteristic *>::const_iterator getCharacteristics() const = 0;
+    virtual const vector<BluetoothGattCharacteristic *> getCharacteristics() const = 0;
+
+    virtual void addCharacteristic(BluetoothGattCharacteristic* characteristic) = 0;
 };
 
 class BluetoothDevice {
@@ -119,7 +127,7 @@ public:
 
     virtual void discoverServices() = 0;
 
-    virtual vector<BluetoothGattService *>::const_iterator getServices() const = 0;
+    virtual const vector<BluetoothGattService *> getServices() const = 0;
 };
 
 class BluetoothAdapter {
@@ -179,11 +187,10 @@ public:
     static AttributeData fromByteBuffer(ByteBuffer &value);
 
     const uint16_t handle;
-    const uint16_t endGroupHandle;
-    const ByteBuffer value;
+    ByteBuffer value;
 
 private:
-    AttributeData(uint16_t handle, uint16_t groupEndHandle, ByteBuffer value);
+    AttributeData(uint16_t handle, ByteBuffer value);
 };
 
 BluetoothAdapter &getAdapter(int hciDevice);
diff --git a/BluetoothImpl.h b/BluetoothImpl.h
index 0263da5..3fad164 100644
--- a/BluetoothImpl.h
+++ b/BluetoothImpl.h
@@ -20,52 +20,75 @@ typedef boost::uuids::uuid uuid_t;
 
 class DefaultBluetoothGattCharacteristic : public BluetoothGattCharacteristic {
 public:
-    DefaultBluetoothGattCharacteristic(BluetoothGattService& service, uuid_t uuid) : service(service), uuid(uuid) {
+    DefaultBluetoothGattCharacteristic(BluetoothGattService &service, uint16_t handle, uuid_t uuid, uint8_t properties, uint16_t valueHandle)
+            : service(service), handle(handle), uuid(uuid), properties(properties), valueHandle(valueHandle) {
     }
 
     virtual ~DefaultBluetoothGattCharacteristic() {
     }
 
+    BluetoothGattService &getService() const {
+        return service;
+    }
+
+    uint16_t getHandle() const {
+        return handle;
+    }
+
     const uuid_t getUuid() const {
         return uuid;
-    };
+    }
 
-    BluetoothGattService& getService() const {
-        return service;
-    };
+    uint8_t getProperties() const {
+        return properties;
+    }
+
+    uint16_t getValueHandle() const {
+        return valueHandle;
+    }
 
 protected:
-    BluetoothGattService& service;
+    BluetoothGattService &service;
+    uint16_t handle;
     uuid_t uuid;
+    uint8_t properties;
+    uint16_t valueHandle;
 };
 
 class DefaultBluetoothGattService : public BluetoothGattService {
 public:
-    DefaultBluetoothGattService(BluetoothDevice& device, const uuid_t uuid, const uint16_t handle, const uint16_t endGroupHandle) :
-            device(device), uuid(uuid), handle(handle), endGroupHandle(endGroupHandle) {
+    DefaultBluetoothGattService(BluetoothDevice &device, const uuid_t uuid, const uint16_t handle, const uint16_t endGroupHandle)
+            : device(device), uuid(uuid), handle(handle), endGroupHandle(endGroupHandle) {
+        DF;
     }
 
     virtual ~DefaultBluetoothGattService() {
+        DF;
+        removeCharacteristics();
     }
 
-    BluetoothDevice& getDevice() const {
+    virtual BluetoothDevice &getDevice() const {
         return device;
     }
 
-    uuid_t getUuid() const {
+    virtual uuid_t getUuid() const {
         return uuid;
     }
 
-    uint16_t getHandle() const {
-            return handle;
+    virtual uint16_t getHandle() const {
+        return handle;
     }
 
-     uint16_t getEndGroupHandle() const {
-         return endGroupHandle;
-     }
+    virtual uint16_t getEndGroupHandle() const {
+        return endGroupHandle;
+    }
+
+    virtual const vector<BluetoothGattCharacteristic *> getCharacteristics() const {
+        return characteristics;
+    }
 
-    vector<BluetoothGattCharacteristic* >::const_iterator getCharacteristics() const {
-        return characteristics.begin();
+    virtual void addCharacteristic(BluetoothGattCharacteristic *characteristic) {
+        characteristics.push_back(characteristic);
     }
 
 protected:
@@ -73,22 +96,45 @@ protected:
     const uuid_t uuid;
     const uint16_t handle;
     const uint16_t endGroupHandle;
-    const vector<BluetoothGattCharacteristic*> characteristics;
+    vector<BluetoothGattCharacteristic *> characteristics;
+
+    void removeCharacteristics() {
+        DF;
+        for (auto &c: characteristics) {
+            delete c;
+        }
+        characteristics.clear();
+    }
 };
 
 class DefaultBluetoothDevice : public BluetoothDevice {
 public:
-    virtual vector<BluetoothGattService*>::const_iterator getServices() const {
-        return services.begin();
+    virtual const vector<BluetoothGattService *> getServices() const {
+        return services;
     };
 
+    virtual void addService(BluetoothGattService *service) {
+        services.push_back(service);
+    }
+
 protected:
+    DefaultBluetoothDevice() {
+        DF;
+    }
+
     virtual ~DefaultBluetoothDevice() {
+        DF;
+        removeServices();
+    }
+
+    void removeServices() {
         for (auto s: services) {
             delete s;
         }
+        services.clear();
     }
-    vector<BluetoothGattService*> services;
+
+    vector<BluetoothGattService *> services;
 };
 
 BluetoothAdapter &getAdapterImpl(int hciDevice);
diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp
index 22d657e..4b7f0e0 100644
--- a/LinuxBluetooth.cpp
+++ b/LinuxBluetooth.cpp
@@ -68,9 +68,13 @@ public:
 
 private:
     vector<AttributeData> discoverServices(uint16_t startHandle);
+
     vector<AttributeData> discoverCharacteristics(uint16_t startHandle, uint16_t endHandle);
+
     ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size);
 
+    uuid_t readUuid(const ByteBuffer &bytes) const;
+
     LinuxBluetoothAdapter &_adapter;
     Mac _mac;
     int l2cap;
@@ -153,45 +157,51 @@ void LinuxBluetoothDevice::connect() {
 }
 
 void LinuxBluetoothDevice::disconnect() {
-    DF << "mac=" << _mac.str();
+    DF << "mac = " << _mac.str();
     close(l2cap);
 }
 
-uuid_t readUuid(ByteBuffer &bytes) {
-    size_t s = data.value.getBytesLeft();
+uuid_t LinuxBluetoothDevice::readUuid(const ByteBuffer &bytes) const {
+    size_t bytesLeft = bytes.getBytesLeft();
 
-    if (s == 2) {
+    uuid_t u;
+
+    if (bytesLeft == 2) {
         uint8_t bs[16] = BLUETOOTH_UUID_INITIALIZER;
-        bs[2] = data.value.get8(1);
-        bs[3] = data.value.get8(0);
+        bs[2] = bytes.get8(1);
+        bs[3] = bytes.get8(0);
         memcpy(&u, bs, 16);
-    } else if (s == 16) {
+    } else if (bytesLeft == 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);
+        bs[15] = bytes.get8(0);
+        bs[14] = bytes.get8(1);
+        bs[13] = bytes.get8(2);
+        bs[12] = bytes.get8(3);
+        bs[11] = bytes.get8(4);
+        bs[10] = bytes.get8(5);
+        bs[9] = bytes.get8(6);
+        bs[8] = bytes.get8(7);
+        bs[7] = bytes.get8(8);
+        bs[6] = bytes.get8(9);
+        bs[5] = bytes.get8(10);
+        bs[4] = bytes.get8(11);
+        bs[3] = bytes.get8(12);
+        bs[2] = bytes.get8(13);
+        bs[1] = bytes.get8(14);
+        bs[0] = bytes.get8(15);
         memcpy(&u, bs, 16);
     } else {
-        throw BluetoothException(this, "Unexpected size: " + to_string(data.value.getSize()));
+        throw BluetoothException(this, "Unexpected bytes left: " + to_string(bytesLeft));
     }
+
+    return u;
 }
 
 void LinuxBluetoothDevice::discoverServices() {
     uint16_t startHandle = 0x0001;
 
+    removeServices();
+
     do {
         vector<AttributeData> values = discoverServices(startHandle);
 
@@ -200,29 +210,41 @@ void LinuxBluetoothDevice::discoverServices() {
             break;
         }
 
+        uint16_t endGroupHandle;
+
         for (auto &data: values) {
-            D << "handle: 0x" << hex << setw(4) << setfill('0') << data.handle <<
-                        ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << data.endGroupHandle <<
-                        ", value: " << data.value.toString();
+            endGroupHandle = data.value.read16le();
+
+//            D << "handle: 0x" << hex << setw(4) << setfill('0') << data.handle <<
+//                        ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle <<
+//                        ", value: " << data.value.toString();
 
             uuid_t u = readUuid(data.value);
 
-            services.push_back(new DefaultBluetoothGattService(*this, u, data.handle, data.endGroupHandle));
+            addService(new DefaultBluetoothGattService(*this, u, data.handle, endGroupHandle));
         }
 
         auto last = values.back();
 
-        startHandle = last.endGroupHandle;
+        startHandle = endGroupHandle;
     } while (startHandle != 0xffff);
 
-    for (auto &s : services) {
-        D << "service: " << to_string(s->getUuid()) << ", handle: " << s->getHandle() << ", end group handle: " << s->getEndGroupHandle();
-    }
+//    for (auto &s : services) {
+//        D << "service: " << to_string(s->getUuid()) << ", handle: " << s->getHandle() << ", end group handle: " << s->getEndGroupHandle();
+//        D << "s= " << (uint64_t) s;
+//    }
 
     auto it = services.begin(),
-         end = services.end();
+            end = services.end();
+
+    if (it == end) {
+        return;
+    }
 
     startHandle = 0x0001;
+    vector<AttributeData> chars;
+
+    auto s = *it;
 
     do {
         vector<AttributeData> values = discoverCharacteristics(startHandle, 0xffff);
@@ -231,32 +253,32 @@ void LinuxBluetoothDevice::discoverServices() {
             break;
         }
 
-        for (auto c : values) {
-            uint8_t properties = c.data.read8();
-            uint16_t valueHandle = c.data.read16le();
+        uint16_t lastHandle = 0xffff;
 
-            if (values.data.getBytesLeft() == 2) {
-                uint16_t uuid = values.data.read16le();
-            } else {
+        for (auto &c : values) {
+            while (c.handle > s->getEndGroupHandle() && it != end) {
+                s = *it++;
+//                D << "service: " << s->getHandle();
             }
-        }
-
-        auto last = values.back();
 
-        startHandle = last.endGroupHandle;
-    } while (startHandle != 0xffff);
+            lastHandle = c.handle;
 
-    /*
-    if (it != end) {
-        auto &service = *it;
+            uint8_t properties = c.value.read8();
+            uint16_t valueHandle = c.value.read16le();
+            uuid_t uuid = readUuid(c.value);
 
-        for (auto &s : services) {
-            vector<AttributeData> values = discoverCharacteristics(s->getHandle(), 0xffff);
+//            D << "characteristic: handle: " << setw(2) << setfill('0') << hex << (int) c.handle <<
+//                        ", properties: " << setw(2) << setfill('0') << hex << (int) properties <<
+//                        ", valueHandle: 0x" << setw(4) << setfill('0') << hex << (int) valueHandle <<
+//                        ", uuid: " << uuid;
 
-//            service = accumulate(services.begin(), services.end(), [](a&, b&){});
+            s->addCharacteristic(new DefaultBluetoothGattCharacteristic(*s, c.handle, uuid, properties, valueHandle));
         }
-    }
-    */
+
+        auto last = values.back();
+
+        startHandle = lastHandle + (uint8_t) 2;
+    } while (startHandle != 0xffff);
 }
 
 ByteBuffer LinuxBluetoothDevice::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size) {
@@ -343,8 +365,6 @@ LinuxBluetoothAdapter::~LinuxBluetoothAdapter() {
 
     close(hciSocket);
 
-    D << "Depeting " << devices.size() << " devices";
-
     for (auto &pair : devices) {
         delete pair.second;
     }
diff --git a/main.cpp b/main.cpp
index 7ae4c1b..361311c 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,5 +1,7 @@
 #include <exception>
 #include <iostream>
+#include <vector>
+#include <boost/uuid/uuid_io.hpp>
 #include "Bluetooth.h"
 
 using namespace std;
@@ -15,12 +17,25 @@ void scan_callback(BluetoothDevice &device) {
         return;
     }
 
-    cout << "found device: " << device.mac().str() << endl;
+    cout << "Connecting to device: " << device.mac().str() << endl;
 
     device.connect();
 
     device.discoverServices();
 
+    vector<BluetoothGattService *> services = device.getServices();
+    cout << "Device has " << services.size() << " services" << endl;
+    
+    for (auto &s: services) {
+        const vector<BluetoothGattCharacteristic *> characteristics = s->getCharacteristics();
+
+        cout << "Service: UUID: " << s->getUuid() << ", has " << characteristics.size() << " characteristics" << endl;
+
+        for (auto &c: characteristics) {
+            cout << "Characteristic: UUID: " << c->getUuid() << ", properties: " << (int) c->getProperties() << endl;
+        }
+    }
+
     device.disconnect();
 }
 
@@ -32,18 +47,18 @@ int main(int argc, char *argv[]) {
 
     int e;
 //    try {
-        Mac mac = Mac::parseMac(argv[1]);
-        targetMac = &mac;
+    Mac mac = Mac::parseMac(argv[1]);
+    targetMac = &mac;
 
-        BluetoothAdapter &adapter = getAdapter(0);
+    BluetoothAdapter &adapter = getAdapter(0);
 
-        BluetoothDevice &device = adapter.getDevice(mac);
+    BluetoothDevice &device = adapter.getDevice(mac);
 
-        scan_callback(device);
+    scan_callback(device);
 
 //        adapter->runScan(scan_callback);
 
-        e = EXIT_SUCCESS;
+    e = EXIT_SUCCESS;
 //    } catch (std::runtime_error ex) {
 //        W << "std::runtime_error: " << ex.what();
 //        e = EXIT_FAILURE;