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. --- ble/Bluetooth.cpp | 4 ++ ble/BluetoothImpl.h | 55 +++++++++++++++- ble/ByteBuffer.cpp | 85 +++++++++++++------------ ble/LinuxBluetooth.cpp | 169 ++++++++++++++++++++++++++++++++++--------------- ble/att.cpp | 58 ++++++++++++++--- 5 files changed, 270 insertions(+), 101 deletions(-) (limited to 'ble') 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 -- cgit v1.2.3