BleAdvParser.cpp

Go to the documentation of this file.

 
#include "cdc_hal/IBluetoothController.h"
#include <cstring>
 
namespace cdc::hal::BleAdvParser {

static constexpr uint8_t AD_TYPE_SHORTENED_NAME            = 0x08;
static constexpr uint8_t AD_TYPE_COMPLETE_NAME             = 0x09;
static constexpr uint8_t AD_TYPE_INCOMPLETE_UUID128        = 0x06;
static constexpr uint8_t AD_TYPE_COMPLETE_UUID128          = 0x07;
static constexpr uint8_t AD_TYPE_MANUFACTURER_SPECIFIC     = 0xFF;
 
static constexpr uint8_t UUID128_SIZE                      = 16;
static constexpr uint8_t COMPANY_ID_SIZE                   = 2;

template <typename Callback>
static bool walkAdStructures(const uint8_t* advData, uint8_t len, Callback callback) {
    if (advData == nullptr || len == 0) {
        return false;
    }
 
    uint8_t offset = 0;
    while (offset < len) {
        const uint8_t adLength = advData[offset];
 
        // Length of 0 signals the end of significant AD structures
        if (adLength == 0) {
            break;
        }
 
        // The full structure occupies (1 + adLength) bytes.
        // Verify it fits within the remaining data.
        if (offset + 1 + adLength > len) {
            break;
        }
 
        const uint8_t adType       = advData[offset + 1];
        const uint8_t* adPayload   = &advData[offset + 2];
        const uint8_t adPayloadLen = adLength - 1; // exclude the type byte
 
        if (callback(adType, adPayload, adPayloadLen)) {
            return true;
        }
 
        offset += 1 + adLength;
    }
 
    return false;
}

bool findManufacturerData(const uint8_t* advData, uint8_t len,
                           uint16_t* companyId, const uint8_t** data, uint8_t* dataLen) {
    if (companyId == nullptr || data == nullptr || dataLen == nullptr) {
        return false;
    }
 
    return walkAdStructures(advData, len,
        [&](uint8_t adType, const uint8_t* payload, uint8_t payloadLen) -> bool {
            if (adType != AD_TYPE_MANUFACTURER_SPECIFIC) {
                return false;
            }
 
            // Manufacturer data must contain at least the 2-byte company ID
            if (payloadLen < COMPANY_ID_SIZE) {
                return false;
            }
 
            // Company ID is little-endian
            *companyId = static_cast<uint16_t>(payload[0]) |
                         (static_cast<uint16_t>(payload[1]) << 8);
            *data      = &payload[COMPANY_ID_SIZE];
            *dataLen   = payloadLen - COMPANY_ID_SIZE;
 
            return true;
        });
}

bool findServiceUuid128(const uint8_t* advData, uint8_t len,
                          const uint8_t uuid128[16]) {
    if (uuid128 == nullptr) {
        return false;
    }
 
    return walkAdStructures(advData, len,
        [&](uint8_t adType, const uint8_t* payload, uint8_t payloadLen) -> bool {
            if (adType != AD_TYPE_INCOMPLETE_UUID128 && adType != AD_TYPE_COMPLETE_UUID128) {
                return false;
            }
 
            // Payload contains one or more 16-byte UUIDs packed consecutively
            uint8_t remaining = payloadLen;
            const uint8_t* ptr = payload;
            while (remaining >= UUID128_SIZE) {
                if (std::memcmp(ptr, uuid128, UUID128_SIZE) == 0) {
                    return true;
                }
                ptr       += UUID128_SIZE;
                remaining -= UUID128_SIZE;
            }
 
            return false;
        });
}

bool findName(const uint8_t* advData, uint8_t len,
                char* name, uint8_t nameMaxLen) {
    if (name == nullptr || nameMaxLen == 0) {
        return false;
    }
 
    // Ensure the output is always null-terminated even on failure
    name[0] = '\0';
 
    return walkAdStructures(advData, len,
        [&](uint8_t adType, const uint8_t* payload, uint8_t payloadLen) -> bool {
            if (adType != AD_TYPE_COMPLETE_NAME && adType != AD_TYPE_SHORTENED_NAME) {
                return false;
            }
 
            // Determine how many characters we can copy (reserve 1 byte for null)
            const uint8_t copyLen = (payloadLen < static_cast<uint8_t>(nameMaxLen - 1))
                                    ? payloadLen
                                    : static_cast<uint8_t>(nameMaxLen - 1);
 
            std::memcpy(name, payload, copyLen);
            name[copyLen] = '\0';
 
            return true;
        });
}
 
} // namespace cdc::hal::BleAdvParser