Add support for AttFindByTypeValueReq/Rsp (GATT server mode)

4 files changed, 293 insertions, 20 deletions

diff --git a/api/direct_bt/ATTPDUTypes.hpp b/api/direct_bt/ATTPDUTypes.hpp
index aab9e91a..6940546f 100644
--- a/api/direct_bt/ATTPDUTypes.hpp
+++ b/api/direct_bt/ATTPDUTypes.hpp
@@ -1863,6 +1863,199 @@ namespace direct_bt {
             }
     };
 
+    /**
+     * BT Core Spec v5.2: Vol 3, Part F ATT: 3.4.3.3 ATT_FIND_BY_TYPE_VALUE_REQ
+     * <p>
+     * FIND_BY_TYPE_VALUE_REQ
+     * </p>
+     * Used in:
+     * <p>
+     * BT Core Spec v5.2: Vol 3, Part G GATT: 4.4.2 Discover Primary Service by Service UUID
+     * </p>
+     */
+    class AttFindByTypeValueReq : public AttPDUMsg
+    {
+        private:
+            jau::uuid_t::TypeSize getAttValueTypeSize() const {
+                return jau::uuid_t::toTypeSize( getPDUValueSize() );
+            }
+
+            constexpr static jau::nsize_t pdu_value_offset = 1 + 2 + 2 + 2;
+
+        public:
+            AttFindByTypeValueReq(const uint8_t* source, const jau::nsize_t length)
+            : AttPDUMsg(source, length)
+            {
+                checkOpcode(Opcode::FIND_BY_TYPE_VALUE_REQ);
+
+                if( getPDUValueSize() == 0 ) {
+                    throw AttValueException("AttFindByTypeValueReq: Invalid packet size: pdu-value-size "+std::to_string(getPDUValueSize())+
+                            " not > 0 ", E_FILE_LINE);
+                }
+                getAttValueTypeSize(); // validates att-value type-size
+            }
+
+            AttFindByTypeValueReq(const uint16_t startHandle, const uint16_t endHandle,
+                                  const jau::uuid16_t &att_type, const jau::uuid_t& att_value)
+            : AttPDUMsg(Opcode::FIND_BY_TYPE_VALUE_REQ, getPDUValueOffset()+att_value.getTypeSizeInt())
+            {
+                pdu.put_uint16_nc(1, startHandle);
+                pdu.put_uint16_nc(1+2, endHandle);
+                pdu.put_uuid_nc  (1+2+2, att_type);
+                pdu.put_uuid_nc  (1+2+2+2, att_value);
+            }
+
+            /** opcode + handle_start + handle_end + att_type */
+            constexpr_cxx20 jau::nsize_t getPDUValueOffset() const noexcept override { return pdu_value_offset; }
+
+            constexpr uint16_t getStartHandle() const noexcept { return pdu.get_uint16_nc( 1 ); }
+
+            constexpr uint16_t getEndHandle() const noexcept { return pdu.get_uint16_nc( 1 + 2 ); }
+
+            jau::uuid16_t getAttType() const noexcept { return pdu.get_uuid16_nc( 1 + 2 + 2 ); }
+
+            std::unique_ptr<const jau::uuid_t> getAttValue() const noexcept { return pdu.get_uuid(pdu_value_offset, getAttValueTypeSize()); }
+
+            constexpr_cxx20 std::string getName() const noexcept override {
+                return "AttFindByTypeValueReq";
+            }
+
+        protected:
+            std::string valueString() const noexcept override {
+                return "handle ["+jau::to_hexstring(getStartHandle())+".."+jau::to_hexstring(getEndHandle())+
+                       "], type "+getAttType().toString()+", value "+getAttValue()->toString();
+            }
+    };
+
+    /**
+     * BT Core Spec v5.2: Vol 3, Part F ATT: 3.4.3.4 ATT_FIND_BY_TYPE_VALUE_RSP
+     * <p>
+     * FIND_BY_TYPE_VALUE_RSP
+     * </p>
+     * <p>
+     * Contains a list of elements, each comprised of { att_handle_start, group_end_handle } pair.
+     * The handles are comprised of two octets, i.e. uint16_t,
+     * hence one element is of size 4 octets.
+     * <pre>
+     *  element := { uint16_t handle_start, uint16_t handle_end }
+     * </pre>
+     * </p>
+     * Used in:
+     * <p>
+     * BT Core Spec v5.2: Vol 3, Part G GATT: 4.4.2 Discover Primary Service by Service UUID
+     * </p>
+     */
+    class AttFindByTypeValueRsp: public AttPDUMsg
+    {
+        public:
+            AttFindByTypeValueRsp(const uint8_t* source, const jau::nsize_t length)
+            : AttPDUMsg(source, length)
+            {
+                checkOpcode(Opcode::FIND_BY_TYPE_VALUE_RSP);
+                if( getPDUValueSize() % getElementSize() != 0 ) {
+                    throw AttValueException("AttFindInfoRsp: Invalid packet size: pdu-value-size "+std::to_string(getPDUValueSize())+
+                            " not multiple of element-size "+std::to_string(getElementSize()), E_FILE_LINE);
+                }
+            }
+
+            /** opcode */
+            constexpr_cxx20 jau::nsize_t getPDUValueOffset() const noexcept override { return 1; }
+
+            /**
+             * Returns element size, 4 octets
+             * <p>
+             * element := { uint16_t handle_start, uint16_t handle_end }
+             * </p>
+             */
+            constexpr jau::nsize_t getElementSize() const {
+                return 2 /* handle */ + 2 /* handle_end */;
+            }
+
+            /** Number of elements */
+            constexpr_cxx20 jau::nsize_t getElementCount() const noexcept {
+                /** getPDUValueSize() =
+                 * - `pdu.size - getAuthSigSize() - value-offset` or
+                 * - `getPDUParamSize() - getPDUValueOffset() + 1`
+                 */
+                return getPDUValueSize()  / getElementSize();
+            }
+
+            /**
+             * Fixate element count
+             * @param count
+             */
+            void setElementCount(const jau::nsize_t count) {
+                const jau::nsize_t element_length = getElementSize();
+                const jau::nsize_t new_size = getPDUValueOffset() + element_length * count;
+                if( pdu.size() < new_size ) {
+                    throw jau::IllegalArgumentException(getName()+": "+std::to_string(getPDUValueOffset())+
+                            " + element[len "+std::to_string(element_length)+
+                            " * count "+std::to_string(count)+" > pdu "+std::to_string(pdu.size()), E_FILE_LINE);
+                }
+                pdu.resize( new_size );
+                if( getPDUValueSize() % getElementSize() != 0 ) {
+                    throw AttValueException(getName()+": Invalid packet size: pdu-value-size "+std::to_string(getPDUValueSize())+
+                            " not multiple of element-size "+std::to_string(getElementSize()), E_FILE_LINE);
+                }
+            }
+
+            jau::nsize_t getElementPDUOffset(const jau::nsize_t elementIdx) const {
+                return getPDUValueOffset() + elementIdx * getElementSize();
+            }
+
+            uint8_t const * getElementPtr(const jau::nsize_t elementIdx) const {
+                return pdu.get_ptr(getElementPDUOffset(elementIdx));
+            }
+
+            /**
+             * Create an incomplete response with maximal (MTU) length.
+             *
+             * User shall set all elements via the set*() methods
+             * and finally use setElementSize() to fixate element length and element count.
+             *
+             * @param total_length maximum
+             */
+            AttFindByTypeValueRsp(const jau::nsize_t total_length)
+            : AttPDUMsg(Opcode::FIND_BY_TYPE_VALUE_RSP, total_length)
+            {
+            }
+
+            uint16_t getElementHandle(const jau::nsize_t elementIdx) const {
+                return pdu.get_uint16( getElementPDUOffset(elementIdx) );
+            }
+            uint16_t getElementHandleEnd(const jau::nsize_t elementIdx) const {
+                return pdu.get_uint16( getElementPDUOffset(elementIdx) + 2 );
+            }
+
+            void setElementHandles(const jau::nsize_t elementIdx, const uint16_t handle, const uint16_t handle_end) {
+                const jau::nsize_t offset = getElementPDUOffset(elementIdx);
+                pdu.put_uint16_nc( offset, handle );
+                pdu.put_uint16_nc( offset, handle_end );
+            }
+
+            constexpr_cxx20 std::string getName() const noexcept override {
+                return "AttFindByTypeValueRsp";
+            }
+
+        protected:
+            std::string elementString(const jau::nsize_t idx) const {
+                return "handle["+jau::to_hexstring(getElementHandle(idx))+
+                       ".."+jau::to_hexstring(getElementHandleEnd(idx))+"]";
+            }
+
+            std::string valueString() const noexcept override {
+                std::string res = "size "+std::to_string(getPDUValueSize())+", elements[count "+std::to_string(getElementCount())+", "+
+                        "size "+std::to_string(getElementSize())+": ";
+                const jau::nsize_t count = getElementCount();
+                for(jau::nsize_t i=0; i<count; i++) {
+                    res += std::to_string(i)+"["+elementString(i)+"],";
+                }
+                res += "]";
+                return res;
+            }
+
+    };
+
 } // namespace direct_bt
 
 /** \example dbt_scanner10.cpp
diff --git a/api/direct_bt/BTGattHandler.hpp b/api/direct_bt/BTGattHandler.hpp
index 6c58b892..102c12e8 100644
--- a/api/direct_bt/BTGattHandler.hpp
+++ b/api/direct_bt/BTGattHandler.hpp
@@ -223,6 +223,7 @@ namespace direct_bt {
             void replyWriteReq(const AttPDUMsg * pdu);
             void replyReadReq(const AttPDUMsg * pdu);
             void replyFindInfoReq(const AttFindInfoReq * pdu);
+            void replyFindByTypeValueReq(const AttFindByTypeValueReq * pdu);
             void replyReadByTypeReq(const AttReadByNTypeReq * pdu);
             void replyReadByGroupTypeReq(const AttReadByNTypeReq * pdu);
             void replyAttPDUReq(std::unique_ptr<const AttPDUMsg> && pdu);
diff --git a/src/direct_bt/ATTPDUTypes.cpp b/src/direct_bt/ATTPDUTypes.cpp
index 31568b8b..b48aad98 100644
--- a/src/direct_bt/ATTPDUTypes.cpp
+++ b/src/direct_bt/ATTPDUTypes.cpp
@@ -125,8 +125,8 @@ std::unique_ptr<const AttPDUMsg> AttPDUMsg::getSpecialized(const uint8_t * buffe
         case Opcode::EXCHANGE_MTU_RSP:              return std::make_unique<AttExchangeMTU>(buffer, buffer_size);
         case Opcode::FIND_INFORMATION_REQ:          return std::make_unique<AttFindInfoReq>(buffer, buffer_size);
         case Opcode::FIND_INFORMATION_RSP:          return std::make_unique<AttFindInfoRsp>(buffer, buffer_size);
-        case Opcode::FIND_BY_TYPE_VALUE_REQ:        return std::make_unique<AttPDUMsg>(buffer, buffer_size); // TODO
-        case Opcode::FIND_BY_TYPE_VALUE_RSP:        return std::make_unique<AttPDUMsg>(buffer, buffer_size); // TODO
+        case Opcode::FIND_BY_TYPE_VALUE_REQ:        return std::make_unique<AttFindByTypeValueReq>(buffer, buffer_size);
+        case Opcode::FIND_BY_TYPE_VALUE_RSP:        return std::make_unique<AttFindByTypeValueRsp>(buffer, buffer_size);
         case Opcode::READ_BY_TYPE_REQ:              return std::make_unique<AttReadByNTypeReq>(buffer, buffer_size);
         case Opcode::READ_BY_TYPE_RSP:              return std::make_unique<AttReadByTypeRsp>(buffer, buffer_size);
         case Opcode::READ_REQ:                      return std::make_unique<AttReadReq>(buffer, buffer_size);
diff --git a/src/direct_bt/BTGattHandler.cpp b/src/direct_bt/BTGattHandler.cpp
index 96855cbe..c946b4e1 100644
--- a/src/direct_bt/BTGattHandler.cpp
+++ b/src/direct_bt/BTGattHandler.cpp
@@ -673,6 +673,76 @@ void BTGattHandler::replyFindInfoReq(const AttFindInfoReq * pdu) {
     send(err);
 }
 
+void BTGattHandler::replyFindByTypeValueReq(const AttFindByTypeValueReq * pdu) {
+    // BT Core Spec v5.2: Vol 3, Part F ATT: 3.4.3.3 ATT_FIND_BY_TYPE_VALUE_REQ
+    // BT Core Spec v5.2: Vol 3, Part F ATT: 3.4.3.4 ATT_FIND_BY_TYPE_VALUE_RSP
+    // BT Core Spec v5.2: Vol 3, Part G GATT: 4.4.2 Discover Primary Service by Service UUID
+    if( 0 == pdu->getStartHandle() ) {
+        AttErrorRsp err(AttErrorRsp::ErrorCode::INVALID_HANDLE, pdu->getOpcode(), 0);
+        COND_PRINT(env.DEBUG_DATA, "GATT-Req: TYPEVALUE.0: %s -> %s from %s", pdu->toString().c_str(), err.toString().c_str(), toString().c_str());
+        send(err);
+        return;
+    }
+    if( pdu->getStartHandle() > pdu->getEndHandle() ) {
+        AttErrorRsp err(AttErrorRsp::ErrorCode::INVALID_HANDLE, pdu->getOpcode(), pdu->getStartHandle());
+        COND_PRINT(env.DEBUG_DATA, "GATT-Req: TYPEVALUE.1: %s -> %s from %s", pdu->toString().c_str(), err.toString().c_str(), toString().c_str());
+        send(err);
+        return;
+    }
+    const jau::uuid16_t uuid_prim_service = jau::uuid16_t(GattAttributeType::PRIMARY_SERVICE);
+    const jau::uuid16_t uuid_secd_service = jau::uuid16_t(GattAttributeType::SECONDARY_SERVICE);
+    const uint16_t end_handle = pdu->getEndHandle();
+    const uint16_t start_handle = pdu->getStartHandle();
+    const jau::uuid16_t att_type = pdu->getAttType();
+    std::unique_ptr<const jau::uuid_t> att_value = pdu->getAttValue();
+
+    uint16_t req_group_type;
+    if( att_type.equivalent( uuid_prim_service ) ) {
+        req_group_type = GattAttributeType::PRIMARY_SERVICE;
+    } else if( att_type.equivalent( uuid_secd_service ) ) {
+        req_group_type = GattAttributeType::SECONDARY_SERVICE;
+    } else {
+        // not handled
+        req_group_type = 0;
+    }
+
+
+    // const jau::nsize_t rspMaxSize = std::min<jau::nsize_t>(255, usedMTU.load()-2);
+    AttFindByTypeValueRsp rsp(usedMTU); // maximum size
+    jau::nsize_t rspSize = 0;
+    jau::nsize_t rspCount = 0;
+
+    if( nullptr != gattServerData ) {
+        const jau::nsize_t size = 2 + 2;
+
+        for(DBGattServiceRef& s : gattServerData->getServices()) {
+            if( start_handle <= s->getHandle() && s->getHandle() <= end_handle ) {
+                if( ( ( GattAttributeType::PRIMARY_SERVICE   == req_group_type &&  s->isPrimary() ) ||
+                      ( GattAttributeType::SECONDARY_SERVICE == req_group_type && !s->isPrimary() )
+                    ) &&
+                    s->getType()->equivalent(*att_value) )
+                {
+                    rsp.setElementHandles(rspCount, s->getHandle(), s->getEndHandle());
+                    rspSize += size;
+                    ++rspCount;
+                    COND_PRINT(env.DEBUG_DATA, "GATT-Req: TYPEVALUE.4: %s -> %s from %s", pdu->toString().c_str(), rsp.toString().c_str(), toString().c_str());
+                    send(rsp);
+                    return; // done
+                }
+            }
+        }
+        if( 0 < rspCount ) { // loop completed, elements added and all fitting in ATT_MTU
+            rsp.setElementCount(rspCount);
+            COND_PRINT(env.DEBUG_DATA, "GATT-Req: TYPEVALUE.5: %s -> %s from %s", pdu->toString().c_str(), rsp.toString().c_str(), toString().c_str());
+            send(rsp);
+            return;
+        }
+    }
+    AttErrorRsp err(AttErrorRsp::ErrorCode::ATTRIBUTE_NOT_FOUND, pdu->getOpcode(), start_handle);
+    COND_PRINT(env.DEBUG_DATA, "GATT-Req: TYPEVALUE.6: %s -> %s from %s", pdu->toString().c_str(), err.toString().c_str(), toString().c_str());
+    send(err);
+}
+
 void BTGattHandler::replyReadByTypeReq(const AttReadByNTypeReq * pdu) {
     // BT Core Spec v5.2: Vol 3, Part F ATT: 3.4.4.1 ATT_READ_BY_TYPE_REQ
     // BT Core Spec v5.2: Vol 3, Part F ATT: 3.4.4.2 ATT_READ_BY_TYPE_RSP
@@ -857,7 +927,7 @@ void BTGattHandler::replyAttPDUReq(std::unique_ptr<const AttPDUMsg> && pdu) {
         return;
     }
     switch( pdu->getOpcode() ) {
-        case AttPDUMsg::Opcode::EXCHANGE_MTU_REQ: {
+        case AttPDUMsg::Opcode::EXCHANGE_MTU_REQ: { // 2
             const AttExchangeMTU * p = static_cast<const AttExchangeMTU*>(pdu.get());
             const uint16_t clientMTU = p->getMTUSize();
             usedMTU = std::min(serverMTU, clientMTU);
@@ -884,49 +954,58 @@ void BTGattHandler::replyAttPDUReq(std::unique_ptr<const AttPDUMsg> && pdu) {
             send(rsp);
             return;
         }
-        case AttPDUMsg::Opcode::READ_BY_TYPE_REQ: {
-            replyReadByTypeReq( static_cast<const AttReadByNTypeReq*>( pdu.get() ) );
+
+        case AttPDUMsg::Opcode::FIND_INFORMATION_REQ: { // 4
+            replyFindInfoReq( static_cast<const AttFindInfoReq*>( pdu.get() ) );
             return;
         }
-        case AttPDUMsg::Opcode::READ_BY_GROUP_TYPE_REQ: {
-            replyReadByGroupTypeReq( static_cast<const AttReadByNTypeReq*>( pdu.get() ) );
+
+        case AttPDUMsg::Opcode::FIND_BY_TYPE_VALUE_REQ: { // 6
+            replyFindByTypeValueReq( static_cast<const AttFindByTypeValueReq*>( pdu.get() ) );
             return;
         }
-        case AttPDUMsg::Opcode::FIND_INFORMATION_REQ: {
-            replyFindInfoReq( static_cast<const AttFindInfoReq*>( pdu.get() ) );
+
+        case AttPDUMsg::Opcode::READ_BY_TYPE_REQ: { // 8
+            replyReadByTypeReq( static_cast<const AttReadByNTypeReq*>( pdu.get() ) );
             return;
         }
-        case AttPDUMsg::Opcode::READ_REQ:
+
+        case AttPDUMsg::Opcode::READ_REQ: // 10
             [[fallthrough]];
-        case AttPDUMsg::Opcode::READ_BLOB_REQ: {
+        case AttPDUMsg::Opcode::READ_BLOB_REQ: { // 12
             replyReadReq( pdu.get() );
             return;
         }
-        case AttPDUMsg::Opcode::WRITE_REQ:
+
+        case AttPDUMsg::Opcode::READ_BY_GROUP_TYPE_REQ: { // 16
+            replyReadByGroupTypeReq( static_cast<const AttReadByNTypeReq*>( pdu.get() ) );
+            return;
+        }
+
+        case AttPDUMsg::Opcode::WRITE_REQ: // 18
             [[fallthrough]];
-        case AttPDUMsg::Opcode::WRITE_CMD:
+        case AttPDUMsg::Opcode::WRITE_CMD: // 18 + 64 = 82
             [[fallthrough]];
-        case AttPDUMsg::Opcode::PREPARE_WRITE_REQ:
+        case AttPDUMsg::Opcode::PREPARE_WRITE_REQ: // 22
             [[fallthrough]];
-        case AttPDUMsg::Opcode::EXECUTE_WRITE_REQ: {
+        case AttPDUMsg::Opcode::EXECUTE_WRITE_REQ: { // 24
             replyWriteReq( pdu.get() );
             return;
         }
 
         // TODO: Add support for the following requests
 
-        case AttPDUMsg::Opcode::FIND_BY_TYPE_VALUE_REQ:
+        case AttPDUMsg::Opcode::READ_MULTIPLE_REQ: // 14
             [[fallthrough]];
-        case AttPDUMsg::Opcode::READ_MULTIPLE_REQ:
+        case AttPDUMsg::Opcode::READ_MULTIPLE_VARIABLE_REQ: // 32
             [[fallthrough]];
-        case AttPDUMsg::Opcode::READ_MULTIPLE_VARIABLE_REQ:
-            [[fallthrough]];
-        case AttPDUMsg::Opcode::SIGNED_WRITE_CMD: {
+        case AttPDUMsg::Opcode::SIGNED_WRITE_CMD: { // 18 + 64 + 128 = 210
             AttErrorRsp rsp(AttErrorRsp::ErrorCode::UNSUPPORTED_REQUEST, pdu->getOpcode(), 0);
             WARN_PRINT("GATT Req: Ignored: %s -> %s from %s", pdu->toString().c_str(), rsp.toString().c_str(), toString().c_str());
             send(rsp);
             return;
         }
+
         default:
             AttErrorRsp rsp(AttErrorRsp::ErrorCode::FORBIDDEN_VALUE, pdu->getOpcode(), 0);
             ERR_PRINT("GATT Req: Unhandled: %s -> %s from %s", pdu->toString().c_str(), rsp.toString().c_str(), toString().c_str());