1 files changed, 49 insertions, 9 deletions

diff --git a/ble/att.cpp b/ble/att.cpp
index 585e914..301918d 100644
--- a/ble/att.cpp
+++ b/ble/att.cpp
@@ -74,7 +74,7 @@ std::string to_string(AttPduType t) {
 }
 
 o<AttPduType> attPduType(const AttVariant &v) {
-    return visit([](auto &&arg) -> o<AttPduType>{
+    return visit([](auto &&arg) -> o<AttPduType> {
         using T = std::decay_t<decltype(arg)>;
 
         if constexpr (std::is_same_v<T, std::monostate>) {
@@ -96,12 +96,17 @@ AttPduType AttPdu::getType() {
     return (AttPduType) bytes.peek8(0);
 }
 
-void AttPdu::makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu)
-{
+void AttPdu::makeExchangeMtuRes(ByteBuffer &bytes, uint16_t serverRxMtu) {
     bytes.write8(AttPduType::EXCHANGE_MTU_RES).
         write16le(serverRxMtu);
 }
 
+void AttPdu::makeFindInformationReq(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle) {
+    bytes.write8(AttPduType::FIND_INFORMATION_REQ);
+    bytes.write16le(startHandle);
+    bytes.write16le(endHandle);
+}
+
 void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) {
     bytes.write8(AttPduType::READ_BY_GROUP_TYPE_REQ);
     bytes.write16le(startHandle);
@@ -129,7 +134,8 @@ void AttPdu::makeWrite(ByteBuffer &req, uint16_t handle, const ByteBuffer &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()));
+        throw BluetoothException(
+            "Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize()));
     }
 
     uint8_t length = bytes.read8();
@@ -138,8 +144,8 @@ vector<AttributeData> AttPdu::parseAttributeData(ByteBuffer &bytes) {
 
     vector<AttributeData> values;
     for (int i = 0; i < count; i++) {
-        auto data = bytes.viewAndSkip(length);
-        auto&& x = AttributeData::fromByteBuffer(data);
+        auto data = bytes.viewForwardAndSkip(length);
+        auto &&x = AttributeData::fromByteBuffer(data);
         values.emplace_back(x);
     }
 
@@ -154,6 +160,8 @@ AttVariant AttPdu::parse(ByteBuffer &bytes) {
             return parseExchangeMtuReq(bytes);
         case AttPduType::EXCHANGE_MTU_RES:
             return parseExchangeMtuRes(bytes);
+        case AttPduType::FIND_INFORMATION_RES:
+            return parseFindInformationRes(bytes);
         case AttPduType::READ_BY_GROUP_TYPE_RES:
             return ReadByGroupTypeRes{parseAttributeData(bytes)};
         case AttPduType::READ_BY_TYPE_RES:
@@ -171,6 +179,23 @@ ExchangeMtuRes AttPdu::parseExchangeMtuRes(ByteBuffer &bytes) {
     return {bytes.read16le()};
 }
 
+FindInformationRes AttPdu::parseFindInformationRes(ByteBuffer &bytes) {
+    auto format = bytes.read8();
+
+    std::vector<InformationData> information;
+    if (format == InformationData::FORMAT_SHORT_UUID) {
+        while (bytes.getBytesLeft()) {
+            information.push_back(InformationData::fromByteBufferShortUuid(bytes));
+        }
+    } else if (format == InformationData::FORMAT_LONG_UUID) {
+        while (bytes.getBytesLeft()) {
+            information.push_back(InformationData::fromByteBufferLongUuid(bytes));
+        }
+    }
+
+    return {format, information};
+}
+
 void AttPdu::parseRead(ByteBuffer &bytes) {
     auto t = static_cast<AttPduType>(bytes.read8());
 
@@ -180,10 +205,11 @@ void AttPdu::parseRead(ByteBuffer &bytes) {
 }
 
 void AttPdu::parseWrite(ByteBuffer &bytes) {
-    auto t = static_cast<AttPduType>(bytes.read8());
+    auto b = bytes.read8();
+    auto t = static_cast<AttPduType>(b);
 
     if (t != WRITE_RES) {
-        throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(WRITE_RES));
+        throw BluetoothException("Unexpected type: " + to_string(b) + "/" + to_string(t) + ", expected " + to_string(WRITE_RES));
     }
 }
 
@@ -196,7 +222,7 @@ AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) {
 
     auto raw = make_unique<uint8_t[]>(bytes.getBytesLeft());
 
-    bytes.copy(raw.get(), bytes.getBytesLeft());
+    bytes.copyTo(raw.get(), bytes.getBytesLeft());
 
     ByteBuffer buffer = ByteBuffer{raw.get(), bytes.getBytesLeft()};
     return AttributeData(handle, buffer, std::move(raw));
@@ -208,5 +234,19 @@ AttributeData::AttributeData(uint16_t handle, ByteBuffer value, std::shared_ptr<
 
 AttributeData::~AttributeData() = default;
 
+// -----------------------------------------------------------------------
+// InformationData
+// -----------------------------------------------------------------------
+
+InformationData InformationData::fromByteBufferShortUuid(ByteBuffer &value) {
+    return {value.read16le(), ShortUuid(value).toLong()};
+}
+
+InformationData InformationData::fromByteBufferLongUuid(ByteBuffer &value) {
+    return {value.read16le(), Uuid(value)};
+}
+
+InformationData::InformationData(uint16_t handle, const Uuid &uuid) : handle(handle), uuid(uuid) {}
+
 } // namespace bluetooth
 } // namespace trygvis