1// Copyright (C) 2016 The Qt Company Ltd.
2// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR LGPL-3.0-only OR GPL-2.0-only OR GPL-3.0-only
3// Qt-Security score:significant reason:trusted-data
4
5#include "qleadvertiser_bluez_p.h"
6
7#include "bluez/bluez_data_p.h"
8#include "bluez/hcimanager_p.h"
9#include "qbluetoothsocketbase_p.h"
10
11#include <QtCore/qloggingcategory.h>
12
13#include <cstring>
14
15QT_BEGIN_NAMESPACE
16
17Q_DECLARE_LOGGING_CATEGORY(QT_BT_BLUEZ)
18
19QLeAdvertiser::~QLeAdvertiser()
20 = default;
21
22struct AdvParams {
23 quint16 minInterval;
24 quint16 maxInterval;
25 quint8 type;
26 quint8 ownAddrType;
27 quint8 directAddrType;
28 bdaddr_t directAddr;
29 quint8 channelMap;
30 quint8 filterPolicy;
31} __attribute__ ((packed));
32
33struct AdvData {
34 quint8 length;
35 quint8 data[31];
36};
37
38struct WhiteListParams {
39 quint8 addrType;
40 bdaddr_t addr;
41};
42
43
44template <typename T>
45static QByteArray byteArrayFromStruct(const T &data)
46{
47 return QByteArray(reinterpret_cast<const char *>(&data), sizeof data);
48}
49
50QLeAdvertiserBluez::QLeAdvertiserBluez(const QLowEnergyAdvertisingParameters &params,
51 const QLowEnergyAdvertisingData &advertisingData,
52 const QLowEnergyAdvertisingData &scanResponseData,
53 std::shared_ptr<HciManager> hciManager, QObject *parent)
54 : QLeAdvertiser(params, advertisingData, scanResponseData, parent), m_hciManager(hciManager)
55{
56 Q_ASSERT(m_hciManager);
57 connect(sender: m_hciManager.get(), signal: &HciManager::commandCompleted, context: this,
58 slot: &QLeAdvertiserBluez::handleCommandCompleted);
59}
60
61QLeAdvertiserBluez::~QLeAdvertiserBluez()
62{
63 disconnect(sender: m_hciManager.get(), signal: &HciManager::commandCompleted, receiver: this,
64 slot: &QLeAdvertiserBluez::handleCommandCompleted);
65 doStopAdvertising();
66}
67
68void QLeAdvertiserBluez::doStartAdvertising()
69{
70 if (!m_hciManager->monitorEvent(event: HciManager::HciEvent::EVT_CMD_COMPLETE)) {
71 handleError();
72 return;
73 }
74
75 m_sendPowerLevel = advertisingData().includePowerLevel()
76 || scanResponseData().includePowerLevel();
77 if (m_sendPowerLevel)
78 queueReadTxPowerLevelCommand();
79 else
80 queueAdvertisingCommands();
81 sendNextCommand();
82}
83
84void QLeAdvertiserBluez::doStopAdvertising()
85{
86 toggleAdvertising(enable: false);
87 sendNextCommand();
88}
89
90void QLeAdvertiserBluez::queueCommand(QBluezConst::OpCodeCommandField ocf, const QByteArray &data)
91{
92 m_pendingCommands << Command(ocf, data);
93}
94
95void QLeAdvertiserBluez::sendNextCommand()
96{
97 if (m_pendingCommands.isEmpty()) {
98 // TODO: Unmonitor event.
99 return;
100 }
101 const Command &c = m_pendingCommands.first();
102 if (!m_hciManager->sendCommand(ogf: QBluezConst::OgfLinkControl, ocf: c.ocf, parameters: c.data)) {
103 handleError();
104 return;
105 }
106}
107
108void QLeAdvertiserBluez::queueAdvertisingCommands()
109{
110 toggleAdvertising(enable: false); // Stop advertising first, in case it's currently active.
111 setWhiteList();
112 setAdvertisingParams();
113 setAdvertisingData();
114 setScanResponseData();
115 toggleAdvertising(enable: true);
116}
117
118void QLeAdvertiserBluez::queueReadTxPowerLevelCommand()
119{
120 // Spec v4.2, Vol 2, Part E, 7.8.6
121 queueCommand(ocf: QBluezConst::OcfLeReadTxPowerLevel, data: QByteArray());
122}
123
124void QLeAdvertiserBluez::toggleAdvertising(bool enable)
125{
126 // Spec v4.2, Vol 2, Part E, 7.8.9
127 queueCommand(ocf: QBluezConst::OcfLeSetAdvEnable, data: QByteArray(1, enable));
128}
129
130void QLeAdvertiserBluez::setAdvertisingParams()
131{
132 // Spec v4.2, Vol 2, Part E, 7.8.5
133 // or Spec v5.3, Vol 4, Part E, 7.8.5
134 AdvParams params;
135 static_assert(sizeof params == 15, "unexpected struct size");
136 using namespace std;
137 memset(s: &params, c: 0, n: sizeof params);
138 setAdvertisingInterval(params);
139 params.type = parameters().mode();
140 params.filterPolicy = parameters().filterPolicy();
141 if (params.filterPolicy != QLowEnergyAdvertisingParameters::IgnoreWhiteList
142 && advertisingData().discoverability() == QLowEnergyAdvertisingData::DiscoverabilityLimited) {
143 qCWarning(QT_BT_BLUEZ) << "limited discoverability is incompatible with "
144 "using a white list; disabling filtering";
145 params.filterPolicy = QLowEnergyAdvertisingParameters::IgnoreWhiteList;
146 }
147 params.ownAddrType = QLowEnergyController::PublicAddress; // TODO: Make configurable.
148
149 // TODO: For ADV_DIRECT_IND.
150 // params.directAddrType = xxx;
151 // params.direct_bdaddr = xxx;
152
153 params.channelMap = 0x7; // All channels.
154
155 const QByteArray paramsData = byteArrayFromStruct(data: params);
156 qCDebug(QT_BT_BLUEZ) << "advertising parameters:" << paramsData.toHex();
157 queueCommand(ocf: QBluezConst::OcfLeSetAdvParams, data: paramsData);
158}
159
160static quint16 forceIntoRange(quint16 val, quint16 min, quint16 max)
161{
162 return qMin(a: qMax(a: val, b: min), b: max);
163}
164
165void QLeAdvertiserBluez::setAdvertisingInterval(AdvParams &params)
166{
167 const double multiplier = 0.625;
168 const quint16 minVal = parameters().minimumInterval() / multiplier;
169 const quint16 maxVal = parameters().maximumInterval() / multiplier;
170 Q_ASSERT(minVal <= maxVal);
171 const quint16 specMinimum =
172 parameters().mode() == QLowEnergyAdvertisingParameters::AdvScanInd
173 || parameters().mode() == QLowEnergyAdvertisingParameters::AdvNonConnInd ? 0xa0 : 0x20;
174 const quint16 specMaximum = 0x4000;
175 params.minInterval = qToLittleEndian(source: forceIntoRange(val: minVal, min: specMinimum, max: specMaximum));
176 params.maxInterval = qToLittleEndian(source: forceIntoRange(val: maxVal, min: specMinimum, max: specMaximum));
177 Q_ASSERT(params.minInterval <= params.maxInterval);
178}
179
180void QLeAdvertiserBluez::setPowerLevel(AdvData &advData)
181{
182 if (m_sendPowerLevel) {
183 advData.data[advData.length++] = 2;
184 advData.data[advData.length++]= 0xa;
185 advData.data[advData.length++] = m_powerLevel;
186 }
187}
188
189void QLeAdvertiserBluez::setFlags(AdvData &advData)
190{
191 // TODO: Discoverability flags are incompatible with ADV_DIRECT_IND
192 quint8 flags = 0;
193 if (advertisingData().discoverability() == QLowEnergyAdvertisingData::DiscoverabilityLimited)
194 flags |= 0x1;
195 else if (advertisingData().discoverability() == QLowEnergyAdvertisingData::DiscoverabilityGeneral)
196 flags |= 0x2;
197 flags |= 0x4; // "BR/EDR not supported". Otherwise clients might try to connect over Bluetooth classic.
198 if (flags) {
199 advData.data[advData.length++] = 2;
200 advData.data[advData.length++] = 0x1;
201 advData.data[advData.length++] = flags;
202 }
203}
204
205template<typename T> static quint8 servicesType(bool dataComplete);
206template<> quint8 servicesType<quint16>(bool dataComplete)
207{
208 return dataComplete ? 0x3 : 0x2;
209}
210template<> quint8 servicesType<quint32>(bool dataComplete)
211{
212 return dataComplete ? 0x5 : 0x4;
213}
214template<> quint8 servicesType<QUuid::Id128Bytes>(bool dataComplete)
215{
216 return dataComplete ? 0x7 : 0x6;
217}
218
219template<typename T>
220static void addServicesData(AdvData &data, const QList<T> &services)
221{
222 if (services.isEmpty())
223 return;
224 constexpr auto sizeofT = static_cast<int>(sizeof(T)); // signed is more convenient
225 const qsizetype spaceAvailable = sizeof data.data - data.length;
226 // Determine how many services will be set, space may limit the number
227 const qsizetype maxServices = (std::min)((spaceAvailable - 2) / sizeofT, services.size());
228 if (maxServices <= 0) {
229 qCWarning(QT_BT_BLUEZ) << "services data does not fit into advertising data packet";
230 return;
231 }
232 const bool dataComplete = maxServices == services.size();
233 if (!dataComplete) {
234 qCWarning(QT_BT_BLUEZ) << "only" << maxServices << "out of" << services.size()
235 << "services fit into the advertising data";
236 }
237 data.data[data.length++] = 1 + maxServices * sizeofT;
238 data.data[data.length++] = servicesType<T>(dataComplete);
239 for (qsizetype i = 0; i < maxServices; ++i) {
240 memcpy(data.data + data.length, &services.at(i), sizeofT);
241 data.length += sizeofT;
242 }
243}
244
245void QLeAdvertiserBluez::setServicesData(const QLowEnergyAdvertisingData &src, AdvData &dest)
246{
247 QList<quint16> services16;
248 QList<quint32> services32;
249 QList<QUuid::Id128Bytes> services128;
250 const QList<QBluetoothUuid> services = src.services();
251 for (const QBluetoothUuid &service : services) {
252 bool ok;
253 const quint16 service16 = service.toUInt16(ok: &ok);
254 if (ok) {
255 services16 << qToLittleEndian(source: service16);
256 continue;
257 }
258 const quint32 service32 = service.toUInt32(ok: &ok);
259 if (ok) {
260 services32 << qToLittleEndian(source: service32);
261 continue;
262 }
263
264 // QUuid::toBytes() is defaults to Big-Endian
265 services128 << service.toBytes(order: QSysInfo::LittleEndian);
266 }
267 addServicesData(data&: dest, services: services16);
268 addServicesData(data&: dest, services: services32);
269 addServicesData(data&: dest, services: services128);
270}
271
272void QLeAdvertiserBluez::setManufacturerData(const QLowEnergyAdvertisingData &src, AdvData &dest)
273{
274 if (src.manufacturerId() == QLowEnergyAdvertisingData::invalidManufacturerId())
275 return;
276
277 const QByteArray manufacturerData = src.manufacturerData();
278 if (dest.length >= sizeof dest.data - 1 - 1 - 2 - manufacturerData.size()) {
279 qCWarning(QT_BT_BLUEZ) << "manufacturer data does not fit into advertising data packet";
280 return;
281 }
282
283 dest.data[dest.length++] = manufacturerData.size() + 1 + 2;
284 dest.data[dest.length++] = 0xff;
285 putBtData(src: src.manufacturerId(), dst: dest.data + dest.length);
286 dest.length += sizeof(quint16);
287 std::memcpy(dest: dest.data + dest.length, src: manufacturerData.data(), n: manufacturerData.size());
288 dest.length += manufacturerData.size();
289}
290
291void QLeAdvertiserBluez::setLocalNameData(const QLowEnergyAdvertisingData &src, AdvData &dest)
292{
293 if (src.localName().isEmpty())
294 return;
295 if (dest.length >= sizeof dest.data - 3) {
296 qCWarning(QT_BT_BLUEZ) << "local name does not fit into advertising data";
297 return;
298 }
299
300 const QByteArray localNameUtf8 = src.localName().toUtf8();
301 const qsizetype fullSize = localNameUtf8.size() + 1 + 1;
302 const qsizetype size = (std::min)(a: fullSize, b: qsizetype(sizeof dest.data - dest.length));
303 const bool isComplete = size == fullSize;
304 dest.data[dest.length++] = size - 1;
305 const int dataType = isComplete ? 0x9 : 0x8;
306 dest.data[dest.length++] = dataType;
307 std::memcpy(dest: dest.data + dest.length, src: localNameUtf8, n: size - 2);
308 dest.length += size - 2;
309}
310
311void QLeAdvertiserBluez::setData(bool isScanResponseData)
312{
313 // Spec v4.2, Vol 3, Part C, 11 and Supplement, Part 1
314 AdvData theData;
315 static_assert(sizeof theData == 32, "unexpected struct size");
316 theData.length = 0;
317
318 const QLowEnergyAdvertisingData &sourceData = isScanResponseData
319 ? scanResponseData() : advertisingData();
320
321 if (const QByteArray rawData = sourceData.rawData(); !rawData.isEmpty()) {
322 theData.length = (std::min)(a: qsizetype(sizeof theData.data), b: rawData.size());
323 std::memcpy(dest: theData.data, src: rawData.data(), n: theData.length);
324 } else {
325 if (sourceData.includePowerLevel())
326 setPowerLevel(theData);
327 if (!isScanResponseData)
328 setFlags(theData);
329
330 // Insert new constant-length data here.
331
332 setLocalNameData(src: sourceData, dest&: theData);
333 setServicesData(src: sourceData, dest&: theData);
334 setManufacturerData(src: sourceData, dest&: theData);
335 }
336
337 std::memset(s: theData.data + theData.length, c: 0, n: sizeof theData.data - theData.length);
338 const QByteArray dataToSend = byteArrayFromStruct(data: theData);
339
340 if (!isScanResponseData) {
341 qCDebug(QT_BT_BLUEZ) << "advertising data:" << dataToSend.toHex();
342 queueCommand(ocf: QBluezConst::OcfLeSetAdvData, data: dataToSend);
343 } else if ((parameters().mode() == QLowEnergyAdvertisingParameters::AdvScanInd
344 || parameters().mode() == QLowEnergyAdvertisingParameters::AdvInd)
345 && theData.length > 0) {
346 qCDebug(QT_BT_BLUEZ) << "scan response data:" << dataToSend.toHex();
347 queueCommand(ocf: QBluezConst::OcfLeSetScanResponseData, data: dataToSend);
348 }
349}
350
351void QLeAdvertiserBluez::setAdvertisingData()
352{
353 // Spec v4.2, Vol 2, Part E, 7.8.7
354 setData(false);
355}
356
357void QLeAdvertiserBluez::setScanResponseData()
358{
359 // Spec v4.2, Vol 2, Part E, 7.8.8
360 setData(true);
361}
362
363void QLeAdvertiserBluez::setWhiteList()
364{
365 // Spec v4.2, Vol 2, Part E, 7.8.15-16
366 if (parameters().filterPolicy() == QLowEnergyAdvertisingParameters::IgnoreWhiteList)
367 return;
368 queueCommand(ocf: QBluezConst::OcfLeClearWhiteList, data: QByteArray());
369 const QList<QLowEnergyAdvertisingParameters::AddressInfo> whiteListInfos
370 = parameters().whiteList();
371 for (const auto &addressInfo : whiteListInfos) {
372 WhiteListParams commandParam;
373 static_assert(sizeof commandParam == 7, "unexpected struct size");
374 commandParam.addrType = addressInfo.type;
375 convertAddress(from: addressInfo.address.toUInt64(), to&: commandParam.addr.b);
376 queueCommand(ocf: QBluezConst::OcfLeAddToWhiteList, data: byteArrayFromStruct(data: commandParam));
377 }
378}
379
380void QLeAdvertiserBluez::handleCommandCompleted(quint16 opCode, quint8 status,
381 const QByteArray &data)
382{
383 if (m_pendingCommands.isEmpty())
384 return;
385 const QBluezConst::OpCodeCommandField ocf = QBluezConst::OpCodeCommandField(ocfFromOpCode(opCode));
386 const Command currentCmd = m_pendingCommands.first();
387 if (currentCmd.ocf != ocf)
388 return; // Not one of our commands.
389 m_pendingCommands.takeFirst();
390 if (status != 0) {
391 qCDebug(QT_BT_BLUEZ) << "command" << ocf
392 << "failed with status" << (HciManager::HciError)status
393 << "status code" << status;
394 if (ocf == QBluezConst::OcfLeSetAdvEnable && status == 0xc && currentCmd.data == QByteArray(1, '\0')) {
395 // we ignore OcfLeSetAdvEnable if it tries to disable an active advertisement
396 // it seems the platform often automatically turns off advertisements
397 // subsequently the explicit stopAdvertisement call fails when re-issued
398 qCDebug(QT_BT_BLUEZ) << "Advertising disable failed, ignoring";
399 sendNextCommand();
400 return;
401 }
402 if (ocf == QBluezConst::OcfLeReadTxPowerLevel) {
403 qCDebug(QT_BT_BLUEZ) << "reading power level failed, leaving it out of the "
404 "advertising data";
405 m_sendPowerLevel = false;
406 } else {
407 handleError();
408 return;
409 }
410 } else {
411 qCDebug(QT_BT_BLUEZ) << "command" << ocf << "executed successfully";
412 }
413
414 switch (ocf) {
415 case QBluezConst::OcfLeReadTxPowerLevel:
416 if (m_sendPowerLevel) {
417 if (!data.isEmpty())
418 m_powerLevel = data.at(i: 0);
419 else
420 m_powerLevel = 0;
421 qCDebug(QT_BT_BLUEZ) << "TX power level is" << m_powerLevel;
422 }
423 queueAdvertisingCommands();
424 break;
425 default:
426 break;
427 }
428
429 sendNextCommand();
430}
431
432void QLeAdvertiserBluez::handleError()
433{
434 m_pendingCommands.clear();
435 // TODO: Unmonitor event
436 emit errorOccurred();
437}
438
439QT_END_NAMESPACE
440
441#include "moc_qleadvertiser_bluez_p.cpp"
442

source code of qtconnectivity/src/bluetooth/qleadvertiser_bluez.cpp