From 91e54cf9150b37036447d423857d2bd18e4bf02b Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Wed, 5 Sep 2018 14:14:42 +0200 Subject: Major overhaul of BLE code: o Starting to remove shared_ptr. The code shouldn't be shared between threads, any thread safety will have to be built on the outside. o Better service discovery, don't fail when there are multiple requests that have to be done. o AttributeData was buggy, now it is just less than ideal. o Much better ByteBuffer. Now it is a simple view + cursor. --- CMakeLists.txt | 2 +- apps/SoilMoisture.cpp | 3 +- apps/ble-bts.cpp | 2 +- apps/ble-inspect-device.cpp | 1 + ble/Bluetooth.cpp | 126 +--------------------- ble/BluetoothImpl.h | 9 +- ble/ByteBuffer.cpp | 113 ++++++++------------ ble/LinuxBluetooth.cpp | 251 ++++++++++++++++++++++++++++++++++++-------- ble/att.cpp | 212 +++++++++++++++++++++++++++++++++++++ ble/misc.cpp | 44 ++++++++ include/ble/Bluetooth.h | 174 +----------------------------- include/ble/ByteBuffer.h | 69 +++++++----- include/ble/att.h | 186 ++++++++++++++++++++++++++++++++ include/ble/misc.h | 84 +++++++++++++++ test/ByteBufferTest.cpp | 59 ++++++----- 15 files changed, 871 insertions(+), 464 deletions(-) create mode 100644 ble/att.cpp create mode 100644 ble/misc.cpp create mode 100644 include/ble/att.h create mode 100644 include/ble/misc.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c64151..6eaa5cb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ project(ble_toys C CXX) find_package(PkgConfig) # Use Clang by default: http://stackoverflow.com/a/7031553/245614 -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -DDEBUG") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -DDEBUG") diff --git a/apps/SoilMoisture.cpp b/apps/SoilMoisture.cpp index d92752d..03aeb00 100644 --- a/apps/SoilMoisture.cpp +++ b/apps/SoilMoisture.cpp @@ -19,6 +19,7 @@ const Uuid temperature_characteristic = makeUuid(bluetooth_base_uuid, 0x2a, 0x1e const Uuid light_characteristic = makeUuid(trygvis_io_base_uuid, 0x00, 0x12); using namespace trygvis::bluetooth; +using std::to_string; static ByteBuffer createGetSensorCount() { @@ -97,7 +98,7 @@ SoilMoisture::SoilMoisture(const shared_ptr &gatt, ByteBuffer SoilMoisture::writeAndRead(const BluetoothGattCharacteristicPtr &c, ByteBuffer &requestBytes) { requestBytes.setCursor(0); - uint8_t expectedCode = requestBytes.get8(0); + uint8_t expectedCode = requestBytes.peek8(0); gatt->writeValue(c, requestBytes); diff --git a/apps/ble-bts.cpp b/apps/ble-bts.cpp index 1e76f60..82d6d43 100644 --- a/apps/ble-bts.cpp +++ b/apps/ble-bts.cpp @@ -94,7 +94,7 @@ public: cout << "bytes " << buf.getSize() << endl; for (int i = 0; i < buf.getSize(); i++) { - cout << "byte " << i << " = " << hex << buf.get8(i) << endl; + cout << "byte " << i << " = " << hex << buf.peek8(i) << endl; } } }; diff --git a/apps/ble-inspect-device.cpp b/apps/ble-inspect-device.cpp index 8f85e5c..883faed 100644 --- a/apps/ble-inspect-device.cpp +++ b/apps/ble-inspect-device.cpp @@ -26,6 +26,7 @@ public: cout << "Inspecting device: " << device->getMac().str() << endl; auto gatt = device->connectGatt(); + cout << "Connected, discovering services" << endl; gatt->discoverServices(); diff --git a/ble/Bluetooth.cpp b/ble/Bluetooth.cpp index 4160acc..2528416 100644 --- a/ble/Bluetooth.cpp +++ b/ble/Bluetooth.cpp @@ -54,124 +54,12 @@ Mac Mac::parseMac(string s) { &bytes[5], &bytes[4], &bytes[3], &bytes[2], &bytes[1], &bytes[0]); if (count != 6) { - throw BluetoothException("Unable to parse mac: " + s); + throw BluetoothException("Unable to parseAttributeData mac: " + s); } return Mac((uint8_t) bytes[5], (uint8_t) bytes[4], (uint8_t) bytes[3], (uint8_t) bytes[2], (uint8_t) bytes[1], (uint8_t) bytes[0]); } -AttPdu::AttPdu(ByteBuffer &bytes) : bytes(bytes) { -} - -AttPdu::AttPdu(ByteBuffer &bytes, AttPduType type) : bytes(bytes) { - bytes.write8(type); -} - -AttPduType AttPdu::getType() { - return (AttPduType) bytes.get8(0); -} - -void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) { - bytes.write8(AttPduType::READ_BY_GROUP_TYPE_REQ); - bytes.write16le(startHandle); - bytes.write16le(endHandle); - bytes.write16le(uuid.value); -} - -void AttPdu::makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) { - bytes.write8(AttPduType::READ_BY_TYPE_REQ); - bytes.write16le(startHandle); - bytes.write16le(endHandle); - 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) { - // cout << "bytes: " << bytes.toString(); - - auto t = static_cast(bytes.read8()); - - if (t == ERROR) { - return vector(); - } - - if (t != type) { - throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(type)); - } - - if (bytes.getSize() < 4) { - throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize())); - } - - uint8_t length = bytes.read8(); - // cout << "length=" << (int) length; - - size_t count = (bytes.getSize() - 2) / length; - // cout << "count=" << count; - - vector values; - for (int i = 0; i < count; i++) { - auto data = bytes.view(length); - // cout << "data, size=" << data.getSize() << ", bytes=" << data.toString(); - bytes.skip(length); - values.push_back(AttributeData::fromByteBuffer(data)); - } - - return values; -} - -vector AttPdu::parseReadByGroupType(ByteBuffer &bytes) { - return parse(bytes, READ_BY_GROUP_TYPE_RES); -} - -vector AttPdu::parseReadByType(ByteBuffer &bytes) { - return parse(bytes, READ_BY_TYPE_RES); -} - -static uint16_t parseExchangeMtuReq(ByteBuffer &bytes); - -void AttPdu::parseRead(ByteBuffer &bytes) { - AttPduType t = (AttPduType) bytes.read8(); - - if (t != READ_RES) { - throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(READ_RES)); - } -} - -void AttPdu::parseWrite(ByteBuffer &bytes) { - AttPduType t = (AttPduType) bytes.read8(); - - if (t != WRITE_RES) { - throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(WRITE_RES)); - } -} - -// ----------------------------------------------------------------------- -// AttributeData -// ----------------------------------------------------------------------- - -AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) { - uint16_t handle = bytes.read16le(); - - return AttributeData(handle, bytes.view()); -} - -AttributeData::AttributeData(uint16_t handle, ByteBuffer value) : - handle(handle), value(value) { -} - -AttributeData::~AttributeData() = default; - // ----------------------------------------------------------------------- // Gatt // ----------------------------------------------------------------------- @@ -216,13 +104,5 @@ shared_ptr BluetoothSystem::getAdapter(string name) { return it->second; } -Uuid makeUuid(const Uuid &base, uint8_t a, uint8_t b) { - uint8_t value[16]; - memcpy(value, base.value, 16); - value[2] = a; - value[3] = b; - return Uuid{value}; -} - -} -}; +} // namespace bluetooth +} // namespace trygvis diff --git a/ble/BluetoothImpl.h b/ble/BluetoothImpl.h index c660b4b..802c844 100644 --- a/ble/BluetoothImpl.h +++ b/ble/BluetoothImpl.h @@ -108,7 +108,7 @@ protected: template class DefaultBluetoothGattService : public BluetoothGattService { public: - DefaultBluetoothGattService(DeviceType &device, const Uuid uuid, const uint16_t handle, + DefaultBluetoothGattService(DeviceType &device, const Uuid& uuid, const uint16_t handle, const uint16_t endGroupHandle) : device(device), uuid(uuid), handle(handle), endGroupHandle(endGroupHandle) { } @@ -267,7 +267,10 @@ protected: shared_ptr getAdapterImpl(string name); -} -}; +template struct overloaded : Ts... { using Ts::operator()...; }; +template overloaded(Ts...) -> overloaded; + +} // namespace bluetooth +} // namespace trygvis #endif diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp index da61427..1a294f1 100644 --- a/ble/ByteBuffer.cpp +++ b/ble/ByteBuffer.cpp @@ -5,57 +5,38 @@ #include #include #include +#include -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()) { - ptr = const_cast(zero); -} -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); -} +using namespace std; -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 = const_cast(this->zero); +ByteBuffer ByteBuffer::alloc(std::size_t size) { + return {new uint8_t[size], size}; } -ByteBuffer::ByteBuffer(const std::shared_ptr bytes, size_t capacity, const uint8_t *zero, const uint8_t *end) : - bytes(bytes), capacity(capacity), zero(zero), end(end), ptr((uint8_t *) zero) { +ByteBuffer::ByteBuffer(uint8_t* bytes, size_t size) : + zero(bytes), end_(&bytes[size]), cursor(bytes) { } ByteBuffer &ByteBuffer::write8(uint8_t value) { - checkAndUpdateEnd(1); - (*ptr++) = value; + assertCanAccessRelative(1); + (*cursor++) = value; return *this; } ByteBuffer &ByteBuffer::write16le(uint16_t value) { - checkAndUpdateEnd(2); - (*ptr++) = (uint8_t) (value & 0xff); - (*ptr++) = (uint8_t) ((value >> 8) & 0xff); + assertCanAccessRelative(2); + (*cursor++) = (uint8_t) (value & 0xff); + (*cursor++) = (uint8_t) ((value >> 8) & 0xff); return *this; } ByteBuffer &ByteBuffer::write32le(uint32_t value) { - checkAndUpdateEnd(4); - (*ptr++) = (uint8_t) (value & 0xff); - (*ptr++) = (uint8_t) ((value >> 8) & 0xff); - (*ptr++) = (uint8_t) ((value >> 16) & 0xff); - (*ptr++) = (uint8_t) ((value >> 24) & 0xff); + 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); return *this; } @@ -64,11 +45,11 @@ ByteBuffer &ByteBuffer::write(const ByteBuffer &value) { } ByteBuffer &ByteBuffer::write(const uint8_t *bytes, size_t len) { - checkAndUpdateEnd(len); + assertCanAccessRelative(len); - memcpy(ptr, bytes, len); + memcpy(cursor, bytes, len); - ptr += len; + cursor += len; return *this; } @@ -133,29 +114,34 @@ ByteBuffer &ByteBuffer::writeFLOAT(double d) { uint8_t ByteBuffer::get8(size_t index) const { assertCanAccessRelative(index); - return ptr[index]; + return zero[index]; +} + +uint8_t ByteBuffer::peek8(size_t relative_index) const { + assertCanAccessRelative(relative_index); + return cursor[relative_index]; } uint8_t ByteBuffer::read8() { assertCanAccessRelative(0); - return *ptr++; + return *cursor++; } uint16_t ByteBuffer::read16le() { assertCanAccessRelative(1); uint16_t value; - value = *ptr++; - value |= ((uint16_t) *ptr++) << 8; + value = *cursor++; + value |= ((uint16_t) *cursor++) << 8; return value; } uint32_t ByteBuffer::read32le() { assertCanAccessRelative(3); uint32_t value; - value = *ptr++; - value |= ((uint32_t) *ptr++) << 8; - value |= ((uint32_t) *ptr++) << 16; - value |= ((uint32_t) *ptr++) << 24; + value = *cursor++; + value |= ((uint32_t) *cursor++) << 8; + value |= ((uint32_t) *cursor++) << 16; + value |= ((uint32_t) *cursor++) << 24; return value; } @@ -163,7 +149,7 @@ double ByteBuffer::readFLOAT() { uint32_t data = read32le(); int32_t mantissa = data & 0xFFFFFF; - int8_t exponent = (int8_t) (data >> 24); + auto exponent = static_cast(data >> 24); double output = 0; if (mantissa >= FLOAT::positive_infinity && @@ -188,38 +174,29 @@ double ByteBuffer::readFLOAT() { void ByteBuffer::copy(uint8_t *bytes, size_t length) const { assertCanAccessRelative(length - 1); - memcpy(bytes, ptr, length); + memcpy(bytes, cursor, length); } -ByteBuffer ByteBuffer::view() const { -// DF << "cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end - ptr << ", ptr=" << (uint64_t) ptr << ", zero=" << (uint64_t) zero; - return view(ptr, end); +void ByteBuffer::reset() { + cursor = const_cast(zero); } -ByteBuffer ByteBuffer::view(size_t length) const { - return ByteBuffer(bytes, length, ptr, ptr + length); -} - -ByteBuffer ByteBuffer::view(uint8_t *ptr, const uint8_t *end) const { - return ByteBuffer(bytes, end - ptr, ptr, end); +ByteBuffer ByteBuffer::view() const { +// DF << "cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end_ - cursor << ", cursor=" << (uint64_t) cursor << ", zero=" << (uint64_t) zero; + return {cursor, getBytesLeft()}; } -void ByteBuffer::checkAndUpdateEnd(size_t newBytes) { - uint8_t *newEnd = ptr + newBytes; - if (newEnd >= end) { - if (newEnd >= &zero[capacity]) { - throw ByteBufferException("New size is too large! cursor=" + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity) + ", new bytes=" + to_string(newBytes)); - } - end = newEnd; - } +ByteBuffer ByteBuffer::view(size_t length) const { + assertCanAccessRelative(length - 1); + return {cursor, length}; } void ByteBuffer::assertCanAccessRelative(size_t diff) const { - assertCanAccessIndex(ptr + diff); + assertCanAccessIndex(cursor + diff); } void ByteBuffer::assertCanAccessIndex(uint8_t *p) const { - if (p >= end || p < zero) { + if (p >= end_ || p < zero) { throw ByteBufferException("Out of bounds! size=" + to_string(getSize()) + ", index=" + to_string(p - zero)); } } @@ -227,7 +204,7 @@ void ByteBuffer::assertCanAccessIndex(uint8_t *p) const { std::string ByteBuffer::toString() const { std::stringstream s; - for (uint8_t *i = (uint8_t *) zero; i < end; i++) { + for (auto *i = zero; i < end_; i++) { s << hex << setfill('0') << setw(2) << (int) *i << " "; } diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp index 7e7c9dc..b546a73 100644 --- a/ble/LinuxBluetooth.cpp +++ b/ble/LinuxBluetooth.cpp @@ -6,19 +6,25 @@ #include #include #include +#include +#include +#include +#include #include #include +#include +#include // Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c #define ATT_CID 4 -#define MAX_MTU 256 - namespace trygvis { namespace bluetooth { namespace linux { using namespace uuids; +using namespace std::chrono_literals; +using std::to_string; class LinuxBluetoothGatt; @@ -30,8 +36,8 @@ class LinuxBluetoothManager; class LinuxBluetoothGattService : public DefaultBluetoothGattService { public: - 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; @@ -39,7 +45,7 @@ public: class LinuxBluetoothAdapter final : public DefaultBluetoothAdapter { public: - LinuxBluetoothAdapter(int hciDeviceId, Mac &mac); + explicit LinuxBluetoothAdapter(int hciDeviceId, Mac &mac); ~LinuxBluetoothAdapter() override; @@ -108,8 +114,21 @@ private: vector discoverCharacteristics(uint16_t startHandle, uint16_t endHandle); - ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr buffer, size_t size); + void writeL2cap(ByteBuffer &buffer); + + void writeAndRead(ByteBuffer &buffer); + + AttVariant processAvailableMessages(ByteBuffer &buffer); + + template + AttVariant exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point time); + template + T exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point time, bool fail_on_error); + + void logError(const ErrorRes&); + + uint16_t mtu = 256; int l2cap; }; @@ -120,6 +139,10 @@ string errnoAsString() { return string(strerror(errno)); }; +string errnoAsString(int err) { + return string(strerror(err)); +}; + // ----------------------------------------------------------------------- // Mac // ----------------------------------------------------------------------- @@ -223,18 +246,20 @@ void LinuxBluetoothGatt::disconnect() { close(l2cap); } -Uuid readUuid(BluetoothDevice *device, const ByteBuffer &bytes) { +Uuid readUuid(BluetoothDevice *device, ByteBuffer &bytes) { size_t bytesLeft = bytes.getBytesLeft(); if (bytesLeft == 2) { uint8_t bs[16] = BLUETOOTH_UUID_INITIALIZER; - bs[2] = bytes.get8(1); - bs[3] = bytes.get8(0); + bs[2] = bytes.read8(); + bs[3] = bytes.read8(); return Uuid(bs); } else if (bytesLeft == 16) { - return {bytes.get8(0), bytes.get8(1), bytes.get8(2), bytes.get8(3), bytes.get8(4), bytes.get8(5), bytes.get8(6), bytes.get8(7), - bytes.get8(8), bytes.get8(9), bytes.get8(10), bytes.get8(11), bytes.get8(12), bytes.get8(13), bytes.get8(14), bytes.get8(15)}; + return {bytes.read8(), bytes.read8(), bytes.read8(), bytes.read8(), + bytes.read8(), bytes.read8(), bytes.read8(), bytes.read8(), + bytes.read8(), bytes.read8(), bytes.read8(), bytes.read8(), + bytes.read8(), bytes.read8(), bytes.read8(), bytes.read8()}; } else { throw BluetoothException(device, "Unexpected bytes left: " + to_string(bytesLeft)); } @@ -243,29 +268,29 @@ Uuid readUuid(BluetoothDevice *device, const ByteBuffer &bytes) { void LinuxBluetoothGatt::writeValue(const BluetoothGattCharacteristicPtr &c, const ByteBuffer &bytes) { LOG_DEBUG("Writing to characteristic " << c->getUuid() << ": " << bytes.toString()); - shared_ptr buffer(new uint8_t[MAX_MTU]); - ByteBuffer out = ByteBuffer(buffer, MAX_MTU); + uint8_t b[mtu]; + ByteBuffer buffer{b, mtu}; - AttPdu::makeWrite(out, c->getValueHandle(), bytes); + AttPdu::makeWrite(buffer, c->getValueHandle(), bytes); - ByteBuffer in = writeAndRead(out, buffer, MAX_MTU); + writeAndRead(buffer); - AttPdu::parseWrite(in); + AttPdu::parseWrite(buffer); } ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristicPtr &c) { - shared_ptr buffer(new uint8_t[MAX_MTU]); - ByteBuffer out = ByteBuffer(buffer, MAX_MTU); + uint8_t b[mtu]; + ByteBuffer buffer{b, mtu}; - AttPdu::makeRead(out, c->getValueHandle()); + AttPdu::makeRead(buffer, c->getValueHandle()); - ByteBuffer in = writeAndRead(out, buffer, MAX_MTU); + writeAndRead(buffer); - AttPdu::parseRead(in); + AttPdu::parseRead(buffer); // D << "READ response has " + to_string(in.getBytesLeft()) + " bytes"; - auto response = in.view(); + auto response = buffer.view(); LOG_DEBUG("Value of characteristic " << c->getUuid() << "=" << response.toString()); @@ -277,6 +302,8 @@ void LinuxBluetoothGatt::discoverServices() { removeServices(); + LOG_DEBUG("Discovering services"); + do { vector values = discoverServices(startHandle); @@ -290,9 +317,9 @@ void LinuxBluetoothGatt::discoverServices() { for (auto &data: values) { endGroupHandle = data.value.read16le(); -// D << "handle: 0x" << hex << setw(4) << setfill('0') << data.handle << -// ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle << -// ", value: " << data.value.toString(); + LOG_DEBUG("service handle: 0x" << hex << setw(4) << setfill('0') << data.handle << + ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle << + ", value: " << data.value.toString()); auto u = readUuid(&device, data.value); @@ -353,11 +380,11 @@ void LinuxBluetoothGatt::discoverServices() { } while (startHandle != 0xffff); } -ByteBuffer LinuxBluetoothGatt::writeAndRead(ByteBuffer &out, shared_ptr buffer, size_t size) { +void LinuxBluetoothGatt::writeAndRead(ByteBuffer &buffer) { // LOG_DEBUG("pdu size=" << out.getCursor()); - auto to_be_written = out.getCursor(); + auto to_be_written = buffer.getCursor(); - ssize_t written = write(l2cap, buffer.get(), 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 " + @@ -366,28 +393,152 @@ ByteBuffer LinuxBluetoothGatt::writeAndRead(ByteBuffer &out, shared_ptr // LOG_DEBUG("written=" << written); - ssize_t r = read(l2cap, buffer.get(), size); + buffer.reset(); + ssize_t r = read(l2cap, buffer.begin(), buffer.getBytesLeft()); if (r == -1) { throw BluetoothException(&device, "read(): " + errnoAsString()); } - auto in = ByteBuffer(buffer, (size_t) r, (size_t) r); +// LOG_DEBUG("read: " << r << " bytes: " << buffer.toString()); +} + +void LinuxBluetoothGatt::writeL2cap(ByteBuffer &buffer) { + auto to_be_written = buffer.getCursor(); + 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(); +} + +AttVariant LinuxBluetoothGatt::processAvailableMessages(ByteBuffer &buffer) { + ssize_t bytes_available = 1; + + int ret = ioctl(l2cap, FIONREAD, &bytes_available); +// 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()); + if (r == -1) { + throw BluetoothException(&device, "read(): " + errnoAsString()); + } + + auto packet_buffer = buffer.view(size_t(r)); + auto type = static_cast(packet_buffer.get8(0)); + LOG_DEBUG("Got ATT message " << to_string(type) << ", size: " << to_string(r)); + + if (type == AttPduType::READ_BY_GROUP_TYPE_RES) { + LOG_DEBUG("READ_BY_GROUP_TYPE_REQ!!"); + } + + auto v = AttPdu::parse(packet_buffer); + + if (holds_alternative(v)) { + auto req = get(v); + LOG_DEBUG("Got exchange MTU request, clientRxMtu=" + to_string(req.clientRxMtu)); + + this->mtu = std::min(123, req.clientRxMtu); + + AttPdu::makeExchangeMtuRes(buffer, mtu); + writeL2cap(buffer); + } else { + return v; + } + + ret = ioctl(l2cap, FIONREAD, &bytes_available); +// LOG_DEBUG("ret=" << to_string(ret) << "/" << errnoAsString(ret) << +// ", bytes available: " << to_string(bytes_available)); + } + + return monostate{}; +} + +static BluetoothException makeUnexpectedTypeException(const AttVariant &variant) { + auto otype = attPduType(variant); + auto msg = otype ? to_string(otype.value()) : "Unknown"; + return BluetoothException("Unexpected message type: " + msg); +} + +template +static +void assertIsVariant(const AttVariant &variant) { + if(holds_alternative(variant)) { + return; + } + + throw makeUnexpectedTypeException(variant); +}; + +void LinuxBluetoothGatt::logError(const ErrorRes &error) { + LOG_WARN(std::hex << std::showbase << std::setw(2) << + "Got error: request opcode=" << int(error.requestOpcode) << + std::setw(4) << ", attribute handle=" << error.attributeHandle << + std::setw(2) << ", error code=" << int(error.errorCode)); +} + +template +T LinuxBluetoothGatt::exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point time, bool fail_on_error) { + auto v = exchangeAndWaitFor(buffer, time); + + if (holds_alternative(v)) { + return get(v); + } + + if (fail_on_error && holds_alternative(v)) { + return get(v); + } + + throw makeUnexpectedTypeException(v); +} + +template +AttVariant LinuxBluetoothGatt::exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point time) { + uint8_t bytes[mtu]; + ByteBuffer buffer2{bytes, mtu}; + auto v = processAvailableMessages(buffer2); + assertIsVariant(v); + + LOG_DEBUG("Writing message " << to_string(static_cast(buffer.get8(0)))); + this->writeL2cap(buffer); + + fd_set rfds; + struct timeval tv{}; + + FD_ZERO(&rfds); + FD_SET(l2cap, &rfds); + + auto start = clock::now(); + auto seconds = std::chrono::duration_cast(time - start); + auto ms = std::chrono::duration_cast(time - start) - seconds; + + tv.tv_sec = seconds.count(); + tv.tv_usec = ms.count(); -// LOG_DEBUG("read: " << r << " bytes: " << in.toString()); + int ret = select(l2cap + 1, &rfds, nullptr, nullptr, &tv); + if (ret == -1) { + throw BluetoothException("select() failed."); + } else if (ret == 0) { + disconnect(); - return in; + throw BluetoothException("Timeout while waiting for correct response."); + } + + return processAvailableMessages(buffer); } vector LinuxBluetoothGatt::discoverServices(uint16_t startHandle) { - shared_ptr buffer(new uint8_t[MAX_MTU]); - ByteBuffer out = ByteBuffer(buffer, MAX_MTU); + uint8_t bytes[mtu]; + ByteBuffer buffer{bytes, mtu}; - AttPdu::makeReadByGroupType(out, startHandle, 0xffff, uuids::PRIMARY_SERVICE); + AttPdu::makeReadByGroupType(buffer, startHandle, 0xffff, uuids::PRIMARY_SERVICE); - ByteBuffer in = writeAndRead(out, buffer, MAX_MTU); + auto res = exchangeAndWaitFor(buffer, std::chrono::system_clock::now() + 10s, true); - vector values = AttPdu::parseReadByGroupType(in); + auto values = res.attributes; LOG_DEBUG("READ_BY_GROUP_TYPE response has " + to_string(values.size()) + " values"); @@ -395,18 +546,30 @@ vector LinuxBluetoothGatt::discoverServices(uint16_t startHandle) } vector LinuxBluetoothGatt::discoverCharacteristics(uint16_t startHandle, uint16_t endHandle) { - shared_ptr buffer(new uint8_t[MAX_MTU]); - ByteBuffer out = ByteBuffer(buffer, MAX_MTU); + uint8_t bytes[mtu]; + ByteBuffer buffer{bytes, mtu}; - AttPdu::makeReadByType(out, startHandle, endHandle, uuids::CHARACTERISTIC); + AttPdu::makeReadByType(buffer, startHandle, endHandle, uuids::CHARACTERISTIC); - ByteBuffer in = writeAndRead(out, buffer, MAX_MTU); + auto res = exchangeAndWaitFor(buffer, std::chrono::system_clock::now() + 10s); - vector values = AttPdu::parseReadByType(in); + if(holds_alternative(res)) { + auto values = get(res).attributes; + LOG_DEBUG("READ_BY_TYPE response has " + to_string(values.size()) + " values"); + return values; + } - LOG_DEBUG("READ_BY_TYPE response has " + to_string(values.size()) + " values"); + if(holds_alternative(res)) { + ErrorRes err = get(res); - return values; + if (err.errorCode == ErrorCode::ATTRIBUTE_NOT_FOUND) { + return {}; + } + + logError(err); + } + + throw makeUnexpectedTypeException(res); } // ----------------------------------------------------------------------- diff --git a/ble/att.cpp b/ble/att.cpp new file mode 100644 index 0000000..585e914 --- /dev/null +++ b/ble/att.cpp @@ -0,0 +1,212 @@ +#include + +#include "ble/att.h" +#include "ble/Bluetooth.h" +#include + +namespace trygvis { +namespace bluetooth { + +using namespace std; +using std::to_string; + +std::string to_string(AttPduType t) { + switch (t) { + case ERROR: + return "ERROR"; + case EXCHANGE_MTU_REQ: + return "EXCHANGE_MTU_REQ"; + case EXCHANGE_MTU_RES: + return "EXCHANGE_MTU_RES"; + case FIND_INFORMATION_REQ: + return "FIND_INFORMATION_REQ"; + case FIND_INFORMATION_RES: + return "FIND_INFORMATION_RES"; + case FIND_BY_TYPE_VALUE_REQ: + return "FIND_BY_TYPE_VALUE_REQ"; + case FIND_BY_TYPE_VALUE_RES: + return "FIND_BY_TYPE_VALUE_RES"; + case READ_BY_TYPE_REQ: + return "READ_BY_TYPE_REQ"; + case READ_BY_TYPE_RES: + return "READ_BY_TYPE_RES"; + case READ_REQ: + return "READ_REQ"; + case READ_RES: + return "READ_RES"; + case READ_BLOB_REQ: + return "READ_BLOB_REQ"; + case READ_BLOB_RES: + return "READ_BLOB_RES"; + case READ_MULTIPLE_REQ: + return "READ_MULTIPLE_REQ"; + case READ_MULTIPLE_RES: + return "READ_MULTIPLE_RES"; + case READ_BY_GROUP_TYPE_REQ: + return "READ_BY_GROUP_TYPE_REQ"; + case READ_BY_GROUP_TYPE_RES: + return "READ_BY_GROUP_TYPE_RES"; + case WRITE_REQ: + return "WRITE_REQ"; + case WRITE_RES: + return "WRITE_RES"; + case WRITE_CMD: + return "WRITE_CMD"; + case PREPARE_WRITE_REQ: + return "PREPARE_WRITE_REQ"; + case PREPARE_WRITE_RES: + return "PREPARE_WRITE_RES"; + case EXECUTE_WRITE_REQ: + return "EXECUTE_WRITE_REQ"; + case EXECUTE_WRITE_RES: + return "EXECUTE_WRITE_RES"; + case HANDLE_VALUE_NOTIFICATION: + return "HANDLE_VALUE_NOTIFICATION"; + case HANDLE_VALUE_INDICATION: + return "HANDLE_VALUE_INDICATION"; + case HANDLE_VALUE_CONFIRMATION: + return "HANDLE_VALUE_CONFIRMATION"; + case SIGNED_WRITE_COMMAND: + return "SIGNED_WRITE_COMMAND"; + default: + return "UNKNOWN"; + } +} + +o attPduType(const AttVariant &v) { + return visit([](auto &&arg) -> o{ + using T = std::decay_t; + + if constexpr (std::is_same_v) { + return {}; + } else { + return {T::att_pdu_type}; + } + }, v); +} + +AttPdu::AttPdu(ByteBuffer &bytes) : bytes(bytes) { +} + +AttPdu::AttPdu(ByteBuffer &bytes, AttPduType type) : bytes(bytes) { + bytes.write8(type); +} + +AttPduType AttPdu::getType() { + return (AttPduType) bytes.peek8(0); +} + +void AttPdu::makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu) +{ + bytes.write8(AttPduType::EXCHANGE_MTU_RES). + write16le(serverRxMtu); +} + +void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) { + bytes.write8(AttPduType::READ_BY_GROUP_TYPE_REQ); + bytes.write16le(startHandle); + bytes.write16le(endHandle); + bytes.write16le(uuid.value); +} + +void AttPdu::makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) { + bytes.write8(AttPduType::READ_BY_TYPE_REQ); + bytes.write16le(startHandle); + bytes.write16le(endHandle); + 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::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())); + } + + uint8_t length = bytes.read8(); + + size_t count = (bytes.getSize() - 2) / length; + + vector values; + for (int i = 0; i < count; i++) { + auto data = bytes.viewAndSkip(length); + auto&& x = AttributeData::fromByteBuffer(data); + values.emplace_back(x); + } + + return values; +} + +AttVariant AttPdu::parse(ByteBuffer &bytes) { + switch (static_cast(bytes.read8())) { + case AttPduType::ERROR: + return ErrorRes::parse(bytes); + case AttPduType::EXCHANGE_MTU_REQ: + return parseExchangeMtuReq(bytes); + case AttPduType::EXCHANGE_MTU_RES: + return parseExchangeMtuRes(bytes); + case AttPduType::READ_BY_GROUP_TYPE_RES: + return ReadByGroupTypeRes{parseAttributeData(bytes)}; + case AttPduType::READ_BY_TYPE_RES: + return ReadByTypeRes{parseAttributeData(bytes)}; + default: + return {}; + } +} + +ExchangeMtuReq AttPdu::parseExchangeMtuReq(ByteBuffer &bytes) { + return {bytes.read16le()}; +} + +ExchangeMtuRes AttPdu::parseExchangeMtuRes(ByteBuffer &bytes) { + return {bytes.read16le()}; +} + +void AttPdu::parseRead(ByteBuffer &bytes) { + auto t = static_cast(bytes.read8()); + + if (t != READ_RES) { + throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(READ_RES)); + } +} + +void AttPdu::parseWrite(ByteBuffer &bytes) { + auto t = static_cast(bytes.read8()); + + if (t != WRITE_RES) { + throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(WRITE_RES)); + } +} + +// ----------------------------------------------------------------------- +// AttributeData +// ----------------------------------------------------------------------- + +AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) { + uint16_t handle = bytes.read16le(); + + auto raw = make_unique(bytes.getBytesLeft()); + + bytes.copy(raw.get(), bytes.getBytesLeft()); + + ByteBuffer buffer = ByteBuffer{raw.get(), bytes.getBytesLeft()}; + return AttributeData(handle, buffer, std::move(raw)); +} + +AttributeData::AttributeData(uint16_t handle, ByteBuffer value, std::shared_ptr raw) : + handle(handle), value(value), raw(std::move(raw)) { +} + +AttributeData::~AttributeData() = default; + +} // namespace bluetooth +} // namespace trygvis diff --git a/ble/misc.cpp b/ble/misc.cpp new file mode 100644 index 0000000..afd89cb --- /dev/null +++ b/ble/misc.cpp @@ -0,0 +1,44 @@ +#include "ble/misc.h" + +#include +#include +#include +#include + + +namespace trygvis { +namespace bluetooth { + +std::ostream &operator<<(std::ostream &s, Uuid const &uuid) { + s << uuid.str(); + return s; +} + +std::string Uuid::str() const { + auto v = value; + + std::stringstream s; + s << std::hex << std::setw(2) << + int(v[0]) << int(v[1]) << ":" << + int(v[2]) << int(v[3]) << ":" << + int(v[4]) << int(v[5]) << ":" << + int(v[6]) << int(v[7]) << ":" << + int(v[8]) << int(v[9]) << ":" << + int(v[10]) << int(v[11]) << ":" << + int(v[12]) << int(v[13]) << ":" << + int(v[14]) << int(v[15]) << ":" << + std::resetiosflags(std::ios_base::basefield); + + return s.str(); +} + +Uuid makeUuid(const Uuid &base, uint8_t a, uint8_t b) { + uint8_t value[16]; + memcpy(value, base.value, 16); + value[2] = a; + value[3] = b; + return Uuid{value}; +} + +} // namespace bluetooth +} // namespace trygvis diff --git a/include/ble/Bluetooth.h b/include/ble/Bluetooth.h index abddc94..477bd29 100644 --- a/include/ble/Bluetooth.h +++ b/include/ble/Bluetooth.h @@ -11,7 +11,8 @@ #include #include -#include "ByteBuffer.h" +#include "ble/ByteBuffer.h" +#include "ble/att.h" namespace trygvis { namespace bluetooth { @@ -20,74 +21,6 @@ using namespace std; template using o = std::experimental::optional; -struct Uuid { - uint8_t value[16]; - - explicit Uuid(uint8_t value[16]) noexcept : value() { - memcpy(this->value, value, 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} {} - - bool operator==(const Uuid &other) { - return std::memcmp(value, other.value, 16) == 0; - } - - bool operator==(const Uuid &other) const { - return std::memcmp(value, other.value, 16) == 0; - } - - friend std::ostream &operator<<(std::ostream &s, Uuid const &uuid) { - auto &v = uuid.value; - s << std::hex << - v[0] << v[1] << ":" << - v[2] << v[3] << ":" << - v[4] << v[5] << ":" << - v[6] << v[7] << ":" << - v[8] << v[9] << ":" << - v[10] << v[11] << ":" << - v[12] << v[13] << ":" << - v[14] << v[15] << ":" << - std::endl; - return s; - } - - static Uuid fromShort(uint8_t b2, uint8_t b3) { - return {0x00, 0x00, b2, b3, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb}; - } -}; - -struct ShortUuid { -private: -public: - explicit ShortUuid(uint16_t value) : value(value) {} - - Uuid toLong() - { - auto b2 = static_cast(value >> 8); - auto b3 = static_cast(value & 0xff); - return Uuid::fromShort(b2, b3); - } - - uint16_t value; -}; - - -namespace uuids { - -const ShortUuid HealthTermometerService{0x1809}; -const ShortUuid DeviceInformationService{0x180a}; -const ShortUuid BatteryService{0x180f}; - -const ShortUuid PRIMARY_SERVICE{0x2800}; -const ShortUuid SECONDARY_SERVICE{0x2801}; -const ShortUuid CHARACTERISTIC{0x2803}; - -const ShortUuid TemperatureMeasurement{0x2A1C}; -} - class BluetoothAdapter; class BluetoothDevice; @@ -102,18 +35,6 @@ typedef shared_ptr BluetoothGattPtr; typedef shared_ptr BluetoothGattCharacteristicPtr; typedef shared_ptr BluetoothGattServicePtr; -class BluetoothException : public runtime_error { -public: - BluetoothException(const BluetoothAdapter *adapter, string const &what) : runtime_error(what), adapter(adapter), device(nullptr) {} - - BluetoothException(const BluetoothDevice *device, string const &what) : runtime_error(what), adapter(nullptr), device(device) {} - - explicit BluetoothException(string const &what) : runtime_error(what), adapter(nullptr), device(nullptr) {} - - const BluetoothAdapter *adapter; - const BluetoothDevice *device; -}; - class Mac { public: explicit Mac(uint8_t _5, uint8_t _4, uint8_t _3, uint8_t _2, uint8_t _1, uint8_t _0) : bytes() { @@ -269,94 +190,7 @@ private: map> adapters; }; -/** - * BLUETOOTH SPECIFICATION Version 4.0 [Vol 3] - Attribute Protocol (ATT) - 3.4.8 Attribute Opcode Summary - * Table 3.37 - */ -enum AttPduType { - ERROR = 0x01, - EXCHANGE_MTU_REQ = 0x02, - EXCHANGE_MTU_RES = 0x03, - FIND_INFORMATION_REQ = 0x04, - FIND_INFORMATION_RES = 0x05, - FIND_BY_TYPE_VALUE_REQ = 0x06, - FIND_BY_TYPE_VALUE_RES = 0x07, - READ_BY_TYPE_REQ = 0x08, - READ_BY_TYPE_RES = 0x09, - READ_REQ = 0x0a, - READ_RES = 0x0b, - READ_BLOB_REQ = 0x0c, - READ_BLOB_RES = 0x0d, - READ_MULTIPLE_REQ = 0x0e, - READ_MULTIPLE_RES = 0x0f, - READ_BY_GROUP_TYPE_REQ = 0x10, - READ_BY_GROUP_TYPE_RES = 0x11, - WRITE_REQ = 0x12, - WRITE_RES = 0x13, - - WRITE_CMD = 0x52, - PREPARE_WRITE_REQ = 0x16, - PREPARE_WRITE_RES = 0x17, - EXECUTE_WRITE_REQ = 0x18, - EXECUTE_WRITE_RES = 0x19, - HANDLE_VALUE_NOTIFICATION = 0x1b, - HANDLE_VALUE_INDICATION = 0x1d, - HANDLE_VALUE_CONFIRMATION = 0x1e, - SIGNED_WRITE_COMMAND = 0xd2, -}; - -class AttributeData; - -class AttPdu { -public: - explicit AttPdu(ByteBuffer &bytes); - - AttPdu(ByteBuffer &bytes, AttPduType type); - - AttPduType getType(); - - static vector parseReadByGroupType(ByteBuffer &bytes); - - static vector parseReadByType(ByteBuffer &bytes); - - static uint16_t parseExchangeMtuReq(ByteBuffer &bytes); - - static void parseRead(ByteBuffer &bytes); - - static void parseWrite(ByteBuffer &bytes); - - 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); - - 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); - - static vector parse(ByteBuffer &bytes, AttPduType type); - - ByteBuffer &bytes; -}; - -class AttributeData { -public: - ~AttributeData(); - - static AttributeData fromByteBuffer(ByteBuffer &value); - - const uint16_t handle; - ByteBuffer value; - -private: - AttributeData(uint16_t handle, ByteBuffer value); -}; - -Uuid makeUuid(const Uuid& base, uint8_t a, uint8_t b); - -} -} +} // namespace bluetooth +} // namespace trygvis #endif diff --git a/include/ble/ByteBuffer.h b/include/ble/ByteBuffer.h index 1032187..5e904c2 100644 --- a/include/ble/ByteBuffer.h +++ b/include/ble/ByteBuffer.h @@ -33,43 +33,53 @@ static const double precision = 10000000; class ByteBufferException : public std::runtime_error { public: - ByteBufferException(std::string const &what) : std::runtime_error(what) { - } + explicit ByteBufferException(std::string const &what) : std::runtime_error(what) {} }; 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 size); + template + explicit ByteBuffer(uint8_t (&bytes)[N]) : zero(bytes), end_(&bytes[N]), cursor(bytes) {} - ByteBuffer(const std::shared_ptr bytes, size_t capacity, size_t size, size_t zero); + ByteBuffer(uint8_t* bytes, size_t capacity); inline size_t getSize() const { - return end - zero; - } - - inline size_t getCapacity() const { - return capacity; + return end_ - zero; } inline size_t getCursor() const { - return ptr - zero; + return cursor - zero; } inline size_t getBytesLeft() const { - return end - ptr; + return end_ - cursor; } inline ByteBuffer &setCursor(size_t newCursor) { - ptr = (uint8_t *) &zero[newCursor]; + cursor = (uint8_t *) &zero[newCursor]; return *this; } inline void skip(size_t length) { - ptr += length; + cursor += length; + } + + inline const uint8_t *cbegin() { + return zero; + } + + inline const uint8_t *cend() { + return end_; + } + + inline uint8_t *begin() { + return const_cast(zero); + } + + inline uint8_t *end() { + return const_cast(end_); } ByteBuffer &write8(uint8_t value); @@ -87,8 +97,16 @@ public: ByteBuffer &writeFLOAT(double d); + /** + * Reads a uint8_t from the start of the buffer. + */ uint8_t get8(size_t index) const; + /** + * Reads a uint8_t relative to the pointer. + */ + uint8_t peek8(size_t relative_index) const; + uint8_t read8(); uint16_t read16le(); @@ -102,32 +120,31 @@ public: void copy(uint8_t *bytes, size_t length) const; + void reset(); + /** * Creates a view from cursor to size. */ ByteBuffer view() const; - // TODO: should return const ByteBuffer view(size_t length) const; + ByteBuffer viewAndSkip(size_t length) { + auto v = view(length); + skip(length); + return v; + } + std::string toString() const; private: - ByteBuffer(const std::shared_ptr bytes, size_t capacity, const uint8_t *zero, const uint8_t *end); - - ByteBuffer view(uint8_t *ptr, const uint8_t *end) const; - - void checkAndUpdateEnd(size_t count); - void assertCanAccessRelative(size_t diff) const; void assertCanAccessIndex(uint8_t *p) const; - const std::shared_ptr bytes; - const size_t capacity; const uint8_t *zero; - const uint8_t *end; - uint8_t *ptr; + const uint8_t *end_; + uint8_t *cursor; }; #endif diff --git a/include/ble/att.h b/include/ble/att.h new file mode 100644 index 0000000..3c7914f --- /dev/null +++ b/include/ble/att.h @@ -0,0 +1,186 @@ +#pragma once + +#include +#include +#include + +#include "ble/ByteBuffer.h" +#include "ble/misc.h" + +namespace trygvis { +namespace bluetooth { + +template +using o = std::experimental::optional; + +/** + * BLUETOOTH SPECIFICATION Version 4.0 [Vol 3] - Attribute Protocol (ATT) - 3.4.8 Attribute Opcode Summary + * Table 3.37 + */ +enum AttPduType { + ERROR = 0x01, + EXCHANGE_MTU_REQ = 0x02, + EXCHANGE_MTU_RES = 0x03, + FIND_INFORMATION_REQ = 0x04, + FIND_INFORMATION_RES = 0x05, + FIND_BY_TYPE_VALUE_REQ = 0x06, + FIND_BY_TYPE_VALUE_RES = 0x07, + READ_BY_TYPE_REQ = 0x08, + READ_BY_TYPE_RES = 0x09, + READ_REQ = 0x0a, + READ_RES = 0x0b, + READ_BLOB_REQ = 0x0c, + READ_BLOB_RES = 0x0d, + READ_MULTIPLE_REQ = 0x0e, + READ_MULTIPLE_RES = 0x0f, + READ_BY_GROUP_TYPE_REQ = 0x10, + READ_BY_GROUP_TYPE_RES = 0x11, + WRITE_REQ = 0x12, + WRITE_RES = 0x13, + + WRITE_CMD = 0x52, + PREPARE_WRITE_REQ = 0x16, + PREPARE_WRITE_RES = 0x17, + EXECUTE_WRITE_REQ = 0x18, + EXECUTE_WRITE_RES = 0x19, + HANDLE_VALUE_NOTIFICATION = 0x1b, + HANDLE_VALUE_INDICATION = 0x1d, + HANDLE_VALUE_CONFIRMATION = 0x1e, + SIGNED_WRITE_COMMAND = 0xd2, +}; + +std::string to_string(AttPduType t); + +class AttributeData; + +/** + * Application Error 0x80 – 0xFF Application error code defined by a higher layer specification. + * + * BLUETOOTH SPECIFICATION Version 4.0 [Vol 3] - 3.4.1.1 Error Response - Table 3.3 + */ +enum ErrorCode : uint8_t { + INVALID_HANDLE = 0x01, ///< The attribute handle given was not valid on this server. + READ_NOT_PERMITTED = 0x02,///< The attribute cannot be read. + WRITE_NOT_PERMITTED = 0x03,///< The attribute cannot be written. + INVALID_PDU = 0x04, /// attributes; +}; + +struct ReadByTypeRes { + static constexpr auto att_pdu_type = AttPduType::READ_BY_TYPE_RES; + + std::vector attributes; +}; + +using AttVariant = std::variant; + +o attPduType(const AttVariant& v); + +class AttPdu { +public: + explicit AttPdu(ByteBuffer &bytes); + + AttPdu(ByteBuffer &bytes, AttPduType type); + + AttPduType getType(); + + static AttVariant parse(ByteBuffer &bytes); + + static ExchangeMtuReq parseExchangeMtuReq(ByteBuffer &bytes); + static ExchangeMtuRes parseExchangeMtuRes(ByteBuffer &bytes); + + static void parseRead(ByteBuffer &bytes); + + static void parseWrite(ByteBuffer &bytes); + + static void makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu); + + 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); + + 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); + + static std::vector parseAttributeData(ByteBuffer &bytes); + + ByteBuffer &bytes; +}; + +class AttributeData { +public: + ~AttributeData(); + + static AttributeData fromByteBuffer(ByteBuffer &value); + + const uint16_t handle; + ByteBuffer value; + +private: + AttributeData(uint16_t handle, ByteBuffer value, std::shared_ptr raw); + + std::shared_ptr raw; +}; + +} // namespace bluetooth +} // namespace trygvis diff --git a/include/ble/misc.h b/include/ble/misc.h new file mode 100644 index 0000000..089c3a1 --- /dev/null +++ b/include/ble/misc.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#include +#include + +namespace trygvis { +namespace bluetooth { + +class BluetoothAdapter; +class BluetoothDevice; + +class BluetoothException : public std::runtime_error { +public: + BluetoothException(const BluetoothAdapter *adapter, std::string const &what) : runtime_error(what), adapter(adapter), device(nullptr) {} + + BluetoothException(const BluetoothDevice *device, std::string const &what) : runtime_error(what), adapter(nullptr), device(device) {} + + explicit BluetoothException(std::string const &what) : runtime_error(what), adapter(nullptr), device(nullptr) {} + + const BluetoothAdapter *adapter; + const BluetoothDevice *device; +}; + +struct Uuid { + uint8_t value[16]; + + explicit Uuid(uint8_t value[16]) noexcept : value() { + std::memcpy(this->value, value, 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} {} + + bool operator==(const Uuid &other) { + return std::memcmp(value, other.value, 16) == 0; + } + + bool operator==(const Uuid &other) const { + return std::memcmp(value, other.value, 16) == 0; + } + + friend std::ostream &operator<<(std::ostream &s, Uuid const &uuid); + + std::string str() const; + + static Uuid fromShort(uint8_t b2, uint8_t b3) { + return {0x00, 0x00, b2, b3, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb}; + } +}; + +struct ShortUuid { +private: +public: + explicit ShortUuid(uint16_t value) : value(value) {} + + Uuid toLong() + { + auto b2 = static_cast(value >> 8); + auto b3 = static_cast(value & 0xff); + return Uuid::fromShort(b2, b3); + } + + uint16_t value; +}; + +Uuid makeUuid(const Uuid& base, uint8_t a, uint8_t b); + +namespace uuids { + +const ShortUuid HealthTermometerService{0x1809}; +const ShortUuid DeviceInformationService{0x180a}; +const ShortUuid BatteryService{0x180f}; + +const ShortUuid PRIMARY_SERVICE{0x2800}; +const ShortUuid SECONDARY_SERVICE{0x2801}; +const ShortUuid CHARACTERISTIC{0x2803}; + +const ShortUuid TemperatureMeasurement{0x2A1C}; +} + +} // namespace bluetooth +} // namespace trygvis diff --git a/test/ByteBufferTest.cpp b/test/ByteBufferTest.cpp index 4a612d2..ca7a999 100644 --- a/test/ByteBufferTest.cpp +++ b/test/ByteBufferTest.cpp @@ -6,36 +6,41 @@ #include -#define checkBuffer(buffer, capacity, size, cursor) \ - if(false) {cout << "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) - using namespace std; +static void checkBuffer(const ByteBuffer& buffer, size_t size, size_t cursor) { + if (false) { + cout << "size=" << buffer.getSize() << ", cursor=" << buffer.getCursor(); + } + BOOST_CHECK_EQUAL(buffer.getSize(), size); + BOOST_CHECK_EQUAL(buffer.getCursor(), cursor); +} + class Bytes { public: - Bytes(size_t size) : capacity(size) { - uint8_t *secret = new uint8_t[size + 0x100]; + explicit Bytes(size_t size) : capacity(size), secret(new uint8_t[size + 0x100]) { auto tmp = (uint8_t *) (((uint64_t) &secret[0x100]) & 0xffffffffffffff00); for (int i = 0; i < size; i++) { tmp[i] = (uint8_t) i; } + bytes = tmp; + } - bytes = shared_ptr(tmp, [secret](uint8_t *) { - delete[]secret; - }); + ~Bytes() { + delete secret; } - shared_ptr bytes; + + uint8_t* bytes; size_t capacity; + + uint8_t* secret; }; BOOST_AUTO_TEST_CASE(empty_buffer) { Bytes b(0); ByteBuffer buffer(b.bytes, b.capacity); - checkBuffer(buffer, 0, 0, 0); + checkBuffer(buffer, 0, 0); try { buffer.read8(); @@ -46,48 +51,48 @@ BOOST_AUTO_TEST_CASE(empty_buffer) { BOOST_AUTO_TEST_CASE(basic) { Bytes b(1000); - ByteBuffer buffer(b.bytes, 1000, 1000); - checkBuffer(buffer, 1000, 1000, 0); + ByteBuffer buffer(b.bytes, 1000); + checkBuffer(buffer, 1000, 0); BOOST_CHECK_EQUAL(buffer.read8(), 0); - checkBuffer(buffer, 1000, 1000, 1); + checkBuffer(buffer, 1000, 1); for (int i = 1; i < b.capacity; i++) { - BOOST_CHECK_EQUAL(buffer.read8(), b.bytes.get()[i]); + BOOST_CHECK_EQUAL(buffer.read8(), b.bytes[i]); } } BOOST_AUTO_TEST_CASE(setCursor) { Bytes b(1000); - ByteBuffer buffer(b.bytes, 1000, 10, 0); - checkBuffer(buffer, 1000, 10, 0); + ByteBuffer buffer(b.bytes, 1000); + checkBuffer(buffer, 1000, 0); BOOST_CHECK_EQUAL(buffer.read8(), 0); - checkBuffer(buffer, 1000, 10, 1); + checkBuffer(buffer, 1000, 1); buffer.setCursor(0); - checkBuffer(buffer, 1000, 10, 0); + checkBuffer(buffer, 1000, 0); BOOST_CHECK_EQUAL(buffer.read8(), 0); - checkBuffer(buffer, 1000, 10, 1); + checkBuffer(buffer, 1000, 1); buffer.setCursor(9); - checkBuffer(buffer, 1000, 10, 9); + checkBuffer(buffer, 1000, 9); } BOOST_AUTO_TEST_CASE(view) { Bytes b(1000); - ByteBuffer buffer(b.bytes, b.capacity, 10, 0); + ByteBuffer buffer(b.bytes, b.capacity); BOOST_CHECK_EQUAL(buffer.read8(), 0); ByteBuffer view1 = buffer.view(); - checkBuffer(view1, 9, 9, 0); + checkBuffer(view1, 999, 0); BOOST_CHECK_EQUAL(view1.read8(), 1); BOOST_CHECK_EQUAL(view1.read8(), 2); ByteBuffer view2 = view1.view(); - checkBuffer(view2, 7, 7, 0); + checkBuffer(view2, 997, 0); BOOST_CHECK_EQUAL(view1.read8(), 3); } @@ -119,7 +124,7 @@ void c(double input, uint32_t hx, double sigma = 0.00001) { double output = buffer.readFLOAT(); stringstream str; - auto actual_hex = ((uint32_t *) b.bytes.get())[0]; + auto actual_hex = ((uint32_t *) b.bytes)[0]; str << "input=" << setw(20) << setprecision(10) << input << ", output=" << setw(20) << setprecision(10) << output << ", diff=" << setw(20) << setprecision(10) << fabs(input - output) << -- cgit v1.2.3