diff options
Diffstat (limited to 'ble')
| -rw-r--r-- | ble/Bluetooth.cpp | 126 | ||||
| -rw-r--r-- | ble/BluetoothImpl.h | 9 | ||||
| -rw-r--r-- | ble/ByteBuffer.cpp | 113 | ||||
| -rw-r--r-- | ble/LinuxBluetooth.cpp | 251 | ||||
| -rw-r--r-- | ble/att.cpp | 212 | ||||
| -rw-r--r-- | ble/misc.cpp | 44 |
6 files changed, 517 insertions, 238 deletions
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<AttributeData> AttPdu::parse(ByteBuffer &bytes, AttPduType type) { - // cout << "bytes: " << bytes.toString(); - - auto t = static_cast<AttPduType>(bytes.read8()); - - if (t == ERROR) { - return vector<AttributeData>(); - } - - 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<AttributeData> 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<AttributeData> AttPdu::parseReadByGroupType(ByteBuffer &bytes) { - return parse(bytes, READ_BY_GROUP_TYPE_RES); -} - -vector<AttributeData> 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<BluetoothAdapter> 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<typename DeviceType> 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 <BluetoothAdapter> getAdapterImpl(string name); -} -}; +template<class... Ts> struct overloaded : Ts... { using Ts::operator()...; }; +template<class... Ts> overloaded(Ts...) -> overloaded<Ts...>; + +} // 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 <iomanip> #include <cmath> #include <iostream> +#include <ble/ByteBuffer.h> -using namespace std; - -ByteBuffer ByteBuffer::alloc(std::size_t capacity) { - auto bytes = shared_ptr<uint8_t>(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<uint8_t> bytes, size_t capacity) : - bytes(bytes), capacity(capacity), zero(bytes.get()), end(bytes.get()) { - ptr = const_cast<uint8_t *>(zero); -} -ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t size) : - bytes(bytes), capacity(capacity), zero(bytes.get()), end(&bytes.get()[size]) { - assert(size <= capacity); - ptr = const_cast<uint8_t *>(this->zero); -} +using namespace std; -ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> 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<uint8_t *>(this->zero); +ByteBuffer ByteBuffer::alloc(std::size_t size) { + return {new uint8_t[size], size}; } -ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> 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<int8_t>(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<uint8_t *>(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 <bluetooth/l2cap.h> #include <map> #include <iomanip> +#include <iostream> +#include <variant> +#include <chrono> +#include <sstream> #include <sys/types.h> #include <sys/socket.h> +#include <sys/ioctl.h> +#include <error.h> // 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<LinuxBluetoothDevice> { 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<AttributeData> discoverCharacteristics(uint16_t startHandle, uint16_t endHandle); - ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size); + void writeL2cap(ByteBuffer &buffer); + + void writeAndRead(ByteBuffer &buffer); + + AttVariant processAvailableMessages(ByteBuffer &buffer); + + template<typename clock = std::chrono::system_clock> + AttVariant exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point<clock> time); + template<typename T, typename clock = std::chrono::system_clock> + T exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point<clock> 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<uint8_t> 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<uint8_t> 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<AttributeData> 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<uint8_t> 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<uint8_t> // 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<AttPduType>(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<ExchangeMtuReq>(v)) { + auto req = get<ExchangeMtuReq>(v); + LOG_DEBUG("Got exchange MTU request, clientRxMtu=" + to_string(req.clientRxMtu)); + + this->mtu = std::min<uint16_t>(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<typename T> +static +void assertIsVariant(const AttVariant &variant) { + if(holds_alternative<T>(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<typename T, typename clock> +T LinuxBluetoothGatt::exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point<clock> time, bool fail_on_error) { + auto v = exchangeAndWaitFor<clock>(buffer, time); + + if (holds_alternative<T>(v)) { + return get<T>(v); + } + + if (fail_on_error && holds_alternative<ErrorRes>(v)) { + return get<T>(v); + } + + throw makeUnexpectedTypeException(v); +} + +template<typename clock> +AttVariant LinuxBluetoothGatt::exchangeAndWaitFor(ByteBuffer &buffer, std::chrono::time_point<clock> time) { + uint8_t bytes[mtu]; + ByteBuffer buffer2{bytes, mtu}; + auto v = processAvailableMessages(buffer2); + assertIsVariant<monostate>(v); + + LOG_DEBUG("Writing message " << to_string(static_cast<AttPduType>(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<std::chrono::seconds>(time - start); + auto ms = std::chrono::duration_cast<std::chrono::microseconds>(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<AttributeData> LinuxBluetoothGatt::discoverServices(uint16_t startHandle) { - shared_ptr<uint8_t> 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<ReadByGroupTypeRes>(buffer, std::chrono::system_clock::now() + 10s, true); - vector<AttributeData> 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<AttributeData> LinuxBluetoothGatt::discoverServices(uint16_t startHandle) } vector<AttributeData> LinuxBluetoothGatt::discoverCharacteristics(uint16_t startHandle, uint16_t endHandle) { - shared_ptr<uint8_t> 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<AttributeData> values = AttPdu::parseReadByType(in); + if(holds_alternative<ReadByTypeRes>(res)) { + auto values = get<ReadByTypeRes>(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<ErrorRes>(res)) { + ErrorRes err = get<ErrorRes>(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 <ble/att.h> + +#include "ble/att.h" +#include "ble/Bluetooth.h" +#include <iostream> + +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> attPduType(const AttVariant &v) { + return visit([](auto &&arg) -> o<AttPduType>{ + using T = std::decay_t<decltype(arg)>; + + if constexpr (std::is_same_v<T, std::monostate>) { + 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<AttributeData> 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<AttributeData> 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<AttPduType>(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<AttPduType>(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<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(); + + auto raw = make_unique<uint8_t[]>(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<uint8_t[]> 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 <ios> +#include <ostream> +#include <iomanip> +#include <ble/misc.h> + + +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 |