3 files changed, 26 insertions, 23 deletions

diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp
index 1a294f1..40ba06c 100644
--- a/ble/ByteBuffer.cpp
+++ b/ble/ByteBuffer.cpp
@@ -7,13 +7,8 @@
 #include <iostream>
 #include <ble/ByteBuffer.h>
 
-
 using namespace std;
 
-ByteBuffer ByteBuffer::alloc(std::size_t size) {
-    return {new uint8_t[size], size};
-}
-
 ByteBuffer::ByteBuffer(uint8_t* bytes, size_t size) :
     zero(bytes), end_(&bytes[size]), cursor(bytes) {
 }
@@ -47,7 +42,7 @@ ByteBuffer &ByteBuffer::write(const ByteBuffer &value) {
 ByteBuffer &ByteBuffer::write(const uint8_t *bytes, size_t len) {
     assertCanAccessRelative(len);
 
-    memcpy(cursor, bytes, len);
+    std::memcpy(cursor, bytes, len);
 
     cursor += len;
 
@@ -174,7 +169,13 @@ double ByteBuffer::readFLOAT() {
 void ByteBuffer::copy(uint8_t *bytes, size_t length) const {
     assertCanAccessRelative(length - 1);
 
-    memcpy(bytes, cursor, length);
+    std::memcpy(bytes, cursor, length);
+}
+
+void ByteBuffer::copy(ByteBuffer& other) const {
+    other.assertCanAccessRelative(getBytesLeft());
+
+    std::memcpy(other.cursor, cursor, getBytesLeft());
 }
 
 void ByteBuffer::reset() {
@@ -182,7 +183,7 @@ void ByteBuffer::reset() {
 }
 
 ByteBuffer ByteBuffer::view() const {
-//    DF << "cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end_ - cursor << ", cursor=" << (uint64_t) cursor << ", zero=" << (uint64_t) zero;
+//    LOG_DEBUG("cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end_ - cursor << ", cursor=" << (uint64_t) cursor << ", zero=" << (uint64_t) zero);
     return {cursor, getBytesLeft()};
 }
 
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) {
diff --git a/ble/misc.cpp b/ble/misc.cpp
index a8df2cb..a4f4b57 100644
--- a/ble/misc.cpp
+++ b/ble/misc.cpp
@@ -50,7 +50,7 @@ o<Uuid> Uuid::fromString(const std::string &str) {
                 }
 
                 auto tmp = str.substr(i, 2);
-                cout << "str=" << tmp << endl;
+                // cout << "str=" << tmp << endl;
                 unsigned long x = std::stoul(tmp, nullptr, 16);
 
                 if (x > std::numeric_limits<uint8_t>::max()) {