1 files changed, 16 insertions, 14 deletions
diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp
index e567d62..d3fe80b 100644
--- a/ble/LinuxBluetooth.cpp
+++ b/ble/LinuxBluetooth.cpp
@@ -103,7 +103,7 @@ public:
 
     void writeValue(const BluetoothGattCharacteristicPtr &c, const ByteBuffer &bytes) override;
 
-    ByteBuffer readValue(const BluetoothGattCharacteristicPtr &c) override;
+    ByteBuffer readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer& response) override;
 
 private:
     void connect();
@@ -116,7 +116,7 @@ private:
 
     void writeL2cap(ByteBuffer &buffer);
 
-    void writeAndRead(ByteBuffer &buffer);
+    void writeAndRead(const ByteBuffer &buffer, ByteBuffer& response);
 
     AttVariant processAvailableMessages(ByteBuffer &buffer);
 
@@ -276,28 +276,30 @@ void LinuxBluetoothGatt::writeValue(const BluetoothGattCharacteristicPtr &c, con
 
     AttPdu::makeWrite(buffer, c->getValueHandle(), bytes);
 
-    writeAndRead(buffer);
+    writeAndRead(buffer, buffer);
 
     AttPdu::parseWrite(buffer);
 }
 
-ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c) {
+ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer& response) {
     uint8_t b[mtu];
     ByteBuffer buffer{b, mtu};
 
     AttPdu::makeRead(buffer, c->getValueHandle());
 
-    writeAndRead(buffer);
+    writeAndRead(buffer, response);
 
-    AttPdu::parseRead(buffer);
+    auto cursor = response.getCursor();
+    response.setCursor(0);
 
-//    D << "READ response has " + to_string(in.getBytesLeft()) + " bytes";
+    AttPdu::parseRead(response);
 
-    auto response = buffer.view();
+    auto view = response.view(cursor - response.getCursor());
 
-    LOG_DEBUG("Value of characteristic " << c->getUuid() << "=" << response.toString());
+    LOG_DEBUG("READ response has " + to_string(view.getSize()) + " bytes");
+    LOG_DEBUG("Value of characteristic " << c->getUuid() << "=" << view.toString());
 
-    return response;
+    return view;
 }
 
 void LinuxBluetoothGatt::discoverServices() {
@@ -383,7 +385,7 @@ void LinuxBluetoothGatt::discoverServices() {
     } while (startHandle != 0xffff);
 }
 
-void LinuxBluetoothGatt::writeAndRead(ByteBuffer &buffer) {
+void LinuxBluetoothGatt::writeAndRead(const ByteBuffer &buffer, ByteBuffer& response) {
 //    LOG_DEBUG("pdu size=" << out.getCursor());
     auto to_be_written = buffer.getCursor();
 
@@ -396,14 +398,14 @@ void LinuxBluetoothGatt::writeAndRead(ByteBuffer &buffer) {
 
 //    LOG_DEBUG("written=" << written);
 
-    buffer.reset();
-    ssize_t r = read(l2cap, buffer.begin(), buffer.getBytesLeft());
+    ssize_t r = read(l2cap, response.begin(), response.getSize());
 
     if (r == -1) {
         throw BluetoothException(&device, "read(): " + errnoAsString());
     }
 
-//    LOG_DEBUG("read: " << r << " bytes: " << buffer.toString());
+    LOG_DEBUG("read: " << r << " bytes: " << response.toString());
+    response.setCursor(static_cast<size_t>(r));
 }
 
 void LinuxBluetoothGatt::writeL2cap(ByteBuffer &buffer) {