1 files changed, 49 insertions, 17 deletions

diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp
index 7be7c9a..7733592 100644
--- a/LinuxBluetooth.cpp
+++ b/LinuxBluetooth.cpp
@@ -18,6 +18,10 @@ namespace trygvis {
 namespace bluetooth {
 namespace linux {
 
+typedef boost::uuids::uuid uuid_t;
+
+using namespace uuids;
+
 class LinuxBluetoothDevice;
 
 class LinuxBluetoothAdapter;
@@ -62,7 +66,9 @@ public:
     void discoverServices() override;
 
 private:
-    vector <AttributeData> discoverServices(uint16_t startHandle);
+    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);
 
     LinuxBluetoothAdapter &_adapter;
     Mac _mac;
@@ -163,10 +169,10 @@ void LinuxBluetoothDevice::discoverServices() {
 
         for (auto &data: values) {
             D << "handle: 0x" << hex << setw(4) << setfill('0') << data.handle <<
-                        ", groupEndHandle: 0x" << hex << setw(4) << setfill('0') << data.groupEndHandle <<
+                        ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << data.endGroupHandle <<
                         ", value: " << data.value.toString();
 
-            boost::uuids::uuid u;
+            uuid_t u;
 
             size_t s = data.value.getSize();
 
@@ -194,38 +200,34 @@ void LinuxBluetoothDevice::discoverServices() {
                 bs[1] = data.value.get8(14);
                 bs[0] = data.value.get8(15);
                 memcpy(&u, bs, 16);
-            }
-            else {
+            } else {
                 throw BluetoothException(this, "Unexpected size: " + to_string(data.value.getSize()));
             }
 
-            services.push_back(new DefaultBluetoothGattService(*this, u));
+            services.push_back(new DefaultBluetoothGattService(*this, u, data.handle, data.endGroupHandle));
         }
 
         auto last = values.back();
 
-        startHandle = last.groupEndHandle;
+        startHandle = last.endGroupHandle;
     } while (startHandle != 0xffff);
 
-    for(auto s : services) {
+    for (auto &s : services) {
         D << "service: " << to_string(s->getUuid());
     }
-}
 
-vector<AttributeData> LinuxBluetoothDevice::discoverServices(uint16_t startHandle) {
-    DF;
-
-    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
-    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);
-
-    AttPdu::makeReadByGroupType(out, startHandle, 0xffff, UUID_PRIMARY_SERVICE);
+    for (auto &s : services) {
+        discoverCharacteristics(s->getHandle(), s->getEndGroupHandle());
+    }
+}
 
+ByteBuffer LinuxBluetoothDevice::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size) {
     D << "pdu size=" << out.getCursor();
     ssize_t written = write(l2cap, buffer.get(), out.getCursor());
 
     D << "written=" << written;
 
-    ssize_t r = read(l2cap, buffer.get(), MAX_MTU);
+    ssize_t r = read(l2cap, buffer.get(), size);
 
     if (r == -1) {
         throw BluetoothException(this, "read(): " + errnoAsString());
@@ -235,6 +237,19 @@ vector<AttributeData> LinuxBluetoothDevice::discoverServices(uint16_t startHandl
 
     D << "read: " << r << " bytes: " << in.toString();
 
+    return in;
+}
+
+vector<AttributeData> LinuxBluetoothDevice::discoverServices(uint16_t startHandle) {
+    DF;
+
+    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
+    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);
+
+    AttPdu::makeReadByGroupType(out, startHandle, 0xffff, uuids::PRIMARY_SERVICE);
+
+    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);
+
     vector<AttributeData> values = AttPdu::parseReadByGroupType(in);
 
     D << "READ_BY_GROUP_TYPE response has " + to_string(values.size()) + " values";
@@ -242,6 +257,23 @@ vector<AttributeData> LinuxBluetoothDevice::discoverServices(uint16_t startHandl
     return values;
 }
 
+vector<AttributeData> LinuxBluetoothDevice::discoverCharacteristics(uint16_t startHandle, uint16_t endHandle) {
+    DF;
+
+    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
+    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);
+
+    AttPdu::makeReadByType(out, startHandle, endHandle, uuids::CHARACTERISTIC);
+
+    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);
+
+    vector<AttributeData> values = AttPdu::parseReadByType(in);
+
+    D << "READ_BY_TYPE response has " + to_string(values.size()) + " values";
+
+    return values;
+}
+
 // -----------------------------------------------------------------------
 // Adapter
 // -----------------------------------------------------------------------