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. --- CMakeLists.txt | 1 + apps/ble-inspect-device.cpp | 1 + apps/sm-get-value.cpp | 47 +++++++++++++++++++++++++++-- apps/soil-moisture.h | 6 +++- ble/Bluetooth.cpp | 30 +++++++++++++++++-- ble/Bluetooth.h | 28 +++++++++++++---- ble/BluetoothImpl.h | 73 +++++++++++++++++++++++---------------------- ble/ByteBuffer.cpp | 37 ++++++++++++++++++++--- ble/ByteBuffer.h | 20 ++++++++----- ble/LinuxBluetooth.cpp | 44 +++++++++++++++++++++++++-- test/ByteBufferTest.cpp | 33 ++++++++++---------- test/CMakeLists.txt | 1 - 12 files changed, 244 insertions(+), 77 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a6e2d6e..6b9fbfc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,3 +15,4 @@ add_definitions(-DBOOST_ALL_DYN_LINK) add_subdirectory(ble) add_subdirectory(apps) add_subdirectory(test) +enable_testing() diff --git a/apps/ble-inspect-device.cpp b/apps/ble-inspect-device.cpp index e50f6ae..b9abe11 100644 --- a/apps/ble-inspect-device.cpp +++ b/apps/ble-inspect-device.cpp @@ -3,6 +3,7 @@ #include #include #include "Bluetooth.h" +#include "log.h" using namespace std; using namespace trygvis::bluetooth; diff --git a/apps/sm-get-value.cpp b/apps/sm-get-value.cpp index 4d84c45..30ef49a 100644 --- a/apps/sm-get-value.cpp +++ b/apps/sm-get-value.cpp @@ -2,6 +2,7 @@ #include #include #include "Bluetooth.h" +#include "soil-moisture.h" #include "log.h" using namespace std; @@ -30,6 +31,8 @@ uuid_t trygvis_io_base_uuid = { uuid_t soil_moisture_service = makeUuid(trygvis_io_base_uuid, 0x00, 0x10); uuid_t soil_moisture_characteristic = makeUuid(trygvis_io_base_uuid, 0x00, 0x11); +ByteBuffer &operator<<(ByteBuffer &bytes, const sm_req &req); + int main(int argc, char *argv[]) { if (argc != 2) { cerr << "usage: " << argv[0] << " [mac]" << endl; @@ -57,14 +60,22 @@ int main(int argc, char *argv[]) { return EXIT_FAILURE; } - auto c = (*service)->findCharacteristic(soil_moisture_characteristic); + auto c = service->findCharacteristic(soil_moisture_characteristic); if (!c) { cout << "The device is missing the soil moisture characteristic" << endl; return EXIT_FAILURE; } -// auto bytes = gatt.readValue(c); + sm_req req = { + .code = SM_CMD_GET_SENSOR_COUNT, + }; + + auto requestBytes = ByteBuffer::alloc(sizeof(struct sm_req)); + requestBytes << req; + gatt.writeValue(*c, requestBytes); + + auto responseBytes = gatt.readValue(*c); gatt.disconnect(); return EXIT_SUCCESS; @@ -76,3 +87,35 @@ int main(int argc, char *argv[]) { return EXIT_FAILURE; } } + +ByteBuffer &operator<<(ByteBuffer &bytes, const sm_req &req) { + bytes.write8(req.code); + switch (req.code) { + case SM_CMD_GET_SENSOR_COUNT: + return bytes; + case SM_CMD_GET_VALUE: + return bytes. + write8(req.get_value.sensor); + case SM_CMD_SET_WARNING_VALUE: + return bytes. + write8(req.set_warning_value.sensor). + write16le(req.set_warning_value.warning_value); + case SM_CMD_GET_WARNING_VALUE: + return bytes. + write8(req.set_warning_value.sensor); + case SM_CMD_SET_SENSOR_NAME: + return bytes. + write8(req.set_sensor_name.sensor). + write8(req.set_sensor_name.length). + write(req.set_sensor_name.name, req.set_sensor_name.length); + case SM_CMD_GET_SENSOR_NAME: + return bytes. + write8(req.get_sensor_name.sensor); + case SM_CMD_SET_UPDATE_INTERVAL: + return bytes. + write8(req.set_update_interval.sensor). + write8(req.set_update_interval.interval_in_seconds); + default: + throw runtime_error("Unknown code: " + to_string(req.code)); + } +} diff --git a/apps/soil-moisture.h b/apps/soil-moisture.h index 4f19de1..c960f79 100644 --- a/apps/soil-moisture.h +++ b/apps/soil-moisture.h @@ -71,8 +71,13 @@ struct sm_set_update_interval_req { struct sm_set_update_interval_res { } __attribute__((packed)); +#define SM_REQ_HEADER_SIZE 1 + struct sm_req { + // header uint8_t code; + + // body union { struct sm_get_sensor_count_req get_sensor_count; struct sm_get_value_req get_value; @@ -84,7 +89,6 @@ struct sm_req { } __attribute__((packed)); } __attribute__((packed)); -// len + code #define SM_RES_HEADER_SIZE 1 struct sm_res { 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(); diff --git a/test/ByteBufferTest.cpp b/test/ByteBufferTest.cpp index 7ed75e1..bf18387 100644 --- a/test/ByteBufferTest.cpp +++ b/test/ByteBufferTest.cpp @@ -1,12 +1,13 @@ #include "ByteBuffer.h" +#include "log.h" #define BOOST_TEST_MODULE "ByteBuffer" #include #include "Bluetooth.h" -#define checkBuffer(buffer, size, capacity, cursor) \ - D << "size=" << buffer.getSize() << ", capacity=" << buffer.getCapacity() << ", cursor=" << buffer.getCursor(); \ +#define checkBuffer(buffer, capacity, size, cursor) \ + if(false) {D << "capacity=" << buffer.getCapacity() << ", size=" << buffer.getSize() << ", cursor=" << buffer.getCursor();} \ BOOST_CHECK_EQUAL(buffer.getSize(), size); \ BOOST_CHECK_EQUAL(buffer.getCapacity(), capacity); \ BOOST_CHECK_EQUAL(buffer.getCursor(), cursor) @@ -22,15 +23,17 @@ struct do_nothing { class Bytes { public: Bytes(size_t size) : capacity(size) { - _bytes = shared_ptr(new uint8_t[size]); - uint8_t *tmp = (uint8_t *) (((uint64_t) &_bytes.get()[0x100]) & 0xffffffffffffff00); - bytes = shared_ptr(tmp, ::do_nothing()); + uint8_t *secret = new uint8_t[size + 0x100]; + auto tmp = (uint8_t *) (((uint64_t) &secret[0x100]) & 0xffffffffffffff00); for (int i = 0; i < size; i++) { - bytes.get()[i] = (uint8_t) i; + tmp[i] = (uint8_t) i; } + + bytes = shared_ptr(tmp, [secret](uint8_t *) { + delete[]secret; + }); } shared_ptr bytes; - shared_ptr _bytes; size_t capacity; }; @@ -49,7 +52,7 @@ BOOST_AUTO_TEST_CASE(empty_buffer) { BOOST_AUTO_TEST_CASE(basic) { Bytes b(1000); - ByteBuffer buffer(b.bytes, 1000, 0, 1000); + ByteBuffer buffer(b.bytes, 1000, 1000); checkBuffer(buffer, 1000, 1000, 0); BOOST_CHECK_EQUAL(buffer.read8(), 0); @@ -62,25 +65,25 @@ BOOST_AUTO_TEST_CASE(basic) { BOOST_AUTO_TEST_CASE(setCursor) { Bytes b(1000); - ByteBuffer buffer(b.bytes, 1000, 0, 10); - checkBuffer(buffer, 10, 1000, 0); + ByteBuffer buffer(b.bytes, 1000, 10, 0); + checkBuffer(buffer, 1000, 10, 0); BOOST_CHECK_EQUAL(buffer.read8(), 0); - checkBuffer(buffer, 10, 1000, 1); + checkBuffer(buffer, 1000, 10, 1); buffer.setCursor(0); - checkBuffer(buffer, 10, 1000, 0); + checkBuffer(buffer, 1000, 10, 0); BOOST_CHECK_EQUAL(buffer.read8(), 0); - checkBuffer(buffer, 10, 1000, 1); + checkBuffer(buffer, 1000, 10, 1); buffer.setCursor(9); - checkBuffer(buffer, 10, 1000, 9); + checkBuffer(buffer, 1000, 10, 9); } BOOST_AUTO_TEST_CASE(view) { Bytes b(1000); - ByteBuffer buffer(b.bytes, b.capacity, 0, 10); + ByteBuffer buffer(b.bytes, b.capacity, 10, 0); BOOST_CHECK_EQUAL(buffer.read8(), 0); ByteBuffer view1 = buffer.view(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 14dcdf7..a417d2b 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,4 +1,3 @@ -enable_testing() find_package(Boost COMPONENTS log unit_test_framework REQUIRED) # If we can change directory here add_definition and test-specific stuff could be moved to the test directory -- cgit v1.2.3