1 /*
2 * Copyright 2022 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 #include "hci/distance_measurement_manager.h"
17
18 #include <bluetooth/log.h>
19 #include <com_android_bluetooth_flags.h>
20 #include <math.h>
21
22 #include <complex>
23 #include <unordered_map>
24
25 #include "acl_manager/assembler.h"
26 #include "common/strings.h"
27 #include "hal/ranging_hal.h"
28 #include "hci/acl_manager.h"
29 #include "hci/distance_measurement_interface.h"
30 #include "hci/event_checkers.h"
31 #include "hci/hci_layer.h"
32 #include "module.h"
33 #include "os/handler.h"
34 #include "os/log.h"
35 #include "os/repeating_alarm.h"
36 #include "packet/packet_view.h"
37 #include "ras/ras_packets.h"
38
39 using namespace bluetooth::ras;
40 using bluetooth::hci::acl_manager::PacketViewForRecombination;
41
42 namespace bluetooth {
43 namespace hci {
44
45 const ModuleFactory DistanceMeasurementManager::Factory =
__anon69390fc90102() 46 ModuleFactory([]() { return new DistanceMeasurementManager(); });
47 static constexpr uint16_t kIllegalConnectionHandle = 0xffff;
48 static constexpr uint8_t kTxPowerNotAvailable = 0xfe;
49 static constexpr int8_t kRSSIDropOffAt1M = 41;
50 static constexpr uint8_t kCsMaxTxPower = 12; // 12 dBm
51 static constexpr CsSyncAntennaSelection kCsSyncAntennaSelection = CsSyncAntennaSelection::ANTENNA_2;
52 static constexpr uint8_t kConfigId = 0x01; // Use 0x01 to create config and enable procedure
53 static constexpr uint8_t kMinMainModeSteps = 0x02;
54 static constexpr uint8_t kMaxMainModeSteps = 0x05;
55 static constexpr uint8_t kMainModeRepetition = 0x00; // No repetition
56 static constexpr uint8_t kMode0Steps =
57 0x03; // Maximum number of mode-0 steps to increase success subevent rate
58 static constexpr uint8_t kChannelMapRepetition = 0x01; // No repetition
59 static constexpr uint8_t kCh3cJump = 0x03; // Skip 3 Channels
60 static constexpr uint16_t kMaxProcedureLen = 0x4E20; // 12.5s
61 static constexpr uint16_t kMinProcedureInterval = 0x01;
62 static constexpr uint16_t kMaxProcedureInterval = 0xFF;
63 static constexpr uint16_t kMaxProcedureCount = 0x01;
64 static constexpr uint32_t kMinSubeventLen = 0x0004E2; // 1250us
65 static constexpr uint32_t kMaxSubeventLen = 0x3d0900; // 4s
66 static constexpr uint8_t kToneAntennaConfigSelection = 0x00; // 1x1
67 static constexpr uint8_t kTxPwrDelta = 0x00;
68 static constexpr uint8_t kProcedureDataBufferSize = 0x10; // Buffer size of Procedure data
69 static constexpr uint16_t kMtuForRasData = 507; // 512 - 5
70 static constexpr uint16_t kRangingCounterMask = 0x0FFF;
71
72 struct DistanceMeasurementManager::impl : bluetooth::hal::RangingHalCallback {
73 struct CsProcedureData {
CsProcedureDatabluetooth::hci::DistanceMeasurementManager::impl::CsProcedureData74 CsProcedureData(
75 uint16_t procedure_counter,
76 uint8_t num_antenna_paths,
77 uint8_t configuration_id,
78 uint8_t selected_tx_power)
79 : counter(procedure_counter), num_antenna_paths(num_antenna_paths) {
80 local_status = CsProcedureDoneStatus::PARTIAL_RESULTS;
81 remote_status = CsProcedureDoneStatus::PARTIAL_RESULTS;
82 // In ascending order of antenna position with tone extension data at the end
83 uint16_t num_tone_data = num_antenna_paths + 1;
84 for (uint8_t i = 0; i < num_tone_data; i++) {
85 std::vector<std::complex<double>> empty_complex_vector;
86 tone_pct_initiator.push_back(empty_complex_vector);
87 tone_pct_reflector.push_back(empty_complex_vector);
88 std::vector<uint8_t> empty_vector;
89 tone_quality_indicator_initiator.push_back(empty_vector);
90 tone_quality_indicator_reflector.push_back(empty_vector);
91 }
92 // RAS data
93 segmentation_header_.first_segment_ = 1;
94 segmentation_header_.last_segment_ = 0;
95 segmentation_header_.rolling_segment_counter_ = 0;
96 ranging_header_.ranging_counter_ = counter;
97 ranging_header_.configuration_id_ = configuration_id;
98 ranging_header_.selected_tx_power_ = selected_tx_power;
99 ranging_header_.antenna_paths_mask_ = 0;
100 for (uint8_t i = 0; i < num_antenna_paths; i++) {
101 ranging_header_.antenna_paths_mask_ |= (1 << i);
102 }
103 ranging_header_.pct_format_ = PctFormat::IQ;
104 }
105 // Procedure counter
106 uint16_t counter;
107 // Number of antenna paths (1 to 4) reported in the procedure
108 uint8_t num_antenna_paths;
109 // Frequency Compensation indicates fractional frequency offset (FFO) value of initiator, in
110 // 0.01ppm
111 std::vector<uint16_t> frequency_compensation;
112 // The channel indices of every step in a CS procedure (in time order)
113 std::vector<uint8_t> step_channel;
114 // Measured Frequency Offset from mode 0, relative to the remote device, in 0.01ppm
115 std::vector<uint16_t> measured_freq_offset;
116 // Initiator's PCT (complex value) measured from mode-2 or mode-3 steps in a CS procedure (in
117 // time order)
118 std::vector<std::vector<std::complex<double>>> tone_pct_initiator;
119 // Reflector's PCT (complex value) measured from mode-2 or mode-3 steps in a CS procedure (in
120 // time order)
121 std::vector<std::vector<std::complex<double>>> tone_pct_reflector;
122 std::vector<std::vector<uint8_t>> tone_quality_indicator_initiator;
123 std::vector<std::vector<uint8_t>> tone_quality_indicator_reflector;
124 CsProcedureDoneStatus local_status;
125 CsProcedureDoneStatus remote_status;
126 // If the procedure is aborted by either the local or remote side.
127 bool aborted = false;
128 // RAS data
129 SegmentationHeader segmentation_header_;
130 RangingHeader ranging_header_;
131 std::vector<uint8_t> ras_raw_data_; // raw data for multi_subevents;
132 uint16_t ras_raw_data_index_ = 0;
133 RasSubeventHeader ras_subevent_header_;
134 std::vector<uint8_t> ras_subevent_data_;
135 uint8_t ras_subevent_counter_ = 0;
136 };
137
OnOpenedbluetooth::hci::DistanceMeasurementManager::impl138 void OnOpened(
139 uint16_t connection_handle,
140 const std::vector<bluetooth::hal::VendorSpecificCharacteristic>& vendor_specific_reply) {
141 log::info(
142 "connection_handle:0x{:04x}, vendor_specific_reply size:{}",
143 connection_handle,
144 vendor_specific_reply.size());
145 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
146 log::error("Can't find CS tracker for connection_handle {}", connection_handle);
147 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
148 cs_trackers_[connection_handle].address, REASON_INTERNAL_ERROR, METHOD_CS);
149 return;
150 }
151
152 auto& tracker = cs_trackers_[connection_handle];
153 if (!vendor_specific_reply.empty()) {
154 // Send reply to remote
155 distance_measurement_callbacks_->OnVendorSpecificReply(
156 tracker.address, vendor_specific_reply);
157 return;
158 }
159
160 start_distance_measurement_with_cs(tracker.address, connection_handle, tracker.interval_ms);
161 }
162
OnOpenFailedbluetooth::hci::DistanceMeasurementManager::impl163 void OnOpenFailed(uint16_t connection_handle) {
164 log::info("connection_handle:0x{:04x}", connection_handle);
165 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
166 cs_trackers_[connection_handle].address, REASON_INTERNAL_ERROR, METHOD_CS);
167 }
168
OnHandleVendorSpecificReplyCompletebluetooth::hci::DistanceMeasurementManager::impl169 void OnHandleVendorSpecificReplyComplete(uint16_t connection_handle, bool success) {
170 log::info("connection_handle:0x{:04x}, success:{}", connection_handle, success);
171 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
172 log::error("Can't find CS tracker for connection_handle {}", connection_handle);
173 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
174 cs_trackers_[connection_handle].address, REASON_INTERNAL_ERROR, METHOD_CS);
175 return;
176 }
177 distance_measurement_callbacks_->OnHandleVendorSpecificReplyComplete(
178 cs_trackers_[connection_handle].address, success);
179 }
180
OnResultbluetooth::hci::DistanceMeasurementManager::impl181 void OnResult(uint16_t connection_handle, const bluetooth::hal::RangingResult& ranging_result) {
182 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
183 log::warn("Can't find CS tracker for connection_handle {}", connection_handle);
184 return;
185 }
186 log::debug(
187 "address {}, resultMeters {}",
188 cs_trackers_[connection_handle].address,
189 ranging_result.result_meters_);
190 distance_measurement_callbacks_->OnDistanceMeasurementResult(
191 cs_trackers_[connection_handle].address,
192 ranging_result.result_meters_ * 100,
193 0.0,
194 -1,
195 -1,
196 -1,
197 -1,
198 DistanceMeasurementMethod::METHOD_CS);
199 }
200
~implbluetooth::hci::DistanceMeasurementManager::impl201 ~impl() {}
startbluetooth::hci::DistanceMeasurementManager::impl202 void start(
203 os::Handler* handler,
204 hal::RangingHal* ranging_hal,
205 hci::HciLayer* hci_layer,
206 hci::AclManager* acl_manager) {
207 handler_ = handler;
208 ranging_hal_ = ranging_hal;
209 hci_layer_ = hci_layer;
210 acl_manager_ = acl_manager;
211 hci_layer_->RegisterLeEventHandler(
212 hci::SubeventCode::TRANSMIT_POWER_REPORTING,
213 handler_->BindOn(this, &impl::on_transmit_power_reporting));
214 if (!com::android::bluetooth::flags::channel_sounding_in_stack()) {
215 log::info("IS_FLAG_ENABLED channel_sounding_in_stack: false");
216 return;
217 }
218 distance_measurement_interface_ = hci_layer_->GetDistanceMeasurementInterface(
219 handler_->BindOn(this, &DistanceMeasurementManager::impl::handle_event));
220 distance_measurement_interface_->EnqueueCommand(
221 LeCsReadLocalSupportedCapabilitiesBuilder::Create(),
222 handler_->BindOnceOn(this, &impl::on_cs_read_local_supported_capabilities));
223 if (ranging_hal_->IsBound()) {
224 ranging_hal_->RegisterCallback(this);
225 };
226 }
227
stopbluetooth::hci::DistanceMeasurementManager::impl228 void stop() {
229 hci_layer_->UnregisterLeEventHandler(hci::SubeventCode::TRANSMIT_POWER_REPORTING);
230 }
231
register_distance_measurement_callbacksbluetooth::hci::DistanceMeasurementManager::impl232 void register_distance_measurement_callbacks(DistanceMeasurementCallbacks* callbacks) {
233 distance_measurement_callbacks_ = callbacks;
234 if (ranging_hal_->IsBound()) {
235 distance_measurement_callbacks_->OnVendorSpecificCharacteristics(
236 ranging_hal_->GetVendorSpecificCharacteristics());
237 }
238 }
239
start_distance_measurementbluetooth::hci::DistanceMeasurementManager::impl240 void start_distance_measurement(
241 const Address& address, uint16_t interval, DistanceMeasurementMethod method) {
242 log::info("Address:{}, method:{}", address, method);
243 uint16_t connection_handle = acl_manager_->HACK_GetLeHandle(address);
244
245 // Remove this check if we support any connection less method
246 if (connection_handle == kIllegalConnectionHandle) {
247 log::warn("Can't find any LE connection for {}", address);
248 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
249 address, REASON_NO_LE_CONNECTION, method);
250 return;
251 }
252
253 switch (method) {
254 case METHOD_AUTO:
255 case METHOD_RSSI: {
256 if (rssi_trackers.find(address) == rssi_trackers.end()) {
257 rssi_trackers[address].handle = connection_handle;
258 rssi_trackers[address].interval_ms = interval;
259 rssi_trackers[address].remote_tx_power = kTxPowerNotAvailable;
260 rssi_trackers[address].started = false;
261 rssi_trackers[address].repeating_alarm = std::make_unique<os::RepeatingAlarm>(handler_);
262 hci_layer_->EnqueueCommand(
263 LeReadRemoteTransmitPowerLevelBuilder::Create(
264 acl_manager_->HACK_GetLeHandle(address), 0x01),
265 handler_->BindOnceOn(
266 this, &impl::on_read_remote_transmit_power_level_status, address));
267 } else {
268 rssi_trackers[address].interval_ms = interval;
269 }
270 } break;
271 case METHOD_CS: {
272 start_distance_measurement_with_cs(address, connection_handle, interval);
273 } break;
274 }
275 }
276
start_distance_measurement_with_csbluetooth::hci::DistanceMeasurementManager::impl277 void start_distance_measurement_with_cs(
278 const Address& cs_remote_address, uint16_t connection_handle, uint16_t interval) {
279 log::info("connection_handle: {}, address: {}", connection_handle, cs_remote_address);
280 if (!com::android::bluetooth::flags::channel_sounding_in_stack()) {
281 log::error("Channel Sounding is not enabled");
282 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
283 cs_remote_address, REASON_INTERNAL_ERROR, METHOD_CS);
284 return;
285 }
286
287 if (cs_trackers_.find(connection_handle) != cs_trackers_.end() &&
288 cs_trackers_[connection_handle].address != cs_remote_address) {
289 log::warn("Remove old tracker for {}", cs_remote_address);
290 cs_trackers_.erase(connection_handle);
291 }
292
293 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
294 // Create a cs tracker with role initiator
295 cs_trackers_[connection_handle].address = cs_remote_address;
296 // TODO: Check ROLE via CS config. (b/304295768)
297 cs_trackers_[connection_handle].role = CsRole::INITIATOR;
298 cs_trackers_[connection_handle].repeating_alarm =
299 std::make_unique<os::RepeatingAlarm>(handler_);
300 }
301 cs_trackers_[connection_handle].interval_ms = interval;
302 cs_trackers_[connection_handle].waiting_for_start_callback = true;
303
304 if (!cs_trackers_[connection_handle].ras_connected) {
305 log::info("Waiting for RAS connected");
306 return;
307 }
308
309 if (!cs_trackers_[connection_handle].setup_complete) {
310 send_le_cs_read_remote_supported_capabilities(connection_handle);
311 return;
312 }
313 if (!cs_trackers_[connection_handle].config_set) {
314 send_le_cs_create_config(connection_handle);
315 return;
316 }
317 log::info(
318 "enable cs procedure regularly with interval: {} ms",
319 cs_trackers_[connection_handle].interval_ms);
320 cs_trackers_[connection_handle].repeating_alarm->Cancel();
321 send_le_cs_procedure_enable(connection_handle, Enable::ENABLED);
322 cs_trackers_[connection_handle].repeating_alarm->Schedule(
323 common::Bind(
324 &impl::send_le_cs_procedure_enable,
325 common::Unretained(this),
326 connection_handle,
327 Enable::ENABLED),
328 std::chrono::milliseconds(cs_trackers_[connection_handle].interval_ms));
329 }
330
stop_distance_measurementbluetooth::hci::DistanceMeasurementManager::impl331 void stop_distance_measurement(const Address& address, DistanceMeasurementMethod method) {
332 log::info("Address:{}, method:{}", address, method);
333 switch (method) {
334 case METHOD_AUTO:
335 case METHOD_RSSI: {
336 if (rssi_trackers.find(address) == rssi_trackers.end()) {
337 log::warn("Can't find rssi tracker for {}", address);
338 } else {
339 hci_layer_->EnqueueCommand(
340 LeSetTransmitPowerReportingEnableBuilder::Create(
341 rssi_trackers[address].handle, 0x00, 0x00),
342 handler_->BindOnce(check_complete<LeSetTransmitPowerReportingEnableCompleteView>));
343 rssi_trackers[address].repeating_alarm->Cancel();
344 rssi_trackers[address].repeating_alarm.reset();
345 rssi_trackers.erase(address);
346 }
347 } break;
348 case METHOD_CS: {
349 uint16_t connection_handle = acl_manager_->HACK_GetLeHandle(address);
350 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
351 log::warn("Can't find CS tracker for {}", address);
352 } else {
353 cs_trackers_[connection_handle].repeating_alarm->Cancel();
354 cs_trackers_[connection_handle].repeating_alarm.reset();
355 send_le_cs_procedure_enable(connection_handle, Enable::DISABLED);
356 cs_trackers_.erase(connection_handle);
357 }
358 } break;
359 }
360 }
361
handle_ras_connected_eventbluetooth::hci::DistanceMeasurementManager::impl362 void handle_ras_connected_event(
363 const Address address,
364 uint16_t att_handle,
365 const std::vector<hal::VendorSpecificCharacteristic> vendor_specific_data) {
366 uint16_t connection_handle = acl_manager_->HACK_GetLeHandle(address);
367 log::info(
368 "address:{}, connection_handle 0x{:04x}, att_handle 0x{:04x}, size of "
369 "vendor_specific_data {}",
370 address,
371 connection_handle,
372 att_handle,
373 vendor_specific_data.size());
374
375 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
376 log::warn("can't find tracker for 0x{:04x}", connection_handle);
377 return;
378 }
379 auto& tracker = cs_trackers_[connection_handle];
380 if (tracker.ras_connected) {
381 log::debug("Already connected");
382 return;
383 }
384 tracker.ras_connected = true;
385
386 if (ranging_hal_->IsBound()) {
387 ranging_hal_->OpenSession(connection_handle, att_handle, vendor_specific_data);
388 return;
389 }
390 start_distance_measurement_with_cs(tracker.address, connection_handle, tracker.interval_ms);
391 }
392
handle_vendor_specific_replybluetooth::hci::DistanceMeasurementManager::impl393 void handle_vendor_specific_reply(
394 const Address address,
395 const std::vector<hal::VendorSpecificCharacteristic> vendor_specific_reply) {
396 uint16_t connection_handle = acl_manager_->HACK_GetLeHandle(address);
397 cs_trackers_[connection_handle].address = address;
398 if (ranging_hal_->IsBound()) {
399 ranging_hal_->HandleVendorSpecificReply(connection_handle, vendor_specific_reply);
400 return;
401 }
402 }
403
handle_vendor_specific_reply_completebluetooth::hci::DistanceMeasurementManager::impl404 void handle_vendor_specific_reply_complete(const Address address, bool success) {
405 uint16_t connection_handle = acl_manager_->HACK_GetLeHandle(address);
406 log::info(
407 "address:{}, connection_handle:0x{:04x}, success:{}", address, connection_handle, success);
408 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
409 log::warn("can't find tracker for 0x{:04x}", connection_handle);
410 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
411 address, REASON_INTERNAL_ERROR, METHOD_CS);
412 return;
413 }
414
415 if (!success) {
416 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
417 address, REASON_INTERNAL_ERROR, METHOD_CS);
418 return;
419 }
420
421 auto& tracker = cs_trackers_[connection_handle];
422 start_distance_measurement_with_cs(tracker.address, connection_handle, tracker.interval_ms);
423 }
424
send_read_rssibluetooth::hci::DistanceMeasurementManager::impl425 void send_read_rssi(const Address& address) {
426 if (rssi_trackers.find(address) == rssi_trackers.end()) {
427 log::warn("Can't find rssi tracker for {}", address);
428 return;
429 }
430 uint16_t connection_handle = acl_manager_->HACK_GetLeHandle(address);
431 if (connection_handle == kIllegalConnectionHandle) {
432 log::warn("Can't find connection for {}", address);
433 if (rssi_trackers.find(address) != rssi_trackers.end()) {
434 distance_measurement_callbacks_->OnDistanceMeasurementStopped(
435 address, REASON_NO_LE_CONNECTION, METHOD_RSSI);
436 rssi_trackers[address].repeating_alarm->Cancel();
437 rssi_trackers[address].repeating_alarm.reset();
438 rssi_trackers.erase(address);
439 }
440 return;
441 }
442
443 hci_layer_->EnqueueCommand(
444 ReadRssiBuilder::Create(connection_handle),
445 handler_->BindOnceOn(this, &impl::on_read_rssi_complete, address));
446 }
447
handle_eventbluetooth::hci::DistanceMeasurementManager::impl448 void handle_event(LeMetaEventView event) {
449 if (!event.IsValid()) {
450 log::error("Received invalid LeMetaEventView");
451 return;
452 }
453 switch (event.GetSubeventCode()) {
454 case hci::SubeventCode::LE_CS_TEST_END_COMPLETE:
455 case hci::SubeventCode::LE_CS_READ_REMOTE_FAE_TABLE_COMPLETE: {
456 log::warn("Unhandled subevent {}", hci::SubeventCodeText(event.GetSubeventCode()));
457 } break;
458 case hci::SubeventCode::LE_CS_SUBEVENT_RESULT_CONTINUE:
459 case hci::SubeventCode::LE_CS_SUBEVENT_RESULT: {
460 on_cs_subevent(event);
461 } break;
462 case hci::SubeventCode::LE_CS_PROCEDURE_ENABLE_COMPLETE: {
463 on_cs_procedure_enable_complete(LeCsProcedureEnableCompleteView::Create(event));
464 } break;
465 case hci::SubeventCode::LE_CS_CONFIG_COMPLETE: {
466 on_cs_config_complete(LeCsConfigCompleteView::Create(event));
467 } break;
468 case hci::SubeventCode::LE_CS_SECURITY_ENABLE_COMPLETE: {
469 on_cs_security_enable_complete(LeCsSecurityEnableCompleteView::Create(event));
470 } break;
471 case hci::SubeventCode::LE_CS_READ_REMOTE_SUPPORTED_CAPABILITIES_COMPLETE: {
472 on_cs_read_remote_supported_capabilities_complete(
473 LeCsReadRemoteSupportedCapabilitiesCompleteView::Create(event));
474 } break;
475 default:
476 log::info("Unknown subevent {}", hci::SubeventCodeText(event.GetSubeventCode()));
477 }
478 }
479
send_le_cs_read_remote_supported_capabilitiesbluetooth::hci::DistanceMeasurementManager::impl480 void send_le_cs_read_remote_supported_capabilities(uint16_t connection_handle) {
481 hci_layer_->EnqueueCommand(
482 LeCsReadRemoteSupportedCapabilitiesBuilder::Create(connection_handle),
483 handler_->BindOnce(check_status<LeCsReadRemoteSupportedCapabilitiesStatusView>));
484 }
485
send_le_cs_security_enablebluetooth::hci::DistanceMeasurementManager::impl486 void send_le_cs_security_enable(uint16_t connection_handle) {
487 hci_layer_->EnqueueCommand(
488 LeCsSecurityEnableBuilder::Create(connection_handle),
489 handler_->BindOnce(check_status<LeCsSecurityEnableStatusView>));
490 }
491
send_le_cs_set_default_settingsbluetooth::hci::DistanceMeasurementManager::impl492 void send_le_cs_set_default_settings(uint16_t connection_handle) {
493 uint8_t role_enable = (1 << (uint8_t)CsRole::INITIATOR) | 1 << ((uint8_t)CsRole::REFLECTOR);
494 hci_layer_->EnqueueCommand(
495 LeCsSetDefaultSettingsBuilder::Create(
496 connection_handle,
497 role_enable,
498 kCsSyncAntennaSelection,
499 kCsMaxTxPower // max_tx_power
500 ),
501 handler_->BindOnceOn(this, &impl::on_cs_set_default_settings_complete));
502 }
503
send_le_cs_create_configbluetooth::hci::DistanceMeasurementManager::impl504 void send_le_cs_create_config(uint16_t connection_handle) {
505 auto channel_vector = common::FromHexString("1FFFFFFFFFFFFC7FFFFC"); // use all 72 Channel
506 std::array<uint8_t, 10> channel_map;
507 std::copy(channel_vector->begin(), channel_vector->end(), channel_map.begin());
508 std::reverse(channel_map.begin(), channel_map.end());
509 hci_layer_->EnqueueCommand(
510 LeCsCreateConfigBuilder::Create(
511 connection_handle,
512 kConfigId,
513 CsCreateContext::BOTH_LOCAL_AND_REMOTE_CONTROLLER,
514 CsMainModeType::MODE_2,
515 CsSubModeType::UNUSED,
516 kMinMainModeSteps,
517 kMaxMainModeSteps,
518 kMainModeRepetition,
519 kMode0Steps,
520 CsRole::INITIATOR,
521 CsConfigRttType::RTT_WITH_128_BIT_RANDOM_SEQUENCE,
522 CsSyncPhy::LE_1M_PHY,
523 channel_map,
524 kChannelMapRepetition,
525 CsChannelSelectionType::TYPE_3B,
526 CsCh3cShape::HAT_SHAPE,
527 kCh3cJump,
528 Enable::DISABLED),
529 handler_->BindOnce(check_status<LeCsCreateConfigStatusView>));
530 }
531
send_le_cs_set_procedure_parametersbluetooth::hci::DistanceMeasurementManager::impl532 void send_le_cs_set_procedure_parameters(uint16_t connection_handle) {
533 CsPreferredPeerAntenna preferred_peer_antenna;
534 hci_layer_->EnqueueCommand(
535 LeCsSetProcedureParametersBuilder::Create(
536 connection_handle,
537 kConfigId,
538 kMaxProcedureLen,
539 kMinProcedureInterval,
540 kMaxProcedureInterval,
541 kMaxProcedureCount,
542 kMinSubeventLen,
543 kMaxSubeventLen,
544 kToneAntennaConfigSelection,
545 CsPhy::LE_1M_PHY,
546 kTxPwrDelta,
547 preferred_peer_antenna),
548 handler_->BindOnceOn(this, &impl::on_cs_set_procedure_parameters));
549 }
550
send_le_cs_procedure_enablebluetooth::hci::DistanceMeasurementManager::impl551 void send_le_cs_procedure_enable(uint16_t connection_handle, Enable enable) {
552 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
553 log::warn("Can't find cs tracker for connection {}", connection_handle);
554 return;
555 }
556 Address address = cs_trackers_[connection_handle].address;
557 // Check if the connection still exists
558 uint16_t connection_handle_from_acl_manager = acl_manager_->HACK_GetLeHandle(address);
559 if (connection_handle_from_acl_manager == kIllegalConnectionHandle) {
560 log::warn("Can't find connection for {}", address);
561 distance_measurement_callbacks_->OnDistanceMeasurementStopped(
562 address, REASON_NO_LE_CONNECTION, METHOD_CS);
563 cs_trackers_[connection_handle].repeating_alarm->Cancel();
564 cs_trackers_[connection_handle].repeating_alarm.reset();
565 cs_trackers_.erase(connection_handle);
566 return;
567 }
568 hci_layer_->EnqueueCommand(
569 LeCsProcedureEnableBuilder::Create(connection_handle, kConfigId, enable),
570 handler_->BindOnce(check_status<LeCsProcedureEnableStatusView>));
571 }
572
on_cs_read_local_supported_capabilitiesbluetooth::hci::DistanceMeasurementManager::impl573 void on_cs_read_local_supported_capabilities(CommandCompleteView view) {
574 auto complete_view = LeCsReadLocalSupportedCapabilitiesCompleteView::Create(view);
575 if (!complete_view.IsValid()) {
576 log::warn("Get invalid LeCsReadLocalSupportedCapabilitiesComplete");
577 return;
578 } else if (complete_view.GetStatus() != ErrorCode::SUCCESS) {
579 std::string error_code = ErrorCodeText(complete_view.GetStatus());
580 log::warn(
581 "Received LeCsReadLocalSupportedCapabilitiesComplete with error code {}", error_code);
582 return;
583 }
584 cs_subfeature_supported_ = complete_view.GetOptionalSubfeaturesSupported();
585 }
586
on_cs_read_remote_supported_capabilities_completebluetooth::hci::DistanceMeasurementManager::impl587 void on_cs_read_remote_supported_capabilities_complete(
588 LeCsReadRemoteSupportedCapabilitiesCompleteView event_view) {
589 if (!event_view.IsValid()) {
590 log::warn("Get invalid LeCsReadRemoteSupportedCapabilitiesCompleteView");
591 return;
592 } else if (event_view.GetStatus() != ErrorCode::SUCCESS) {
593 std::string error_code = ErrorCodeText(event_view.GetStatus());
594 log::warn(
595 "Received LeCsReadRemoteSupportedCapabilitiesCompleteView with error code {}",
596 error_code);
597 return;
598 }
599 uint16_t connection_handle = event_view.GetConnectionHandle();
600 send_le_cs_set_default_settings(event_view.GetConnectionHandle());
601 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
602 // Create a cs tracker with role reflector
603 // TODO: Check ROLE via CS config. (b/304295768)
604 cs_trackers_[connection_handle].role = CsRole::REFLECTOR;
605 cs_trackers_[connection_handle].address = acl_manager_->HACK_GetLeAddress(connection_handle);
606 } else {
607 send_le_cs_security_enable(connection_handle);
608 }
609
610 if (event_view.GetOptionalSubfeaturesSupported().phase_based_ranging_ == 0x01) {
611 cs_trackers_[connection_handle].remote_support_phase_based_ranging = true;
612 }
613 log::info(
614 "connection_handle:{}, num_antennas_supported:{}, max_antenna_paths_supported:{}, "
615 "roles_supported:{}, phase_based_ranging_supported: {}",
616 event_view.GetConnectionHandle(),
617 event_view.GetNumAntennasSupported(),
618 event_view.GetMaxAntennaPathsSupported(),
619 event_view.GetRolesSupported().ToString(),
620 event_view.GetOptionalSubfeaturesSupported().phase_based_ranging_);
621 }
622
on_cs_set_default_settings_completebluetooth::hci::DistanceMeasurementManager::impl623 void on_cs_set_default_settings_complete(CommandCompleteView view) {
624 auto complete_view = LeCsSetDefaultSettingsCompleteView::Create(view);
625 if (!complete_view.IsValid()) {
626 log::warn("Get invalid LeCsSetDefaultSettingsComplete");
627 return;
628 } else if (complete_view.GetStatus() != ErrorCode::SUCCESS) {
629 std::string error_code = ErrorCodeText(complete_view.GetStatus());
630 log::warn("Received LeCsSetDefaultSettingsComplete with error code {}", error_code);
631 return;
632 }
633 }
634
on_cs_security_enable_completebluetooth::hci::DistanceMeasurementManager::impl635 void on_cs_security_enable_complete(LeCsSecurityEnableCompleteView event_view) {
636 if (!event_view.IsValid()) {
637 log::warn("Get invalid LeCsSecurityEnableCompleteView");
638 return;
639 } else if (event_view.GetStatus() != ErrorCode::SUCCESS) {
640 std::string error_code = ErrorCodeText(event_view.GetStatus());
641 log::warn("Received LeCsSecurityEnableCompleteView with error code {}", error_code);
642 return;
643 }
644 uint16_t connection_handle = event_view.GetConnectionHandle();
645 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
646 log::warn("Can't find cs tracker for connection_handle {}", connection_handle);
647 return;
648 }
649 cs_trackers_[connection_handle].setup_complete = true;
650 log::info(
651 "Setup phase complete, connection_handle: {}, address: {}",
652 connection_handle,
653 cs_trackers_[connection_handle].address);
654 if (cs_trackers_[connection_handle].role == CsRole::INITIATOR) {
655 send_le_cs_create_config(connection_handle);
656 }
657 }
658
on_cs_config_completebluetooth::hci::DistanceMeasurementManager::impl659 void on_cs_config_complete(LeCsConfigCompleteView event_view) {
660 if (!event_view.IsValid()) {
661 log::warn("Get invalid LeCsConfigCompleteView");
662 return;
663 } else if (event_view.GetStatus() != ErrorCode::SUCCESS) {
664 std::string error_code = ErrorCodeText(event_view.GetStatus());
665 log::warn("Received LeCsConfigCompleteView with error code {}", error_code);
666 return;
667 }
668 uint16_t connection_handle = event_view.GetConnectionHandle();
669 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
670 log::warn("Can't find cs tracker for connection_handle {}", connection_handle);
671 return;
672 }
673 if (event_view.GetAction() == CsAction::CONFIG_REMOVED) {
674 return;
675 }
676 log::info("Get {}", event_view.ToString());
677 cs_trackers_[connection_handle].role = event_view.GetRole();
678 cs_trackers_[connection_handle].config_set = true;
679 cs_trackers_[connection_handle].main_mode_type = event_view.GetMainModeType();
680 cs_trackers_[connection_handle].sub_mode_type = event_view.GetSubModeType();
681 cs_trackers_[connection_handle].rtt_type = event_view.GetRttType();
682
683 if (cs_trackers_[connection_handle].role == CsRole::INITIATOR) {
684 send_le_cs_set_procedure_parameters(event_view.GetConnectionHandle());
685 }
686 }
687
on_cs_set_procedure_parametersbluetooth::hci::DistanceMeasurementManager::impl688 void on_cs_set_procedure_parameters(CommandCompleteView view) {
689 auto complete_view = LeCsSetProcedureParametersCompleteView::Create(view);
690 if (!complete_view.IsValid()) {
691 log::warn("Get Invalid LeCsSetProcedureParametersCompleteView");
692 return;
693 } else if (complete_view.GetStatus() != ErrorCode::SUCCESS) {
694 std::string error_code = ErrorCodeText(complete_view.GetStatus());
695 log::warn("Received LeCsSetProcedureParametersCompleteView with error code {}", error_code);
696 return;
697 }
698 uint16_t connection_handle = complete_view.GetConnectionHandle();
699 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
700 log::warn("Can't find cs tracker for connection_handle {}", connection_handle);
701 return;
702 }
703
704 if (cs_trackers_[connection_handle].role == CsRole::INITIATOR) {
705 log::info(
706 "enable cs procedure regularly with interval: {} ms",
707 cs_trackers_[connection_handle].interval_ms);
708 cs_trackers_[connection_handle].repeating_alarm->Cancel();
709 send_le_cs_procedure_enable(connection_handle, Enable::ENABLED);
710 cs_trackers_[connection_handle].repeating_alarm->Schedule(
711 common::Bind(
712 &impl::send_le_cs_procedure_enable,
713 common::Unretained(this),
714 connection_handle,
715 Enable::ENABLED),
716 std::chrono::milliseconds(cs_trackers_[connection_handle].interval_ms));
717 }
718 }
719
on_cs_procedure_enable_completebluetooth::hci::DistanceMeasurementManager::impl720 void on_cs_procedure_enable_complete(LeCsProcedureEnableCompleteView event_view) {
721 log::assert_that(event_view.IsValid(), "assert failed: event_view.IsValid()");
722 uint16_t connection_handle = event_view.GetConnectionHandle();
723 if (event_view.GetStatus() != ErrorCode::SUCCESS) {
724 std::string error_code = ErrorCodeText(event_view.GetStatus());
725 log::warn("Received LeCsProcedureEnableCompleteView with error code {}", error_code);
726 if (cs_trackers_.find(connection_handle) != cs_trackers_.end() &&
727 cs_trackers_[connection_handle].waiting_for_start_callback) {
728 cs_trackers_[connection_handle].waiting_for_start_callback = false;
729 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
730 cs_trackers_[connection_handle].address, REASON_INTERNAL_ERROR, METHOD_CS);
731 }
732 return;
733 }
734
735 if (event_view.GetState() == Enable::ENABLED) {
736 log::debug("Procedure enabled, {}", event_view.ToString());
737 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
738 return;
739 }
740 cs_trackers_[connection_handle].config_id = event_view.GetConfigId();
741 cs_trackers_[connection_handle].selected_tx_power = event_view.GetSelectedTxPower();
742
743 if (cs_trackers_[connection_handle].waiting_for_start_callback) {
744 cs_trackers_[connection_handle].waiting_for_start_callback = false;
745 distance_measurement_callbacks_->OnDistanceMeasurementStarted(
746 cs_trackers_[connection_handle].address, METHOD_CS);
747 }
748 }
749 cs_delete_obsolete_data(event_view.GetConnectionHandle());
750 }
751
on_cs_subeventbluetooth::hci::DistanceMeasurementManager::impl752 void on_cs_subevent(LeMetaEventView event) {
753 if (!event.IsValid()) {
754 log::error("Received invalid LeMetaEventView");
755 return;
756 }
757
758 // Common data for LE_CS_SUBEVENT_RESULT and LE_CS_SUBEVENT_RESULT_CONTINUE,
759 uint16_t connection_handle = 0;
760 CsProcedureDoneStatus procedure_done_status;
761 CsSubeventDoneStatus subevent_done_status;
762 ProcedureAbortReason procedure_abort_reason;
763 SubeventAbortReason subevent_abort_reason;
764 std::vector<LeCsResultDataStructure> result_data_structures;
765 if (event.GetSubeventCode() == SubeventCode::LE_CS_SUBEVENT_RESULT) {
766 auto cs_event_result = LeCsSubeventResultView::Create(event);
767 if (!cs_event_result.IsValid()) {
768 log::warn("Get invalid LeCsSubeventResultView");
769 return;
770 }
771 connection_handle = cs_event_result.GetConnectionHandle();
772 procedure_done_status = cs_event_result.GetProcedureDoneStatus();
773 subevent_done_status = cs_event_result.GetSubeventDoneStatus();
774 procedure_abort_reason = cs_event_result.GetProcedureAbortReason();
775 subevent_abort_reason = cs_event_result.GetSubeventAbortReason();
776 result_data_structures = cs_event_result.GetResultDataStructures();
777 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
778 log::warn("Can't find any tracker for {}", connection_handle);
779 return;
780 }
781 CsProcedureData* procedure_data = init_cs_procedure_data(
782 connection_handle,
783 cs_event_result.GetProcedureCounter(),
784 cs_event_result.GetNumAntennaPaths(),
785 true);
786 if (cs_trackers_[connection_handle].role == CsRole::INITIATOR) {
787 procedure_data->frequency_compensation.push_back(
788 cs_event_result.GetFrequencyCompensation());
789 }
790 // RAS
791 log::debug("RAS Update subevent_header counter:{}", procedure_data->ras_subevent_counter_++);
792 auto& ras_subevent_header = procedure_data->ras_subevent_header_;
793 ras_subevent_header.start_acl_conn_event_ = cs_event_result.GetStartAclConnEvent();
794 ras_subevent_header.frequency_compensation_ = cs_event_result.GetFrequencyCompensation();
795 ras_subevent_header.reference_power_level_ = cs_event_result.GetReferencePowerLevel();
796 ras_subevent_header.num_steps_reported_ = 0;
797 } else {
798 auto cs_event_result = LeCsSubeventResultContinueView::Create(event);
799 if (!cs_event_result.IsValid()) {
800 log::warn("Get invalid LeCsSubeventResultContinueView");
801 return;
802 }
803 connection_handle = cs_event_result.GetConnectionHandle();
804 procedure_done_status = cs_event_result.GetProcedureDoneStatus();
805 subevent_done_status = cs_event_result.GetSubeventDoneStatus();
806 procedure_abort_reason = cs_event_result.GetProcedureAbortReason();
807 subevent_abort_reason = cs_event_result.GetSubeventAbortReason();
808 result_data_structures = cs_event_result.GetResultDataStructures();
809 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
810 log::warn("Can't find any tracker for {}", connection_handle);
811 return;
812 }
813 }
814
815 uint16_t counter = cs_trackers_[connection_handle].local_counter;
816 log::debug(
817 "Connection_handle {}, procedure_done_status: {}, subevent_done_status: {}, counter: {}",
818 connection_handle,
819 CsProcedureDoneStatusText(procedure_done_status),
820 CsSubeventDoneStatusText(subevent_done_status),
821 counter);
822
823 if (procedure_done_status == CsProcedureDoneStatus::ABORTED ||
824 subevent_done_status == CsSubeventDoneStatus::ABORTED) {
825 log::warn(
826 "Received CS Subevent with procedure_abort_reason:{}, subevent_abort_reason:{}, "
827 "connection_handle:{}, counter:{}",
828 ProcedureAbortReasonText(procedure_abort_reason),
829 SubeventAbortReasonText(subevent_abort_reason),
830 connection_handle,
831 counter);
832 }
833
834 CsProcedureData* procedure_data = get_procedure_data(connection_handle, counter);
835 if (procedure_data == nullptr) {
836 return;
837 }
838 procedure_data->ras_subevent_header_.num_steps_reported_ += result_data_structures.size();
839
840 if (procedure_abort_reason != ProcedureAbortReason::NO_ABORT ||
841 subevent_abort_reason != SubeventAbortReason::NO_ABORT) {
842 // Even the procedure is aborted, we should keep following process and
843 // handle it when all corresponding remote data received.
844 procedure_data->aborted = true;
845 procedure_data->ras_subevent_header_.ranging_abort_reason_ =
846 static_cast<RangingAbortReason>(procedure_abort_reason);
847 procedure_data->ras_subevent_header_.subevent_abort_reason_ =
848 static_cast<bluetooth::ras::SubeventAbortReason>(subevent_abort_reason);
849 }
850 parse_cs_result_data(
851 result_data_structures, *procedure_data, cs_trackers_[connection_handle].role);
852 // Update procedure status
853 procedure_data->local_status = procedure_done_status;
854 check_cs_procedure_complete(procedure_data, connection_handle);
855
856 if (cs_trackers_[connection_handle].role == CsRole::INITIATOR) {
857 // Skip to send remote
858 return;
859 }
860
861 // Send data to RAS server
862 if (subevent_done_status != CsSubeventDoneStatus::PARTIAL_RESULTS) {
863 procedure_data->ras_subevent_header_.ranging_done_status_ =
864 static_cast<RangingDoneStatus>(procedure_done_status);
865 procedure_data->ras_subevent_header_.subevent_done_status_ =
866 static_cast<SubeventDoneStatus>(subevent_done_status);
867 auto builder = RasSubeventBuilder::Create(
868 procedure_data->ras_subevent_header_, procedure_data->ras_subevent_data_);
869 auto subevent_raw = builder_to_bytes(std::move(builder));
870 append_vector(procedure_data->ras_raw_data_, subevent_raw);
871 // erase buffer
872 procedure_data->ras_subevent_data_.clear();
873 send_on_demand_data(cs_trackers_[connection_handle].address, procedure_data);
874 }
875 }
876
send_on_demand_databluetooth::hci::DistanceMeasurementManager::impl877 void send_on_demand_data(Address address, CsProcedureData* procedure_data) {
878 // Check is last segment or not.
879 uint16_t unsent_data_size =
880 procedure_data->ras_raw_data_.size() - procedure_data->ras_raw_data_index_;
881 if (procedure_data->local_status != CsProcedureDoneStatus::PARTIAL_RESULTS &&
882 unsent_data_size <= kMtuForRasData) {
883 procedure_data->segmentation_header_.last_segment_ = 1;
884 } else if (procedure_data->ras_raw_data_.size() < kMtuForRasData) {
885 log::verbose("waiting for more data, current size {}", procedure_data->ras_raw_data_.size());
886 return;
887 }
888
889 // Create raw data for segment_data;
890 uint16_t copy_size = unsent_data_size < kMtuForRasData ? unsent_data_size : kMtuForRasData;
891 auto copy_start = procedure_data->ras_raw_data_.begin() + procedure_data->ras_raw_data_index_;
892 auto copy_end = copy_start + copy_size;
893 std::vector<uint8_t> subevent_data(copy_start, copy_end);
894 procedure_data->ras_raw_data_index_ += copy_size;
895
896 auto builder =
897 RangingDataSegmentBuilder::Create(procedure_data->segmentation_header_, subevent_data);
898 auto segment_data = builder_to_bytes(std::move(builder));
899
900 log::debug("counter: {}, size:{}", procedure_data->counter, (uint16_t)segment_data.size());
901 distance_measurement_callbacks_->OnRasFragmentReady(
902 address,
903 procedure_data->counter,
904 procedure_data->segmentation_header_.last_segment_,
905 segment_data);
906
907 procedure_data->segmentation_header_.first_segment_ = 0;
908 procedure_data->segmentation_header_.rolling_segment_counter_++;
909 procedure_data->segmentation_header_.rolling_segment_counter_ %= 64;
910 if (procedure_data->segmentation_header_.last_segment_) {
911 // last segment sent, clear buffer
912 procedure_data->ras_raw_data_.clear();
913 } else if (unsent_data_size > kMtuForRasData) {
914 send_on_demand_data(address, procedure_data);
915 }
916 }
917
handle_remote_databluetooth::hci::DistanceMeasurementManager::impl918 void handle_remote_data(const Address address, const std::vector<uint8_t> raw_data) {
919 uint16_t connection_handle = acl_manager_->HACK_GetLeHandle(address);
920 log::debug(
921 "address:{}, connection_handle 0x{:04x}, size:{}",
922 address.ToString(),
923 connection_handle,
924 raw_data.size());
925
926 if (cs_trackers_.find(connection_handle) == cs_trackers_.end()) {
927 log::warn("can't find tracker for 0x{:04x}", connection_handle);
928 return;
929 }
930 auto& tracker = cs_trackers_[connection_handle];
931
932 SegmentationHeader segmentation_header;
933 PacketView<kLittleEndian> packet_bytes_view(std::make_shared<std::vector<uint8_t>>(raw_data));
934 auto after = SegmentationHeader::Parse(&segmentation_header, packet_bytes_view.begin());
935 if (after == packet_bytes_view.begin()) {
936 log::warn("Invalid segment data");
937 return;
938 }
939
940 log::debug(
941 "Receive segment for segment counter {}, size {}",
942 segmentation_header.rolling_segment_counter_,
943 raw_data.size());
944
945 PacketView<kLittleEndian> segment_data(std::make_shared<std::vector<uint8_t>>(raw_data));
946 if (segmentation_header.first_segment_) {
947 auto segment = FirstRangingDataSegmentView::Create(segment_data);
948 if (!segment.IsValid()) {
949 log::warn("Invalid segment data");
950 return;
951 }
952 tracker.ranging_header_ = segment.GetRangingHeader();
953
954 auto begin = segment.GetSegmentationHeader().size() + segment.GetRangingHeader().size();
955 tracker.segment_data_ =
956 PacketViewForRecombination(segment.GetLittleEndianSubview(begin, segment.size()));
957 } else {
958 auto segment = RangingDataSegmentView::Create(segment_data);
959 if (!segment.IsValid()) {
960 log::warn("Invalid segment data");
961 return;
962 }
963 tracker.segment_data_.AppendPacketView(
964 segment.GetLittleEndianSubview(segmentation_header.size(), segment.size()));
965 }
966
967 if (segmentation_header.last_segment_) {
968 parse_ras_segments(tracker.ranging_header_, tracker.segment_data_, connection_handle);
969 }
970 }
971
parse_ras_segmentsbluetooth::hci::DistanceMeasurementManager::impl972 void parse_ras_segments(
973 RangingHeader ranging_header,
974 PacketViewForRecombination& segment_data,
975 uint16_t connection_handle) {
976 log::debug("Data size {}, Ranging_header {}", segment_data.size(), ranging_header.ToString());
977 auto procedure_data =
978 get_procedure_data_for_ras(connection_handle, ranging_header.ranging_counter_);
979 if (procedure_data == nullptr) {
980 return;
981 }
982
983 uint8_t num_antenna_paths = 0;
984 for (uint8_t i = 0; i < 4; i++) {
985 if ((ranging_header.antenna_paths_mask_ & (1 << i)) != 0) {
986 num_antenna_paths++;
987 }
988 }
989
990 // Get role of the remote device
991 CsRole role = cs_trackers_[connection_handle].role == CsRole::INITIATOR ? CsRole::REFLECTOR
992 : CsRole::INITIATOR;
993
994 auto parse_index = segment_data.begin();
995 uint16_t remaining_data_size = std::distance(parse_index, segment_data.end());
996
997 // Parse subevents
998 while (remaining_data_size > 0) {
999 RasSubeventHeader subevent_header;
1000 // Parse header
1001 auto after = RasSubeventHeader::Parse(&subevent_header, parse_index);
1002 if (after == parse_index) {
1003 log::warn("Received invalid subevent_header data");
1004 return;
1005 }
1006 parse_index = after;
1007 log::debug("subevent_header: {}", subevent_header.ToString());
1008
1009 // Parse step data
1010 for (uint8_t i = 0; i < subevent_header.num_steps_reported_; i++) {
1011 StepMode step_mode;
1012 after = StepMode::Parse(&step_mode, parse_index);
1013 if (after == parse_index) {
1014 log::warn("Received invalid step_mode data");
1015 return;
1016 }
1017 parse_index = after;
1018 log::verbose("step:{}, {}", (uint16_t)i, step_mode.ToString());
1019 if (step_mode.aborted_) {
1020 continue;
1021 }
1022
1023 switch (step_mode.mode_type_) {
1024 case 0: {
1025 if (role == CsRole::INITIATOR) {
1026 LeCsMode0InitatorData tone_data;
1027 after = LeCsMode0InitatorData::Parse(&tone_data, parse_index);
1028 if (after == parse_index) {
1029 log::warn(
1030 "Error invalid mode {} data, role:{}", step_mode.mode_type_, CsRoleText(role));
1031 return;
1032 }
1033 parse_index = after;
1034 } else {
1035 LeCsMode0ReflectorData tone_data;
1036 after = LeCsMode0ReflectorData::Parse(&tone_data, parse_index);
1037 if (after == parse_index) {
1038 log::warn(
1039 "Error invalid mode {} data, role:{}", step_mode.mode_type_, CsRoleText(role));
1040 return;
1041 }
1042 }
1043 parse_index = after;
1044 } break;
1045 case 2: {
1046 uint8_t num_tone_data = num_antenna_paths + 1;
1047 uint8_t data_len = 1 + (4 * num_tone_data);
1048 remaining_data_size = std::distance(parse_index, segment_data.end());
1049 if (remaining_data_size < data_len) {
1050 log::warn(
1051 "insufficient length for LeCsMode2Data, num_tone_data {}, remaining_data_size {}",
1052 num_tone_data,
1053 remaining_data_size);
1054 return;
1055 }
1056 std::vector<uint8_t> vector_for_num_tone_data = {num_tone_data};
1057 PacketView<kLittleEndian> packet_view_for_num_tone_data(
1058 std::make_shared<std::vector<uint8_t>>(vector_for_num_tone_data));
1059 PacketViewForRecombination packet_bytes_view =
1060 PacketViewForRecombination(packet_view_for_num_tone_data);
1061 auto subview_begin = std::distance(segment_data.begin(), parse_index);
1062 packet_bytes_view.AppendPacketView(
1063 segment_data.GetLittleEndianSubview(subview_begin, subview_begin + data_len));
1064 LeCsMode2Data tone_data;
1065 after = LeCsMode2Data::Parse(&tone_data, packet_bytes_view.begin());
1066 if (after == packet_bytes_view.begin()) {
1067 log::warn(
1068 "Error invalid mode {} data, role:{}", step_mode.mode_type_, CsRoleText(role));
1069 return;
1070 }
1071 parse_index += data_len;
1072 uint8_t permutation_index = tone_data.antenna_permutation_index_;
1073
1074 // Parse in ascending order of antenna position with tone extension data at the end
1075 for (uint8_t k = 0; k < num_tone_data; k++) {
1076 uint8_t antenna_path = k == num_antenna_paths
1077 ? num_antenna_paths
1078 : cs_antenna_permutation_array_[permutation_index][k] - 1;
1079 double i_value = get_iq_value(tone_data.tone_data_[k].i_sample_);
1080 double q_value = get_iq_value(tone_data.tone_data_[k].q_sample_);
1081 uint8_t tone_quality_indicator = tone_data.tone_data_[k].tone_quality_indicator_;
1082 log::verbose(
1083 "antenna_path {}, {:f}, {:f}", (uint16_t)(antenna_path + 1), i_value, q_value);
1084 if (role == CsRole::INITIATOR) {
1085 procedure_data->tone_pct_initiator[antenna_path].emplace_back(i_value, q_value);
1086 procedure_data->tone_quality_indicator_initiator[antenna_path].emplace_back(
1087 tone_quality_indicator);
1088 } else {
1089 procedure_data->tone_pct_reflector[antenna_path].emplace_back(i_value, q_value);
1090 procedure_data->tone_quality_indicator_reflector[antenna_path].emplace_back(
1091 tone_quality_indicator);
1092 }
1093 }
1094 } break;
1095 default:
1096 log::error("Unexpect mode: {}", step_mode.mode_type_);
1097 return;
1098 }
1099 }
1100 remaining_data_size = std::distance(parse_index, segment_data.end());
1101 log::debug("Parse subevent done with remaining data size {}", remaining_data_size);
1102 procedure_data->remote_status = (CsProcedureDoneStatus)subevent_header.ranging_done_status_;
1103 }
1104 check_cs_procedure_complete(procedure_data, connection_handle);
1105 }
1106
init_cs_procedure_databluetooth::hci::DistanceMeasurementManager::impl1107 CsProcedureData* init_cs_procedure_data(
1108 uint16_t connection_handle,
1109 uint16_t procedure_counter,
1110 uint8_t num_antenna_paths,
1111 bool local) {
1112 // Update procedure count
1113 if (local) {
1114 cs_trackers_[connection_handle].local_counter = procedure_counter;
1115 } else {
1116 cs_trackers_[connection_handle].remote_counter = procedure_counter;
1117 }
1118
1119 std::vector<CsProcedureData>& data_list = cs_trackers_[connection_handle].procedure_data_list;
1120 for (auto& data : data_list) {
1121 if (data.counter == procedure_counter) {
1122 // Data already exists, return
1123 return &data;
1124 }
1125 }
1126 log::info("Create data for procedure_counter: {}", procedure_counter);
1127 data_list.emplace_back(
1128 procedure_counter,
1129 num_antenna_paths,
1130 cs_trackers_[connection_handle].config_id,
1131 cs_trackers_[connection_handle].selected_tx_power);
1132
1133 // Append ranging header raw data
1134 std::vector<uint8_t> ranging_header_raw = {};
1135 BitInserter bi(ranging_header_raw);
1136 data_list.back().ranging_header_.Serialize(bi);
1137 append_vector(data_list.back().ras_raw_data_, ranging_header_raw);
1138
1139 if (data_list.size() > kProcedureDataBufferSize) {
1140 log::warn("buffer full, drop procedure data with counter: {}", data_list.front().counter);
1141 data_list.erase(data_list.begin());
1142 }
1143 return &data_list.back();
1144 }
1145
cs_delete_obsolete_databluetooth::hci::DistanceMeasurementManager::impl1146 void cs_delete_obsolete_data(uint16_t connection_handle) {
1147 std::vector<CsProcedureData>& data_list = cs_trackers_[connection_handle].procedure_data_list;
1148 while (!data_list.empty()) {
1149 data_list.erase(data_list.begin());
1150 }
1151 }
1152
get_procedure_databluetooth::hci::DistanceMeasurementManager::impl1153 CsProcedureData* get_procedure_data(uint16_t connection_handle, uint16_t counter) {
1154 std::vector<CsProcedureData>& data_list = cs_trackers_[connection_handle].procedure_data_list;
1155 CsProcedureData* procedure_data = nullptr;
1156 for (uint8_t i = 0; i < data_list.size(); i++) {
1157 if (data_list[i].counter == counter) {
1158 procedure_data = &data_list[i];
1159 break;
1160 }
1161 }
1162 if (procedure_data == nullptr) {
1163 log::warn(
1164 "Can't find data for connection_handle:{}, counter: {}", connection_handle, counter);
1165 }
1166 return procedure_data;
1167 }
1168
get_procedure_data_for_rasbluetooth::hci::DistanceMeasurementManager::impl1169 CsProcedureData* get_procedure_data_for_ras(
1170 uint16_t connection_handle, uint16_t ranging_counter) {
1171 std::vector<CsProcedureData>& data_list = cs_trackers_[connection_handle].procedure_data_list;
1172 CsProcedureData* procedure_data = nullptr;
1173 for (uint8_t i = 0; i < data_list.size(); i++) {
1174 if ((data_list[i].counter & kRangingCounterMask) == ranging_counter) {
1175 procedure_data = &data_list[i];
1176 break;
1177 }
1178 }
1179 if (procedure_data == nullptr) {
1180 log::warn(
1181 "Can't find data for connection_handle:{}, ranging_counter: {}",
1182 connection_handle,
1183 ranging_counter);
1184 }
1185 return procedure_data;
1186 }
1187
check_cs_procedure_completebluetooth::hci::DistanceMeasurementManager::impl1188 void check_cs_procedure_complete(CsProcedureData* procedure_data, uint16_t connection_handle) {
1189 if (procedure_data->local_status == CsProcedureDoneStatus::ALL_RESULTS_COMPLETE &&
1190 procedure_data->remote_status == CsProcedureDoneStatus::ALL_RESULTS_COMPLETE &&
1191 !procedure_data->aborted) {
1192 log::debug(
1193 "Procedure complete counter:{} data size:{}, main_mode_type:{}, sub_mode_type:{}",
1194 (uint16_t)procedure_data->counter,
1195 (uint16_t)procedure_data->step_channel.size(),
1196 (uint16_t)cs_trackers_[connection_handle].main_mode_type,
1197 (uint16_t)cs_trackers_[connection_handle].sub_mode_type);
1198
1199 if (ranging_hal_->IsBound()) {
1200 // Use algorithm in the HAL
1201 bluetooth::hal::ChannelSoundingRawData raw_data;
1202 raw_data.num_antenna_paths_ = procedure_data->num_antenna_paths;
1203 raw_data.step_channel_ = procedure_data->step_channel;
1204 raw_data.tone_pct_initiator_ = procedure_data->tone_pct_initiator;
1205 raw_data.tone_quality_indicator_initiator_ =
1206 procedure_data->tone_quality_indicator_initiator;
1207 raw_data.tone_pct_reflector_ = procedure_data->tone_pct_reflector;
1208 raw_data.tone_quality_indicator_reflector_ =
1209 procedure_data->tone_quality_indicator_reflector;
1210 ranging_hal_->WriteRawData(connection_handle, raw_data);
1211 return;
1212 }
1213 }
1214
1215 // If the procedure is completed or aborted, delete all previous data
1216 if (procedure_data->local_status != CsProcedureDoneStatus::PARTIAL_RESULTS &&
1217 procedure_data->remote_status != CsProcedureDoneStatus::PARTIAL_RESULTS) {
1218 std::vector<CsProcedureData>& data_list = cs_trackers_[connection_handle].procedure_data_list;
1219 uint16_t counter = procedure_data->counter; // Get value from pointer first.
1220 while (data_list.begin()->counter < counter) {
1221 log::debug("Delete obsolete procedure data, counter:{}", data_list.begin()->counter);
1222 data_list.erase(data_list.begin());
1223 }
1224 }
1225 }
1226
parse_cs_result_databluetooth::hci::DistanceMeasurementManager::impl1227 void parse_cs_result_data(
1228 std::vector<LeCsResultDataStructure> result_data_structures,
1229 CsProcedureData& procedure_data,
1230 CsRole role) {
1231 uint8_t num_antenna_paths = procedure_data.num_antenna_paths;
1232 auto& ras_data = procedure_data.ras_subevent_data_;
1233 for (auto result_data_structure : result_data_structures) {
1234 uint16_t mode = result_data_structure.step_mode_;
1235 uint16_t step_channel = result_data_structure.step_channel_;
1236 uint16_t data_length = result_data_structure.step_data_.size();
1237 log::verbose(
1238 "mode: {}, channel: {}, data_length: {}",
1239 mode,
1240 step_channel,
1241 (uint16_t)result_data_structure.step_data_.size());
1242 ras_data.emplace_back(mode);
1243 if (data_length == 0) {
1244 ras_data.back() |= (1 << 7); // set step aborted
1245 continue;
1246 }
1247 append_vector(ras_data, result_data_structure.step_data_);
1248
1249 // Parse data into structs from an iterator
1250 auto bytes = std::make_shared<std::vector<uint8_t>>();
1251 if (mode == 0x02 || mode == 0x03) {
1252 // Add one byte for the length of Tone_PCT[k], Tone_Quality_Indicator[k]
1253 bytes->emplace_back(num_antenna_paths + 1);
1254 }
1255 bytes->reserve(bytes->size() + result_data_structure.step_data_.size());
1256 bytes->insert(
1257 bytes->end(),
1258 result_data_structure.step_data_.begin(),
1259 result_data_structure.step_data_.end());
1260 Iterator<packet::kLittleEndian> iterator(bytes);
1261 switch (mode) {
1262 case 0: {
1263 if (role == CsRole::INITIATOR) {
1264 LeCsMode0InitatorData tone_data_view;
1265 auto after = LeCsMode0InitatorData::Parse(&tone_data_view, iterator);
1266 if (after == iterator) {
1267 log::warn("Received invalid mode {} data, role:{}", mode, CsRoleText(role));
1268 print_raw_data(result_data_structure.step_data_);
1269 continue;
1270 }
1271 log::verbose("step_data: {}", tone_data_view.ToString());
1272 procedure_data.measured_freq_offset.push_back(tone_data_view.measured_freq_offset_);
1273 } else {
1274 LeCsMode0ReflectorData tone_data_view;
1275 auto after = LeCsMode0ReflectorData::Parse(&tone_data_view, iterator);
1276 if (after == iterator) {
1277 log::warn("Received invalid mode {} data, role:{}", mode, CsRoleText(role));
1278 print_raw_data(result_data_structure.step_data_);
1279 continue;
1280 }
1281 log::verbose("step_data: {}", tone_data_view.ToString());
1282 }
1283 } break;
1284 case 2: {
1285 LeCsMode2Data tone_data_view;
1286 auto after = LeCsMode2Data::Parse(&tone_data_view, iterator);
1287 if (after == iterator) {
1288 log::warn("Received invalid mode {} data, role:{}", mode, CsRoleText(role));
1289 print_raw_data(result_data_structure.step_data_);
1290 continue;
1291 }
1292 log::verbose("step_data: {}", tone_data_view.ToString());
1293 if (role == CsRole::INITIATOR) {
1294 procedure_data.step_channel.push_back(step_channel);
1295 }
1296 auto tone_data = tone_data_view.tone_data_;
1297 uint8_t permutation_index = tone_data_view.antenna_permutation_index_;
1298 // Parse in ascending order of antenna position with tone extension data at the end
1299 uint16_t num_tone_data = num_antenna_paths + 1;
1300 for (uint16_t k = 0; k < num_tone_data; k++) {
1301 uint8_t antenna_path = k == num_antenna_paths
1302 ? num_antenna_paths
1303 : cs_antenna_permutation_array_[permutation_index][k] - 1;
1304 double i_value = get_iq_value(tone_data[k].i_sample_);
1305 double q_value = get_iq_value(tone_data[k].q_sample_);
1306 uint8_t tone_quality_indicator = tone_data[k].tone_quality_indicator_;
1307 log::verbose(
1308 "antenna_path {}, {:f}, {:f}", (uint16_t)(antenna_path + 1), i_value, q_value);
1309 if (role == CsRole::INITIATOR) {
1310 procedure_data.tone_pct_initiator[antenna_path].emplace_back(i_value, q_value);
1311 procedure_data.tone_quality_indicator_initiator[antenna_path].emplace_back(
1312 tone_quality_indicator);
1313 } else {
1314 procedure_data.tone_pct_reflector[antenna_path].emplace_back(i_value, q_value);
1315 procedure_data.tone_quality_indicator_reflector[antenna_path].emplace_back(
1316 tone_quality_indicator);
1317 }
1318 }
1319 } break;
1320 case 1:
1321 case 3:
1322 log::debug("Unsupported mode: {}", mode);
1323 break;
1324 default: {
1325 log::warn("Invalid mode {}", mode);
1326 }
1327 }
1328 }
1329 }
1330
get_iq_valuebluetooth::hci::DistanceMeasurementManager::impl1331 double get_iq_value(uint16_t sample) {
1332 int16_t signed_sample = convert_to_signed(sample, 12);
1333 double value = 1.0 * signed_sample / 2048;
1334 return value;
1335 }
1336
convert_to_signedbluetooth::hci::DistanceMeasurementManager::impl1337 int16_t convert_to_signed(uint16_t num_unsigned, uint8_t bits) {
1338 unsigned msb_mask = 1 << (bits - 1); // setup a mask for most significant bit
1339 int16_t num_signed = num_unsigned;
1340 if ((num_signed & msb_mask) != 0) {
1341 num_signed |= ~(msb_mask - 1); // extend the MSB
1342 }
1343 return num_signed;
1344 }
1345
print_raw_databluetooth::hci::DistanceMeasurementManager::impl1346 void print_raw_data(std::vector<uint8_t> raw_data) {
1347 std::string raw_data_str = "";
1348 auto for_end = raw_data.size() - 1;
1349 for (size_t i = 0; i < for_end; i++) {
1350 char buff[10];
1351 snprintf(buff, sizeof(buff), "%02x ", (uint8_t)raw_data[i]);
1352 std::string buffAsStdStr = buff;
1353 raw_data_str.append(buffAsStdStr);
1354 if (i % 100 == 0 && i != 0) {
1355 log::verbose("{}", raw_data_str);
1356 raw_data_str = "";
1357 }
1358 }
1359 char buff[10];
1360 snprintf(buff, sizeof(buff), "%02x", (uint8_t)raw_data[for_end]);
1361 std::string buffAsStdStr = buff;
1362 raw_data_str.append(buffAsStdStr);
1363 log::verbose("{}", raw_data_str);
1364 }
1365
on_read_remote_transmit_power_level_statusbluetooth::hci::DistanceMeasurementManager::impl1366 void on_read_remote_transmit_power_level_status(Address address, CommandStatusView view) {
1367 auto status_view = LeReadRemoteTransmitPowerLevelStatusView::Create(view);
1368 if (!status_view.IsValid()) {
1369 log::warn("Invalid LeReadRemoteTransmitPowerLevelStatus event");
1370 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
1371 address, REASON_INTERNAL_ERROR, METHOD_RSSI);
1372 rssi_trackers.erase(address);
1373 } else if (status_view.GetStatus() != ErrorCode::SUCCESS) {
1374 std::string error_code = ErrorCodeText(status_view.GetStatus());
1375 log::warn("Received LeReadRemoteTransmitPowerLevelStatus with error code {}", error_code);
1376 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
1377 address, REASON_INTERNAL_ERROR, METHOD_RSSI);
1378 rssi_trackers.erase(address);
1379 }
1380 }
1381
on_transmit_power_reportingbluetooth::hci::DistanceMeasurementManager::impl1382 void on_transmit_power_reporting(LeMetaEventView event) {
1383 auto event_view = LeTransmitPowerReportingView::Create(event);
1384 if (!event_view.IsValid()) {
1385 log::warn("Dropping invalid LeTransmitPowerReporting event");
1386 return;
1387 }
1388
1389 if (event_view.GetReason() == ReportingReason::LOCAL_TRANSMIT_POWER_CHANGED) {
1390 log::warn("Dropping local LeTransmitPowerReporting event");
1391 return;
1392 }
1393
1394 Address address = Address::kEmpty;
1395 for (auto& rssi_tracker : rssi_trackers) {
1396 if (rssi_tracker.second.handle == event_view.GetConnectionHandle()) {
1397 address = rssi_tracker.first;
1398 }
1399 }
1400
1401 if (address.IsEmpty()) {
1402 log::warn("Can't find rssi tracker for connection {}", event_view.GetConnectionHandle());
1403 return;
1404 }
1405
1406 auto status = event_view.GetStatus();
1407 if (status != ErrorCode::SUCCESS) {
1408 log::warn("Received LeTransmitPowerReporting with error code {}", ErrorCodeText(status));
1409 } else {
1410 rssi_trackers[address].remote_tx_power = event_view.GetTransmitPowerLevel();
1411 }
1412
1413 if (event_view.GetReason() == ReportingReason::READ_COMMAND_COMPLETE &&
1414 !rssi_trackers[address].started) {
1415 if (status == ErrorCode::SUCCESS) {
1416 hci_layer_->EnqueueCommand(
1417 LeSetTransmitPowerReportingEnableBuilder::Create(
1418 event_view.GetConnectionHandle(), 0x00, 0x01),
1419 handler_->BindOnceOn(
1420 this, &impl::on_set_transmit_power_reporting_enable_complete, address));
1421 } else {
1422 log::warn("Read remote transmit power level fail");
1423 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
1424 address, REASON_INTERNAL_ERROR, METHOD_RSSI);
1425 rssi_trackers.erase(address);
1426 }
1427 }
1428 }
1429
on_set_transmit_power_reporting_enable_completebluetooth::hci::DistanceMeasurementManager::impl1430 void on_set_transmit_power_reporting_enable_complete(Address address, CommandCompleteView view) {
1431 auto complete_view = LeSetTransmitPowerReportingEnableCompleteView::Create(view);
1432 if (!complete_view.IsValid()) {
1433 log::warn("Invalid LeSetTransmitPowerReportingEnableComplete event");
1434 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
1435 address, REASON_INTERNAL_ERROR, METHOD_RSSI);
1436 rssi_trackers.erase(address);
1437 return;
1438 } else if (complete_view.GetStatus() != ErrorCode::SUCCESS) {
1439 std::string error_code = ErrorCodeText(complete_view.GetStatus());
1440 log::warn(
1441 "Received LeSetTransmitPowerReportingEnableComplete with error code {}", error_code);
1442 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
1443 address, REASON_INTERNAL_ERROR, METHOD_RSSI);
1444 rssi_trackers.erase(address);
1445 return;
1446 }
1447
1448 if (rssi_trackers.find(address) == rssi_trackers.end()) {
1449 log::warn("Can't find rssi tracker for {}", address);
1450 distance_measurement_callbacks_->OnDistanceMeasurementStartFail(
1451 address, REASON_INTERNAL_ERROR, METHOD_RSSI);
1452 rssi_trackers.erase(address);
1453 } else {
1454 log::info("Track rssi for address {}", address);
1455 rssi_trackers[address].started = true;
1456 distance_measurement_callbacks_->OnDistanceMeasurementStarted(address, METHOD_RSSI);
1457 rssi_trackers[address].repeating_alarm->Schedule(
1458 common::Bind(&impl::send_read_rssi, common::Unretained(this), address),
1459 std::chrono::milliseconds(rssi_trackers[address].interval_ms));
1460 }
1461 }
1462
on_read_rssi_completebluetooth::hci::DistanceMeasurementManager::impl1463 void on_read_rssi_complete(Address address, CommandCompleteView view) {
1464 auto complete_view = ReadRssiCompleteView::Create(view);
1465 if (!complete_view.IsValid()) {
1466 log::warn("Dropping invalid read RSSI complete event");
1467 return;
1468 }
1469 if (rssi_trackers.find(address) == rssi_trackers.end()) {
1470 log::warn("Can't find rssi tracker for {}", address);
1471 return;
1472 }
1473 double remote_tx_power = (int8_t)rssi_trackers[address].remote_tx_power;
1474 int8_t rssi = complete_view.GetRssi();
1475 double pow_value = (remote_tx_power - rssi - kRSSIDropOffAt1M) / 20.0;
1476 double distance = pow(10.0, pow_value);
1477 distance_measurement_callbacks_->OnDistanceMeasurementResult(
1478 address,
1479 distance * 100,
1480 distance * 100,
1481 -1,
1482 -1,
1483 -1,
1484 -1,
1485 DistanceMeasurementMethod::METHOD_RSSI);
1486 }
1487
builder_to_bytesbluetooth::hci::DistanceMeasurementManager::impl1488 std::vector<uint8_t> builder_to_bytes(std::unique_ptr<PacketBuilder<true>> builder) {
1489 std::shared_ptr<std::vector<uint8_t>> bytes = std::make_shared<std::vector<uint8_t>>();
1490 BitInserter bi(*bytes);
1491 builder->Serialize(bi);
1492 return *bytes;
1493 }
1494
append_vectorbluetooth::hci::DistanceMeasurementManager::impl1495 void append_vector(std::vector<uint8_t>& v1, const std::vector<uint8_t>& v2) {
1496 v1.reserve(v2.size());
1497 v1.insert(v1.end(), v2.begin(), v2.end());
1498 }
1499
1500 struct RSSITracker {
1501 uint16_t handle;
1502 uint16_t interval_ms;
1503 uint8_t remote_tx_power;
1504 bool started;
1505 std::unique_ptr<os::RepeatingAlarm> repeating_alarm;
1506 };
1507
1508 struct CsTracker {
1509 Address address;
1510 uint16_t local_counter;
1511 uint16_t remote_counter;
1512 CsRole role;
1513 bool ras_connected = false;
1514 bool setup_complete = false;
1515 bool config_set = false;
1516 CsMainModeType main_mode_type;
1517 CsSubModeType sub_mode_type;
1518 CsRttType rtt_type;
1519 bool remote_support_phase_based_ranging = false;
1520 uint8_t config_id = 0;
1521 uint8_t selected_tx_power = 0;
1522 std::vector<CsProcedureData> procedure_data_list;
1523 uint16_t interval_ms;
1524 bool waiting_for_start_callback = false;
1525 std::unique_ptr<os::RepeatingAlarm> repeating_alarm;
1526 // RAS data
1527 RangingHeader ranging_header_;
1528 PacketViewForRecombination segment_data_;
1529 };
1530
1531 os::Handler* handler_;
1532 hal::RangingHal* ranging_hal_;
1533 hci::HciLayer* hci_layer_;
1534 hci::AclManager* acl_manager_;
1535 hci::DistanceMeasurementInterface* distance_measurement_interface_;
1536 std::unordered_map<Address, RSSITracker> rssi_trackers;
1537 std::unordered_map<uint16_t, CsTracker> cs_trackers_;
1538 DistanceMeasurementCallbacks* distance_measurement_callbacks_;
1539 CsOptionalSubfeaturesSupported cs_subfeature_supported_;
1540 // Antenna path permutations. See Channel Sounding CR_PR for the details.
1541 uint8_t cs_antenna_permutation_array_[24][4] = {
1542 {1, 2, 3, 4}, {2, 1, 3, 4}, {1, 3, 2, 4}, {3, 1, 2, 4}, {3, 2, 1, 4}, {2, 3, 1, 4},
1543 {1, 2, 4, 3}, {2, 1, 4, 3}, {1, 4, 2, 3}, {4, 1, 2, 3}, {4, 2, 1, 3}, {2, 4, 1, 3},
1544 {1, 4, 3, 2}, {4, 1, 3, 2}, {1, 3, 4, 2}, {3, 1, 4, 2}, {3, 4, 1, 2}, {4, 3, 1, 2},
1545 {4, 2, 3, 1}, {2, 4, 3, 1}, {4, 3, 2, 1}, {3, 4, 2, 1}, {3, 2, 4, 1}, {2, 3, 4, 1}};
1546 };
1547
DistanceMeasurementManager()1548 DistanceMeasurementManager::DistanceMeasurementManager() {
1549 pimpl_ = std::make_unique<impl>();
1550 }
1551
1552 DistanceMeasurementManager::~DistanceMeasurementManager() = default;
1553
ListDependencies(ModuleList * list) const1554 void DistanceMeasurementManager::ListDependencies(ModuleList* list) const {
1555 list->add<hal::RangingHal>();
1556 list->add<hci::HciLayer>();
1557 list->add<hci::AclManager>();
1558 }
1559
Start()1560 void DistanceMeasurementManager::Start() {
1561 pimpl_->start(
1562 GetHandler(),
1563 GetDependency<hal::RangingHal>(),
1564 GetDependency<hci::HciLayer>(),
1565 GetDependency<AclManager>());
1566 }
1567
Stop()1568 void DistanceMeasurementManager::Stop() {
1569 pimpl_->stop();
1570 }
1571
ToString() const1572 std::string DistanceMeasurementManager::ToString() const {
1573 return "Distance Measurement Manager";
1574 }
1575
RegisterDistanceMeasurementCallbacks(DistanceMeasurementCallbacks * callbacks)1576 void DistanceMeasurementManager::RegisterDistanceMeasurementCallbacks(
1577 DistanceMeasurementCallbacks* callbacks) {
1578 CallOn(pimpl_.get(), &impl::register_distance_measurement_callbacks, callbacks);
1579 }
1580
StartDistanceMeasurement(const Address & address,uint16_t interval,DistanceMeasurementMethod method)1581 void DistanceMeasurementManager::StartDistanceMeasurement(
1582 const Address& address, uint16_t interval, DistanceMeasurementMethod method) {
1583 CallOn(pimpl_.get(), &impl::start_distance_measurement, address, interval, method);
1584 }
1585
StopDistanceMeasurement(const Address & address,DistanceMeasurementMethod method)1586 void DistanceMeasurementManager::StopDistanceMeasurement(
1587 const Address& address, DistanceMeasurementMethod method) {
1588 CallOn(pimpl_.get(), &impl::stop_distance_measurement, address, method);
1589 }
1590
HandleRasConnectedEvent(const Address & address,uint16_t att_handle,const std::vector<hal::VendorSpecificCharacteristic> & vendor_specific_data)1591 void DistanceMeasurementManager::HandleRasConnectedEvent(
1592 const Address& address,
1593 uint16_t att_handle,
1594 const std::vector<hal::VendorSpecificCharacteristic>& vendor_specific_data) {
1595 CallOn(
1596 pimpl_.get(), &impl::handle_ras_connected_event, address, att_handle, vendor_specific_data);
1597 }
1598
HandleVendorSpecificReply(const Address & address,const std::vector<hal::VendorSpecificCharacteristic> & vendor_specific_reply)1599 void DistanceMeasurementManager::HandleVendorSpecificReply(
1600 const Address& address,
1601 const std::vector<hal::VendorSpecificCharacteristic>& vendor_specific_reply) {
1602 CallOn(pimpl_.get(), &impl::handle_vendor_specific_reply, address, vendor_specific_reply);
1603 }
1604
HandleVendorSpecificReplyComplete(const Address & address,bool success)1605 void DistanceMeasurementManager::HandleVendorSpecificReplyComplete(
1606 const Address& address, bool success) {
1607 CallOn(pimpl_.get(), &impl::handle_vendor_specific_reply_complete, address, success);
1608 }
1609
HandleRemoteData(const Address & address,const std::vector<uint8_t> & raw_data)1610 void DistanceMeasurementManager::HandleRemoteData(
1611 const Address& address, const std::vector<uint8_t>& raw_data) {
1612 CallOn(pimpl_.get(), &impl::handle_remote_data, address, raw_data);
1613 }
1614
1615 } // namespace hci
1616 } // namespace bluetooth
1617