Revision control
++ // Copyright (C) 2016 The Qt Company Ltd.
++ // SPDX-License-Identifier: LicenseRef-Qt-Commercial OR LGPL-3.0-only OR GPL-2.0-only OR GPL-3.0-only
++ // Qt-Security score:significant reason:trusted-data
++
++ #include "qleadvertiser_bluez_p.h"
++
++ #include "bluez/bluez_data_p.h"
++ #include "bluez/hcimanager_p.h"
++ #include "qbluetoothsocketbase_p.h"
++
++ #include <QtCore/qloggingcategory.h>
++
++ #include <cstring>
++
++ QT_BEGIN_NAMESPACE
++
++ Q_DECLARE_LOGGING_CATEGORY(QT_BT_BLUEZ)
++
++ QLeAdvertiser::~QLeAdvertiser()
++ = default;
++
++ struct AdvParams {
++ quint16 minInterval;
++ quint16 maxInterval;
++ quint8 type;
++ quint8 ownAddrType;
++ quint8 directAddrType;
++ bdaddr_t directAddr;
++ quint8 channelMap;
++ quint8 filterPolicy;
++ } __attribute__ ((packed));
++
++ struct AdvData {
++ quint8 length;
++ quint8 data[31];
++ };
++
++ struct WhiteListParams {
++ quint8 addrType;
++ bdaddr_t addr;
++ };
++
++
++ template <typename T>
++ static QByteArray byteArrayFromStruct(const T &data)
++ {
++ return QByteArray(reinterpret_cast<const char *>(&data), sizeof data);
++ }
++
++ QLeAdvertiserBluez::QLeAdvertiserBluez(const QLowEnergyAdvertisingParameters ¶ms,
++ const QLowEnergyAdvertisingData &advertisingData,
++ const QLowEnergyAdvertisingData &scanResponseData,
++ std::shared_ptr<HciManager> hciManager, QObject *parent)
++ : QLeAdvertiser(params, advertisingData, scanResponseData, parent), m_hciManager(hciManager)
++ {
++ Q_ASSERT(m_hciManager);
++ connect(m_hciManager.get(), &HciManager::commandCompleted, this,
++ &QLeAdvertiserBluez::handleCommandCompleted);
++ }
++
++ QLeAdvertiserBluez::~QLeAdvertiserBluez()
++ {
++ disconnect(m_hciManager.get(), &HciManager::commandCompleted, this,
++ &QLeAdvertiserBluez::handleCommandCompleted);
++ doStopAdvertising();
++ }
++
++ void QLeAdvertiserBluez::doStartAdvertising()
++ {
++ if (!m_hciManager->monitorEvent(HciManager::HciEvent::EVT_CMD_COMPLETE)) {
++ handleError();
++ return;
++ }
++
++ m_sendPowerLevel = advertisingData().includePowerLevel()
++ || scanResponseData().includePowerLevel();
++ if (m_sendPowerLevel)
++ queueReadTxPowerLevelCommand();
++ else
++ queueAdvertisingCommands();
++ sendNextCommand();
++ }
++
++ void QLeAdvertiserBluez::doStopAdvertising()
++ {
++ toggleAdvertising(false);
++ sendNextCommand();
++ }
++
++ void QLeAdvertiserBluez::queueCommand(QBluezConst::OpCodeCommandField ocf, const QByteArray &data)
++ {
++ m_pendingCommands << Command(ocf, data);
++ }
++
++ void QLeAdvertiserBluez::sendNextCommand()
++ {
++ if (m_pendingCommands.isEmpty()) {
++ // TODO: Unmonitor event.
++ return;
++ }
++ const Command &c = m_pendingCommands.first();
++ if (!m_hciManager->sendCommand(QBluezConst::OgfLinkControl, c.ocf, c.data)) {
++ handleError();
++ return;
++ }
++ }
++
++ void QLeAdvertiserBluez::queueAdvertisingCommands()
++ {
++ toggleAdvertising(false); // Stop advertising first, in case it's currently active.
++ setWhiteList();
++ setAdvertisingParams();
++ setAdvertisingData();
++ setScanResponseData();
++ toggleAdvertising(true);
++ }
++
++ void QLeAdvertiserBluez::queueReadTxPowerLevelCommand()
++ {
++ // Spec v4.2, Vol 2, Part E, 7.8.6
++ queueCommand(QBluezConst::OcfLeReadTxPowerLevel, QByteArray());
++ }
++
++ void QLeAdvertiserBluez::toggleAdvertising(bool enable)
++ {
++ // Spec v4.2, Vol 2, Part E, 7.8.9
++ queueCommand(QBluezConst::OcfLeSetAdvEnable, QByteArray(1, enable));
++ }
++
++ void QLeAdvertiserBluez::setAdvertisingParams()
++ {
++ // Spec v4.2, Vol 2, Part E, 7.8.5
++ // or Spec v5.3, Vol 4, Part E, 7.8.5
++ AdvParams params;
++ static_assert(sizeof params == 15, "unexpected struct size");
++ using namespace std;
++ memset(¶ms, 0, sizeof params);
++ setAdvertisingInterval(params);
++ params.type = parameters().mode();
++ params.filterPolicy = parameters().filterPolicy();
++ if (params.filterPolicy != QLowEnergyAdvertisingParameters::IgnoreWhiteList
++ && advertisingData().discoverability() == QLowEnergyAdvertisingData::DiscoverabilityLimited) {
++ qCWarning(QT_BT_BLUEZ) << "limited discoverability is incompatible with "
++ "using a white list; disabling filtering";
++ params.filterPolicy = QLowEnergyAdvertisingParameters::IgnoreWhiteList;
++ }
++ params.ownAddrType = QLowEnergyController::PublicAddress; // TODO: Make configurable.
++
++ // TODO: For ADV_DIRECT_IND.
++ // params.directAddrType = xxx;
++ // params.direct_bdaddr = xxx;
++
++ params.channelMap = 0x7; // All channels.
++
++ const QByteArray paramsData = byteArrayFromStruct(params);
++ qCDebug(QT_BT_BLUEZ) << "advertising parameters:" << paramsData.toHex();
++ queueCommand(QBluezConst::OcfLeSetAdvParams, paramsData);
++ }
++
++ static quint16 forceIntoRange(quint16 val, quint16 min, quint16 max)
++ {
++ return qMin(qMax(val, min), max);
++ }
++
++ void QLeAdvertiserBluez::setAdvertisingInterval(AdvParams ¶ms)
++ {
++ const double multiplier = 0.625;
++ const quint16 minVal = parameters().minimumInterval() / multiplier;
++ const quint16 maxVal = parameters().maximumInterval() / multiplier;
++ Q_ASSERT(minVal <= maxVal);
++ const quint16 specMinimum =
++ parameters().mode() == QLowEnergyAdvertisingParameters::AdvScanInd
++ || parameters().mode() == QLowEnergyAdvertisingParameters::AdvNonConnInd ? 0xa0 : 0x20;
++ const quint16 specMaximum = 0x4000;
++ params.minInterval = qToLittleEndian(forceIntoRange(minVal, specMinimum, specMaximum));
++ params.maxInterval = qToLittleEndian(forceIntoRange(maxVal, specMinimum, specMaximum));
++ Q_ASSERT(params.minInterval <= params.maxInterval);
++ }
++
++ void QLeAdvertiserBluez::setPowerLevel(AdvData &advData)
++ {
++ if (m_sendPowerLevel) {
++ advData.data[advData.length++] = 2;
++ advData.data[advData.length++]= 0xa;
++ advData.data[advData.length++] = m_powerLevel;
++ }
++ }
++
++ void QLeAdvertiserBluez::setFlags(AdvData &advData)
++ {
++ // TODO: Discoverability flags are incompatible with ADV_DIRECT_IND
++ quint8 flags = 0;
++ if (advertisingData().discoverability() == QLowEnergyAdvertisingData::DiscoverabilityLimited)
++ flags |= 0x1;
++ else if (advertisingData().discoverability() == QLowEnergyAdvertisingData::DiscoverabilityGeneral)
++ flags |= 0x2;
++ flags |= 0x4; // "BR/EDR not supported". Otherwise clients might try to connect over Bluetooth classic.
++ if (flags) {
++ advData.data[advData.length++] = 2;
++ advData.data[advData.length++] = 0x1;
++ advData.data[advData.length++] = flags;
++ }
++ }
++
++ template<typename T> static quint8 servicesType(bool dataComplete);
++ template<> quint8 servicesType<quint16>(bool dataComplete)
++ {
++ return dataComplete ? 0x3 : 0x2;
++ }
++ template<> quint8 servicesType<quint32>(bool dataComplete)
++ {
++ return dataComplete ? 0x5 : 0x4;
++ }
++ template<> quint8 servicesType<QUuid::Id128Bytes>(bool dataComplete)
++ {
++ return dataComplete ? 0x7 : 0x6;
++ }
++
++ template<typename T>
++ static void addServicesData(AdvData &data, const QList<T> &services)
++ {
++ if (services.isEmpty())
++ return;
++ constexpr auto sizeofT = static_cast<int>(sizeof(T)); // signed is more convenient
++ const qsizetype spaceAvailable = sizeof data.data - data.length;
++ // Determine how many services will be set, space may limit the number
++ const qsizetype maxServices = (std::min)((spaceAvailable - 2) / sizeofT, services.size());
++ if (maxServices <= 0) {
++ qCWarning(QT_BT_BLUEZ) << "services data does not fit into advertising data packet";
++ return;
++ }
++ const bool dataComplete = maxServices == services.size();
++ if (!dataComplete) {
++ qCWarning(QT_BT_BLUEZ) << "only" << maxServices << "out of" << services.size()
++ << "services fit into the advertising data";
++ }
++ data.data[data.length++] = 1 + maxServices * sizeofT;
++ data.data[data.length++] = servicesType<T>(dataComplete);
++ for (qsizetype i = 0; i < maxServices; ++i) {
++ memcpy(data.data + data.length, &services.at(i), sizeofT);
++ data.length += sizeofT;
++ }
++ }
++
++ void QLeAdvertiserBluez::setServicesData(const QLowEnergyAdvertisingData &src, AdvData &dest)
++ {
++ QList<quint16> services16;
++ QList<quint32> services32;
++ QList<QUuid::Id128Bytes> services128;
++ const QList<QBluetoothUuid> services = src.services();
++ for (const QBluetoothUuid &service : services) {
++ bool ok;
++ const quint16 service16 = service.toUInt16(&ok);
++ if (ok) {
++ services16 << qToLittleEndian(service16);
++ continue;
++ }
++ const quint32 service32 = service.toUInt32(&ok);
++ if (ok) {
++ services32 << qToLittleEndian(service32);
++ continue;
++ }
++
++ // QUuid::toBytes() is defaults to Big-Endian
++ services128 << service.toBytes(QSysInfo::LittleEndian);
++ }
++ addServicesData(dest, services16);
++ addServicesData(dest, services32);
++ addServicesData(dest, services128);
++ }
++
++ void QLeAdvertiserBluez::setManufacturerData(const QLowEnergyAdvertisingData &src, AdvData &dest)
++ {
++ if (src.manufacturerId() == QLowEnergyAdvertisingData::invalidManufacturerId())
++ return;
++
++ const QByteArray manufacturerData = src.manufacturerData();
++ if (qsizetype(sizeof dest.data) - dest.length < 1 + 1 + 2 + manufacturerData.size()) {
++ qCWarning(QT_BT_BLUEZ) << "manufacturer data does not fit into advertising data packet";
++ return;
++ }
++
++ dest.data[dest.length++] = manufacturerData.size() + 1 + 2;
++ dest.data[dest.length++] = 0xff;
++ putBtData(src.manufacturerId(), dest.data + dest.length);
++ dest.length += sizeof(quint16);
++ std::memcpy(dest.data + dest.length, manufacturerData.data(), manufacturerData.size());
++ dest.length += manufacturerData.size();
++ }
++
++ void QLeAdvertiserBluez::setLocalNameData(const QLowEnergyAdvertisingData &src, AdvData &dest)
++ {
++ if (src.localName().isEmpty())
++ return;
++ if (dest.length >= sizeof dest.data - 3) {
++ qCWarning(QT_BT_BLUEZ) << "local name does not fit into advertising data";
++ return;
++ }
++
++ const QByteArray localNameUtf8 = src.localName().toUtf8();
++ const qsizetype fullSize = localNameUtf8.size() + 1 + 1;
++ const qsizetype size = (std::min)(fullSize, qsizetype(sizeof dest.data - dest.length));
++ const bool isComplete = size == fullSize;
++ dest.data[dest.length++] = size - 1;
++ const int dataType = isComplete ? 0x9 : 0x8;
++ dest.data[dest.length++] = dataType;
++ std::memcpy(dest.data + dest.length, localNameUtf8, size - 2);
++ dest.length += size - 2;
++ }
++
++ void QLeAdvertiserBluez::setData(bool isScanResponseData)
++ {
++ // Spec v4.2, Vol 3, Part C, 11 and Supplement, Part 1
++ AdvData theData;
++ static_assert(sizeof theData == 32, "unexpected struct size");
++ theData.length = 0;
++
++ const QLowEnergyAdvertisingData &sourceData = isScanResponseData
++ ? scanResponseData() : advertisingData();
++
++ if (const QByteArray rawData = sourceData.rawData(); !rawData.isEmpty()) {
++ theData.length = (std::min)(qsizetype(sizeof theData.data), rawData.size());
++ std::memcpy(theData.data, rawData.data(), theData.length);
++ } else {
++ if (sourceData.includePowerLevel())
++ setPowerLevel(theData);
++ if (!isScanResponseData)
++ setFlags(theData);
++
++ // Insert new constant-length data here.
++
++ setLocalNameData(sourceData, theData);
++ setServicesData(sourceData, theData);
++ setManufacturerData(sourceData, theData);
++ }
++
++ std::memset(theData.data + theData.length, 0, sizeof theData.data - theData.length);
++ const QByteArray dataToSend = byteArrayFromStruct(theData);
++
++ if (!isScanResponseData) {
++ qCDebug(QT_BT_BLUEZ) << "advertising data:" << dataToSend.toHex();
++ queueCommand(QBluezConst::OcfLeSetAdvData, dataToSend);
++ } else if ((parameters().mode() == QLowEnergyAdvertisingParameters::AdvScanInd
++ || parameters().mode() == QLowEnergyAdvertisingParameters::AdvInd)
++ && theData.length > 0) {
++ qCDebug(QT_BT_BLUEZ) << "scan response data:" << dataToSend.toHex();
++ queueCommand(QBluezConst::OcfLeSetScanResponseData, dataToSend);
++ }
++ }
++
++ void QLeAdvertiserBluez::setAdvertisingData()
++ {
++ // Spec v4.2, Vol 2, Part E, 7.8.7
++ setData(false);
++ }
++
++ void QLeAdvertiserBluez::setScanResponseData()
++ {
++ // Spec v4.2, Vol 2, Part E, 7.8.8
++ setData(true);
++ }
++
++ void QLeAdvertiserBluez::setWhiteList()
++ {
++ // Spec v4.2, Vol 2, Part E, 7.8.15-16
++ if (parameters().filterPolicy() == QLowEnergyAdvertisingParameters::IgnoreWhiteList)
++ return;
++ queueCommand(QBluezConst::OcfLeClearWhiteList, QByteArray());
++ const QList<QLowEnergyAdvertisingParameters::AddressInfo> whiteListInfos
++ = parameters().whiteList();
++ for (const auto &addressInfo : whiteListInfos) {
++ WhiteListParams commandParam;
++ static_assert(sizeof commandParam == 7, "unexpected struct size");
++ commandParam.addrType = addressInfo.type;
++ convertAddress(addressInfo.address.toUInt64(), commandParam.addr.b);
++ queueCommand(QBluezConst::OcfLeAddToWhiteList, byteArrayFromStruct(commandParam));
++ }
++ }
++
++ void QLeAdvertiserBluez::handleCommandCompleted(quint16 opCode, quint8 status,
++ const QByteArray &data)
++ {
++ if (m_pendingCommands.isEmpty())
++ return;
++ const QBluezConst::OpCodeCommandField ocf = QBluezConst::OpCodeCommandField(ocfFromOpCode(opCode));
++ const Command currentCmd = m_pendingCommands.first();
++ if (currentCmd.ocf != ocf)
++ return; // Not one of our commands.
++ m_pendingCommands.takeFirst();
++ if (status != 0) {
++ qCDebug(QT_BT_BLUEZ) << "command" << ocf
++ << "failed with status" << (HciManager::HciError)status
++ << "status code" << status;
++ if (ocf == QBluezConst::OcfLeSetAdvEnable && status == 0xc && currentCmd.data == QByteArray(1, '\0')) {
++ // we ignore OcfLeSetAdvEnable if it tries to disable an active advertisement
++ // it seems the platform often automatically turns off advertisements
++ // subsequently the explicit stopAdvertisement call fails when re-issued
++ qCDebug(QT_BT_BLUEZ) << "Advertising disable failed, ignoring";
++ sendNextCommand();
++ return;
++ }
++ if (ocf == QBluezConst::OcfLeReadTxPowerLevel) {
++ qCDebug(QT_BT_BLUEZ) << "reading power level failed, leaving it out of the "
++ "advertising data";
++ m_sendPowerLevel = false;
++ } else {
++ handleError();
++ return;
++ }
++ } else {
++ qCDebug(QT_BT_BLUEZ) << "command" << ocf << "executed successfully";
++ }
++
++ switch (ocf) {
++ case QBluezConst::OcfLeReadTxPowerLevel:
++ if (m_sendPowerLevel) {
++ if (!data.isEmpty())
++ m_powerLevel = data.at(0);
++ else
++ m_powerLevel = 0;
++ qCDebug(QT_BT_BLUEZ) << "TX power level is" << m_powerLevel;
++ }
++ queueAdvertisingCommands();
++ break;
++ default:
++ break;
++ }
++
++ sendNextCommand();
++ }
++
++ void QLeAdvertiserBluez::handleError()
++ {
++ m_pendingCommands.clear();
++ // TODO: Unmonitor event
++ emit errorOccurred();
++ }
++
++ QT_END_NAMESPACE
++
++ #include "moc_qleadvertiser_bluez_p.cpp"