From 71167b3c8fb1ce47feb72b9376b899e40ffd3907 Mon Sep 17 00:00:00 2001
From: Trygve Laugstøl <trygvis@inamo.no>
Date: Fri, 20 Feb 2015 17:53:33 +0100
Subject: wip

---
 ByteBuffer.h       |  10 +++---
 LinuxBluetooth.cpp | 104 ++++++++++++++++++++++++++++++++++++-----------------
 2 files changed, 75 insertions(+), 39 deletions(-)

diff --git a/ByteBuffer.h b/ByteBuffer.h
index b3e06cf..3836966 100644
--- a/ByteBuffer.h
+++ b/ByteBuffer.h
@@ -17,14 +17,8 @@ public:
 
 class ByteBuffer {
 public:
-    /**
-     * Wrapping constructor, the size will be equal to the capacity.
-     */
     ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity);
 
-    /**
-    * Wrapping constructor.
-    */
     ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t zero, size_t size);
 
     inline size_t getSize() const {
@@ -40,6 +34,10 @@ public:
         return ptr - zero;
     }
 
+    inline size_t getBytesLeft() const {
+        return end - ptr;
+    }
+
     inline void setCursor(size_t newCursor) {
 //        assertCanAccessRelative(newCursor);
         ptr = (uint8_t *) &zero[newCursor];
diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp
index 3d5cded..22d657e 100644
--- a/LinuxBluetooth.cpp
+++ b/LinuxBluetooth.cpp
@@ -8,6 +8,7 @@
 #include <map>
 #include <sstream>
 #include <iomanip>
+#include <numeric>
 
 // Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c
 #define ATT_CID 4
@@ -156,6 +157,38 @@ void LinuxBluetoothDevice::disconnect() {
     close(l2cap);
 }
 
+uuid_t readUuid(ByteBuffer &bytes) {
+    size_t s = data.value.getBytesLeft();
+
+    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()));
+    }
+}
+
 void LinuxBluetoothDevice::discoverServices() {
     uint16_t startHandle = 0x0001;
 
@@ -172,37 +205,7 @@ void LinuxBluetoothDevice::discoverServices() {
                         ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << data.endGroupHandle <<
                         ", value: " << data.value.toString();
 
-            uuid_t 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()));
-            }
+            uuid_t u = readUuid(data.value);
 
             services.push_back(new DefaultBluetoothGattService(*this, u, data.handle, data.endGroupHandle));
         }
@@ -216,9 +219,44 @@ void LinuxBluetoothDevice::discoverServices() {
         D << "service: " << to_string(s->getUuid()) << ", handle: " << s->getHandle() << ", end group handle: " << s->getEndGroupHandle();
     }
 
-    for (auto &s : services) {
-        discoverCharacteristics(s->getHandle(), s->getEndGroupHandle());
+    auto it = services.begin(),
+         end = services.end();
+
+    startHandle = 0x0001;
+
+    do {
+        vector<AttributeData> values = discoverCharacteristics(startHandle, 0xffff);
+
+        if (values.size() == 0) {
+            break;
+        }
+
+        for (auto c : values) {
+            uint8_t properties = c.data.read8();
+            uint16_t valueHandle = c.data.read16le();
+
+            if (values.data.getBytesLeft() == 2) {
+                uint16_t uuid = values.data.read16le();
+            } else {
+            }
+        }
+
+        auto last = values.back();
+
+        startHandle = last.endGroupHandle;
+    } while (startHandle != 0xffff);
+
+    /*
+    if (it != end) {
+        auto &service = *it;
+
+        for (auto &s : services) {
+            vector<AttributeData> values = discoverCharacteristics(s->getHandle(), 0xffff);
+
+//            service = accumulate(services.begin(), services.end(), [](a&, b&){});
+        }
     }
+    */
 }
 
 ByteBuffer LinuxBluetoothDevice::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size) {
-- 
cgit v1.2.3