From d88eee8fcf23e20ae76b3dd346b36af693849ccd Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Thu, 22 Nov 2018 19:30:14 +0100 Subject: o Working enabling of notifications. --- apps/SoilMoisture.cpp | 4 +- apps/ble-read-characteristic.cpp | 15 +++- ble/Bluetooth.cpp | 4 + ble/BluetoothImpl.h | 55 ++++++++++++- ble/ByteBuffer.cpp | 85 +++++++++++--------- ble/LinuxBluetooth.cpp | 169 +++++++++++++++++++++++++++------------ ble/att.cpp | 58 +++++++++++--- include/ble/Bluetooth.h | 52 ++++++------ include/ble/ByteBuffer.h | 71 ++++++++-------- include/ble/att.h | 37 ++++++++- include/ble/misc.h | 46 ++++++++++- test/ByteBufferTest.cpp | 6 +- 12 files changed, 432 insertions(+), 170 deletions(-) diff --git a/apps/SoilMoisture.cpp b/apps/SoilMoisture.cpp index d7c03bb..3129564 100644 --- a/apps/SoilMoisture.cpp +++ b/apps/SoilMoisture.cpp @@ -95,7 +95,7 @@ SoilMoisture::SoilMoisture(const shared_ptr &gatt, temperatureCharacteristic(temperatureCharacteristic), lightCharacteristic(lightCharacteristic) {} void SoilMoisture::writeAndRead(const BluetoothGattCharacteristicPtr &c, ByteBuffer &buffer) { - buffer.setCursor(0); + buffer.setPosition(0); uint8_t expectedCode = buffer.peek8(0); @@ -137,7 +137,7 @@ string SoilMoisture::getName(uint8_t sensor) { writeAndRead(soilMoistureCharacteristic, buffer); size_t bytesLeft = buffer.getBytesLeft(); uint8_t bytes[bytesLeft]; - buffer.copy(bytes, bytesLeft); + buffer.copyTo(bytes, bytesLeft); if (bytesLeft == 0 || bytesLeft < (bytes[0] + 1)) { throw runtime_error("Bad response from device. buffer size: " + to_string(bytesLeft) + ", buffer[0]=" + diff --git a/apps/ble-read-characteristic.cpp b/apps/ble-read-characteristic.cpp index 8a0a7b6..a11b0cf 100644 --- a/apps/ble-read-characteristic.cpp +++ b/apps/ble-read-characteristic.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include #include "ble/Bluetooth.h" @@ -100,8 +101,18 @@ public: cout << "Got data, size=" << response.getSize() << endl; } else if (op_mode == op::NOTIFY) { auto cccd = characteristic.get()->getDescriptor(trygvis::bluetooth::uuids::CLIENT_CHARACTERISTIC_CONFIG); -// cccd->setValue(BluetoothGattDescriptor::ENABLE_NOTIFICATION_VALUE); - gatt->setCharacteristicNotification(cccd, true); + + if (!cccd) { + cout << "The characteristic does not have a CCCD" << endl; + } else { + cerr << "Enabling notifications" << endl; + cccd->setValue(ByteBuffer::wrapInitialized(BluetoothGattDescriptor::ENABLE_NOTIFICATION_VALUE)); + gatt->setCharacteristicNotification(cccd, true); + gatt->write(cccd); + + cout << "sleeping" << endl; + std::this_thread::sleep_for(10s); + } } else { cout << "Unsupported op mode." << endl; } diff --git a/ble/Bluetooth.cpp b/ble/Bluetooth.cpp index 2528416..16f4224 100644 --- a/ble/Bluetooth.cpp +++ b/ble/Bluetooth.cpp @@ -8,6 +8,10 @@ namespace trygvis { namespace bluetooth { using namespace std; +const uint8_t BluetoothGattDescriptor::DISABLE_NOTIFICATION_VALUE[] = {0x00, 0x00}; +const uint8_t BluetoothGattDescriptor::ENABLE_INDICATION_VALUE[] = {0x02, 0x00}; +const uint8_t BluetoothGattDescriptor::ENABLE_NOTIFICATION_VALUE[] = {0x01, 0x00}; + // ----------------------------------------------------------------------- // Mac // ----------------------------------------------------------------------- diff --git a/ble/BluetoothImpl.h b/ble/BluetoothImpl.h index 975a011..5b53cd4 100644 --- a/ble/BluetoothImpl.h +++ b/ble/BluetoothImpl.h @@ -40,6 +40,46 @@ protected: // Shared classes +class DefaultBluetoothGattDescriptor : public BluetoothGattDescriptor { +public: + explicit DefaultBluetoothGattDescriptor(const BluetoothGattCharacteristicPtr& characteristic, uint16_t handle, + const Uuid &uuid) : characteristic(characteristic), handle(handle), uuid(uuid), data() {} + + ~DefaultBluetoothGattDescriptor() override { + delete data.underlying(); + }; + + BluetoothGattCharacteristicPtr getCharacteristic() const { + return characteristic; + }; + + uint16_t getHandle() const override { + return handle; + } + + const Uuid getUuid() const override { + return uuid; + } + + void setValue(const ByteBuffer &buffer) override { + delete[] data.underlying(); + + auto sz = buffer.getSize(); + data = ByteBuffer(new uint8_t[sz], sz); + data.copyFromEntire(buffer); + }; + + ByteBuffer getValue() const override { + return data; + }; + +private: + BluetoothGattCharacteristicPtr characteristic; + uint16_t handle; + Uuid uuid; + ByteBuffer data; +}; + class DefaultBluetoothGattCharacteristic : protected LogSetup, public BluetoothGattCharacteristic { public: DefaultBluetoothGattCharacteristic(const BluetoothGattServicePtr &service, uint16_t handle, Uuid uuid, @@ -62,8 +102,8 @@ public: return uuid; } - uint8_t getProperties() const override { - return properties; + CharacteristicProperties getProperties() const override { + return {properties}; } uint16_t getValueHandle() const override { @@ -71,15 +111,26 @@ public: } shared_ptr getDescriptor(Uuid uuid) const override { + for (auto &d: descriptors) { + if (d->getUuid() == uuid) { + return d; + } + } + return {}; } + void addDescriptor(shared_ptr &&d) { + descriptors.push_back(std::move(d)); + } + protected: BluetoothGattServicePtr service; uint16_t handle; Uuid uuid; uint8_t properties; uint16_t valueHandle; + vector descriptors; }; template diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp index 40ba06c..8aea2c8 100644 --- a/ble/ByteBuffer.cpp +++ b/ble/ByteBuffer.cpp @@ -9,29 +9,25 @@ using namespace std; -ByteBuffer::ByteBuffer(uint8_t* bytes, size_t size) : - zero(bytes), end_(&bytes[size]), cursor(bytes) { -} - ByteBuffer &ByteBuffer::write8(uint8_t value) { - assertCanAccessRelative(1); - (*cursor++) = value; + assertCanAccessPtr(cursor + 1); + *cursor++ = value; return *this; } ByteBuffer &ByteBuffer::write16le(uint16_t value) { - assertCanAccessRelative(2); - (*cursor++) = (uint8_t) (value & 0xff); - (*cursor++) = (uint8_t) ((value >> 8) & 0xff); + assertCanAccessPtr(cursor + 2); + *cursor++ = (uint8_t) (value & 0xff); + *cursor++ = (uint8_t) ((value >> 8) & 0xff); return *this; } ByteBuffer &ByteBuffer::write32le(uint32_t value) { - assertCanAccessRelative(4); - (*cursor++) = (uint8_t) (value & 0xff); - (*cursor++) = (uint8_t) ((value >> 8) & 0xff); - (*cursor++) = (uint8_t) ((value >> 16) & 0xff); - (*cursor++) = (uint8_t) ((value >> 24) & 0xff); + assertCanAccessPtr(cursor + 4); + *cursor++ = (uint8_t) (value & 0xff); + *cursor++ = (uint8_t) ((value >> 8) & 0xff); + *cursor++ = (uint8_t) ((value >> 16) & 0xff); + *cursor++ = (uint8_t) ((value >> 24) & 0xff); return *this; } @@ -40,7 +36,7 @@ ByteBuffer &ByteBuffer::write(const ByteBuffer &value) { } ByteBuffer &ByteBuffer::write(const uint8_t *bytes, size_t len) { - assertCanAccessRelative(len); + assertCanAccessPtr(cursor + len); std::memcpy(cursor, bytes, len); @@ -100,7 +96,7 @@ ByteBuffer &ByteBuffer::writeFLOAT(double d) { mdiff = abs(smantissa - rmantissa); } - uint32_t int_mantissa = (uint32_t) round(sgn * mantissa); + auto int_mantissa = uint32_t(round(sgn * mantissa)); result = (exponent << 24) | (int_mantissa & 0xFFFFFF); } @@ -108,22 +104,22 @@ ByteBuffer &ByteBuffer::writeFLOAT(double d) { } uint8_t ByteBuffer::get8(size_t index) const { - assertCanAccessRelative(index); + assertCanAccessPosition(index); return zero[index]; } uint8_t ByteBuffer::peek8(size_t relative_index) const { - assertCanAccessRelative(relative_index); + assertCanAccessPtr(cursor + relative_index); return cursor[relative_index]; } uint8_t ByteBuffer::read8() { - assertCanAccessRelative(0); + assertCanAccessPtr(cursor); return *cursor++; } uint16_t ByteBuffer::read16le() { - assertCanAccessRelative(1); + assertCanAccessPtr(cursor + 1); uint16_t value; value = *cursor++; value |= ((uint16_t) *cursor++) << 8; @@ -131,7 +127,7 @@ uint16_t ByteBuffer::read16le() { } uint32_t ByteBuffer::read32le() { - assertCanAccessRelative(3); + assertCanAccessPtr(cursor + 3); uint32_t value; value = *cursor++; value |= ((uint32_t) *cursor++) << 8; @@ -166,39 +162,46 @@ double ByteBuffer::readFLOAT() { return output; } -void ByteBuffer::copy(uint8_t *bytes, size_t length) const { - assertCanAccessRelative(length - 1); +void ByteBuffer::copyFromEntire(const ByteBuffer &other) const { + size_t length = other.getPosition(); - std::memcpy(bytes, cursor, length); + assertCanAccessPosition(length - 1); + std::memcpy(cursor, other.zero, length); } -void ByteBuffer::copy(ByteBuffer& other) const { - other.assertCanAccessRelative(getBytesLeft()); +void ByteBuffer::copyTo(uint8_t *bytes, size_t length) const { + assertCanAccessPtr(&cursor[length - 1]); - std::memcpy(other.cursor, cursor, getBytesLeft()); -} - -void ByteBuffer::reset() { - cursor = const_cast(zero); + std::memcpy(bytes, cursor, length); } -ByteBuffer ByteBuffer::view() const { +ByteBuffer ByteBuffer::viewCursorToEnd() const { // LOG_DEBUG("cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end_ - cursor << ", cursor=" << (uint64_t) cursor << ", zero=" << (uint64_t) zero); return {cursor, getBytesLeft()}; } -ByteBuffer ByteBuffer::view(size_t length) const { - assertCanAccessRelative(length - 1); +ByteBuffer ByteBuffer::viewBeginningToCursor() const { + return {zero, getPosition()}; +} + +ByteBuffer ByteBuffer::viewForward(size_t length) const { + assertCanAccessPtr(&cursor[length - 1]); return {cursor, length}; } -void ByteBuffer::assertCanAccessRelative(size_t diff) const { - assertCanAccessIndex(cursor + diff); +ByteBuffer ByteBuffer::viewForwardAndSkip(size_t length){ + auto v = viewForward(length); + skip(length); + return v; +} + +void ByteBuffer::assertCanAccessPosition(size_t position) const { + assertCanAccessPtr(&zero[position]); } -void ByteBuffer::assertCanAccessIndex(uint8_t *p) const { - if (p >= end_ || p < zero) { - throw ByteBufferException("Out of bounds! size=" + to_string(getSize()) + ", index=" + to_string(p - zero)); +void ByteBuffer::assertCanAccessPtr(uint8_t *ptr) const { + if (ptr >= end_ || ptr < zero) { + throw ByteBufferException("Out of bounds! size=" + to_string(getSize()) + ", index=" + to_string(ptr - zero)); } } @@ -211,3 +214,7 @@ std::string ByteBuffer::toString() const { return string(s.str()); } + +const ByteBuffer ByteBuffer::wrap(const uint8_t *bytes, size_t capacity, size_t cursor) noexcept { + return {const_cast(bytes), capacity, cursor}; +} diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp index 444e3a2..cc6516d 100644 --- a/ble/LinuxBluetooth.cpp +++ b/ble/LinuxBluetooth.cpp @@ -38,8 +38,9 @@ class LinuxBluetoothManager; class LinuxBluetoothGattService : public DefaultBluetoothGattService { public: - explicit LinuxBluetoothGattService(LinuxBluetoothDevice &device, const Uuid& uuid, const uint16_t handle, - const uint16_t endGroupHandle) : DefaultBluetoothGattService(device, uuid, handle, endGroupHandle) { + explicit LinuxBluetoothGattService(LinuxBluetoothDevice &device, const Uuid &uuid, const uint16_t handle, + const uint16_t endGroupHandle) : + DefaultBluetoothGattService(device, uuid, handle, endGroupHandle) { }; LinuxBluetoothGattService(const LinuxBluetoothGattService &o) = delete; @@ -53,7 +54,7 @@ public: LinuxBluetoothAdapter(const LinuxBluetoothAdapter &) = delete; - LinuxBluetoothAdapter& operator=(const LinuxBluetoothAdapter&) = delete; + LinuxBluetoothAdapter &operator=(const LinuxBluetoothAdapter &) = delete; void runScan(std::function &device)>) override; @@ -81,7 +82,7 @@ public: LinuxBluetoothDevice(const LinuxBluetoothDevice &) = delete; - LinuxBluetoothDevice& operator=(const LinuxBluetoothDevice&) = delete; + LinuxBluetoothDevice &operator=(const LinuxBluetoothDevice &) = delete; shared_ptr connectGatt() override; @@ -95,9 +96,9 @@ public: ~LinuxBluetoothGatt() override; - LinuxBluetoothGatt(const LinuxBluetoothGatt&) = delete; + LinuxBluetoothGatt(const LinuxBluetoothGatt &) = delete; - LinuxBluetoothGatt& operator=(const LinuxBluetoothGatt&) = delete; + LinuxBluetoothGatt &operator=(const LinuxBluetoothGatt &) = delete; bool isConnected() const override; @@ -105,7 +106,9 @@ public: void writeValue(const BluetoothGattCharacteristicPtr &c, const ByteBuffer &bytes) override; - ByteBuffer readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer& response) override; + void write(const BluetoothGattDescriptorPtr &c) override; + + ByteBuffer readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer &response) override; void setCharacteristicNotification(const BluetoothGattDescriptorPtr &c, bool enable) override; @@ -118,9 +121,13 @@ private: vector discoverCharacteristics(uint16_t startHandle, uint16_t endHandle); + void discoverDescriptors(shared_ptr &c); + + void writeHandle(uint16_t handle, const ByteBuffer &bytes); + void writeL2cap(ByteBuffer &buffer); - void writeAndRead(const ByteBuffer &buffer, ByteBuffer& response); + void writeAndRead(const ByteBuffer &buffer, ByteBuffer &response); AttVariant processAvailableMessages(ByteBuffer &buffer); @@ -130,7 +137,7 @@ private: template T exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point time, bool fail_on_error); - void logError(const ErrorRes&); + void logError(const ErrorRes &); uint16_t mtu = 256; @@ -160,7 +167,7 @@ Mac parseMac(bdaddr_t &a) { // ----------------------------------------------------------------------- LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &mac) : - DefaultBluetoothDevice(adapter, mac), gatt(nullptr) { + DefaultBluetoothDevice(adapter, mac), gatt(nullptr) { } LinuxBluetoothDevice::~LinuxBluetoothDevice() { @@ -183,7 +190,7 @@ shared_ptr LinuxBluetoothDevice::connectGatt() { // ----------------------------------------------------------------------- LinuxBluetoothGatt::LinuxBluetoothGatt(LinuxBluetoothDevice &device) : - DefaultBluetoothGatt(device), l2cap() { + DefaultBluetoothGatt(device), l2cap() { connect(); } @@ -229,12 +236,12 @@ void LinuxBluetoothGatt::connect() { addr.l2_cid = htobs(ATT_CID); addr.l2_bdaddr_type = BDADDR_LE_RANDOM; device.getMac().copy( - addr.l2_bdaddr.b[5], - addr.l2_bdaddr.b[4], - addr.l2_bdaddr.b[3], - addr.l2_bdaddr.b[2], - addr.l2_bdaddr.b[1], - addr.l2_bdaddr.b[0]); + addr.l2_bdaddr.b[5], + addr.l2_bdaddr.b[4], + addr.l2_bdaddr.b[3], + addr.l2_bdaddr.b[2], + addr.l2_bdaddr.b[1], + addr.l2_bdaddr.b[0]); if (::connect(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) { close(l2cap); @@ -261,8 +268,10 @@ Uuid readUuid(BluetoothDevice *device, ByteBuffer &bytes) { return Uuid(bs); } else if (bytesLeft == 16) { uint8_t bs[16] = { - bytes.get8(15), bytes.get8(14), bytes.get8(13), bytes.get8(12), bytes.get8(11), bytes.get8(10), bytes.get8(9), bytes.get8(8), - bytes.get8(7), bytes.get8(6), bytes.get8(5), bytes.get8(4), bytes.get8(3), bytes.get8(2), bytes.get8(1), bytes.get8(0), + bytes.get8(15), bytes.get8(14), bytes.get8(13), bytes.get8(12), + bytes.get8(11), bytes.get8(10), bytes.get8(9), bytes.get8(8), + bytes.get8(7), bytes.get8(6), bytes.get8(5), bytes.get8(4), + bytes.get8(3), bytes.get8(2), bytes.get8(1), bytes.get8(0), }; bytes.skip(16); @@ -275,17 +284,39 @@ Uuid readUuid(BluetoothDevice *device, ByteBuffer &bytes) { void LinuxBluetoothGatt::writeValue(const BluetoothGattCharacteristicPtr &c, const ByteBuffer &bytes) { LOG_DEBUG("Writing to characteristic " << c->getUuid() << ": " << bytes.toString()); + writeHandle(c->getValueHandle(), bytes); +} + +void LinuxBluetoothGatt::write(const BluetoothGattDescriptorPtr &d) { + auto bytes = d->getValue(); + + LOG_DEBUG("Writing to descriptor " << d->getUuid() << ": " << bytes.toString()); + + writeHandle(d->getHandle(), bytes); +} + +void LinuxBluetoothGatt::writeHandle(uint16_t handle, const ByteBuffer &bytes) { uint8_t b[mtu]; ByteBuffer buffer{b, mtu}; - AttPdu::makeWrite(buffer, c->getValueHandle(), bytes); + AttPdu::makeWrite(buffer, handle, bytes); writeAndRead(buffer, buffer); + auto cursor = buffer.getPosition(); + buffer.setPosition(0); + AttPdu::parseWrite(buffer); + + auto extra_bytes = cursor - buffer.getPosition(); + LOG_DEBUG("WRITE response has " + to_string(extra_bytes) + " extra bytes"); + + if (extra_bytes) { + throw BluetoothException(&device, "Got extra bytes from ::read(): " + to_string(extra_bytes)); + } } -ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer& response) { +ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer &response) { uint8_t b[mtu]; ByteBuffer buffer{b, mtu}; @@ -293,12 +324,12 @@ ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c writeAndRead(buffer, response); - auto cursor = response.getCursor(); - response.setCursor(0); + auto cursor = response.getPosition(); + response.setPosition(0); AttPdu::parseRead(response); - auto view = response.view(cursor - response.getCursor()); + auto view = response.viewForward(cursor - response.getPosition()); LOG_DEBUG("READ response has " + to_string(view.getSize()) + " bytes"); LOG_DEBUG("Value of characteristic " << c->getUuid() << "=" << view.toString()); @@ -306,7 +337,11 @@ ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c return view; } -void LinuxBluetoothGatt::setCharacteristicNotification(const BluetoothGattDescriptorPtr &c, bool enable) { +void LinuxBluetoothGatt::setCharacteristicNotification(const BluetoothGattDescriptorPtr &d, bool enable) { + uint8_t b[mtu]; + ByteBuffer buffer{b, mtu}; + +// AttPdu::makeWrite(buffer, d->getValueHandle()); } void LinuxBluetoothGatt::discoverServices() { @@ -330,16 +365,14 @@ void LinuxBluetoothGatt::discoverServices() { endGroupHandle = data.value.read16le(); LOG_DEBUG("service handle: 0x" << hex << setw(4) << setfill('0') << data.handle << - ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle << - ", value: " << data.value.toString()); + ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle << + ", value: " << data.value.toString()); auto u = readUuid(&device, data.value); addService(make_shared(device, u, data.handle, endGroupHandle)); } -// auto last = values.back(); - startHandle = endGroupHandle; } while (startHandle != 0xffff); @@ -349,7 +382,7 @@ void LinuxBluetoothGatt::discoverServices() { // } auto it = services.begin(), - end = services.end(); + end = services.end(); if (it == end) { return; @@ -381,49 +414,56 @@ void LinuxBluetoothGatt::discoverServices() { auto uuid = readUuid(&device, c.value); LOG_DEBUG("characteristic: handle: " << setw(2) << setfill('0') << hex << (int) c.handle << - ", properties: " << setw(2) << setfill('0') << hex << (int) properties << - ", valueHandle: 0x" << setw(4) << setfill('0') << hex << (int) valueHandle << - ", uuid: " << uuid); + ", properties: " << setw(2) << setfill('0') << hex << (int) properties + << + ", valueHandle: 0x" << setw(4) << setfill('0') << hex + << (int) valueHandle << + ", uuid: " << uuid); + + auto characteristic = make_shared( + s, c.handle, uuid, properties, valueHandle); - s->addCharacteristic(std::move(make_shared(s, c.handle, uuid, properties, valueHandle))); + discoverDescriptors(characteristic); + + s->addCharacteristic(std::move(characteristic)); } startHandle = lastHandle + (uint8_t) 2; } while (startHandle != 0xffff); } -void LinuxBluetoothGatt::writeAndRead(const ByteBuffer &buffer, ByteBuffer& response) { +void LinuxBluetoothGatt::writeAndRead(const ByteBuffer &buffer, ByteBuffer &response) { // LOG_DEBUG("pdu size=" << out.getCursor()); - auto to_be_written = buffer.getCursor(); + auto to_be_written = buffer.getPosition(); - ssize_t written = write(l2cap, buffer.cbegin(), to_be_written); + ssize_t written = ::write(l2cap, buffer.cbegin(), to_be_written); if (to_be_written != written) { throw BluetoothException(&device, "Expected to write " + to_string(to_be_written) + " but wrote only " + - to_string(written)); + to_string(written)); } // LOG_DEBUG("written=" << written); - ssize_t r = read(l2cap, response.begin(), response.getSize()); + ssize_t r = read(l2cap, response.underlying(), response.getSize()); if (r == -1) { throw BluetoothException(&device, "read(): " + errnoAsString()); } - LOG_DEBUG("read: " << r << " bytes: " << response.toString()); - response.setCursor(static_cast(r)); + response.setPosition(static_cast(r)); + LOG_DEBUG("read: " << r << " bytes: " << response.viewBeginningToCursor().toString()); } void LinuxBluetoothGatt::writeL2cap(ByteBuffer &buffer) { - auto to_be_written = buffer.getCursor(); - ssize_t written = write(l2cap, buffer.cbegin(), to_be_written); + auto to_be_written = buffer.getPosition(); + ssize_t written = ::write(l2cap, buffer.cbegin(), to_be_written); if (to_be_written != written) { throw BluetoothException(&device, "Expected to write " + to_string(to_be_written) + " but wrote only " + to_string(written)); } - buffer.reset(); + buffer.setPosition(0); } AttVariant LinuxBluetoothGatt::processAvailableMessages(ByteBuffer &buffer) { @@ -433,13 +473,13 @@ AttVariant LinuxBluetoothGatt::processAvailableMessages(ByteBuffer &buffer) { // LOG_DEBUG("ret=" << to_string(ret) << "/" << errnoAsString(ret) << // ", bytes available: " << to_string(bytes_available)); - while(!ret && bytes_available) { - ssize_t r = read(l2cap, buffer.begin(), buffer.getBytesLeft()); + while (!ret && bytes_available) { + ssize_t r = read(l2cap, buffer.underlying(), buffer.getBytesLeft()); if (r == -1) { throw BluetoothException(&device, "read(): " + errnoAsString()); } - auto packet_buffer = buffer.view(size_t(r)); + auto packet_buffer = buffer.viewForward(size_t(r)); auto type = static_cast(packet_buffer.get8(0)); LOG_DEBUG("Got ATT message " << to_string(type) << ", size: " << to_string(r)); @@ -474,7 +514,7 @@ static BluetoothException makeUnexpectedTypeException(const AttVariant &variant) template static void assertIsVariant(const AttVariant &variant) { - if(holds_alternative(variant)) { + if (holds_alternative(variant)) { return; } @@ -561,13 +601,13 @@ vector LinuxBluetoothGatt::discoverCharacteristics(uint16_t start auto res = exchangeAndWaitFor(buffer, std::chrono::system_clock::now() + 10s); - if(holds_alternative(res)) { + if (holds_alternative(res)) { auto values = get(res).attributes; LOG_DEBUG("READ_BY_TYPE response has " + to_string(values.size()) + " values"); return values; } - if(holds_alternative(res)) { + if (holds_alternative(res)) { ErrorRes err = get(res); if (err.errorCode == ErrorCode::ATTRIBUTE_NOT_FOUND) { @@ -580,12 +620,39 @@ vector LinuxBluetoothGatt::discoverCharacteristics(uint16_t start throw makeUnexpectedTypeException(res); } +void LinuxBluetoothGatt::discoverDescriptors(shared_ptr &c) { + + auto p = c->getProperties(); + + if (!p.indicate() && !p.notify()) { + return; + } + + uint8_t bytes[mtu]; + ByteBuffer buffer{bytes, mtu}; + + uint16_t handle = c->getValueHandle() + uint16_t(1); + AttPdu::makeFindInformationReq(buffer, handle, handle); + + auto res = exchangeAndWaitFor(buffer, std::chrono::system_clock::now() + 10s); + + if (holds_alternative(res)) { + auto values = get(res).information; + LOG_DEBUG("FIND_INFORMATION response has " + to_string(values.size()) + " values"); + + for (auto &value : values) { + c->addDescriptor(std::move(make_shared(c, value.handle, value.uuid))); + LOG_DEBUG("UUID=" + value.uuid.str()); + } + } +} + // ----------------------------------------------------------------------- // Adapter // ----------------------------------------------------------------------- LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId, Mac &mac) : DefaultBluetoothAdapter(mac), - scanning(false), hciFilter() { + scanning(false), hciFilter() { LOG_DEBUG("hciDeviceId=" << hciDeviceId); this->hciDeviceId = hciDeviceId; diff --git a/ble/att.cpp b/ble/att.cpp index 585e914..301918d 100644 --- a/ble/att.cpp +++ b/ble/att.cpp @@ -74,7 +74,7 @@ std::string to_string(AttPduType t) { } o attPduType(const AttVariant &v) { - return visit([](auto &&arg) -> o{ + return visit([](auto &&arg) -> o { using T = std::decay_t; if constexpr (std::is_same_v) { @@ -96,12 +96,17 @@ AttPduType AttPdu::getType() { return (AttPduType) bytes.peek8(0); } -void AttPdu::makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu) -{ +void AttPdu::makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu) { bytes.write8(AttPduType::EXCHANGE_MTU_RES). write16le(serverRxMtu); } +void AttPdu::makeFindInformationReq(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle) { + bytes.write8(AttPduType::FIND_INFORMATION_REQ); + bytes.write16le(startHandle); + bytes.write16le(endHandle); +} + void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) { bytes.write8(AttPduType::READ_BY_GROUP_TYPE_REQ); bytes.write16le(startHandle); @@ -129,7 +134,8 @@ void AttPdu::makeWrite(ByteBuffer &req, uint16_t handle, const ByteBuffer &bytes vector AttPdu::parseAttributeData(ByteBuffer &bytes) { if (bytes.getSize() < 4) { - throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize())); + throw BluetoothException( + "Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize())); } uint8_t length = bytes.read8(); @@ -138,8 +144,8 @@ vector AttPdu::parseAttributeData(ByteBuffer &bytes) { vector values; for (int i = 0; i < count; i++) { - auto data = bytes.viewAndSkip(length); - auto&& x = AttributeData::fromByteBuffer(data); + auto data = bytes.viewForwardAndSkip(length); + auto &&x = AttributeData::fromByteBuffer(data); values.emplace_back(x); } @@ -154,6 +160,8 @@ AttVariant AttPdu::parse(ByteBuffer &bytes) { return parseExchangeMtuReq(bytes); case AttPduType::EXCHANGE_MTU_RES: return parseExchangeMtuRes(bytes); + case AttPduType::FIND_INFORMATION_RES: + return parseFindInformationRes(bytes); case AttPduType::READ_BY_GROUP_TYPE_RES: return ReadByGroupTypeRes{parseAttributeData(bytes)}; case AttPduType::READ_BY_TYPE_RES: @@ -171,6 +179,23 @@ ExchangeMtuRes AttPdu::parseExchangeMtuRes(ByteBuffer &bytes) { return {bytes.read16le()}; } +FindInformationRes AttPdu::parseFindInformationRes(ByteBuffer &bytes) { + auto format = bytes.read8(); + + std::vector information; + if (format == InformationData::FORMAT_SHORT_UUID) { + while (bytes.getBytesLeft()) { + information.push_back(InformationData::fromByteBufferShortUuid(bytes)); + } + } else if (format == InformationData::FORMAT_LONG_UUID) { + while (bytes.getBytesLeft()) { + information.push_back(InformationData::fromByteBufferLongUuid(bytes)); + } + } + + return {format, information}; +} + void AttPdu::parseRead(ByteBuffer &bytes) { auto t = static_cast(bytes.read8()); @@ -180,10 +205,11 @@ void AttPdu::parseRead(ByteBuffer &bytes) { } void AttPdu::parseWrite(ByteBuffer &bytes) { - auto t = static_cast(bytes.read8()); + auto b = bytes.read8(); + auto t = static_cast(b); if (t != WRITE_RES) { - throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(WRITE_RES)); + throw BluetoothException("Unexpected type: " + to_string(b) + "/" + to_string(t) + ", expected " + to_string(WRITE_RES)); } } @@ -196,7 +222,7 @@ AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) { auto raw = make_unique(bytes.getBytesLeft()); - bytes.copy(raw.get(), bytes.getBytesLeft()); + bytes.copyTo(raw.get(), bytes.getBytesLeft()); ByteBuffer buffer = ByteBuffer{raw.get(), bytes.getBytesLeft()}; return AttributeData(handle, buffer, std::move(raw)); @@ -208,5 +234,19 @@ AttributeData::AttributeData(uint16_t handle, ByteBuffer value, std::shared_ptr< AttributeData::~AttributeData() = default; +// ----------------------------------------------------------------------- +// InformationData +// ----------------------------------------------------------------------- + +InformationData InformationData::fromByteBufferShortUuid(ByteBuffer &value) { + return {value.read16le(), ShortUuid(value).toLong()}; +} + +InformationData InformationData::fromByteBufferLongUuid(ByteBuffer &value) { + return {value.read16le(), Uuid(value)}; +} + +InformationData::InformationData(uint16_t handle, const Uuid &uuid) : handle(handle), uuid(uuid) {} + } // namespace bluetooth } // namespace trygvis diff --git a/include/ble/Bluetooth.h b/include/ble/Bluetooth.h index f97f3b8..b0228e5 100644 --- a/include/ble/Bluetooth.h +++ b/include/ble/Bluetooth.h @@ -27,19 +27,15 @@ template using o = std::optional; class BluetoothAdapter; - class BluetoothDevice; - class BluetoothGatt; -typedef shared_ptr BluetoothGattPtr; - class BluetoothGattService; -typedef shared_ptr BluetoothGattServicePtr; - class BluetoothGattCharacteristic; -typedef shared_ptr BluetoothGattCharacteristicPtr; - class BluetoothGattDescriptor; + +typedef shared_ptr BluetoothGattPtr; +typedef shared_ptr BluetoothGattServicePtr; +typedef shared_ptr BluetoothGattCharacteristicPtr; typedef shared_ptr BluetoothGattDescriptorPtr; class Mac { @@ -69,29 +65,29 @@ private: uint8_t bytes[6]; }; -template -class Iterator { -public: -private: -}; - -template -class Collection { -public: - Iterator begin(); - - Iterator end(); -}; - class BluetoothGattDescriptor { public: - static const uint8_t DISABLE_NOTIFICATION_VALUE[]; - static const uint8_t ENABLE_INDICATION_VALUE[]; - static const uint8_t ENABLE_NOTIFICATION_VALUE[]; + static const uint8_t DISABLE_NOTIFICATION_VALUE[2]; + static const uint8_t ENABLE_INDICATION_VALUE[2]; + static const uint8_t ENABLE_NOTIFICATION_VALUE[2]; virtual ~BluetoothGattDescriptor() = default; virtual BluetoothGattCharacteristicPtr getCharacteristic() const = 0; + + virtual uint16_t getHandle() const = 0; + + virtual const Uuid getUuid() const = 0; + + template + void setValue(const uint8_t (&buffer)[N]) { + auto tmp = ByteBuffer::wrap(buffer, N); + setValue(&tmp); + } + + virtual void setValue(const ByteBuffer &buffer) = 0; + + virtual ByteBuffer getValue() const = 0; }; class BluetoothGattCharacteristic { @@ -104,7 +100,7 @@ public: virtual const Uuid getUuid() const = 0; - virtual uint8_t getProperties() const = 0; + virtual CharacteristicProperties getProperties() const = 0; virtual uint16_t getValueHandle() const = 0; @@ -153,7 +149,9 @@ public: virtual void writeValue(const BluetoothGattCharacteristicPtr &c, const ByteBuffer &bytes) = 0; - virtual ByteBuffer readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer& response) = 0; + virtual void write(const BluetoothGattDescriptorPtr &d) = 0; + + virtual ByteBuffer readValue(const BluetoothGattCharacteristicPtr &c, ByteBuffer &response) = 0; virtual void setCharacteristicNotification(const BluetoothGattDescriptorPtr &c, bool enable) = 0; diff --git a/include/ble/ByteBuffer.h b/include/ble/ByteBuffer.h index 09deca5..6b09049 100644 --- a/include/ble/ByteBuffer.h +++ b/include/ble/ByteBuffer.h @@ -1,5 +1,5 @@ -#ifndef BYTE_STREAM_WRAPPER_H -#define BYTE_STREAM_WRAPPER_H +#ifndef BYTE_BUFFER_H +#define BYTE_BUFFER_H #include #include @@ -34,16 +34,29 @@ public: class ByteBuffer { public: + ByteBuffer() noexcept : zero(nullptr), end_(nullptr), cursor(nullptr) {}; + template - explicit ByteBuffer(uint8_t (&bytes)[N]) : zero(bytes), end_(&bytes[N]), cursor(bytes) {} + constexpr explicit ByteBuffer(uint8_t (&bytes)[N]) noexcept : zero(bytes), end_(&bytes[N]), cursor(zero) {}; + + ByteBuffer(uint8_t *bytes, size_t size) noexcept : zero(bytes), end_(&bytes[size]), cursor(zero) {}; - ByteBuffer(uint8_t* bytes, size_t capacity); + ByteBuffer(uint8_t *bytes, size_t size, size_t position) : zero(bytes), end_(&bytes[size]), cursor(zero) { + setPosition(position); + }; + + template + static const ByteBuffer wrapInitialized(const uint8_t (&bytes)[N], size_t cursor = 0) noexcept { + return wrap(bytes, N, N); + }; + + static const ByteBuffer wrap(const uint8_t *bytes, size_t size, size_t cursor = 0) noexcept; inline size_t getSize() const { return end_ - zero; } - inline size_t getCursor() const { + inline size_t getPosition() const { return cursor - zero; } @@ -51,10 +64,8 @@ public: return end_ - cursor; } - inline ByteBuffer &setCursor(size_t newCursor) { - auto tmp = (uint8_t *) &zero[newCursor]; - assertCanAccessIndex(tmp); - cursor = tmp; + inline ByteBuffer &setPosition(size_t newCursor) { + cursor = &zero[newCursor]; return *this; } @@ -70,12 +81,12 @@ public: return end_; } - inline uint8_t *begin() const { + inline uint8_t *underlying() { return const_cast(zero); } inline uint8_t *end() const { - return const_cast(end_); + return const_cast(cend()); } ByteBuffer &write8(uint8_t value); @@ -99,7 +110,7 @@ public: uint8_t get8(size_t index) const; /** - * Reads a uint8_t relative to the pointer. + * Reads a uint8_t relative to the cursor. */ uint8_t peek8(size_t relative_index) const; @@ -114,35 +125,31 @@ public: */ double readFLOAT(); - void copy(uint8_t *bytes, size_t length) const; + void copyFromEntire(const ByteBuffer &other) const; - void copy(ByteBuffer& other) const; + void copyTo(uint8_t *bytes, size_t length) const; - void reset(); + ByteBuffer viewCursorToEnd() const; - /** - * Creates a view from cursor to size. - */ - ByteBuffer view() const; + ByteBuffer viewBeginningToCursor() const; - ByteBuffer view(size_t length) const; + /** + * Creates a view from cursor to cursor + length. + */ + ByteBuffer viewForward(size_t length) const; - ByteBuffer viewAndSkip(size_t length) { - auto v = view(length); - skip(length); - return v; - } + /** + * Creates a view from cursor to cursor + length and then moves the cursor length further. + */ + ByteBuffer viewForwardAndSkip(size_t length); std::string toString() const; -private: - void assertCanAccessRelative(size_t diff) const; - - void assertCanAccessIndex(uint8_t *p) const; + void assertCanAccessPosition(size_t position) const; + void assertCanAccessPtr(uint8_t* ptr) const; - const uint8_t *zero; - const uint8_t *end_; - uint8_t *cursor; +protected: + uint8_t *zero, *end_, *cursor; }; template diff --git a/include/ble/att.h b/include/ble/att.h index 1db560e..db21d2e 100644 --- a/include/ble/att.h +++ b/include/ble/att.h @@ -49,6 +49,8 @@ std::string to_string(AttPduType t); class AttributeData; +class InformationData; + /** * Application Error 0x80 – 0xFF Application error code defined by a higher layer specification. * @@ -92,7 +94,7 @@ struct ErrorRes { */ uint8_t errorCode; - static ErrorRes parse(ByteBuffer& buffer) { + static ErrorRes parse(ByteBuffer &buffer) { return {buffer.read8(), buffer.read16le(), buffer.read8()}; } }; @@ -109,6 +111,13 @@ struct ExchangeMtuRes { uint16_t serverRxMtu; }; +struct FindInformationRes { + static constexpr auto att_pdu_type = AttPduType::FIND_INFORMATION_RES; + + uint8_t format; + std::vector information; +}; + struct ReadByGroupTypeRes { static constexpr auto att_pdu_type = AttPduType::READ_BY_GROUP_TYPE_RES; @@ -124,9 +133,10 @@ struct ReadByTypeRes { using AttVariant = std::variant; -o attPduType(const AttVariant& v); +o attPduType(const AttVariant &v); class AttPdu { public: @@ -139,14 +149,19 @@ public: static AttVariant parse(ByteBuffer &bytes); static ExchangeMtuReq parseExchangeMtuReq(ByteBuffer &bytes); + static ExchangeMtuRes parseExchangeMtuRes(ByteBuffer &bytes); + static FindInformationRes parseFindInformationRes(ByteBuffer &bytes); + static void parseRead(ByteBuffer &bytes); static void parseWrite(ByteBuffer &bytes); static void makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu); + static void makeFindInformationReq(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle); + static void makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid); static void makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid); @@ -178,5 +193,23 @@ private: std::shared_ptr raw; }; +class InformationData { +public: + static constexpr uint8_t FORMAT_SHORT_UUID = 0x01; + static constexpr uint8_t FORMAT_LONG_UUID = 0x02; + + ~InformationData() = default; + + static InformationData fromByteBufferShortUuid(ByteBuffer &value); + + static InformationData fromByteBufferLongUuid(ByteBuffer &value); + + const uint16_t handle; + const Uuid uuid; + +private: + InformationData(uint16_t handle, const Uuid &uuid); +}; + } // namespace bluetooth } // namespace trygvis diff --git a/include/ble/misc.h b/include/ble/misc.h index 2508d8e..70a19f5 100644 --- a/include/ble/misc.h +++ b/include/ble/misc.h @@ -1,7 +1,8 @@ #pragma once -#include +#include "ByteBuffer.h" +#include #include #include #include @@ -34,6 +35,10 @@ struct Uuid { std::memcpy(this->value, value, 16); } + explicit Uuid(ByteBuffer& buffer) noexcept : value() { + std::memcpy(value, buffer.cbegin(), 16); + } + Uuid(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4, uint8_t b5, uint8_t b6, uint8_t b7, uint8_t b8, uint8_t b9, uint8_t b10, uint8_t b11, uint8_t b12, uint8_t b13, uint8_t b14, uint8_t b15) noexcept : value{ b0, b1, b2, b3, b4, b5, b6, b7, b8, b9, b10, b11, b12, b13, b14, b15} {} @@ -62,6 +67,8 @@ private: public: explicit ShortUuid(uint16_t value) noexcept : value(value) {} + explicit ShortUuid(ByteBuffer& buffer) noexcept : value(buffer.read16le()) {} + Uuid toLong() const noexcept { auto b2 = static_cast(value >> 8); @@ -91,5 +98,42 @@ const ShortUuid CLIENT_CHARACTERISTIC_CONFIG{0x2902}; const ShortUuid TemperatureMeasurement{0x2A1C}; } +class CharacteristicProperties { +public: + const uint16_t value; + + bool broadcast() { + return static_cast(value & 0x01); + } + + bool read() { + return static_cast(value & 0x02); + } + + bool writeWithoutResponse() { + return static_cast(value & 0x04); + } + + bool write() { + return static_cast(value & 0x08); + } + + bool notify() { + return static_cast(value & 0x10); + } + + bool indicate() { + return static_cast(value & 0x20); + } + + bool authenticatedSignedWrites() { + return static_cast(value & 0x40); + } + + bool extendedProperties() { + return static_cast(value & 0x80); + } +}; + } // namespace bluetooth } // namespace trygvis diff --git a/test/ByteBufferTest.cpp b/test/ByteBufferTest.cpp index ca7a999..f8190c5 100644 --- a/test/ByteBufferTest.cpp +++ b/test/ByteBufferTest.cpp @@ -27,7 +27,7 @@ public: } ~Bytes() { - delete secret; + delete[] secret; } uint8_t* bytes; @@ -85,13 +85,13 @@ BOOST_AUTO_TEST_CASE(view) { ByteBuffer buffer(b.bytes, b.capacity); BOOST_CHECK_EQUAL(buffer.read8(), 0); - ByteBuffer view1 = buffer.view(); + ByteBuffer view1 = buffer.viewRest(); checkBuffer(view1, 999, 0); BOOST_CHECK_EQUAL(view1.read8(), 1); BOOST_CHECK_EQUAL(view1.read8(), 2); - ByteBuffer view2 = view1.view(); + ByteBuffer view2 = view1.viewRest(); checkBuffer(view2, 997, 0); BOOST_CHECK_EQUAL(view1.read8(), 3); -- cgit v1.2.3