aboutsummaryrefslogtreecommitdiff
path: root/ble
diff options
context:
space:
mode:
authorTrygve Laugstøl <trygvis@inamo.no>2018-09-05 14:14:42 +0200
committerTrygve Laugstøl <trygvis@inamo.no>2018-09-05 14:14:42 +0200
commit91e54cf9150b37036447d423857d2bd18e4bf02b (patch)
tree7738db9a61cb6385f0abc6359b5b89aaa1ff104d /ble
parent25d82b0c52120c81cfed5bc1f245408f08203b7b (diff)
downloadble-toys-91e54cf9150b37036447d423857d2bd18e4bf02b.tar.gz
ble-toys-91e54cf9150b37036447d423857d2bd18e4bf02b.tar.bz2
ble-toys-91e54cf9150b37036447d423857d2bd18e4bf02b.tar.xz
ble-toys-91e54cf9150b37036447d423857d2bd18e4bf02b.zip
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.
Diffstat (limited to 'ble')
-rw-r--r--ble/Bluetooth.cpp126
-rw-r--r--ble/BluetoothImpl.h9
-rw-r--r--ble/ByteBuffer.cpp113
-rw-r--r--ble/LinuxBluetooth.cpp251
-rw-r--r--ble/att.cpp212
-rw-r--r--ble/misc.cpp44
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