1 files changed, 41 insertions, 3 deletions
diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp
index d62bf23..435dd63 100644
--- a/ble/LinuxBluetooth.cpp
+++ b/ble/LinuxBluetooth.cpp
@@ -1,6 +1,5 @@
 #include "BluetoothImpl.h"
 
-#include <string.h>
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/hci.h>
 #include <bluetooth/hci_lib.h>
@@ -8,7 +7,6 @@
 #include <map>
 #include <sstream>
 #include <iomanip>
-#include <numeric>
 
 // Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c
 #define ATT_CID 4
@@ -77,6 +75,10 @@ public:
 
     void discoverServices() override;
 
+    void writeValue(const BluetoothGattCharacteristic &c, const ByteBuffer &bytes) override;
+
+    ByteBuffer readValue(const BluetoothGattCharacteristic &c) override;
+
 private:
     vector<AttributeData> discoverServices(uint16_t startHandle);
 
@@ -224,6 +226,42 @@ uuid_t readUuid(BluetoothDevice *device, const ByteBuffer &bytes) {
     return u;
 }
 
+void LinuxBluetoothGatt::writeValue(const BluetoothGattCharacteristic &c, const ByteBuffer &bytes) {
+    DF;
+
+    D << "Writing to characteristic " << c.getUuid() << ": " << bytes.toString();
+
+    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
+    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);
+
+    AttPdu::makeWrite(out, c.getValueHandle(), bytes);
+
+    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);
+
+    AttPdu::parseWrite(in);
+}
+
+ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristic &c) {
+    DF;
+
+    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
+    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);
+
+    AttPdu::makeRead(out, c.getValueHandle());
+
+    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);
+
+    AttPdu::parseRead(in);
+
+//    D << "READ response has " + to_string(in.getBytesLeft()) + " bytes";
+
+    auto response = in.view();
+
+    D << "Value of characteristic " << c.getUuid() << "=" << response.toString();
+
+    return response;
+}
+
 void LinuxBluetoothGatt::discoverServices() {
     uint16_t startHandle = 0x0001;
 
@@ -320,7 +358,7 @@ ByteBuffer LinuxBluetoothGatt::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t>
         throw BluetoothException(&device, "read(): " + errnoAsString());
     }
 
-    auto in = ByteBuffer(buffer, (size_t) r);
+    auto in = ByteBuffer(buffer, (size_t) r, (size_t) r);
 
     D << "read: " << r << " bytes: " << in.toString();