diff options
Diffstat (limited to 'ble/LinuxBluetooth.cpp')
| -rw-r--r-- | ble/LinuxBluetooth.cpp | 251 |
1 files changed, 207 insertions, 44 deletions
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); } // ----------------------------------------------------------------------- |