o Support for reading and writing characteristics.

6 files changed, 174 insertions, 58 deletions

diff --git a/ble/Bluetooth.cpp b/ble/Bluetooth.cpp
index 31f82e3..fe63b4f 100644
--- a/ble/Bluetooth.cpp
+++ b/ble/Bluetooth.cpp
@@ -3,7 +3,6 @@
 
 #include <sstream>
 #include <iomanip>
-#include <string.h>
 
 namespace trygvis {
 namespace bluetooth {
@@ -49,7 +48,7 @@ void Mac::copy(uint8_t &_0, uint8_t &_1, uint8_t &_2, uint8_t &_3, uint8_t &_4,
     _5 = bytes[5];
 }
 
-Mac Mac::parseMac(string s) throw(BluetoothException) {
+Mac Mac::parseMac(string s) {
     unsigned int bytes[6];
     int count = sscanf(s.c_str(), "%02x:%02x:%02x:%02x:%02x:%02x",
             &bytes[0], &bytes[1], &bytes[2], &bytes[3], &bytes[4], &bytes[5]);
@@ -86,6 +85,17 @@ void AttPdu::makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t en
     bytes.write16le(uuid.value);
 }
 
+void AttPdu::makeRead(ByteBuffer &bytes, uint16_t handle) {
+    bytes.write8(AttPduType::READ_REQ);
+    bytes.write16le(handle);
+}
+
+void AttPdu::makeWrite(ByteBuffer &req, uint16_t handle, const ByteBuffer &bytes) {
+    req.write8(AttPduType::WRITE_REQ);
+    req.write16le(handle);
+    req.write(bytes);
+}
+
 vector<AttributeData> AttPdu::parse(ByteBuffer &bytes, AttPduType type) {
     DF << "bytes: " << bytes.toString();
 
@@ -128,6 +138,22 @@ vector<AttributeData> AttPdu::parseReadByType(ByteBuffer &bytes) {
     return parse(bytes, READ_BY_TYPE_RES);
 }
 
+void AttPdu::parseRead(ByteBuffer &bytes) {
+    AttPduType t = (AttPduType) bytes.read8();
+
+    if (t != READ_RES) {
+        throw BluetoothException("Unexpected type: " + to_string(t));
+    }
+}
+
+void AttPdu::parseWrite(ByteBuffer &bytes) {
+    AttPduType t = (AttPduType) bytes.read8();
+
+    if (t != WRITE_RES) {
+        throw BluetoothException("Unexpected type: " + to_string(t));
+    }
+}
+
 // -----------------------------------------------------------------------
 // AttributeData
 // -----------------------------------------------------------------------
diff --git a/ble/Bluetooth.h b/ble/Bluetooth.h
index d6be783..6ed9c76 100644
--- a/ble/Bluetooth.h
+++ b/ble/Bluetooth.h
@@ -22,9 +22,9 @@ public:
 };
 
 namespace uuids {
-static const SpecUuid PRIMARY_SERVICE = SpecUuid(0x2800);
-static const SpecUuid SECONDARY_SERVICE = SpecUuid(0x2801);
-static const SpecUuid CHARACTERISTIC = SpecUuid(0x2803);
+const SpecUuid PRIMARY_SERVICE = SpecUuid(0x2800);
+const SpecUuid SECONDARY_SERVICE = SpecUuid(0x2801);
+const SpecUuid CHARACTERISTIC = SpecUuid(0x2803);
 }
 
 class BluetoothAdapter;
@@ -72,7 +72,7 @@ public:
 
     void copy(uint8_t &_0, uint8_t &_1, uint8_t &_2, uint8_t &_3, uint8_t &_4, uint8_t &_5) const;
 
-    static Mac parseMac(string s) throw(BluetoothException);
+    static Mac parseMac(string s);
 
     friend bool operator<(const Mac &a, const Mac &b);
 
@@ -113,7 +113,7 @@ public:
 
     virtual void addCharacteristic(BluetoothGattCharacteristic *characteristic) = 0;
 
-    virtual const boost::optional<BluetoothGattCharacteristic *> findCharacteristic(boost::uuids::uuid uuid) const = 0;
+    virtual const boost::optional<BluetoothGattCharacteristic &> findCharacteristic(boost::uuids::uuid uuid) const = 0;
 };
 
 class BluetoothGatt {
@@ -128,11 +128,15 @@ public:
 
     virtual void disconnect() = 0;
 
+    virtual void writeValue(const BluetoothGattCharacteristic &c, const ByteBuffer &bytes) = 0;
+
+    virtual ByteBuffer readValue(const BluetoothGattCharacteristic &c) = 0;
+
     virtual void discoverServices() = 0;
 
     virtual const vector<BluetoothGattService *> getServices() const = 0;
 
-    virtual const boost::optional<BluetoothGattService *> findService(boost::uuids::uuid uuid) const = 0;
+    virtual const boost::optional<BluetoothGattService &> findService(boost::uuids::uuid uuid) const = 0;
 };
 
 class BluetoothDevice {
@@ -179,8 +183,12 @@ enum AttPduType {
     INVALID_HANDLE = 0x01,
     READ_BY_TYPE_REQ = 0x08,
     READ_BY_TYPE_RES = 0x09,
+    READ_REQ = 0x0a,
+    READ_RES = 0x0b,
     READ_BY_GROUP_TYPE_REQ = 0x10,
     READ_BY_GROUP_TYPE_RES = 0x11,
+    WRITE_REQ = 0x12,
+    WRITE_RES = 0x13,
 };
 
 class AttributeData;
@@ -197,10 +205,18 @@ public:
 
     static vector<AttributeData> parseReadByType(ByteBuffer &bytes);
 
+    static void parseRead(ByteBuffer &bytes);
+
+    static void parseWrite(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, SpecUuid uuid);
 
+    static void makeRead(ByteBuffer &bytes, uint16_t handle);
+
+    static void makeWrite(ByteBuffer &req, uint16_t handle, const ByteBuffer &bytes);
+
 private:
     static void checkType(ByteBuffer &bytes, AttPduType type);
 
diff --git a/ble/BluetoothImpl.h b/ble/BluetoothImpl.h
index ef9a733..c9ba80c 100644
--- a/ble/BluetoothImpl.h
+++ b/ble/BluetoothImpl.h
@@ -2,6 +2,7 @@
 #define BLUETOOTH_IMPL_H
 
 #include "Bluetooth.h"
+#include "log.h"
 #include <boost/uuid/uuid_io.hpp>
 #include <cstring>
 
@@ -94,14 +95,14 @@ public:
         characteristics.push_back(characteristic);
     }
 
-    virtual const o<BluetoothGattCharacteristic *> findCharacteristic(uuid_t uuid) const {
+    virtual const o<BluetoothGattCharacteristic &> findCharacteristic(uuid_t uuid) const {
         for (auto c: characteristics) {
             if (memcmp(c->getUuid().data, uuid.data, 16) == 0) {
-                return o<BluetoothGattCharacteristic *>(c);
+                return o<BluetoothGattCharacteristic &>(*c);
             }
         }
 
-        return o<BluetoothGattCharacteristic *>();
+        return o<BluetoothGattCharacteristic &>();
     }
 
 protected:
@@ -120,25 +121,37 @@ protected:
     }
 };
 
-template<class A>
-class DefaultBluetoothDevice : public BluetoothDevice {
+template<class _D>
+class DefaultBluetoothGatt : public BluetoothGatt {
 public:
+    virtual _D &getDevice() const {
+        return device;
+    }
 
-    virtual Mac const &getMac() override {
-        return mac;
+    virtual const vector<BluetoothGattService *> getServices() const {
+        return services;
+    };
+
+    virtual const o<BluetoothGattService &> findService(uuid_t uuid) const {
+        for (auto s: services) {
+            if (memcmp(s->getUuid().data, uuid.data, 16) == 0) {
+                return o<BluetoothGattService &>(*s);
+            }
+        }
+
+        return o<BluetoothGattService &>();
     }
 
-    virtual A &getAdapter() override {
-        return adapter;
+    virtual void addService(BluetoothGattService *service) {
+        services.push_back(service);
     }
 
 protected:
-    DefaultBluetoothDevice(A &adapter, Mac &mac) :
-            adapter(adapter), mac(mac) {
+    DefaultBluetoothGatt(_D &device) : device(device) {
         DF;
     }
 
-    virtual ~DefaultBluetoothDevice() {
+    virtual ~DefaultBluetoothGatt() {
         DF;
         removeServices();
     }
@@ -150,42 +163,29 @@ protected:
         services.clear();
     }
 
-    A &adapter;
-    Mac &mac;
+    _D &device;
     vector<BluetoothGattService *> services;
 };
 
-template<class _D>
-class DefaultBluetoothGatt : public BluetoothGatt {
+template<class A>
+class DefaultBluetoothDevice : public BluetoothDevice {
 public:
-    virtual _D &getDevice() const {
-        return device;
-    }
-
-    virtual const vector<BluetoothGattService *> getServices() const {
-        return services;
-    };
-
-    virtual const o<BluetoothGattService *> findService(uuid_t uuid) const {
-        for (auto s: services) {
-            if (memcmp(s->getUuid().data, uuid.data, 16) == 0) {
-                return o<BluetoothGattService *>(s);
-            }
-        }
 
-        return o<BluetoothGattService *>();
+    virtual Mac const &getMac() override {
+        return mac;
     }
 
-    virtual void addService(BluetoothGattService *service) {
-        services.push_back(service);
+    virtual A &getAdapter() override {
+        return adapter;
     }
 
 protected:
-    DefaultBluetoothGatt(_D &device) : device(device) {
+    DefaultBluetoothDevice(A &adapter, Mac &mac) :
+            adapter(adapter), mac(mac) {
         DF;
     }
 
-    virtual ~DefaultBluetoothGatt() {
+    virtual ~DefaultBluetoothDevice() {
         DF;
         removeServices();
     }
@@ -197,7 +197,8 @@ protected:
         services.clear();
     }
 
-    _D &device;
+    A &adapter;
+    Mac &mac;
     vector<BluetoothGattService *> services;
 };
 
diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp
index 820c638..377334b 100644
--- a/ble/ByteBuffer.cpp
+++ b/ble/ByteBuffer.cpp
@@ -2,19 +2,34 @@
 #include <string.h>
 #include <sstream>
 #include <iomanip>
+#include <cassert>
+#include "log.h"
 
 using namespace std;
 
+ByteBuffer ByteBuffer::alloc(std::size_t capacity) {
+    auto bytes = shared_ptr<uint8_t>(new uint8_t[capacity], [](uint8_t* p) {
+        delete[] p;
+    });
+    return ByteBuffer(bytes, capacity, (size_t) 0, (size_t) 0);
+}
+
 ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity) :
-    bytes(bytes), capacity(capacity), zero(bytes.get()), end(&bytes.get()[capacity]) {
-    ptr = (uint8_t *) zero;
+    bytes(bytes), capacity(capacity), zero(bytes.get()), end(bytes.get()) {
+    ptr = const_cast<uint8_t *>(zero);
 }
 
-ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t zero, size_t size) :
+ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t size) :
+        bytes(bytes), capacity(capacity), zero(bytes.get()), end(&bytes.get()[size]) {
+    assert(size <= capacity);
+    ptr = const_cast<uint8_t *>(this->zero);
+}
+
+ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t size, size_t zero) :
         bytes(bytes), capacity(capacity), zero(&bytes.get()[zero]), end(&bytes.get()[size]) {
     assert(zero <= size);
     assert(size <= capacity);
-    ptr = (uint8_t *) zero;
+    ptr = const_cast<uint8_t *>(this->zero);
 }
 
 ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, const uint8_t *zero, const uint8_t *end) :
@@ -34,6 +49,20 @@ ByteBuffer &ByteBuffer::write16le(uint16_t value) {
     return *this;
 }
 
+ByteBuffer &ByteBuffer::write(const ByteBuffer &value) {
+    return write(value.zero, value.getSize());
+}
+
+ByteBuffer &ByteBuffer::write(const uint8_t *bytes, size_t len) {
+    checkAndUpdateEnd(len);
+
+    memcpy(ptr, bytes, len);
+
+    ptr += len;
+
+    return *this;
+}
+
 uint8_t ByteBuffer::get8(size_t index) const {
     assertCanAccessRelative(index);
     return ptr[index];
diff --git a/ble/ByteBuffer.h b/ble/ByteBuffer.h
index 3836966..f884d6e 100644
--- a/ble/ByteBuffer.h
+++ b/ble/ByteBuffer.h
@@ -5,9 +5,7 @@
 #include <cstdlib>
 #include <string>
 #include <stdexcept>
-
-// For now
-#include "log.h"
+#include <memory>
 
 class ByteBufferException : public std::runtime_error {
 public:
@@ -17,12 +15,15 @@ public:
 
 class ByteBuffer {
 public:
+    static ByteBuffer alloc(std::size_t capacity);
+
     ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity);
 
-    ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t zero, size_t size);
+    ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t size);
+
+    ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t size, size_t zero);
 
     inline size_t getSize() const {
-//        DF << "end=" << (uint64_t)end << ", zero=" << (uint64_t)zero << ", size=" << (end - zero);
         return end - zero;
     }
 
@@ -39,12 +40,10 @@ public:
     }
 
     inline void setCursor(size_t newCursor) {
-//        assertCanAccessRelative(newCursor);
         ptr = (uint8_t *) &zero[newCursor];
     }
 
     inline void skip(size_t length) {
-//        checkAndUpdateEnd(length);
         ptr += length;
     }
 
@@ -52,6 +51,13 @@ public:
 
     ByteBuffer &write16le(uint16_t value);
 
+    /**
+     * Appends the entire buffer. Make a view if you want to write a part of it.
+     */
+    ByteBuffer &write(const ByteBuffer &value);
+
+    ByteBuffer &write(const uint8_t *bytes, size_t len);
+
     uint8_t get8(size_t index) const;
 
     uint8_t read8();
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();