o Reading characteristics, unable to parse just yet.

5 files changed, 99 insertions, 33 deletions

diff --git a/Bluetooth.cpp b/Bluetooth.cpp
index a8b95a1..9c32740 100644
--- a/Bluetooth.cpp
+++ b/Bluetooth.cpp
@@ -72,18 +72,18 @@ AttPduType AttPdu::getType() {
     return (AttPduType) bytes.get8(0);
 }
 
-void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid) {
+void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, SpecUuid uuid) {
     bytes.write8(AttPduType::READ_BY_GROUP_TYPE_REQ);
     bytes.write16le(startHandle);
     bytes.write16le(endHandle);
-    bytes.write16le(uuid);
+    bytes.write16le(uuid.value);
 }
 
-void AttPdu::makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid) {
+void AttPdu::makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, SpecUuid uuid) {
     bytes.write8(AttPduType::READ_BY_TYPE_REQ);
     bytes.write16le(startHandle);
     bytes.write16le(endHandle);
-    bytes.write16le(uuid);
+    bytes.write16le(uuid.value);
 }
 
 void AttPdu::checkType(ByteBuffer &bytes, AttPduType type) {
@@ -99,10 +99,10 @@ void AttPdu::checkType(ByteBuffer &bytes, AttPduType type) {
     }
 }
 
-vector<AttributeData> AttPdu::parseReadByGroupType(ByteBuffer &bytes) {
+vector<AttributeData> AttPdu::parse(ByteBuffer &bytes, AttPduType type) {
     DF << "bytes: " << bytes.toString();
 
-    checkType(bytes, READ_BY_GROUP_TYPE_RES);
+    checkType(bytes, type);
 
     if (bytes.getSize() < 4) {
         throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize()));
@@ -125,6 +125,14 @@ vector<AttributeData> AttPdu::parseReadByGroupType(ByteBuffer &bytes) {
     return values;
 }
 
+vector<AttributeData> AttPdu::parseReadByGroupType(ByteBuffer &bytes) {
+    return parse(bytes, READ_BY_GROUP_TYPE_RES);
+}
+
+vector<AttributeData> AttPdu::parseReadByType(ByteBuffer &bytes) {
+    return parse(bytes, READ_BY_TYPE_RES);
+}
+
 // -----------------------------------------------------------------------
 // AttributeData
 // -----------------------------------------------------------------------
@@ -137,7 +145,7 @@ AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) {
 }
 
 AttributeData::AttributeData(uint16_t handle, uint16_t groupEndHandle, ByteBuffer value) :
-        handle(handle), groupEndHandle(groupEndHandle), value(value) {
+        handle(handle), endGroupHandle(groupEndHandle), value(value) {
 }
 
 AttributeData::~AttributeData() {
diff --git a/Bluetooth.h b/Bluetooth.h
index 157ae0b..80e0193 100644
--- a/Bluetooth.h
+++ b/Bluetooth.h
@@ -9,8 +9,20 @@
 
 namespace trygvis {
 namespace bluetooth {
+
 using namespace std;
-//using namespace boost;
+
+struct SpecUuid {
+public:
+    SpecUuid(uint16_t value) : value(value) {}
+    uint16_t value;
+};
+
+namespace uuids {
+static const SpecUuid PRIMARY_SERVICE = SpecUuid(0x2800);
+static const SpecUuid SECONDARY_SERVICE = SpecUuid(0x2801);
+static const SpecUuid CHARACTERISTIC = SpecUuid(0x2803);
+}
 
 class BluetoothAdapter;
 
@@ -78,6 +90,10 @@ public:
 
     virtual boost::uuids::uuid getUuid() const = 0;
 
+    virtual uint16_t getHandle() const = 0;
+
+    virtual uint16_t getEndGroupHandle() const = 0;
+
     virtual vector<BluetoothGattCharacteristic *>::const_iterator getCharacteristics() const = 0;
 };
 
@@ -136,12 +152,15 @@ public:
 
     static vector<AttributeData> parseReadByGroupType(ByteBuffer &bytes);
 
-    static void makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid);
+    static vector<AttributeData> parseReadByType(ByteBuffer &bytes);
+
+    static void makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, SpecUuid uuid);
 
-    static void makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid);
+    static void makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, SpecUuid uuid);
 
 private:
     static void checkType(ByteBuffer &bytes, AttPduType type);
+    static vector<AttributeData> parse(ByteBuffer &bytes, AttPduType type);
 
     ByteBuffer &bytes;
 };
@@ -153,7 +172,7 @@ public:
     static AttributeData fromByteBuffer(ByteBuffer &value);
 
     const uint16_t handle;
-    const uint16_t groupEndHandle;
+    const uint16_t endGroupHandle;
     const ByteBuffer value;
 
 private:
diff --git a/BluetoothImpl.h b/BluetoothImpl.h
index 3d3ce29..0263da5 100644
--- a/BluetoothImpl.h
+++ b/BluetoothImpl.h
@@ -41,8 +41,8 @@ protected:
 
 class DefaultBluetoothGattService : public BluetoothGattService {
 public:
-    DefaultBluetoothGattService(BluetoothDevice& device, const uuid_t uuid) :
-            device(device), uuid(uuid) {
+    DefaultBluetoothGattService(BluetoothDevice& device, const uuid_t uuid, const uint16_t handle, const uint16_t endGroupHandle) :
+            device(device), uuid(uuid), handle(handle), endGroupHandle(endGroupHandle) {
     }
 
     virtual ~DefaultBluetoothGattService() {
@@ -56,6 +56,14 @@ public:
         return uuid;
     }
 
+    uint16_t getHandle() const {
+            return handle;
+    }
+
+     uint16_t getEndGroupHandle() const {
+         return endGroupHandle;
+     }
+
     vector<BluetoothGattCharacteristic* >::const_iterator getCharacteristics() const {
         return characteristics.begin();
     }
@@ -63,6 +71,8 @@ public:
 protected:
     BluetoothDevice &device;
     const uuid_t uuid;
+    const uint16_t handle;
+    const uint16_t endGroupHandle;
     const vector<BluetoothGattCharacteristic*> characteristics;
 };
 
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
 // -----------------------------------------------------------------------
diff --git a/log.h b/log.h
index dcfe83e..1d62121 100644
--- a/log.h
+++ b/log.h
@@ -11,7 +11,4 @@
 #define IF BOOST_LOG_TRIVIAL(info) << __FUNCTION__ << ": "
 #define WF BOOST_LOG_TRIVIAL(warning) << __FUNCTION__ << ": "
 
-#define UUID_PRIMARY_SERVICE 0x2800
-#define UUID_SECONDARY_SERVICE 0x2801
-
 #endif