From 5926b05afa21eaac36c185e7fc458710efa30b02 Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Sat, 21 Feb 2015 23:58:39 +0100 Subject: o Support for reading and writing characteristics. --- ble/Bluetooth.cpp | 30 +++++++++++++++++++-- ble/Bluetooth.h | 28 ++++++++++++++----- ble/BluetoothImpl.h | 73 +++++++++++++++++++++++++------------------------- ble/ByteBuffer.cpp | 37 ++++++++++++++++++++++--- ble/ByteBuffer.h | 20 +++++++++----- ble/LinuxBluetooth.cpp | 44 +++++++++++++++++++++++++++--- 6 files changed, 174 insertions(+), 58 deletions(-) (limited to 'ble') 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 #include -#include 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 AttPdu::parse(ByteBuffer &bytes, AttPduType type) { DF << "bytes: " << bytes.toString(); @@ -128,6 +138,22 @@ vector 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 findCharacteristic(boost::uuids::uuid uuid) const = 0; + virtual const boost::optional 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 getServices() const = 0; - virtual const boost::optional findService(boost::uuids::uuid uuid) const = 0; + virtual const boost::optional 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 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 #include @@ -94,14 +95,14 @@ public: characteristics.push_back(characteristic); } - virtual const o findCharacteristic(uuid_t uuid) const { + virtual const o findCharacteristic(uuid_t uuid) const { for (auto c: characteristics) { if (memcmp(c->getUuid().data, uuid.data, 16) == 0) { - return o(c); + return o(*c); } } - return o(); + return o(); } protected: @@ -120,25 +121,37 @@ protected: } }; -template -class DefaultBluetoothDevice : public BluetoothDevice { +template +class DefaultBluetoothGatt : public BluetoothGatt { public: + virtual _D &getDevice() const { + return device; + } - virtual Mac const &getMac() override { - return mac; + virtual const vector getServices() const { + return services; + }; + + virtual const o findService(uuid_t uuid) const { + for (auto s: services) { + if (memcmp(s->getUuid().data, uuid.data, 16) == 0) { + return o(*s); + } + } + + return o(); } - 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 services; }; -template -class DefaultBluetoothGatt : public BluetoothGatt { +template +class DefaultBluetoothDevice : public BluetoothDevice { public: - virtual _D &getDevice() const { - return device; - } - - virtual const vector getServices() const { - return services; - }; - - virtual const o findService(uuid_t uuid) const { - for (auto s: services) { - if (memcmp(s->getUuid().data, uuid.data, 16) == 0) { - return o(s); - } - } - return o(); + 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 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 #include #include +#include +#include "log.h" using namespace std; +ByteBuffer ByteBuffer::alloc(std::size_t capacity) { + auto bytes = shared_ptr(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 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(zero); } -ByteBuffer::ByteBuffer(const std::shared_ptr bytes, size_t capacity, size_t zero, size_t size) : +ByteBuffer::ByteBuffer(const std::shared_ptr bytes, size_t capacity, size_t size) : + bytes(bytes), capacity(capacity), zero(bytes.get()), end(&bytes.get()[size]) { + assert(size <= capacity); + ptr = const_cast(this->zero); +} + +ByteBuffer::ByteBuffer(const std::shared_ptr 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(this->zero); } ByteBuffer::ByteBuffer(const std::shared_ptr 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 #include #include - -// For now -#include "log.h" +#include 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 bytes, size_t capacity); - ByteBuffer(const std::shared_ptr bytes, size_t capacity, size_t zero, size_t size); + ByteBuffer(const std::shared_ptr bytes, size_t capacity, size_t size); + + ByteBuffer(const std::shared_ptr 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 #include #include #include @@ -8,7 +7,6 @@ #include #include #include -#include // 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 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 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 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 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(); -- cgit v1.2.3