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.

15 files changed, 871 insertions, 464 deletions

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<BluetoothGatt> &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<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
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 <functional>
 #include <cstring>
 
-#include "ByteBuffer.h"
+#include "ble/ByteBuffer.h"
+#include "ble/att.h"
 
 namespace trygvis {
 namespace bluetooth {
@@ -20,74 +21,6 @@ using namespace std;
 template<typename T>
 using o = std::experimental::optional<T>;
 
-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<uint8_t>(value >> 8);
-        auto b3 = static_cast<uint8_t>(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<BluetoothGatt> BluetoothGattPtr;
 typedef shared_ptr<BluetoothGattCharacteristic> BluetoothGattCharacteristicPtr;
 typedef shared_ptr<BluetoothGattService> 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<string, shared_ptr<BluetoothAdapter>> 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<AttributeData> parseReadByGroupType(ByteBuffer &bytes);
-
-    static vector<AttributeData> 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<AttributeData> 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<uint8_t> bytes, size_t capacity);
-
-    ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t size);
+    template<size_t N>
+    explicit ByteBuffer(uint8_t (&bytes)[N]) : zero(bytes), end_(&bytes[N]), cursor(bytes) {}
 
-    ByteBuffer(const std::shared_ptr<uint8_t> 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<uint8_t *>(zero);
+    }
+
+    inline uint8_t *end() {
+        return const_cast<uint8_t *>(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<uint8_t> 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<uint8_t> 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 <experimental/optional>
+#include <variant>
+#include <vector>
+
+#include "ble/ByteBuffer.h"
+#include "ble/misc.h"
+
+namespace trygvis {
+namespace bluetooth {
+
+template<typename T>
+using o = std::experimental::optional<T>;
+
+/**
+ * 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, ///<The attribute PDU was invalid.
+    INSUFFICIENT_AUTHENTICATION = 0x05, ///<The attribute requires authentication before it can be read or written.
+    REQUEST_NOT_SUPPORTED = 0x06, ///<Attribute server does not support the request received from the client.
+    INVALID_OFFSET = 0x07, ///<Offset specified was past the end of the attribute.
+    INSUFFICIENT_AUTHORIZATION = 0x08, ///<The attribute requires authorization before it can be read or written.
+    PREPARE_QUEUE_FULL = 0x09, ///<Too many prepare writes have been queued.
+    ATTRIBUTE_NOT_FOUND = 0x0a, ///<No attribute found within the given attribute handle range.
+    ATTRIBUTE_NOT_LONG = 0x0b, ///<The attribute cannot be read or written using the Read Blob Request
+    INSUFFICIENT_ENCRYPTION_KEY_SIZE = 0x0c, ///<The Encryption Key Size used for encrypting this link is insufficient.
+    INVALID_ATTRIBUTE_VALUE_LENGTH = 0x0d, ///<The attribute value length is invalid for the operation
+    UNLIKELY_ERROR = 0x0e, ///<The attribute request that was requested has encountered an error that was unlikely, and therefore could not be completed as requested.
+    INSUFFICIENT_ENCRYPTION = 0x0f, ///<The attribute requires encryption before it can be read or written.
+    UNSUPPORTED_GROUP_TYPE = 0x10, ///<The attribute type is not a supported grouping attribute as defined by a higher layer specification.
+    INSUFFICIENT_RESOURCES = 0x11, ///<Insufficient Resources to complete the request Reserved 0x012 – 0x7F Reserved for future use.
+};
+
+struct ErrorRes {
+    static constexpr auto att_pdu_type = AttPduType::EXCHANGE_MTU_REQ;
+
+    /**
+     * The request that generated this error response.
+     */
+    uint8_t requestOpcode;
+
+    /**
+     * The attribute handle that generated this error response.
+     */
+    uint16_t attributeHandle;
+
+    /**
+     * The reason why the request has generated an error response.
+     */
+    uint8_t errorCode;
+
+    static ErrorRes parse(ByteBuffer& buffer) {
+        return {buffer.read8(), buffer.read16le(), buffer.read8()};
+    }
+};
+
+struct ExchangeMtuReq {
+    static constexpr auto att_pdu_type = AttPduType::EXCHANGE_MTU_REQ;
+
+    uint16_t clientRxMtu;
+};
+
+struct ExchangeMtuRes {
+    static constexpr auto att_pdu_type = AttPduType::EXCHANGE_MTU_RES;
+
+    uint16_t serverRxMtu;
+};
+
+struct ReadByGroupTypeRes {
+    static constexpr auto att_pdu_type = AttPduType::READ_BY_GROUP_TYPE_RES;
+
+    std::vector<AttributeData> attributes;
+};
+
+struct ReadByTypeRes {
+    static constexpr auto att_pdu_type = AttPduType::READ_BY_TYPE_RES;
+
+    std::vector<AttributeData> attributes;
+};
+
+using AttVariant = std::variant<std::monostate,
+    ErrorRes,
+    ExchangeMtuReq, ExchangeMtuRes,
+    ReadByGroupTypeRes, ReadByTypeRes>;
+
+o<AttPduType> 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<AttributeData> 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<uint8_t[]> raw);
+
+    std::shared_ptr<uint8_t[]> 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 <cstring>
+#include <cctype>
+#include <stdexcept>
+
+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<uint8_t>(value >> 8);
+        auto b3 = static_cast<uint8_t>(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 <boost/test/unit_test.hpp>
 
-#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<uint8_t>(tmp, [secret](uint8_t *) {
-            delete[]secret;
-        });
+    ~Bytes() {
+      delete secret;
     }
-    shared_ptr<uint8_t> 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) <<