o Working enabling of notifications.

5 files changed, 270 insertions, 101 deletions

diff --git a/ble/Bluetooth.cpp b/ble/Bluetooth.cpp
index 2528416..16f4224 100644
--- a/ble/Bluetooth.cpp
+++ b/ble/Bluetooth.cpp
@@ -8,6 +8,10 @@ namespace trygvis {
 namespace bluetooth {
 using namespace std;
 
+const uint8_t BluetoothGattDescriptor::DISABLE_NOTIFICATION_VALUE[] = {0x00, 0x00};
+const uint8_t BluetoothGattDescriptor::ENABLE_INDICATION_VALUE[] = {0x02, 0x00};
+const uint8_t BluetoothGattDescriptor::ENABLE_NOTIFICATION_VALUE[] = {0x01, 0x00};
+
 // -----------------------------------------------------------------------
 // Mac
 // -----------------------------------------------------------------------
diff --git a/ble/BluetoothImpl.h b/ble/BluetoothImpl.h
index 975a011..5b53cd4 100644
--- a/ble/BluetoothImpl.h
+++ b/ble/BluetoothImpl.h
@@ -40,6 +40,46 @@ protected:
 
 // Shared classes
 
+class DefaultBluetoothGattDescriptor : public BluetoothGattDescriptor {
+public:
+    explicit DefaultBluetoothGattDescriptor(const BluetoothGattCharacteristicPtr& characteristic, uint16_t handle,
+            const Uuid &uuid) : characteristic(characteristic), handle(handle), uuid(uuid), data() {}
+
+    ~DefaultBluetoothGattDescriptor() override {
+        delete data.underlying();
+    };
+
+    BluetoothGattCharacteristicPtr getCharacteristic() const {
+        return characteristic;
+    };
+
+    uint16_t getHandle() const override {
+        return handle;
+    }
+
+    const Uuid getUuid() const override {
+        return uuid;
+    }
+
+    void setValue(const ByteBuffer &buffer) override {
+        delete[] data.underlying();
+
+        auto sz = buffer.getSize();
+        data = ByteBuffer(new uint8_t[sz], sz);
+        data.copyFromEntire(buffer);
+    };
+
+    ByteBuffer getValue() const override {
+        return data;
+    };
+
+private:
+    BluetoothGattCharacteristicPtr characteristic;
+    uint16_t handle;
+    Uuid uuid;
+    ByteBuffer data;
+};
+
 class DefaultBluetoothGattCharacteristic : protected LogSetup, public BluetoothGattCharacteristic {
 public:
     DefaultBluetoothGattCharacteristic(const BluetoothGattServicePtr &service, uint16_t handle, Uuid uuid,
@@ -62,8 +102,8 @@ public:
         return uuid;
     }
 
-    uint8_t getProperties() const override {
-        return properties;
+    CharacteristicProperties getProperties() const override {
+        return {properties};
     }
 
     uint16_t getValueHandle() const override {
@@ -71,15 +111,26 @@ public:
     }
 
     shared_ptr<BluetoothGattDescriptor> getDescriptor(Uuid uuid) const override {
+        for (auto &d: descriptors) {
+            if (d->getUuid() == uuid) {
+                return d;
+            }
+        }
+
         return {};
     }
 
+    void addDescriptor(shared_ptr<BluetoothGattDescriptor> &&d) {
+        descriptors.push_back(std::move(d));
+    }
+
 protected:
     BluetoothGattServicePtr service;
     uint16_t handle;
     Uuid uuid;
     uint8_t properties;
     uint16_t valueHandle;
+    vector<BluetoothGattDescriptorPtr> descriptors;
 };
 
 template<typename DeviceType>
diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp
index 40ba06c..8aea2c8 100644
--- a/ble/ByteBuffer.cpp
+++ b/ble/ByteBuffer.cpp
@@ -9,29 +9,25 @@
 
 using namespace std;
 
-ByteBuffer::ByteBuffer(uint8_t* bytes, size_t size) :
-    zero(bytes), end_(&bytes[size]), cursor(bytes) {
-}
-
 ByteBuffer &ByteBuffer::write8(uint8_t value) {
-    assertCanAccessRelative(1);
-    (*cursor++) = value;
+    assertCanAccessPtr(cursor + 1);
+    *cursor++ = value;
     return *this;
 }
 
 ByteBuffer &ByteBuffer::write16le(uint16_t value) {
-    assertCanAccessRelative(2);
-    (*cursor++) = (uint8_t) (value & 0xff);
-    (*cursor++) = (uint8_t) ((value >> 8) & 0xff);
+    assertCanAccessPtr(cursor + 2);
+    *cursor++ = (uint8_t) (value & 0xff);
+    *cursor++ = (uint8_t) ((value >> 8) & 0xff);
     return *this;
 }
 
 ByteBuffer &ByteBuffer::write32le(uint32_t value) {
-    assertCanAccessRelative(4);
-    (*cursor++) = (uint8_t) (value & 0xff);
-    (*cursor++) = (uint8_t) ((value >> 8) & 0xff);
-    (*cursor++) = (uint8_t) ((value >> 16) & 0xff);
-    (*cursor++) = (uint8_t) ((value >> 24) & 0xff);
+    assertCanAccessPtr(cursor + 4);
+    *cursor++ = (uint8_t) (value & 0xff);
+    *cursor++ = (uint8_t) ((value >> 8) & 0xff);
+    *cursor++ = (uint8_t) ((value >> 16) & 0xff);
+    *cursor++ = (uint8_t) ((value >> 24) & 0xff);
     return *this;
 }
 
@@ -40,7 +36,7 @@ ByteBuffer &ByteBuffer::write(const ByteBuffer &value) {
 }
 
 ByteBuffer &ByteBuffer::write(const uint8_t *bytes, size_t len) {
-    assertCanAccessRelative(len);
+    assertCanAccessPtr(cursor + len);
 
     std::memcpy(cursor, bytes, len);
 
@@ -100,7 +96,7 @@ ByteBuffer &ByteBuffer::writeFLOAT(double d) {
             mdiff = abs(smantissa - rmantissa);
         }
 
-        uint32_t int_mantissa = (uint32_t) round(sgn * mantissa);
+        auto int_mantissa = uint32_t(round(sgn * mantissa));
         result = (exponent << 24) | (int_mantissa & 0xFFFFFF);
     }
 
@@ -108,22 +104,22 @@ ByteBuffer &ByteBuffer::writeFLOAT(double d) {
 }
 
 uint8_t ByteBuffer::get8(size_t index) const {
-    assertCanAccessRelative(index);
+    assertCanAccessPosition(index);
     return zero[index];
 }
 
 uint8_t ByteBuffer::peek8(size_t relative_index) const {
-    assertCanAccessRelative(relative_index);
+    assertCanAccessPtr(cursor + relative_index);
     return cursor[relative_index];
 }
 
 uint8_t ByteBuffer::read8() {
-    assertCanAccessRelative(0);
+    assertCanAccessPtr(cursor);
     return *cursor++;
 }
 
 uint16_t ByteBuffer::read16le() {
-    assertCanAccessRelative(1);
+    assertCanAccessPtr(cursor + 1);
     uint16_t value;
     value = *cursor++;
     value |= ((uint16_t) *cursor++) << 8;
@@ -131,7 +127,7 @@ uint16_t ByteBuffer::read16le() {
 }
 
 uint32_t ByteBuffer::read32le() {
-    assertCanAccessRelative(3);
+    assertCanAccessPtr(cursor + 3);
     uint32_t value;
     value = *cursor++;
     value |= ((uint32_t) *cursor++) << 8;
@@ -166,39 +162,46 @@ double ByteBuffer::readFLOAT() {
     return output;
 }
 
-void ByteBuffer::copy(uint8_t *bytes, size_t length) const {
-    assertCanAccessRelative(length - 1);
+void ByteBuffer::copyFromEntire(const ByteBuffer &other) const {
+    size_t length = other.getPosition();
 
-    std::memcpy(bytes, cursor, length);
+    assertCanAccessPosition(length - 1);
+    std::memcpy(cursor, other.zero, length);
 }
 
-void ByteBuffer::copy(ByteBuffer& other) const {
-    other.assertCanAccessRelative(getBytesLeft());
+void ByteBuffer::copyTo(uint8_t *bytes, size_t length) const {
+    assertCanAccessPtr(&cursor[length - 1]);
 
-    std::memcpy(other.cursor, cursor, getBytesLeft());
-}
-
-void ByteBuffer::reset() {
-    cursor = const_cast<uint8_t *>(zero);
+    std::memcpy(bytes, cursor, length);
 }
 
-ByteBuffer ByteBuffer::view() const {
+ByteBuffer ByteBuffer::viewCursorToEnd() const {
 //    LOG_DEBUG("cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end_ - cursor << ", cursor=" << (uint64_t) cursor << ", zero=" << (uint64_t) zero);
     return {cursor, getBytesLeft()};
 }
 
-ByteBuffer ByteBuffer::view(size_t length) const {
-    assertCanAccessRelative(length - 1);
+ByteBuffer ByteBuffer::viewBeginningToCursor() const {
+    return {zero, getPosition()};
+}
+
+ByteBuffer ByteBuffer::viewForward(size_t length) const {
+    assertCanAccessPtr(&cursor[length - 1]);
     return {cursor, length};
 }
 
-void ByteBuffer::assertCanAccessRelative(size_t diff) const {
-    assertCanAccessIndex(cursor + diff);
+ByteBuffer ByteBuffer::viewForwardAndSkip(size_t length){
+    auto v = viewForward(length);
+    skip(length);
+    return v;
+}
+
+void ByteBuffer::assertCanAccessPosition(size_t position) const {
+    assertCanAccessPtr(&zero[position]);
 }
 
-void ByteBuffer::assertCanAccessIndex(uint8_t *p) const {
-    if (p >= end_ || p < zero) {
-        throw ByteBufferException("Out of bounds! size=" + to_string(getSize()) + ", index=" + to_string(p - zero));
+void ByteBuffer::assertCanAccessPtr(uint8_t *ptr) const {
+    if (ptr >= end_ || ptr < zero) {
+        throw ByteBufferException("Out of bounds! size=" + to_string(getSize()) + ", index=" + to_string(ptr - zero));
     }
 }
 
@@ -211,3 +214,7 @@ std::string ByteBuffer::toString() const {
 
     return string(s.str());
 }
+
+const ByteBuffer ByteBuffer::wrap(const uint8_t *bytes, size_t capacity, size_t cursor) noexcept {
+    return {const_cast<uint8_t *>(bytes), capacity, cursor};
+}
diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp
index 444e3a2..cc6516d 100644
--- a/ble/LinuxBluetooth.cpp
+++ b/ble/LinuxBluetooth.cpp
@@ -38,8 +38,9 @@ class LinuxBluetoothManager;
 
 class LinuxBluetoothGattService : public DefaultBluetoothGattService<LinuxBluetoothDevice> {
 public:
-    explicit LinuxBluetoothGattService(LinuxBluetoothDevice &device, const Uuid& uuid, const uint16_t handle,
-        const uint16_t endGroupHandle) : DefaultBluetoothGattService(device, uuid, handle, endGroupHandle) {
+    explicit LinuxBluetoothGattService(LinuxBluetoothDevice &device, const Uuid &uuid, const uint16_t handle,
+                                       const uint16_t endGroupHandle) :
+                                       DefaultBluetoothGattService(device, uuid, handle, endGroupHandle) {
     };
 
     LinuxBluetoothGattService(const LinuxBluetoothGattService &o) = delete;
@@ -53,7 +54,7 @@ public:
 
     LinuxBluetoothAdapter(const LinuxBluetoothAdapter &) = delete;
 
-    LinuxBluetoothAdapter& operator=(const LinuxBluetoothAdapter&) = delete;
+    LinuxBluetoothAdapter &operator=(const LinuxBluetoothAdapter &) = delete;
 
     void runScan(std::function<void(const shared_ptr<BluetoothDevice> &device)>) override;
 
@@ -81,7 +82,7 @@ public:
 
     LinuxBluetoothDevice(const LinuxBluetoothDevice &) = delete;
 
-    LinuxBluetoothDevice& operator=(const LinuxBluetoothDevice&) = delete;
+    LinuxBluetoothDevice &operator=(const LinuxBluetoothDevice &) = delete;
 
     shared_ptr<BluetoothGatt> connectGatt() override;
 
@@ -95,9 +96,9 @@ public:
 
     ~LinuxBluetoothGatt() override;
 
-    LinuxBluetoothGatt(const LinuxBluetoothGatt&) = delete;
+    LinuxBluetoothGatt(const LinuxBluetoothGatt &) = delete;
 
-    LinuxBluetoothGatt& operator=(const LinuxBluetoothGatt&) = delete;
+    LinuxBluetoothGatt &operator=(const LinuxBluetoothGatt &) = delete;
 
     bool isConnected() const override;
 
@@ -105,7 +106,9 @@ public:
 
     void writeValue(const BluetoothGattCharacteristicPtr &c, const ByteBuffer &bytes) override;
 
-    ByteBuffer readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer& response) override;
+    void write(const BluetoothGattDescriptorPtr &c) override;
+
+    ByteBuffer readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer &response) override;
 
     void setCharacteristicNotification(const BluetoothGattDescriptorPtr &c, bool enable) override;
 
@@ -118,9 +121,13 @@ private:
 
     vector<AttributeData> discoverCharacteristics(uint16_t startHandle, uint16_t endHandle);
 
+    void discoverDescriptors(shared_ptr<DefaultBluetoothGattCharacteristic> &c);
+
+    void writeHandle(uint16_t handle, const ByteBuffer &bytes);
+
     void writeL2cap(ByteBuffer &buffer);
 
-    void writeAndRead(const ByteBuffer &buffer, ByteBuffer& response);
+    void writeAndRead(const ByteBuffer &buffer, ByteBuffer &response);
 
     AttVariant processAvailableMessages(ByteBuffer &buffer);
 
@@ -130,7 +137,7 @@ private:
     template<typename T, typename clock = std::chrono::system_clock>
     T exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point<clock> time, bool fail_on_error);
 
-    void logError(const ErrorRes&);
+    void logError(const ErrorRes &);
 
     uint16_t mtu = 256;
 
@@ -160,7 +167,7 @@ Mac parseMac(bdaddr_t &a) {
 // -----------------------------------------------------------------------
 
 LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &mac) :
-        DefaultBluetoothDevice(adapter, mac), gatt(nullptr) {
+    DefaultBluetoothDevice(adapter, mac), gatt(nullptr) {
 }
 
 LinuxBluetoothDevice::~LinuxBluetoothDevice() {
@@ -183,7 +190,7 @@ shared_ptr<BluetoothGatt> LinuxBluetoothDevice::connectGatt() {
 // -----------------------------------------------------------------------
 
 LinuxBluetoothGatt::LinuxBluetoothGatt(LinuxBluetoothDevice &device) :
-        DefaultBluetoothGatt(device), l2cap() {
+    DefaultBluetoothGatt(device), l2cap() {
     connect();
 }
 
@@ -229,12 +236,12 @@ void LinuxBluetoothGatt::connect() {
     addr.l2_cid = htobs(ATT_CID);
     addr.l2_bdaddr_type = BDADDR_LE_RANDOM;
     device.getMac().copy(
-            addr.l2_bdaddr.b[5],
-            addr.l2_bdaddr.b[4],
-            addr.l2_bdaddr.b[3],
-            addr.l2_bdaddr.b[2],
-            addr.l2_bdaddr.b[1],
-            addr.l2_bdaddr.b[0]);
+        addr.l2_bdaddr.b[5],
+        addr.l2_bdaddr.b[4],
+        addr.l2_bdaddr.b[3],
+        addr.l2_bdaddr.b[2],
+        addr.l2_bdaddr.b[1],
+        addr.l2_bdaddr.b[0]);
 
     if (::connect(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
         close(l2cap);
@@ -261,8 +268,10 @@ Uuid readUuid(BluetoothDevice *device, ByteBuffer &bytes) {
         return Uuid(bs);
     } else if (bytesLeft == 16) {
         uint8_t bs[16] = {
-                bytes.get8(15), bytes.get8(14), bytes.get8(13), bytes.get8(12), bytes.get8(11), bytes.get8(10), bytes.get8(9), bytes.get8(8),
-                bytes.get8(7), bytes.get8(6), bytes.get8(5), bytes.get8(4), bytes.get8(3), bytes.get8(2), bytes.get8(1), bytes.get8(0),
+            bytes.get8(15), bytes.get8(14), bytes.get8(13), bytes.get8(12),
+            bytes.get8(11), bytes.get8(10), bytes.get8(9), bytes.get8(8),
+            bytes.get8(7), bytes.get8(6), bytes.get8(5), bytes.get8(4),
+            bytes.get8(3), bytes.get8(2), bytes.get8(1), bytes.get8(0),
         };
         bytes.skip(16);
 
@@ -275,17 +284,39 @@ Uuid readUuid(BluetoothDevice *device, ByteBuffer &bytes) {
 void LinuxBluetoothGatt::writeValue(const BluetoothGattCharacteristicPtr &c, const ByteBuffer &bytes) {
     LOG_DEBUG("Writing to characteristic " << c->getUuid() << ": " << bytes.toString());
 
+    writeHandle(c->getValueHandle(), bytes);
+}
+
+void LinuxBluetoothGatt::write(const BluetoothGattDescriptorPtr &d) {
+    auto bytes = d->getValue();
+
+    LOG_DEBUG("Writing to descriptor " << d->getUuid() << ": " << bytes.toString());
+
+    writeHandle(d->getHandle(), bytes);
+}
+
+void LinuxBluetoothGatt::writeHandle(uint16_t handle, const ByteBuffer &bytes) {
     uint8_t b[mtu];
     ByteBuffer buffer{b, mtu};
 
-    AttPdu::makeWrite(buffer, c->getValueHandle(), bytes);
+    AttPdu::makeWrite(buffer, handle, bytes);
 
     writeAndRead(buffer, buffer);
 
+    auto cursor = buffer.getPosition();
+    buffer.setPosition(0);
+
     AttPdu::parseWrite(buffer);
+
+    auto extra_bytes = cursor - buffer.getPosition();
+    LOG_DEBUG("WRITE response has " + to_string(extra_bytes) + " extra bytes");
+
+    if (extra_bytes) {
+        throw BluetoothException(&device, "Got extra bytes from ::read(): " + to_string(extra_bytes));
+    }
 }
 
-ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer& response) {
+ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer &response) {
     uint8_t b[mtu];
     ByteBuffer buffer{b, mtu};
 
@@ -293,12 +324,12 @@ ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c
 
     writeAndRead(buffer, response);
 
-    auto cursor = response.getCursor();
-    response.setCursor(0);
+    auto cursor = response.getPosition();
+    response.setPosition(0);
 
     AttPdu::parseRead(response);
 
-    auto view = response.view(cursor - response.getCursor());
+    auto view = response.viewForward(cursor - response.getPosition());
 
     LOG_DEBUG("READ response has " + to_string(view.getSize()) + " bytes");
     LOG_DEBUG("Value of characteristic " << c->getUuid() << "=" << view.toString());
@@ -306,7 +337,11 @@ ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c
     return view;
 }
 
-void LinuxBluetoothGatt::setCharacteristicNotification(const BluetoothGattDescriptorPtr &c, bool enable) {
+void LinuxBluetoothGatt::setCharacteristicNotification(const BluetoothGattDescriptorPtr &d, bool enable) {
+    uint8_t b[mtu];
+    ByteBuffer buffer{b, mtu};
+
+//    AttPdu::makeWrite(buffer, d->getValueHandle());
 }
 
 void LinuxBluetoothGatt::discoverServices() {
@@ -330,16 +365,14 @@ void LinuxBluetoothGatt::discoverServices() {
             endGroupHandle = data.value.read16le();
 
             LOG_DEBUG("service handle: 0x" << hex << setw(4) << setfill('0') << data.handle <<
-                        ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle <<
-                        ", value: " << data.value.toString());
+                                           ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle <<
+                                           ", value: " << data.value.toString());
 
             auto u = readUuid(&device, data.value);
 
             addService(make_shared<LinuxBluetoothGattService>(device, u, data.handle, endGroupHandle));
         }
 
-//        auto last = values.back();
-
         startHandle = endGroupHandle;
     } while (startHandle != 0xffff);
 
@@ -349,7 +382,7 @@ void LinuxBluetoothGatt::discoverServices() {
 //    }
 
     auto it = services.begin(),
-            end = services.end();
+        end = services.end();
 
     if (it == end) {
         return;
@@ -381,49 +414,56 @@ void LinuxBluetoothGatt::discoverServices() {
             auto uuid = readUuid(&device, c.value);
 
             LOG_DEBUG("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);
+                                                 ", properties: " << setw(2) << setfill('0') << hex << (int) properties
+                                                 <<
+                                                 ", valueHandle: 0x" << setw(4) << setfill('0') << hex
+                                                 << (int) valueHandle <<
+                                                 ", uuid: " << uuid);
+
+            auto characteristic = make_shared<DefaultBluetoothGattCharacteristic>(
+                s, c.handle, uuid, properties, valueHandle);
 
-            s->addCharacteristic(std::move(make_shared<DefaultBluetoothGattCharacteristic>(s, c.handle, uuid, properties, valueHandle)));
+            discoverDescriptors(characteristic);
+
+            s->addCharacteristic(std::move(characteristic));
         }
 
         startHandle = lastHandle + (uint8_t) 2;
     } while (startHandle != 0xffff);
 }
 
-void LinuxBluetoothGatt::writeAndRead(const ByteBuffer &buffer, ByteBuffer& response) {
+void LinuxBluetoothGatt::writeAndRead(const ByteBuffer &buffer, ByteBuffer &response) {
 //    LOG_DEBUG("pdu size=" << out.getCursor());
-    auto to_be_written = buffer.getCursor();
+    auto to_be_written = buffer.getPosition();
 
-    ssize_t written = write(l2cap, buffer.cbegin(), to_be_written);
+    ssize_t written = ::write(l2cap, buffer.cbegin(), to_be_written);
 
     if (to_be_written != written) {
         throw BluetoothException(&device, "Expected to write " + to_string(to_be_written) + " but wrote only " +
-                to_string(written));
+                                          to_string(written));
     }
 
 //    LOG_DEBUG("written=" << written);
 
-    ssize_t r = read(l2cap, response.begin(), response.getSize());
+    ssize_t r = read(l2cap, response.underlying(), response.getSize());
 
     if (r == -1) {
         throw BluetoothException(&device, "read(): " + errnoAsString());
     }
 
-    LOG_DEBUG("read: " << r << " bytes: " << response.toString());
-    response.setCursor(static_cast<size_t>(r));
+    response.setPosition(static_cast<size_t>(r));
+    LOG_DEBUG("read: " << r << " bytes: " << response.viewBeginningToCursor().toString());
 }
 
 void LinuxBluetoothGatt::writeL2cap(ByteBuffer &buffer) {
-    auto to_be_written = buffer.getCursor();
-    ssize_t written = write(l2cap, buffer.cbegin(), to_be_written);
+    auto to_be_written = buffer.getPosition();
+    ssize_t written = ::write(l2cap, buffer.cbegin(), to_be_written);
 
     if (to_be_written != written) {
         throw BluetoothException(&device, "Expected to write " + to_string(to_be_written) + " but wrote only " +
                                           to_string(written));
     }
-    buffer.reset();
+    buffer.setPosition(0);
 }
 
 AttVariant LinuxBluetoothGatt::processAvailableMessages(ByteBuffer &buffer) {
@@ -433,13 +473,13 @@ AttVariant LinuxBluetoothGatt::processAvailableMessages(ByteBuffer &buffer) {
 //    LOG_DEBUG("ret=" << to_string(ret) << "/" << errnoAsString(ret) <<
 //                     ", bytes available: " << to_string(bytes_available));
 
-    while(!ret && bytes_available) {
-        ssize_t r = read(l2cap, buffer.begin(), buffer.getBytesLeft());
+    while (!ret && bytes_available) {
+        ssize_t r = read(l2cap, buffer.underlying(), buffer.getBytesLeft());
         if (r == -1) {
             throw BluetoothException(&device, "read(): " + errnoAsString());
         }
 
-        auto packet_buffer = buffer.view(size_t(r));
+        auto packet_buffer = buffer.viewForward(size_t(r));
         auto type = static_cast<AttPduType>(packet_buffer.get8(0));
         LOG_DEBUG("Got ATT message " << to_string(type) << ", size: " << to_string(r));
 
@@ -474,7 +514,7 @@ static BluetoothException makeUnexpectedTypeException(const AttVariant &variant)
 template<typename T>
 static
 void assertIsVariant(const AttVariant &variant) {
-    if(holds_alternative<T>(variant)) {
+    if (holds_alternative<T>(variant)) {
         return;
     }
 
@@ -561,13 +601,13 @@ vector<AttributeData> LinuxBluetoothGatt::discoverCharacteristics(uint16_t start
 
     auto res = exchangeAndWaitFor(buffer, std::chrono::system_clock::now() + 10s);
 
-    if(holds_alternative<ReadByTypeRes>(res)) {
+    if (holds_alternative<ReadByTypeRes>(res)) {
         auto values = get<ReadByTypeRes>(res).attributes;
         LOG_DEBUG("READ_BY_TYPE response has " + to_string(values.size()) + " values");
         return values;
     }
 
-    if(holds_alternative<ErrorRes>(res)) {
+    if (holds_alternative<ErrorRes>(res)) {
         ErrorRes err = get<ErrorRes>(res);
 
         if (err.errorCode == ErrorCode::ATTRIBUTE_NOT_FOUND) {
@@ -580,12 +620,39 @@ vector<AttributeData> LinuxBluetoothGatt::discoverCharacteristics(uint16_t start
     throw makeUnexpectedTypeException(res);
 }
 
+void LinuxBluetoothGatt::discoverDescriptors(shared_ptr<DefaultBluetoothGattCharacteristic> &c) {
+
+    auto p = c->getProperties();
+
+    if (!p.indicate() && !p.notify()) {
+        return;
+    }
+
+    uint8_t bytes[mtu];
+    ByteBuffer buffer{bytes, mtu};
+
+    uint16_t handle = c->getValueHandle() + uint16_t(1);
+    AttPdu::makeFindInformationReq(buffer, handle, handle);
+
+    auto res = exchangeAndWaitFor(buffer, std::chrono::system_clock::now() + 10s);
+
+    if (holds_alternative<FindInformationRes>(res)) {
+        auto values = get<FindInformationRes>(res).information;
+        LOG_DEBUG("FIND_INFORMATION response has " + to_string(values.size()) + " values");
+
+        for (auto &value : values) {
+            c->addDescriptor(std::move(make_shared<DefaultBluetoothGattDescriptor>(c, value.handle, value.uuid)));
+            LOG_DEBUG("UUID=" + value.uuid.str());
+        }
+    }
+}
+
 // -----------------------------------------------------------------------
 // Adapter
 // -----------------------------------------------------------------------
 
 LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId, Mac &mac) : DefaultBluetoothAdapter(mac),
-        scanning(false), hciFilter() {
+                                                                          scanning(false), hciFilter() {
     LOG_DEBUG("hciDeviceId=" << hciDeviceId);
 
     this->hciDeviceId = hciDeviceId;
diff --git a/ble/att.cpp b/ble/att.cpp
index 585e914..301918d 100644
--- a/ble/att.cpp
+++ b/ble/att.cpp
@@ -74,7 +74,7 @@ std::string to_string(AttPduType t) {
 }
 
 o<AttPduType> attPduType(const AttVariant &v) {
-    return visit([](auto &&arg) -> o<AttPduType>{
+    return visit([](auto &&arg) -> o<AttPduType> {
         using T = std::decay_t<decltype(arg)>;
 
         if constexpr (std::is_same_v<T, std::monostate>) {
@@ -96,12 +96,17 @@ AttPduType AttPdu::getType() {
     return (AttPduType) bytes.peek8(0);
 }
 
-void AttPdu::makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu)
-{
+void AttPdu::makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu) {
     bytes.write8(AttPduType::EXCHANGE_MTU_RES).
         write16le(serverRxMtu);
 }
 
+void AttPdu::makeFindInformationReq(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle) {
+    bytes.write8(AttPduType::FIND_INFORMATION_REQ);
+    bytes.write16le(startHandle);
+    bytes.write16le(endHandle);
+}
+
 void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) {
     bytes.write8(AttPduType::READ_BY_GROUP_TYPE_REQ);
     bytes.write16le(startHandle);
@@ -129,7 +134,8 @@ void AttPdu::makeWrite(ByteBuffer &req, uint16_t handle, const ByteBuffer &bytes
 
 vector<AttributeData> AttPdu::parseAttributeData(ByteBuffer &bytes) {
     if (bytes.getSize() < 4) {
-        throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize()));
+        throw BluetoothException(
+            "Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize()));
     }
 
     uint8_t length = bytes.read8();
@@ -138,8 +144,8 @@ vector<AttributeData> AttPdu::parseAttributeData(ByteBuffer &bytes) {
 
     vector<AttributeData> values;
     for (int i = 0; i < count; i++) {
-        auto data = bytes.viewAndSkip(length);
-        auto&& x = AttributeData::fromByteBuffer(data);
+        auto data = bytes.viewForwardAndSkip(length);
+        auto &&x = AttributeData::fromByteBuffer(data);
         values.emplace_back(x);
     }
 
@@ -154,6 +160,8 @@ AttVariant AttPdu::parse(ByteBuffer &bytes) {
             return parseExchangeMtuReq(bytes);
         case AttPduType::EXCHANGE_MTU_RES:
             return parseExchangeMtuRes(bytes);
+        case AttPduType::FIND_INFORMATION_RES:
+            return parseFindInformationRes(bytes);
         case AttPduType::READ_BY_GROUP_TYPE_RES:
             return ReadByGroupTypeRes{parseAttributeData(bytes)};
         case AttPduType::READ_BY_TYPE_RES:
@@ -171,6 +179,23 @@ ExchangeMtuRes AttPdu::parseExchangeMtuRes(ByteBuffer &bytes) {
     return {bytes.read16le()};
 }
 
+FindInformationRes AttPdu::parseFindInformationRes(ByteBuffer &bytes) {
+    auto format = bytes.read8();
+
+    std::vector<InformationData> information;
+    if (format == InformationData::FORMAT_SHORT_UUID) {
+        while (bytes.getBytesLeft()) {
+            information.push_back(InformationData::fromByteBufferShortUuid(bytes));
+        }
+    } else if (format == InformationData::FORMAT_LONG_UUID) {
+        while (bytes.getBytesLeft()) {
+            information.push_back(InformationData::fromByteBufferLongUuid(bytes));
+        }
+    }
+
+    return {format, information};
+}
+
 void AttPdu::parseRead(ByteBuffer &bytes) {
     auto t = static_cast<AttPduType>(bytes.read8());
 
@@ -180,10 +205,11 @@ void AttPdu::parseRead(ByteBuffer &bytes) {
 }
 
 void AttPdu::parseWrite(ByteBuffer &bytes) {
-    auto t = static_cast<AttPduType>(bytes.read8());
+    auto b = bytes.read8();
+    auto t = static_cast<AttPduType>(b);
 
     if (t != WRITE_RES) {
-        throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(WRITE_RES));
+        throw BluetoothException("Unexpected type: " + to_string(b) + "/" + to_string(t) + ", expected " + to_string(WRITE_RES));
     }
 }
 
@@ -196,7 +222,7 @@ AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) {
 
     auto raw = make_unique<uint8_t[]>(bytes.getBytesLeft());
 
-    bytes.copy(raw.get(), bytes.getBytesLeft());
+    bytes.copyTo(raw.get(), bytes.getBytesLeft());
 
     ByteBuffer buffer = ByteBuffer{raw.get(), bytes.getBytesLeft()};
     return AttributeData(handle, buffer, std::move(raw));
@@ -208,5 +234,19 @@ AttributeData::AttributeData(uint16_t handle, ByteBuffer value, std::shared_ptr<
 
 AttributeData::~AttributeData() = default;
 
+// -----------------------------------------------------------------------
+// InformationData
+// -----------------------------------------------------------------------
+
+InformationData InformationData::fromByteBufferShortUuid(ByteBuffer &value) {
+    return {value.read16le(), ShortUuid(value).toLong()};
+}
+
+InformationData InformationData::fromByteBufferLongUuid(ByteBuffer &value) {
+    return {value.read16le(), Uuid(value)};
+}
+
+InformationData::InformationData(uint16_t handle, const Uuid &uuid) : handle(handle), uuid(uuid) {}
+
 } // namespace bluetooth
 } // namespace trygvis