diff options
| -rw-r--r-- | ByteBuffer.h | 10 | ||||
| -rw-r--r-- | LinuxBluetooth.cpp | 104 |
2 files changed, 75 insertions, 39 deletions
diff --git a/ByteBuffer.h b/ByteBuffer.h index b3e06cf..3836966 100644 --- a/ByteBuffer.h +++ b/ByteBuffer.h @@ -17,14 +17,8 @@ public: class ByteBuffer { public: - /** - * Wrapping constructor, the size will be equal to the capacity. - */ ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity); - /** - * Wrapping constructor. - */ ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t zero, size_t size); inline size_t getSize() const { @@ -40,6 +34,10 @@ public: return ptr - zero; } + inline size_t getBytesLeft() const { + return end - ptr; + } + inline void setCursor(size_t newCursor) { // assertCanAccessRelative(newCursor); ptr = (uint8_t *) &zero[newCursor]; diff --git a/LinuxBluetooth.cpp b/LinuxBluetooth.cpp index 3d5cded..22d657e 100644 --- a/LinuxBluetooth.cpp +++ b/LinuxBluetooth.cpp @@ -8,6 +8,7 @@ #include <map> #include <sstream> #include <iomanip> +#include <numeric> // Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c #define ATT_CID 4 @@ -156,6 +157,38 @@ void LinuxBluetoothDevice::disconnect() { close(l2cap); } +uuid_t readUuid(ByteBuffer &bytes) { + size_t s = data.value.getBytesLeft(); + + if (s == 2) { + uint8_t bs[16] = BLUETOOTH_UUID_INITIALIZER; + bs[2] = data.value.get8(1); + bs[3] = data.value.get8(0); + memcpy(&u, bs, 16); + } else if (s == 16) { + uint8_t bs[16]; + bs[15] = data.value.get8(0); + bs[14] = data.value.get8(1); + bs[13] = data.value.get8(2); + bs[12] = data.value.get8(3); + bs[11] = data.value.get8(4); + bs[10] = data.value.get8(5); + bs[9] = data.value.get8(6); + bs[8] = data.value.get8(7); + bs[7] = data.value.get8(8); + bs[6] = data.value.get8(9); + bs[5] = data.value.get8(10); + bs[4] = data.value.get8(11); + bs[3] = data.value.get8(12); + bs[2] = data.value.get8(13); + bs[1] = data.value.get8(14); + bs[0] = data.value.get8(15); + memcpy(&u, bs, 16); + } else { + throw BluetoothException(this, "Unexpected size: " + to_string(data.value.getSize())); + } +} + void LinuxBluetoothDevice::discoverServices() { uint16_t startHandle = 0x0001; @@ -172,37 +205,7 @@ void LinuxBluetoothDevice::discoverServices() { ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << data.endGroupHandle << ", value: " << data.value.toString(); - uuid_t u; - - size_t s = data.value.getSize(); - - if (s == 2) { - uint8_t bs[16] = BLUETOOTH_UUID_INITIALIZER; - bs[2] = data.value.get8(1); - bs[3] = data.value.get8(0); - memcpy(&u, bs, 16); - } else if (s == 16) { - uint8_t bs[16]; - bs[15] = data.value.get8(0); - bs[14] = data.value.get8(1); - bs[13] = data.value.get8(2); - bs[12] = data.value.get8(3); - bs[11] = data.value.get8(4); - bs[10] = data.value.get8(5); - bs[9] = data.value.get8(6); - bs[8] = data.value.get8(7); - bs[7] = data.value.get8(8); - bs[6] = data.value.get8(9); - bs[5] = data.value.get8(10); - bs[4] = data.value.get8(11); - bs[3] = data.value.get8(12); - bs[2] = data.value.get8(13); - bs[1] = data.value.get8(14); - bs[0] = data.value.get8(15); - memcpy(&u, bs, 16); - } else { - throw BluetoothException(this, "Unexpected size: " + to_string(data.value.getSize())); - } + uuid_t u = readUuid(data.value); services.push_back(new DefaultBluetoothGattService(*this, u, data.handle, data.endGroupHandle)); } @@ -216,9 +219,44 @@ void LinuxBluetoothDevice::discoverServices() { D << "service: " << to_string(s->getUuid()) << ", handle: " << s->getHandle() << ", end group handle: " << s->getEndGroupHandle(); } - for (auto &s : services) { - discoverCharacteristics(s->getHandle(), s->getEndGroupHandle()); + auto it = services.begin(), + end = services.end(); + + startHandle = 0x0001; + + do { + vector<AttributeData> values = discoverCharacteristics(startHandle, 0xffff); + + if (values.size() == 0) { + break; + } + + for (auto c : values) { + uint8_t properties = c.data.read8(); + uint16_t valueHandle = c.data.read16le(); + + if (values.data.getBytesLeft() == 2) { + uint16_t uuid = values.data.read16le(); + } else { + } + } + + auto last = values.back(); + + startHandle = last.endGroupHandle; + } while (startHandle != 0xffff); + + /* + if (it != end) { + auto &service = *it; + + for (auto &s : services) { + vector<AttributeData> values = discoverCharacteristics(s->getHandle(), 0xffff); + +// service = accumulate(services.begin(), services.end(), [](a&, b&){}); + } } + */ } ByteBuffer LinuxBluetoothDevice::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size) { |