1 /*
2  * Copyright 2024 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 
17 #include <base/functional/bind.h>
18 #include <base/functional/callback.h>
19 #include <bluetooth/log.h>
20 
21 #include <algorithm>
22 #include <cstdint>
23 #include <cstring>
24 #include <mutex>
25 #include <unordered_map>
26 #include <vector>
27 
28 #include "bta/include/bta_gatt_api.h"
29 #include "bta/include/bta_ras_api.h"
30 #include "bta/ras/ras_types.h"
31 #include "btm_ble_api_types.h"
32 #include "gatt_api.h"
33 #include "gd/hci/controller_interface.h"
34 #include "gd/os/rand.h"
35 #include "hardware/bt_common_types.h"
36 #include "main/shim/entry.h"
37 #include "stack/include/bt_types.h"
38 #include "stack/include/btm_ble_addr.h"
39 #include "stack/include/main_thread.h"
40 #include "types/ble_address_with_type.h"
41 #include "types/bluetooth/uuid.h"
42 #include "types/bt_transport.h"
43 #include "types/raw_address.h"
44 
45 using namespace bluetooth;
46 using namespace ::ras;
47 using namespace ::ras::uuid;
48 using bluetooth::ras::VendorSpecificCharacteristic;
49 
50 namespace {
51 
52 class RasServerImpl;
53 RasServerImpl* instance;
54 
55 static constexpr uint32_t kSupportedFeatures = feature::kRealTimeRangingData;
56 static constexpr uint16_t kBufferSize = 3;
57 
58 class RasServerImpl : public bluetooth::ras::RasServer {
59 public:
60   struct RasCharacteristic {
61     bluetooth::Uuid uuid_;
62     uint16_t attribute_handle_;
63     uint16_t attribute_handle_ccc_;
64   };
65 
66   // Struct to save data of specific ranging counter
67   struct DataBuffer {
DataBuffer__anon46e572cf0111::RasServerImpl::DataBuffer68     DataBuffer(uint16_t ranging_counter) : ranging_counter_(ranging_counter), segments_() {}
69     uint16_t ranging_counter_;
70     std::vector<std::vector<uint8_t>> segments_;
71   };
72 
73   struct PendingWriteResponse {
74     tCONN_ID conn_id_;
75     uint32_t trans_id_;
76     uint16_t write_req_handle_;
77   };
78 
79   struct ClientTracker {
80     tCONN_ID conn_id_;
81     std::unordered_map<Uuid, uint16_t> ccc_values_;
82     std::vector<DataBuffer> buffers_;
83     bool handling_control_point_command_ = false;
84     uint8_t vendor_specific_reply_counter_ = 0;
85     PendingWriteResponse pending_write_response_;
86     uint16_t last_ready_procedure_ = 0;
87     uint16_t last_overwritten_procedure_ = 0;
88   };
89 
Initialize()90   void Initialize() override {
91     do_in_main_thread(base::BindOnce(&RasServerImpl::do_initialize, base::Unretained(this)));
92   }
93 
do_initialize()94   void do_initialize() {
95     auto controller = bluetooth::shim::GetController();
96     if (controller && !controller->SupportsBleChannelSounding()) {
97       log::info("controller does not support channel sounding.");
98       return;
99     }
100     Uuid uuid = Uuid::From128BitBE(bluetooth::os::GenerateRandom<Uuid::kNumBytes128>());
101     app_uuid_ = uuid;
102     log::info("Register server with uuid:{}", app_uuid_.ToString());
103     BTA_GATTS_AppRegister(
104             app_uuid_,
105             [](tBTA_GATTS_EVT event, tBTA_GATTS* p_data) {
106               if (instance && p_data) {
107                 instance->GattsCallback(event, p_data);
108               }
109             },
110             false);
111   }
112 
RegisterCallbacks(bluetooth::ras::RasServerCallbacks * callbacks)113   void RegisterCallbacks(bluetooth::ras::RasServerCallbacks* callbacks) { callbacks_ = callbacks; }
114 
SetVendorSpecificCharacteristic(const std::vector<VendorSpecificCharacteristic> & vendor_specific_characteristics)115   void SetVendorSpecificCharacteristic(
116           const std::vector<VendorSpecificCharacteristic>& vendor_specific_characteristics) {
117     vendor_specific_characteristics_ = vendor_specific_characteristics;
118   }
119 
HandleVendorSpecificReplyComplete(RawAddress address,bool success)120   void HandleVendorSpecificReplyComplete(RawAddress address, bool success) {
121     log::info("address:{}, success:{}", address, success);
122     tBLE_BD_ADDR ble_bd_addr;
123     ResolveAddress(ble_bd_addr, address);
124     if (trackers_.find(ble_bd_addr.bda) == trackers_.end()) {
125       log::warn("Can't find tracker for address {}", address);
126       return;
127     }
128     auto response = trackers_[ble_bd_addr.bda].pending_write_response_;
129     tGATTS_RSP p_msg;
130     p_msg.attr_value.handle = response.write_req_handle_;
131     GattStatus status = success ? GATT_SUCCESS : GATT_ERROR;
132     BTA_GATTS_SendRsp(response.conn_id_, response.trans_id_, status, &p_msg);
133   }
134 
PushProcedureData(RawAddress address,uint16_t procedure_counter,bool is_last,std::vector<uint8_t> data)135   void PushProcedureData(RawAddress address, uint16_t procedure_counter, bool is_last,
136                          std::vector<uint8_t> data) {
137     log::debug("{}, counter:{}, is_last:{}, with size {}", address, procedure_counter, is_last,
138                data.size());
139     tBLE_BD_ADDR ble_bd_addr;
140     ResolveAddress(ble_bd_addr, address);
141 
142     if (trackers_.find(ble_bd_addr.bda) == trackers_.end()) {
143       log::warn("Can't find tracker for {}", ble_bd_addr.bda);
144       return;
145     }
146     ClientTracker& tracker = trackers_[ble_bd_addr.bda];
147     uint16_t ccc_real_time = tracker.ccc_values_[kRasRealTimeRangingDataCharacteristic];
148     uint16_t ccc_data_ready = tracker.ccc_values_[kRasRangingDataReadyCharacteristic];
149     uint16_t ccc_data_over_written = tracker.ccc_values_[kRasRangingDataOverWrittenCharacteristic];
150 
151     if (ccc_real_time != GATT_CLT_CONFIG_NONE) {
152       bool use_notification = ccc_real_time & GATT_CLT_CONFIG_NOTIFICATION;
153       uint16_t attr_id =
154               GetCharacteristic(kRasRealTimeRangingDataCharacteristic)->attribute_handle_;
155       log::debug("Send Real-time Ranging Data is_last {}", is_last);
156       BTA_GATTS_HandleValueIndication(tracker.conn_id_, attr_id, data, !use_notification);
157     }
158 
159     if (ccc_data_ready == GATT_CLT_CONFIG_NONE && ccc_data_over_written == GATT_CLT_CONFIG_NONE) {
160       return;
161     }
162     std::lock_guard<std::mutex> lock(on_demand_ranging_mutex_);
163     DataBuffer& data_buffer = InitDataBuffer(ble_bd_addr.bda, procedure_counter);
164     data_buffer.segments_.push_back(data);
165     tracker.last_ready_procedure_ = procedure_counter;
166 
167     // Send data ready
168     if (is_last) {
169       if (ccc_data_ready == GATT_CLT_CONFIG_NONE || ccc_real_time != GATT_CLT_CONFIG_NONE) {
170         log::debug("Skip Ranging Data Ready");
171       } else {
172         bool need_confirm = ccc_data_ready & GATT_CLT_CONFIG_INDICATION;
173         log::debug("Send data ready, ranging_counter {}, total fragment {}", procedure_counter,
174                    data_buffer.segments_.size());
175         uint16_t attr_id = GetCharacteristic(kRasRangingDataReadyCharacteristic)->attribute_handle_;
176         std::vector<uint8_t> value(kRingingCounterSize);
177         value[0] = (procedure_counter & 0xFF);
178         value[1] = (procedure_counter >> 8) & 0xFF;
179         BTA_GATTS_HandleValueIndication(tracker.conn_id_, attr_id, value, need_confirm);
180       }
181     }
182 
183     // Send data overwritten
184     if (tracker.buffers_.size() > kBufferSize) {
185       auto begin = tracker.buffers_.begin();
186       tracker.last_overwritten_procedure_ = begin->ranging_counter_;
187       if (ccc_data_over_written == GATT_CLT_CONFIG_NONE || ccc_real_time != GATT_CLT_CONFIG_NONE) {
188         log::debug("Skip Ranging Data Over Written");
189         tracker.buffers_.erase(begin);
190         return;
191       }
192       bool need_confirm = ccc_data_over_written & GATT_CLT_CONFIG_INDICATION;
193       log::debug("Send data over written, ranging_counter {}", begin->ranging_counter_);
194       uint16_t attr_id =
195               GetCharacteristic(kRasRangingDataOverWrittenCharacteristic)->attribute_handle_;
196       std::vector<uint8_t> value(kRingingCounterSize);
197       value[0] = (begin->ranging_counter_ & 0xFF);
198       value[1] = (begin->ranging_counter_ >> 8) & 0xFF;
199       BTA_GATTS_HandleValueIndication(tracker.conn_id_, attr_id, value, need_confirm);
200       tracker.buffers_.erase(begin);
201     }
202   }
203 
GattsCallback(tBTA_GATTS_EVT event,tBTA_GATTS * p_data)204   void GattsCallback(tBTA_GATTS_EVT event, tBTA_GATTS* p_data) {
205     log::info("event: {}", gatt_server_event_text(event));
206     switch (event) {
207       case BTA_GATTS_CONNECT_EVT: {
208         OnGattConnect(p_data);
209       } break;
210       case BTA_GATTS_DISCONNECT_EVT: {
211         OnGattDisconnect(p_data);
212       } break;
213       case BTA_GATTS_REG_EVT: {
214         OnGattServerRegister(p_data);
215       } break;
216       case BTA_GATTS_READ_CHARACTERISTIC_EVT: {
217         OnReadCharacteristic(p_data);
218       } break;
219       case BTA_GATTS_READ_DESCRIPTOR_EVT: {
220         OnReadDescriptor(p_data);
221       } break;
222       case BTA_GATTS_WRITE_CHARACTERISTIC_EVT: {
223         OnWriteCharacteristic(p_data);
224       } break;
225       case BTA_GATTS_WRITE_DESCRIPTOR_EVT: {
226         OnWriteDescriptor(p_data);
227       } break;
228       default:
229         log::warn("Unhandled event {}", event);
230     }
231   }
232 
OnGattConnect(tBTA_GATTS * p_data)233   void OnGattConnect(tBTA_GATTS* p_data) {
234     auto address = p_data->conn.remote_bda;
235     log::info("Address: {}, conn_id:{}", address, p_data->conn.conn_id);
236     if (p_data->conn.transport == BT_TRANSPORT_BR_EDR) {
237       log::warn("Skip BE/EDR connection");
238       return;
239     }
240 
241     if (trackers_.find(address) == trackers_.end()) {
242       log::warn("Create new tracker");
243     }
244     trackers_[address].conn_id_ = p_data->conn.conn_id;
245 
246     RawAddress identity_address = p_data->conn.remote_bda;
247     tBLE_ADDR_TYPE address_type = BLE_ADDR_PUBLIC_ID;
248     btm_random_pseudo_to_identity_addr(&identity_address, &address_type);
249     // TODO: optimize, remove this event, initialize the tracker within the GD on demand.
250     callbacks_->OnRasServerConnected(identity_address);
251   }
252 
OnGattDisconnect(tBTA_GATTS * p_data)253   void OnGattDisconnect(tBTA_GATTS* p_data) {
254     auto remote_bda = p_data->conn.remote_bda;
255     log::info("Address: {}, conn_id:{}", remote_bda, p_data->conn.conn_id);
256     if (trackers_.find(remote_bda) != trackers_.end()) {
257       NotifyRasServerDisconnected(remote_bda);
258       trackers_.erase(remote_bda);
259     }
260   }
261 
NotifyRasServerDisconnected(const RawAddress & remote_bda)262   void NotifyRasServerDisconnected(const RawAddress& remote_bda) {
263     tBLE_BD_ADDR ble_identity_bd_addr;
264     ble_identity_bd_addr.bda = remote_bda;
265     ble_identity_bd_addr.type = BLE_ADDR_RANDOM;
266     btm_random_pseudo_to_identity_addr(&ble_identity_bd_addr.bda, &ble_identity_bd_addr.type);
267 
268     callbacks_->OnRasServerDisconnected(ble_identity_bd_addr.bda);
269   }
270 
OnGattServerRegister(tBTA_GATTS * p_data)271   void OnGattServerRegister(tBTA_GATTS* p_data) {
272     tGATT_STATUS status = p_data->reg_oper.status;
273     log::info("status: {}", gatt_status_text(p_data->reg_oper.status));
274 
275     if (status != tGATT_STATUS::GATT_SUCCESS) {
276       log::warn("Register Server fail");
277       return;
278     }
279     server_if_ = p_data->reg_oper.server_if;
280 
281     uint16_t key_mask = ((16 - 7) << 12);
282     std::vector<btgatt_db_element_t> service;
283     // RAS service
284     btgatt_db_element_t ranging_service;
285     ranging_service.uuid = kRangingService;
286     ranging_service.type = BTGATT_DB_PRIMARY_SERVICE;
287     service.push_back(ranging_service);
288 
289     // RAS Features
290     btgatt_db_element_t features_characteristic;
291     features_characteristic.uuid = kRasFeaturesCharacteristic;
292     features_characteristic.type = BTGATT_DB_CHARACTERISTIC;
293     features_characteristic.properties = GATT_CHAR_PROP_BIT_READ;
294     features_characteristic.permissions = GATT_PERM_READ_ENCRYPTED | key_mask;
295     service.push_back(features_characteristic);
296 
297     // Real-time Ranging Data (Optional)
298     btgatt_db_element_t real_time_ranging_data_characteristic;
299     real_time_ranging_data_characteristic.uuid = kRasRealTimeRangingDataCharacteristic;
300     real_time_ranging_data_characteristic.type = BTGATT_DB_CHARACTERISTIC;
301     real_time_ranging_data_characteristic.properties =
302             GATT_CHAR_PROP_BIT_NOTIFY | GATT_CHAR_PROP_BIT_INDICATE;
303     real_time_ranging_data_characteristic.permissions = GATT_PERM_READ_ENCRYPTED | key_mask;
304     service.push_back(real_time_ranging_data_characteristic);
305     btgatt_db_element_t ccc_descriptor;
306     ccc_descriptor.uuid = kClientCharacteristicConfiguration;
307     ccc_descriptor.type = BTGATT_DB_DESCRIPTOR;
308     ccc_descriptor.permissions = GATT_PERM_WRITE | GATT_PERM_READ | key_mask;
309     service.push_back(ccc_descriptor);
310 
311     // On-demand Ranging Data
312     btgatt_db_element_t on_demand_ranging_data_characteristic;
313     on_demand_ranging_data_characteristic.uuid = kRasOnDemandDataCharacteristic;
314     on_demand_ranging_data_characteristic.type = BTGATT_DB_CHARACTERISTIC;
315     on_demand_ranging_data_characteristic.properties =
316             GATT_CHAR_PROP_BIT_NOTIFY | GATT_CHAR_PROP_BIT_INDICATE;
317     on_demand_ranging_data_characteristic.permissions = GATT_PERM_READ_ENCRYPTED | key_mask;
318     service.push_back(on_demand_ranging_data_characteristic);
319     service.push_back(ccc_descriptor);
320 
321     // RAS Control Point (RAS-CP)
322     btgatt_db_element_t ras_control_point;
323     ras_control_point.uuid = kRasControlPointCharacteristic;
324     ras_control_point.type = BTGATT_DB_CHARACTERISTIC;
325     ras_control_point.properties = GATT_CHAR_PROP_BIT_WRITE | GATT_CHAR_PROP_BIT_INDICATE;
326     ras_control_point.permissions = GATT_PERM_WRITE_ENCRYPTED | key_mask;
327     service.push_back(ras_control_point);
328     service.push_back(ccc_descriptor);
329 
330     // Ranging Data Ready
331     btgatt_db_element_t ranging_data_ready_characteristic;
332     ranging_data_ready_characteristic.uuid = kRasRangingDataReadyCharacteristic;
333     ranging_data_ready_characteristic.type = BTGATT_DB_CHARACTERISTIC;
334     ranging_data_ready_characteristic.properties =
335             GATT_CHAR_PROP_BIT_READ | GATT_CHAR_PROP_BIT_NOTIFY | GATT_CHAR_PROP_BIT_INDICATE;
336     ranging_data_ready_characteristic.permissions = GATT_PERM_READ_ENCRYPTED | key_mask;
337     service.push_back(ranging_data_ready_characteristic);
338     service.push_back(ccc_descriptor);
339 
340     // Ranging Data Overwritten
341     btgatt_db_element_t ranging_data_overwritten_characteristic;
342     ranging_data_overwritten_characteristic.uuid = kRasRangingDataOverWrittenCharacteristic;
343     ranging_data_overwritten_characteristic.type = BTGATT_DB_CHARACTERISTIC;
344     ranging_data_overwritten_characteristic.properties =
345             GATT_CHAR_PROP_BIT_READ | GATT_CHAR_PROP_BIT_NOTIFY | GATT_CHAR_PROP_BIT_INDICATE;
346     ranging_data_overwritten_characteristic.permissions = GATT_PERM_READ_ENCRYPTED | key_mask;
347     service.push_back(ranging_data_overwritten_characteristic);
348     service.push_back(ccc_descriptor);
349 
350     for (auto& vendor_specific_characteristics : vendor_specific_characteristics_) {
351       btgatt_db_element_t characteristics;
352       characteristics.uuid = vendor_specific_characteristics.characteristicUuid_;
353       characteristics.type = BTGATT_DB_CHARACTERISTIC;
354       characteristics.properties = GATT_CHAR_PROP_BIT_READ | GATT_CHAR_PROP_BIT_WRITE;
355       characteristics.permissions = GATT_PERM_READ_ENCRYPTED | GATT_PERM_WRITE_ENCRYPTED | key_mask;
356       service.push_back(characteristics);
357       log::info("Push vendor_specific_characteristics uuid {}", characteristics.uuid);
358     }
359 
360     BTA_GATTS_AddService(server_if_, service,
361                          base::BindRepeating([](tGATT_STATUS status, int server_if,
362                                                 std::vector<btgatt_db_element_t> service) {
363                            if (instance) {
364                              instance->OnServiceAdded(status, server_if, service);
365                            }
366                          }));
367   }
368 
OnReadCharacteristic(tBTA_GATTS * p_data)369   void OnReadCharacteristic(tBTA_GATTS* p_data) {
370     uint16_t read_req_handle = p_data->req_data.p_data->read_req.handle;
371     log::info("read_req_handle: 0x{:04x},", read_req_handle);
372 
373     tGATTS_RSP p_msg;
374     p_msg.attr_value.handle = read_req_handle;
375     if (characteristics_.find(read_req_handle) == characteristics_.end()) {
376       log::error("Invalid handle 0x{:04x}", read_req_handle);
377       BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id, GATT_INVALID_HANDLE,
378                         &p_msg);
379       return;
380     }
381 
382     auto uuid = characteristics_[read_req_handle].uuid_;
383     auto vendor_specific_characteristic = GetVendorSpecificCharacteristic(uuid);
384     if (vendor_specific_characteristic != nullptr) {
385       log::debug("Read vendor_specific_characteristic uuid {}", uuid);
386       p_msg.attr_value.len = vendor_specific_characteristic->value_.size();
387       std::copy(vendor_specific_characteristic->value_.begin(),
388                 vendor_specific_characteristic->value_.end(), p_msg.attr_value.value);
389       BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id, GATT_SUCCESS, &p_msg);
390       return;
391     }
392     log::info("Read uuid, {}", getUuidName(uuid));
393     ClientTracker* tracker = &trackers_[p_data->req_data.remote_bda];
394     if (trackers_.find(p_data->req_data.remote_bda) == trackers_.end()) {
395       log::warn("Can't find tracker for {}", p_data->req_data.remote_bda);
396       BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id, GATT_ILLEGAL_PARAMETER,
397                         &p_msg);
398       return;
399     }
400 
401     // Check Characteristic UUID
402     switch (uuid.As16Bit()) {
403       case kRasFeaturesCharacteristic16bit: {
404         p_msg.attr_value.len = kFeatureSize;
405         memcpy(p_msg.attr_value.value, &kSupportedFeatures, sizeof(uint32_t));
406       } break;
407       case kRasRangingDataReadyCharacteristic16bit: {
408         p_msg.attr_value.len = kRingingCounterSize;
409         p_msg.attr_value.value[0] = (tracker->last_ready_procedure_ & 0xFF);
410         p_msg.attr_value.value[1] = (tracker->last_ready_procedure_ >> 8) & 0xFF;
411       } break;
412       case kRasRangingDataOverWrittenCharacteristic16bit: {
413         p_msg.attr_value.len = kRingingCounterSize;
414         p_msg.attr_value.value[0] = (tracker->last_overwritten_procedure_ & 0xFF);
415         p_msg.attr_value.value[1] = (tracker->last_overwritten_procedure_ >> 8) & 0xFF;
416       } break;
417       default:
418         log::warn("Unhandled uuid {}", uuid.ToString());
419         BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id,
420                           GATT_ILLEGAL_PARAMETER, &p_msg);
421         return;
422     }
423     BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id, GATT_SUCCESS, &p_msg);
424   }
425 
OnReadDescriptor(tBTA_GATTS * p_data)426   void OnReadDescriptor(tBTA_GATTS* p_data) {
427     tCONN_ID conn_id = p_data->req_data.conn_id;
428     uint16_t read_req_handle = p_data->req_data.p_data->read_req.handle;
429     RawAddress remote_bda = p_data->req_data.remote_bda;
430     log::info("conn_id:{}, read_req_handle:0x{:04x}", conn_id, read_req_handle);
431 
432     tGATTS_RSP p_msg;
433     p_msg.attr_value.handle = read_req_handle;
434 
435     // Only Client Characteristic Configuration (CCC) descriptor is expected
436     RasCharacteristic* characteristic = GetCharacteristicByCccHandle(read_req_handle);
437     if (characteristic == nullptr) {
438       log::warn("Can't find Characteristic for CCC Descriptor, handle 0x{:04x}", read_req_handle);
439       BTA_GATTS_SendRsp(conn_id, p_data->req_data.trans_id, GATT_INVALID_HANDLE, &p_msg);
440       return;
441     }
442     log::info("Read CCC for uuid, {}", getUuidName(characteristic->uuid_));
443     uint16_t ccc_value = 0;
444     if (trackers_.find(remote_bda) != trackers_.end()) {
445       ccc_value = trackers_[remote_bda].ccc_values_[characteristic->uuid_];
446     }
447 
448     p_msg.attr_value.len = kCccValueSize;
449     memcpy(p_msg.attr_value.value, &ccc_value, sizeof(uint16_t));
450 
451     log::info("Send response for CCC value 0x{:04x}", ccc_value);
452     BTA_GATTS_SendRsp(conn_id, p_data->req_data.trans_id, GATT_SUCCESS, &p_msg);
453   }
454 
OnWriteCharacteristic(tBTA_GATTS * p_data)455   void OnWriteCharacteristic(tBTA_GATTS* p_data) {
456     tCONN_ID conn_id = p_data->req_data.conn_id;
457     uint16_t write_req_handle = p_data->req_data.p_data->write_req.handle;
458     uint16_t len = p_data->req_data.p_data->write_req.len;
459     bool need_rsp = p_data->req_data.p_data->write_req.need_rsp;
460     RawAddress remote_bda = p_data->req_data.remote_bda;
461     log::info("conn_id:{}, write_req_handle:0x{:04x}, need_rsp{}, len:{}", conn_id,
462               write_req_handle, need_rsp, len);
463 
464     tGATTS_RSP p_msg;
465     p_msg.handle = write_req_handle;
466     if (characteristics_.find(write_req_handle) == characteristics_.end()) {
467       log::error("Invalid handle {}", write_req_handle);
468       BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id, GATT_INVALID_HANDLE,
469                         &p_msg);
470       return;
471     }
472 
473     auto uuid = characteristics_[write_req_handle].uuid_;
474     auto vendor_specific_characteristic = GetVendorSpecificCharacteristic(uuid);
475     if (vendor_specific_characteristic != nullptr) {
476       WriteVendorSpecificCharacteristic(vendor_specific_characteristic, p_data, p_msg);
477       return;
478     }
479     log::info("Write uuid, {}", getUuidName(uuid));
480 
481     // Check Characteristic UUID
482     switch (uuid.As16Bit()) {
483       case kRasControlPointCharacteristic16bit: {
484         if (trackers_.find(p_data->req_data.remote_bda) == trackers_.end()) {
485           log::warn("Can't find trackers for {}", p_data->req_data.remote_bda);
486           BTA_GATTS_SendRsp(conn_id, p_data->req_data.trans_id, GATT_ILLEGAL_PARAMETER, &p_msg);
487           return;
488         }
489         ClientTracker* tracker = &trackers_[p_data->req_data.remote_bda];
490         if (need_rsp) {
491           BTA_GATTS_SendRsp(conn_id, p_data->req_data.trans_id, GATT_SUCCESS, &p_msg);
492         }
493         HandleControlPoint(tracker, &p_data->req_data.p_data->write_req);
494       } break;
495       default:
496         log::warn("Unhandled uuid {}", uuid.ToString());
497         BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id,
498                           GATT_ILLEGAL_PARAMETER, &p_msg);
499         return;
500     }
501   }
502 
WriteVendorSpecificCharacteristic(VendorSpecificCharacteristic * vendor_specific_characteristic,tBTA_GATTS * p_data,tGATTS_RSP & p_msg)503   void WriteVendorSpecificCharacteristic(
504           VendorSpecificCharacteristic* vendor_specific_characteristic, tBTA_GATTS* p_data,
505           tGATTS_RSP& p_msg) {
506     log::debug("uuid {}", vendor_specific_characteristic->characteristicUuid_);
507     uint16_t len = p_data->req_data.p_data->write_req.len;
508     RawAddress remote_bda = p_data->req_data.remote_bda;
509 
510     if (trackers_.find(remote_bda) == trackers_.end()) {
511       BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id, GATT_INVALID_HANDLE,
512                         &p_msg);
513       log::warn("Can't find tracker for remote_bda {}", remote_bda);
514       return;
515     }
516 
517     // Update reply value
518     auto& tracker = trackers_[remote_bda];
519     auto value = p_data->req_data.p_data->write_req.value;
520     vendor_specific_characteristic->reply_value_.clear();
521     vendor_specific_characteristic->reply_value_.reserve(len);
522     vendor_specific_characteristic->reply_value_.assign(value, value + len);
523     tracker.vendor_specific_reply_counter_++;
524 
525     if (tracker.vendor_specific_reply_counter_ == vendor_specific_characteristics_.size()) {
526       log::info("All vendor specific characteristics written");
527       tBLE_BD_ADDR ble_bd_addr;
528       ble_bd_addr.bda = remote_bda;
529       ble_bd_addr.type = BLE_ADDR_RANDOM;
530       btm_random_pseudo_to_identity_addr(&ble_bd_addr.bda, &ble_bd_addr.type);
531       tracker.vendor_specific_reply_counter_ = 0;
532       tracker.pending_write_response_.conn_id_ = p_data->req_data.conn_id;
533       tracker.pending_write_response_.trans_id_ = p_data->req_data.trans_id;
534       tracker.pending_write_response_.write_req_handle_ = p_msg.handle;
535       callbacks_->OnVendorSpecificReply(ble_bd_addr.bda, vendor_specific_characteristics_);
536     } else {
537       BTA_GATTS_SendRsp(p_data->req_data.conn_id, p_data->req_data.trans_id, GATT_SUCCESS, &p_msg);
538     }
539   }
540 
OnWriteDescriptor(tBTA_GATTS * p_data)541   void OnWriteDescriptor(tBTA_GATTS* p_data) {
542     tCONN_ID conn_id = p_data->req_data.conn_id;
543     uint16_t write_req_handle = p_data->req_data.p_data->write_req.handle;
544     uint16_t len = p_data->req_data.p_data->write_req.len;
545     RawAddress remote_bda = p_data->req_data.remote_bda;
546     log::info("conn_id:{}, write_req_handle:0x{:04x}, len:{}", conn_id, write_req_handle, len);
547 
548     tGATTS_RSP p_msg;
549     p_msg.handle = write_req_handle;
550 
551     // Only Client Characteristic Configuration (CCC) descriptor is expected
552     RasCharacteristic* characteristic = GetCharacteristicByCccHandle(write_req_handle);
553     if (characteristic == nullptr) {
554       log::warn("Can't find Characteristic for CCC Descriptor, handle 0x{:04x}", write_req_handle);
555       BTA_GATTS_SendRsp(conn_id, p_data->req_data.trans_id, GATT_INVALID_HANDLE, &p_msg);
556       return;
557     }
558 
559     if (trackers_.find(remote_bda) == trackers_.end()) {
560       BTA_GATTS_SendRsp(conn_id, p_data->req_data.trans_id, GATT_ILLEGAL_PARAMETER, &p_msg);
561       return;
562     }
563     ClientTracker* tracker = &trackers_[p_data->req_data.remote_bda];
564     const uint8_t* value = p_data->req_data.p_data->write_req.value;
565     uint16_t ccc_value;
566     STREAM_TO_UINT16(ccc_value, value);
567 
568     // Check that On-demand and Real-time are not registered at the same time
569     uint16_t ccc_on_demand_temp = tracker->ccc_values_[kRasOnDemandDataCharacteristic];
570     uint16_t ccc_real_time_temp = tracker->ccc_values_[kRasRealTimeRangingDataCharacteristic];
571     if (characteristic->uuid_ == kRasRealTimeRangingDataCharacteristic) {
572       ccc_real_time_temp = ccc_value;
573     } else if (characteristic->uuid_ == kRasOnDemandDataCharacteristic) {
574       ccc_on_demand_temp = ccc_value;
575     }
576     if (ccc_real_time_temp != GATT_CLT_CONFIG_NONE && ccc_on_demand_temp != GATT_CLT_CONFIG_NONE) {
577       log::warn("Client Characteristic Configuration Descriptor Improperly Configured");
578       BTA_GATTS_SendRsp(conn_id, p_data->req_data.trans_id, GATT_CCC_CFG_ERR, &p_msg);
579       return;
580     }
581 
582     trackers_[remote_bda].ccc_values_[characteristic->uuid_] = ccc_value;
583     log::info("Write CCC for {}, conn_id:{}, value:0x{:04x}", getUuidName(characteristic->uuid_),
584               conn_id, ccc_value);
585     BTA_GATTS_SendRsp(conn_id, p_data->req_data.trans_id, GATT_SUCCESS, &p_msg);
586   }
587 
HandleControlPoint(ClientTracker * tracker,tGATT_WRITE_REQ * write_req)588   void HandleControlPoint(ClientTracker* tracker, tGATT_WRITE_REQ* write_req) {
589     ControlPointCommand command;
590     ParseControlPointCommand(&command, write_req->value, write_req->len);
591 
592     if (!command.isValid_) {
593       SendResponseCode(ResponseCodeValue::INVALID_PARAMETER, tracker);
594       return;
595     }
596 
597     if (tracker->handling_control_point_command_ && command.opcode_ != Opcode::ABORT_OPERATION) {
598       log::warn("Server busy");
599       SendResponseCode(ResponseCodeValue::SERVER_BUSY, tracker);
600       return;
601     }
602 
603     tracker->handling_control_point_command_ = true;
604 
605     switch (command.opcode_) {
606       case Opcode::GET_RANGING_DATA:
607         OnGetRangingData(&command, tracker);
608         break;
609       case Opcode::ACK_RANGING_DATA:
610         OnAckRangingData(&command, tracker);
611         break;
612       case Opcode::RETRIEVE_LOST_RANGING_DATA_SEGMENTS:
613       case Opcode::ABORT_OPERATION:
614       case Opcode::FILTER:
615         log::warn("Unsupported opcode:0x{:02x}, {}", (uint16_t)command.opcode_,
616                   GetOpcodeText(command.opcode_));
617         SendResponseCode(ResponseCodeValue::OP_CODE_NOT_SUPPORTED, tracker);
618         break;
619       default:
620         log::warn("Unknown opcode:0x{:02x}", (uint16_t)command.opcode_);
621         SendResponseCode(ResponseCodeValue::OP_CODE_NOT_SUPPORTED, tracker);
622     }
623   }
624 
OnGetRangingData(ControlPointCommand * command,ClientTracker * tracker)625   void OnGetRangingData(ControlPointCommand* command, ClientTracker* tracker) {
626     const uint8_t* value = command->parameter_;
627     uint16_t ranging_counter;
628     STREAM_TO_UINT16(ranging_counter, value);
629     log::info("ranging_counter:{}", ranging_counter);
630 
631     uint16_t ccc_value = tracker->ccc_values_[kRasOnDemandDataCharacteristic];
632     uint16_t attr_id = GetCharacteristic(kRasOnDemandDataCharacteristic)->attribute_handle_;
633     bool use_notification = ccc_value & GATT_CLT_CONFIG_NOTIFICATION;
634 
635     std::lock_guard<std::mutex> lock(on_demand_ranging_mutex_);
636     auto it = std::find_if(tracker->buffers_.begin(), tracker->buffers_.end(),
637                            [&ranging_counter](const DataBuffer& buffer) {
638                              return buffer.ranging_counter_ == ranging_counter;
639                            });
640     if (it != tracker->buffers_.end()) {
641       for (uint16_t i = 0; i < it->segments_.size(); i++) {
642         if (ccc_value == GATT_CLT_CONFIG_NONE) {
643           log::warn("On Demand Data is not subscribed, Skip");
644           break;
645         }
646         log::info("Send On Demand Ranging Data, segment {}", i);
647         BTA_GATTS_HandleValueIndication(tracker->conn_id_, attr_id, it->segments_[i],
648                                         !use_notification);
649       }
650       log::info("Send COMPLETE_RANGING_DATA_RESPONSE, ranging_counter:{}", ranging_counter);
651       std::vector<uint8_t> response(3, 0);
652       response[0] = (uint8_t)EventCode::COMPLETE_RANGING_DATA_RESPONSE;
653       response[1] = (ranging_counter & 0xFF);
654       response[2] = (ranging_counter >> 8) & 0xFF;
655       BTA_GATTS_HandleValueIndication(
656               tracker->conn_id_,
657               GetCharacteristic(kRasControlPointCharacteristic)->attribute_handle_, response, true);
658       tracker->handling_control_point_command_ = false;
659       return;
660     } else {
661       log::warn("No Records Found");
662       SendResponseCode(ResponseCodeValue::NO_RECORDS_FOUND, tracker);
663     }
664   }
665 
OnAckRangingData(ControlPointCommand * command,ClientTracker * tracker)666   void OnAckRangingData(ControlPointCommand* command, ClientTracker* tracker) {
667     const uint8_t* value = command->parameter_;
668     uint16_t ranging_counter;
669     STREAM_TO_UINT16(ranging_counter, value);
670     log::info("ranging_counter:{}", ranging_counter);
671 
672     std::lock_guard<std::mutex> lock(on_demand_ranging_mutex_);
673     auto it = std::find_if(tracker->buffers_.begin(), tracker->buffers_.end(),
674                            [&ranging_counter](const DataBuffer& buffer) {
675                              return buffer.ranging_counter_ == ranging_counter;
676                            });
677     // If found, erase it
678     if (it != tracker->buffers_.end()) {
679       tracker->buffers_.erase(it);
680       tracker->handling_control_point_command_ = false;
681       SendResponseCode(ResponseCodeValue::SUCCESS, tracker);
682     } else {
683       log::warn("No Records Found");
684       SendResponseCode(ResponseCodeValue::NO_RECORDS_FOUND, tracker);
685     }
686   }
687 
SendResponseCode(ResponseCodeValue response_code_value,ClientTracker * tracker)688   void SendResponseCode(ResponseCodeValue response_code_value, ClientTracker* tracker) {
689     log::info("0x{:02x}, {}", (uint16_t)response_code_value,
690               GetResponseOpcodeValueText(response_code_value));
691     std::vector<uint8_t> response(2, 0);
692     response[0] = (uint8_t)EventCode::RESPONSE_CODE;
693     response[1] = (uint8_t)response_code_value;
694     BTA_GATTS_HandleValueIndication(
695             tracker->conn_id_, GetCharacteristic(kRasControlPointCharacteristic)->attribute_handle_,
696             response, true);
697     tracker->handling_control_point_command_ = false;
698   }
699 
OnServiceAdded(tGATT_STATUS status,int server_if,std::vector<btgatt_db_element_t> service)700   void OnServiceAdded(tGATT_STATUS status, int server_if,
701                       std::vector<btgatt_db_element_t> service) {
702     log::info("status: {}, server_if: {}", gatt_status_text(status), server_if);
703     RasCharacteristic* current_characteristic;
704     for (uint16_t i = 0; i < service.size(); i++) {
705       uint16_t attribute_handle = service[i].attribute_handle;
706       Uuid uuid = service[i].uuid;
707       if (service[i].type == BTGATT_DB_CHARACTERISTIC) {
708         log::info("Characteristic uuid: 0x{:04x}, handle:0x{:04x}, {}", uuid.As16Bit(),
709                   attribute_handle, getUuidName(uuid));
710         characteristics_[attribute_handle].attribute_handle_ = attribute_handle;
711         characteristics_[attribute_handle].uuid_ = uuid;
712         current_characteristic = &characteristics_[attribute_handle];
713       } else if (service[i].type == BTGATT_DB_DESCRIPTOR) {
714         log::info("\tDescriptor uuid: 0x{:04x}, handle: 0x{:04x}, {}", uuid.As16Bit(),
715                   attribute_handle, getUuidName(uuid));
716         if (service[i].uuid == kClientCharacteristicConfiguration) {
717           current_characteristic->attribute_handle_ccc_ = attribute_handle;
718         }
719       }
720     }
721   }
722 
GetCharacteristic(Uuid uuid)723   RasCharacteristic* GetCharacteristic(Uuid uuid) {
724     for (auto& [attribute_handle, characteristic] : characteristics_) {
725       if (characteristic.uuid_ == uuid) {
726         return &characteristic;
727       }
728     }
729     return nullptr;
730   }
731 
GetCharacteristicByCccHandle(uint16_t descriptor_handle)732   RasCharacteristic* GetCharacteristicByCccHandle(uint16_t descriptor_handle) {
733     for (auto& [attribute_handle, characteristic] : characteristics_) {
734       if (characteristic.attribute_handle_ccc_ == descriptor_handle) {
735         return &characteristic;
736       }
737     }
738     return nullptr;
739   }
740 
ResolveAddress(tBLE_BD_ADDR & ble_bd_addr,const RawAddress & address)741   void ResolveAddress(tBLE_BD_ADDR& ble_bd_addr, const RawAddress& address) {
742     ble_bd_addr.bda = address;
743     ble_bd_addr.type = BLE_ADDR_RANDOM;
744     maybe_resolve_address(&ble_bd_addr.bda, &ble_bd_addr.type);
745   }
746 
InitDataBuffer(RawAddress address,uint16_t procedure_counter)747   DataBuffer& InitDataBuffer(RawAddress address, uint16_t procedure_counter) {
748     std::vector<DataBuffer>& buffers = trackers_[address].buffers_;
749     for (DataBuffer& data_buffer : buffers) {
750       if (data_buffer.ranging_counter_ == procedure_counter) {
751         // Data already exist, return
752         return data_buffer;
753       }
754     }
755     log::info("Create data for ranging_counter: {}, current size {}", procedure_counter,
756               buffers.size());
757     buffers.emplace_back(procedure_counter);
758     return buffers.back();
759   }
760 
GetVendorSpecificCharacteristic(const bluetooth::Uuid & uuid)761   VendorSpecificCharacteristic* GetVendorSpecificCharacteristic(const bluetooth::Uuid& uuid) {
762     for (auto& characteristic : vendor_specific_characteristics_) {
763       if (characteristic.characteristicUuid_ == uuid) {
764         return &characteristic;
765       }
766     }
767     return nullptr;
768   }
769 
770 private:
771   bluetooth::Uuid app_uuid_;
772   uint16_t server_if_;
773   // A map to associate characteristics with handles
774   std::unordered_map<uint16_t, RasCharacteristic> characteristics_;
775   // A map to client trackers with address
776   std::unordered_map<RawAddress, ClientTracker> trackers_;
777   bluetooth::ras::RasServerCallbacks* callbacks_;
778   std::mutex on_demand_ranging_mutex_;
779   std::vector<VendorSpecificCharacteristic> vendor_specific_characteristics_;
780 };
781 
782 }  // namespace
783 
GetRasServer()784 bluetooth::ras::RasServer* bluetooth::ras::GetRasServer() {
785   if (instance == nullptr) {
786     instance = new RasServerImpl();
787   }
788   return instance;
789 }
790