diff --git a/Kconfig b/Kconfig index 5b3ee339d..346abec11 100644 --- a/Kconfig +++ b/Kconfig @@ -233,6 +233,11 @@ choice bool "Not support BLE Stack" endchoice +config BLUETOOTH_LE_CS + bool "Bluetooth LE Channel Sounding supported" + depends on BLUETOOTH_STACK_LE_ZBLUE + default n + config BLUETOOTH_LE_SCANNER_MAX_NUM int "LE Scanner max register number" default 2 diff --git a/Makefile b/Makefile index dfc14bfed..65861d99e 100644 --- a/Makefile +++ b/Makefile @@ -22,7 +22,7 @@ CSRCS += framework/common/*.c CSRCS += framework/api/bluetooth.c CSRCS += framework/api/bt_adapter.c CSRCS += framework/api/bt_device.c - +CSRCS += framework/api/bt_cs.c ifeq ($(CONFIG_BLUETOOTH_A2DP_SINK), y) CSRCS += framework/api/bt_a2dp_sink.c endif #CONFIG_BLUETOOTH_A2DP_SINK @@ -72,6 +72,7 @@ ifeq ($(CONFIG_BLUETOOTH_BLE_AUDIO), y) CSRCS := framework/api/bt_lea*.c endif #CONFIG_BLUETOOTH_BLE_AUDIO + ifeq ($(CONFIG_BLUETOOTH_FRAMEWORK_SOCKET_IPC), y) CSRCS += service/ipc/bluetooth_ipc.c CSRCS += framework/socket/bt_device.c @@ -160,6 +161,11 @@ CSRCS += framework/socket/bt_trace.c CSRCS += service/ipc/socket/src/bt_socket_log.c endif #CONFIG_BLUETOOTH_BLE_AUDIO +ifeq ($(CONFIG_BLUETOOTH_LE_CS), y) +CSRCS += framework/socket/bt_cs.c +CSRCS += service/ipc/socket/src/bt_socket_cs.c +endif #CONFIG_BLUETOOTH_LE_CS + CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/frameworks/connectivity/bluetooth/service/ipc/socket/include ifeq ($(CONFIG_BLUETOOTH_FRAMEWORK_ASYNC), y) CSRCS += framework/socket/async/*.c @@ -265,6 +271,9 @@ endif #CONFIG_BLUETOOTH_GATT_CLIENT ifeq ($(CONFIG_BLUETOOTH_GATT_SERVER), y) CSRCS += service/stacks/zephyr/sal_gatt_server_interface.c endif #CONFIG_BLUETOOTH_GATT_SERVER +ifeq ($(CONFIG_BLUETOOTH_LE_CS), y) + CSRCS += service/stacks/zephyr/sal_le_cs_interface.c +endif #CONFIG_BLUETOOTH_LE_CS endif #CONFIG_BLUETOOTH_STACK_LE_ZBLUE endif @@ -383,6 +392,11 @@ CSRCS += service/utils/btsnoop_writer.c CSRCS += service/utils/btsnoop_filter.c endif #CONFIG_BLUETOOTH_LOG +ifeq ($(CONFIG_BLUETOOTH_LE_CS), y) + CSRCS += service/profiles/cs/*.c + CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/frameworks/connectivity/bluetooth/service/profiles/cs +endif #CONFIG_BLUETOOTH_CS + ifeq ($(CONFIG_BLUETOOTH_HCI_FILTER), y) CSRCS += service/vhal/bt_hci_filter.c endif @@ -508,6 +522,10 @@ ifeq ($(CONFIG_BLUETOOTH_STORAGE_UPDATE), y) CSRCS += tools/storage_update/storage_tool.c endif #CONFIG_BLUETOOTH_STORAGE_UPDATE +ifeq ($(CONFIG_BLUETOOTH_LE_CS), y) + CSRCS += tools/le_cs.c +endif + endif # framework/service/stack/tools dependence @@ -544,7 +562,13 @@ ifneq ($(CONFIG_BLUETOOTH_STACK_BREDR_ZBLUE)$(CONFIG_BLUETOOTH_STACK_LE_ZBLUE),) CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/external/zblue/zblue/port/include/ CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/external/zblue/zblue/subsys/bluetooth/host CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/external/zblue/zblue/subsys/settings/include/settings + CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/external/zblue/zblue/subsys/bluetooth CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/external/zblue/zblue/port/include/kernel/include + CFLAGS += ${INCDIR_PREFIX}${APPDIR}/frameworks/connectivity/bluetooth/framework/include + CFLAGS += ${INCDIR_PREFIX}${APPDIR}/frameworks/connectivity/bluetooth/service/ipc/socket/include + CFLAGS += ${INCDIR_PREFIX}${APPDIR}frameworks/connectivity/bluetooth/service/profiles/cs + CFLAGS += ${INCDIR_PREFIX}${APPDIR}frameworks/connectivity/bluetooth/service/profiles/include + CFLAGS += ${INCDIR_PREFIX}${APPDIR}frameworks/connectivity/bluetooth/service/stacks/include endif CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/frameworks/connectivity/bluetooth/service/ipc endif diff --git a/framework/api/bt_cs.c b/framework/api/bt_cs.c new file mode 100644 index 000000000..b7830190c --- /dev/null +++ b/framework/api/bt_cs.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * Copyright (C) 2024 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#define LOG_TAG "channel_sounding_api" + +#include + +#include "bt_cs.h" +#include "bt_internal.h" +#include "bt_profile.h" +#include "cs_service.h" +#include "service_manager.h" +#include "utils/log.h" + +#ifdef CONFIG_BLUETOOTH_LE_CS + +static bt_cs_interface_t* get_profile_service(void) +{ + return (bt_cs_interface_t*)service_manager_get_profile(PROFILE_CS); +} +void* BTSYMBOLS(bt_cs_register_callbacks)(bt_instance_t* ins, const cs_callbacks_t* callbacks) +{ + bt_cs_interface_t* profile = get_profile_service(); + + return profile->register_callbacks(NULL, callbacks); +} + +bool BTSYMBOLS(bt_cs_unregister_callbacks)(bt_instance_t* ins, void* cookie) +{ + bt_cs_interface_t* profile = get_profile_service(); + + return profile->unregister_callbacks(NULL, cookie); +} + +bt_status_t BTSYMBOLS(bt_cs_start_distance_measurement)(bt_instance_t* ins, bt_distance_measurement_params_t* params) +{ + bt_cs_interface_t* profile = get_profile_service(); + + return profile->start_distance_measurement(params); +} + +bt_status_t BTSYMBOLS(bt_cs_stop_distance_measurement)(bt_instance_t* ins, bt_address_t* addr, uint8_t method, bool timeout) +{ + bt_cs_interface_t* profile = get_profile_service(); + + return profile->stop_distance_measurement(addr, method, timeout); +} + +bt_status_t BTSYMBOLS(bt_cs_test)(bt_instance_t* ins, void* data, uint16_t len) +{ + bt_cs_interface_t* profile = get_profile_service(); + + return profile->cs_test(data, len); +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/framework/include/bluetooth.h b/framework/include/bluetooth.h index 9ce8e5be2..6373bd077 100644 --- a/framework/include/bluetooth.h +++ b/framework/include/bluetooth.h @@ -459,11 +459,13 @@ typedef struct bt_instance { callbacks_list_t* a2dp_source_callbacks; callbacks_list_t* avrcp_target_callbacks; callbacks_list_t* avrcp_control_callbacks; + callbacks_list_t* cs_callbacks; void* adapter_cookie; void* a2dp_sink_cookie; void* a2dp_source_cookie; void* avrcp_target_cookie; void* avrcp_control_cookie; + void* cs_cookie; callbacks_list_t* hfp_ag_callbacks; callbacks_list_t* hfp_hf_callbacks; diff --git a/framework/include/bt_addr.h b/framework/include/bt_addr.h index 97fd0c68d..64a4b6495 100644 --- a/framework/include/bt_addr.h +++ b/framework/include/bt_addr.h @@ -26,8 +26,9 @@ extern "C" { #define BT_ADDR_LENGTH 6 /*define the address length*/ #define BT_ADDR_STR_LENGTH 18 +#ifndef bt_addr_str #define bt_addr_str(addr) bt_addr_bastr(addr) - +#endif /** * @cond */ diff --git a/framework/include/bt_cs.h b/framework/include/bt_cs.h new file mode 100644 index 000000000..eddce5d58 --- /dev/null +++ b/framework/include/bt_cs.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#ifndef __BT_CS_H__ +#define __BT_CS_H__ + +#include "bt_addr.h" +#include "bt_device.h" +#include "bt_internal.h" +#include + +#ifndef BTSYMBOLS +#define BTSYMBOLS(s) s +#endif + +typedef void (*cs_distance_measure_started_cb)(void* cookie, bt_address_t* addr, uint8_t method); +typedef void (*cs_distance_measure_stopped_cb)(void* cookie, bt_address_t* addr, uint8_t reason, uint8_t method); +typedef void (*cs_distance_measure_result_cb)(void* cookie, bt_address_t* addr, uint8_t centimeter, uint8_t errorCentimeter, + uint8_t azimuthAngle, uint8_t errorAzimuthAngle, uint8_t altitudeAngle, uint8_t errorAltitudeAngle, + long elapsedRealtimeNanos, uint8_t confidenceLevel, double delaySpreadMeters, + uint8_t detectedAttackLevel, double velocityMetersPerSecond, uint8_t method); + +/** + * @cond + */ +typedef struct { + bt_address_t addr; + uint8_t method; + uint8_t role; + uint16_t interval_ms; + uint16_t duration_ms; + uint8_t submode; + uint8_t max_steps; + uint8_t mode0_steps; + uint8_t rtt_type; + uint8_t sync_phy; + uint8_t channel_map; + uint8_t antenna_paths_mask; + uint8_t vendor_specific; + uint8_t debug_flags; +} bt_distance_measurement_params_t; + +typedef struct { + size_t size; + cs_distance_measure_started_cb cs_distance_measure_started_cb; + cs_distance_measure_stopped_cb cs_distance_measure_stopped_cb; + cs_distance_measure_result_cb cs_distance_measure_result_cb; +} cs_callbacks_t; + +typedef enum { + METHOD_AUTO, + METHOD_RSSI, + METHOD_CS, +} cs_method_t; + +/** + * @endcond + */ +void* BTSYMBOLS(bt_cs_register_callbacks)(bt_instance_t* ins, const cs_callbacks_t* callbacks); +bool BTSYMBOLS(bt_cs_unregister_callbacks)(bt_instance_t* ins, void* cookie); +bt_status_t BTSYMBOLS(bt_cs_start_distance_measurement)(bt_instance_t* ins, bt_distance_measurement_params_t* params); +bt_status_t BTSYMBOLS(bt_cs_stop_distance_measurement)(bt_instance_t* ins, bt_address_t* addr, uint8_t method, bool timeout); +bt_status_t BTSYMBOLS(bt_get_cs_max_supported_security_level)(bt_instance_t* ins, bt_address_t* addr); +bt_status_t BTSYMBOLS(bt_cs_test)(bt_instance_t* ins, void* data, uint16_t len); + +#endif /* __BT_CS_H__ */ diff --git a/framework/include/bt_gatts.h b/framework/include/bt_gatts.h index 9668a3c6e..3073c4329 100644 --- a/framework/include/bt_gatts.h +++ b/framework/include/bt_gatts.h @@ -344,7 +344,7 @@ typedef struct { gatts_phy_read_cb_t on_phy_read; gatts_phy_updated_cb_t on_phy_updated; gatts_connection_parameter_changed_cb_t on_conn_param_changed; - + } gatts_callbacks_t; /** diff --git a/framework/include/bt_profile.h b/framework/include/bt_profile.h index 6dde97314..7d7374732 100644 --- a/framework/include/bt_profile.h +++ b/framework/include/bt_profile.h @@ -24,6 +24,7 @@ extern "C" { #define PROFILE_A2DP_SINK_NAME "A2DP-Sink" #define PROFILE_AVRCP_CT_NAME "AVRCP-CT" #define PROFILE_AVRCP_TG_NAME "AVRCP-TG" +#define PROFILE_CS_NAME "CS" #define PROFILE_HFP_HF_NAME "HFP-HF" #define PROFILE_HFP_AG_NAME "HFP-AG" #define PROFILE_SPP_NAME "SPP" @@ -60,6 +61,7 @@ enum profile_id { PROFILE_LEAUDIO_MCS, PROFILE_LEAUDIO_TBS, PROFILE_LEAUDIO_VMICP, + PROFILE_CS, PROFILE_UNKOWN, PROFILE_MAX }; diff --git a/framework/socket/bt_cs.c b/framework/socket/bt_cs.c new file mode 100644 index 000000000..a9b079411 --- /dev/null +++ b/framework/socket/bt_cs.c @@ -0,0 +1,135 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#define LOG_TAG "channel_sounding_api" +#include "bt_cs.h" +#include "bt_message.h" +#include "bt_socket.h" +#include "utils/log.h" +#include + +#ifdef CONFIG_BLUETOOTH_LE_CS + +void* bt_cs_register_callbacks(bt_instance_t* ins, const cs_callbacks_t* callbacks) +{ + bt_message_packet_t packet; + bt_status_t status; + void* cookie; + + BT_SOCKET_INS_VALID(ins, NULL); + + if (ins->cs_callbacks != NULL) { + cookie = bt_remote_callbacks_register(ins->cs_callbacks, NULL, (void*)callbacks); + return cookie; + } + + ins->cs_callbacks = bt_callbacks_list_new(CONFIG_BLUETOOTH_MAX_REGISTER_NUM); + + cookie = bt_remote_callbacks_register(ins->cs_callbacks, NULL, (void*)callbacks); + if (cookie == NULL) { + bt_callbacks_list_free(ins->cs_callbacks); + ins->cs_callbacks = NULL; + return cookie; + } + + status = bt_socket_client_sendrecv(ins, &packet, BT_CS_REGISTER_CALLBACKS); + if (status != BT_STATUS_SUCCESS || packet.cs_r.status != BT_STATUS_SUCCESS) { + bt_callbacks_list_free(ins->cs_callbacks); + ins->cs_callbacks = NULL; + return NULL; + } + + return cookie; +} + +bool bt_cs_unregister_callbacks(bt_instance_t* ins, void* cookie) +{ + bt_message_packet_t packet; + bt_status_t status; + + BT_SOCKET_INS_VALID(ins, false); + + if (!ins->cs_callbacks) + return false; + + bt_remote_callbacks_unregister(ins->cs_callbacks, NULL, cookie); + if (bt_callbacks_list_count(ins->cs_callbacks) > 0) { + return true; + } + + bt_callbacks_list_free(ins->cs_callbacks); + ins->cs_callbacks = NULL; + + status = bt_socket_client_sendrecv(ins, &packet, BT_CS_UNREGISTER_CALLBACKS); + if (status != BT_STATUS_SUCCESS || packet.cs_r.status != BT_STATUS_SUCCESS) { + return false; + } + + return true; +} + +bt_status_t bt_cs_start_distance_measurement(bt_instance_t* ins, bt_distance_measurement_params_t* params) +{ + bt_message_packet_t packet; + bt_status_t status; + + BT_SOCKET_INS_VALID(ins, BT_STATUS_PARM_INVALID); + memcpy(&packet.cs_pl._bt_cs_start_distance_measurement.params, params, sizeof(bt_distance_measurement_params_t)); + BT_LOGI("BT_CS_START_DISTANCE_MEASUREMENT:0x%lx", BT_CS_START_DISTANCE_MEASUREMENT); + status = bt_socket_client_sendrecv(ins, &packet, BT_CS_START_DISTANCE_MEASUREMENT); + if (status != BT_STATUS_SUCCESS) { + return status; + } + return packet.cs_r.status; +} + +bt_status_t bt_cs_stop_distance_measurement(bt_instance_t* ins, bt_address_t* addr, uint8_t method, bool timeout) +{ + bt_message_packet_t packet; + bt_status_t status; + + BT_SOCKET_INS_VALID(ins, BT_STATUS_PARM_INVALID); + memcpy(&packet.cs_pl._bt_cs_stop_distance_measurement.addr, addr, sizeof(packet.cs_pl._bt_cs_stop_distance_measurement.addr)); + packet.cs_pl._bt_cs_stop_distance_measurement.method = method; + packet.cs_pl._bt_cs_stop_distance_measurement.timeout_bool = timeout; + status = bt_socket_client_sendrecv(ins, &packet, BT_CS_STOP_DISTANCE_MEASUREMENT); + if (status != BT_STATUS_SUCCESS) { + return status; + } + return packet.cs_r.status; +} + +bt_status_t bt_cs_test(bt_instance_t* ins, void* data, uint16_t len) +{ + bt_message_packet_t packet; + bt_status_t status; + + BT_SOCKET_INS_VALID(ins, BT_STATUS_PARM_INVALID); + if (data) { + memcpy(&packet.cs_pl._bt_cs_test.data, data, len); + } else { + packet.cs_pl._bt_cs_test.data = NULL; + } + + packet.cs_pl._bt_cs_test.len = len; + + status = bt_socket_client_sendrecv(ins, &packet, BT_CS_TEST); + if (status != BT_STATUS_SUCCESS) { + return status; + } + return packet.cs_r.status; +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/framework/socket/bt_device.c b/framework/socket/bt_device.c index d461d05ea..1aefd47da 100644 --- a/framework/socket/bt_device.c +++ b/framework/socket/bt_device.c @@ -575,7 +575,7 @@ bt_status_t bt_device_set_pass_key(bt_instance_t* ins, bt_address_t* addr, uint8 return packet.devs_r.status; } -bt_status_t BTSYMBOLS(bt_device_set_le_legacy_tk)(bt_instance_t* ins, bt_address_t* addr, bt_128key_t tk_val) +bt_status_t bt_device_set_le_legacy_tk(bt_instance_t* ins, bt_address_t* addr, bt_128key_t tk_val) { bt_message_packet_t packet; bt_status_t status; diff --git a/service/ipc/socket/include/bt_ipc_code.h b/service/ipc/socket/include/bt_ipc_code.h index a2b41c178..6b5cc6cbb 100644 --- a/service/ipc/socket/include/bt_ipc_code.h +++ b/service/ipc/socket/include/bt_ipc_code.h @@ -43,6 +43,7 @@ extern "C" { #define BT_IPC_CODE_GROUP_AVRCP_TG (BT_IPC_CODE_GROUP_PROFILES + PROFILE_AVRCP_TG) #define BT_IPC_CODE_GROUP_HFP_HF (BT_IPC_CODE_GROUP_PROFILES + PROFILE_HFP_HF) #define BT_IPC_CODE_GROUP_HFP_AG (BT_IPC_CODE_GROUP_PROFILES + PROFILE_HFP_AG) +#define BT_IPC_CODE_GROUP_CS (BT_IPC_CODE_GROUP_PROFILES + PROFILE_CS) #define BT_IPC_CODE_GROUP_SPP (BT_IPC_CODE_GROUP_PROFILES + PROFILE_SPP) #define BT_IPC_CODE_GROUP_HID_DEV (BT_IPC_CODE_GROUP_PROFILES + PROFILE_HID_DEV) #define BT_IPC_CODE_GROUP_PANU (BT_IPC_CODE_GROUP_PROFILES + PROFILE_PANU) diff --git a/service/ipc/socket/include/bt_message.h b/service/ipc/socket/include/bt_message.h index b1fc8e1eb..71d5a68e5 100644 --- a/service/ipc/socket/include/bt_message.h +++ b/service/ipc/socket/include/bt_message.h @@ -43,6 +43,7 @@ extern "C" { #include "bt_message_pan.h" #include "bt_message_scan.h" #include "bt_message_spp.h" +#include "bt_message_cs.h" #include "service_loop.h" @@ -67,6 +68,7 @@ typedef enum { #include "bt_message_pan.h" #include "bt_message_scan.h" #include "bt_message_spp.h" +#include "bt_message_cs.h" BT_MESSAGE_END, #undef __BT_MESSAGE_CODE__ #define __BT_CALLBACK_CODE__ @@ -88,6 +90,7 @@ typedef enum { #include "bt_message_pan.h" #include "bt_message_scan.h" #include "bt_message_spp.h" +#include "bt_message_cs.h" BT_CALLBACK_END, #undef __BT_MESSAGE_CODE__ } bt_message_type_t; @@ -105,6 +108,7 @@ typedef struct bt_a2dp_source_result_t a2dp_source_r; bt_avrcp_target_result_t avrcp_target_r; bt_avrcp_control_result_t avrcp_control_r; + bt_cs_result_t cs_r; bt_hfp_ag_result_t hfp_ag_r; bt_hfp_hf_result_t hfp_hf_r; bt_advertiser_result_t adv_r; @@ -131,6 +135,7 @@ typedef struct bt_message_a2dp_source_callbacks_t a2dp_source_cb; bt_message_avrcp_target_t avrcp_target_pl; + bt_message_cs_t cs_pl; bt_message_avrcp_target_callbacks_t avrcp_target_cb; bt_message_avrcp_control_t avrcp_control_pl; bt_message_avrcp_control_callbacks_t avrcp_control_cb; diff --git a/service/ipc/socket/include/bt_message_cs.h b/service/ipc/socket/include/bt_message_cs.h new file mode 100644 index 000000000..4fc47e40e --- /dev/null +++ b/service/ipc/socket/include/bt_message_cs.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * Copyright (C) 2022 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#ifndef _BT_MESSAGE_CS_H__ +#define _BT_MESSAGE_CS_H__ + +#include "bt_cs.h" +#include "bt_ipc_code.h" + +#define BT_IPC_CODE_COMMAND_CS_BEGIN BT_IPC_CODE(BT_IPC_CODE_TYPE_COMMAND, BT_IPC_CODE_GROUP_CS, 0) +// TODO: Add new BT IPC Code sequentially +#define CS_SUBCODE_REGISTER_CALLBACKS 1 +#define BT_CS_REGISTER_CALLBACKS BT_IPC_CODE(BT_IPC_CODE_TYPE_COMMAND, BT_IPC_CODE_GROUP_CS, CS_SUBCODE_REGISTER_CALLBACKS) +#define CS_SUBCODE_UNREGISTER_CALLBACKS 2 +#define BT_CS_UNREGISTER_CALLBACKS BT_IPC_CODE(BT_IPC_CODE_TYPE_COMMAND, BT_IPC_CODE_GROUP_CS, CS_SUBCODE_UNREGISTER_CALLBACKS) +#define CS_SUBCODE_START_DISTANCE_MEASUREMENT 3 +#define BT_CS_START_DISTANCE_MEASUREMENT BT_IPC_CODE(BT_IPC_CODE_TYPE_COMMAND, BT_IPC_CODE_GROUP_CS, CS_SUBCODE_START_DISTANCE_MEASUREMENT) + +#define CS_SUBCODE_STOP_DISTANCE_MEASUREMENT 5 +#define BT_CS_STOP_DISTANCE_MEASUREMENT BT_IPC_CODE(BT_IPC_CODE_TYPE_COMMAND, BT_IPC_CODE_GROUP_CS, CS_SUBCODE_STOP_DISTANCE_MEASUREMENT) + +#define CS_SUBCODE_TEST 6 +#define BT_CS_TEST BT_IPC_CODE(BT_IPC_CODE_TYPE_COMMAND, BT_IPC_CODE_GROUP_CS, CS_SUBCODE_TEST) + +#define BT_IPC_CODE_COMMAND_CS_END BT_IPC_CODE(BT_IPC_CODE_TYPE_COMMAND, BT_IPC_CODE_GROUP_CS, BT_IPC_CODE_SUBCODE_MAX_NUM) + +#define BT_IPC_CODE_CALLBACK_CS_BEGIN BT_IPC_CODE(BT_IPC_CODE_TYPE_CALLBACK, BT_IPC_CODE_GROUP_CS, 0) +// TODO: Add new BT IPC Code sequentially + +#define BT_IPC_CODE_CALLBACK_CS_END BT_IPC_CODE(BT_IPC_CODE_TYPE_CALLBACK, BT_IPC_CODE_GROUP_CS, BT_IPC_CODE_SUBCODE_MAX_NUM) + +typedef union { + uint8_t status; /* bt_status_t */ + // uint8_t profile_conn_state; /* profile_connection_state_t */ + // uint8_t value_bool; /* boolean */ +} bt_cs_result_t; + +typedef union { + struct { + bt_distance_measurement_params_t params; + } _bt_cs_start_distance_measurement; + + struct { + bt_address_t addr; + uint8_t method; + uint8_t timeout_bool; /* boolean */ + } _bt_cs_stop_distance_measurement; + + struct { + uint16_t len; + void* data; + } _bt_cs_test; +} bt_message_cs_t; + +typedef union { + struct { + bt_address_t addr; + uint8_t state; /* profile_connection_state_t */ + } _on_connection_state_changed; +} bt_message_cs_callbacks_t; + +#endif /* _BT_MESSAGE_CS_H__ */ diff --git a/service/ipc/socket/include/bt_socket.h b/service/ipc/socket/include/bt_socket.h index 1fdb7639f..591e4224b 100644 --- a/service/ipc/socket/include/bt_socket.h +++ b/service/ipc/socket/include/bt_socket.h @@ -64,6 +64,7 @@ typedef struct { callbacks_list_t* spp_callbacks; callbacks_list_t* hidd_callbacks; callbacks_list_t* l2cap_callbacks; + callbacks_list_t* cs_callbacks; bt_list_t* gattc_remote_list; bt_list_t* gatts_remote_list; @@ -237,6 +238,10 @@ int bt_socket_client_l2cap_callback(service_poll_t* poll, void bt_socket_server_log_process(service_poll_t* poll, int fd, bt_instance_t* ins, bt_message_packet_t* packet); + +/* Channel Sounding */ +void bt_socket_server_cs_process(service_poll_t* poll, + int fd, bt_instance_t* ins, bt_message_packet_t* packet); #ifdef __cplusplus } #endif diff --git a/service/ipc/socket/src/bt_socket_cs.c b/service/ipc/socket/src/bt_socket_cs.c new file mode 100644 index 000000000..40b71e627 --- /dev/null +++ b/service/ipc/socket/src/bt_socket_cs.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * Copyright (C) 2024 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "bluetooth.h" +#include "bt_cs.h" +#include "bt_message.h" +#include "bt_socket.h" +#include "cs_service.h" +#include "service_manager.h" + +#ifdef CONFIG_BLUETOOTH_LE_CS +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define CALLBACK_FOREACH(_list, _struct, _cback, ...) \ + BT_CALLBACK_FOREACH(_list, _struct, _cback, ##__VA_ARGS__) +#define CBLIST (__async ? __async->cs_callbacks : ins->cs_callbacks) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +#if defined(CONFIG_BLUETOOTH_SERVER) && defined(__NuttX__) + +static void on_distance_measure_started_cb(void* cookie, bt_address_t* addr, uint8_t method) +{ +} + +static void on_distance_measure_stopped_cb(void* cookie, bt_address_t* addr, uint8_t reason, uint8_t method) +{ +} +static void on_distance_measure_result_cb(void* cookie, bt_address_t* addr, uint8_t centimeter, uint8_t errorCentimeter, + uint8_t azimuthAngle, uint8_t errorAzimuthAngle, uint8_t altitudeAngle, uint8_t errorAltitudeAngle, + long elapsedRealtimeNanos, uint8_t confidenceLevel, double delaySpreadMeters, + uint8_t detectedAttackLevel, double velocityMetersPerSecond, uint8_t method) +{ +} + +const static cs_callbacks_t g_cs_cbs = { + .size = sizeof(cs_callbacks_t), + .cs_distance_measure_started_cb = on_distance_measure_started_cb, + .cs_distance_measure_stopped_cb = on_distance_measure_stopped_cb, + .cs_distance_measure_result_cb = on_distance_measure_result_cb, +}; +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void bt_socket_server_cs_process(service_poll_t* poll, + int fd, bt_instance_t* ins, bt_message_packet_t* packet) +{ + bt_cs_interface_t* profile; + + switch (BT_IPC_GET_SUBCODE(packet->code)) { + case CS_SUBCODE_REGISTER_CALLBACKS: + if (ins->cs_cookie == NULL) { + profile = (bt_cs_interface_t*)service_manager_get_profile(PROFILE_CS); + if (profile) { + ins->cs_cookie = profile->register_callbacks(ins, &g_cs_cbs); + if (ins->cs_cookie) { + packet->cs_r.status = BT_STATUS_SUCCESS; + } else { + packet->cs_r.status = BT_STATUS_NO_RESOURCES; + } + } else { + packet->cs_r.status = BT_STATUS_SERVICE_NOT_FOUND; + } + } else { + packet->cs_r.status = BT_STATUS_BUSY; + } + break; + case CS_SUBCODE_UNREGISTER_CALLBACKS: + if (ins->cs_cookie) { + profile = (bt_cs_interface_t*)service_manager_get_profile(PROFILE_CS); + if (profile) + profile->unregister_callbacks((void**)&ins, ins->cs_cookie); + ins->cs_cookie = NULL; + packet->cs_r.status = BT_STATUS_SUCCESS; + } else { + packet->cs_r.status = BT_STATUS_NOT_FOUND; + } + break; + case CS_SUBCODE_START_DISTANCE_MEASUREMENT: + packet->cs_r.status = BTSYMBOLS(bt_cs_start_distance_measurement)(ins, + &packet->cs_pl._bt_cs_start_distance_measurement.params); + break; + case CS_SUBCODE_STOP_DISTANCE_MEASUREMENT: + packet->cs_r.status = BTSYMBOLS(bt_cs_stop_distance_measurement)(ins, + &packet->cs_pl._bt_cs_stop_distance_measurement.addr, + packet->cs_pl._bt_cs_stop_distance_measurement.method, + packet->cs_pl._bt_cs_stop_distance_measurement.timeout_bool); + break; + case CS_SUBCODE_TEST: + packet->cs_r.status = BTSYMBOLS(bt_cs_test)(ins, + &packet->cs_pl._bt_cs_test.data, + packet->cs_pl._bt_cs_test.len); + break; + default: + break; + } +} + +#endif + +int bt_socket_client_cs_callback(service_poll_t* poll, + int fd, bt_instance_t* ins, bt_message_packet_t* packet, bool is_async) +{ + bt_socket_async_client_t* __async = NULL; + + if (is_async) + __async = ins->priv; + + switch (packet->code) { + case BT_AVRCP_TARGET_ON_CONNECTION_STATE_CHANGED: + CALLBACK_FOREACH(CBLIST, avrcp_target_callbacks_t, + connection_state_cb, + &packet->avrcp_target_cb._on_connection_state_changed.addr, + packet->avrcp_target_cb._on_connection_state_changed.state); + break; + default: + return BT_STATUS_PARM_INVALID; + } + + return BT_STATUS_SUCCESS; +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/service/ipc/socket/src/bt_socket_server.c b/service/ipc/socket/src/bt_socket_server.c index efb0aa934..3a8fea6ea 100644 --- a/service/ipc/socket/src/bt_socket_server.c +++ b/service/ipc/socket/src/bt_socket_server.c @@ -252,6 +252,10 @@ static int bt_socket_server_receive(service_poll_t* poll, int fd, void* userdata || BT_IPC_CODE_CHECK_RANGE(packet->code, BT_IPC_CODE_COMMAND_LOG_BEGIN, BT_IPC_CODE_COMMAND_LOG_END)) { bt_socket_server_log_process(poll, fd, ins, packet); #endif +#ifdef CONFIG_BLUETOOTH_LE_CS + } else if (BT_IPC_CODE_CHECK_RANGE(packet->code, BT_IPC_CODE_COMMAND_CS_BEGIN, BT_IPC_CODE_COMMAND_CS_END)) { + bt_socket_server_cs_process(poll, fd, ins, packet); +#endif /* CONFIG_BLUETOOTH_LE_CS */ } else { BT_LOGE("%s, Unhandled message:%" PRIu32, __func__, packet->code); assert(0); diff --git a/service/profiles/cs/cs_msg.c b/service/profiles/cs/cs_msg.c new file mode 100644 index 000000000..2bec35fe7 --- /dev/null +++ b/service/profiles/cs/cs_msg.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include +#include + +#include "cs_msg.h" + +#ifdef CONFIG_BLUETOOTH_LE_CS + +cs_msg_t* cs_msg_new(cs_msg_id_t msg, bt_address_t* bd_addr) +{ + cs_msg_t* cs_msg; + + cs_msg = (cs_msg_t*)malloc(sizeof(cs_msg_t)); + if (cs_msg == NULL) + return NULL; + + cs_msg->id = msg; + if (bd_addr != NULL) + memcpy(&cs_msg->cs_data.bd_addr, bd_addr, sizeof(bt_address_t)); + + return cs_msg; +} + +void cs_msg_destory(cs_msg_t* cs_msg) +{ + free(cs_msg); +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ \ No newline at end of file diff --git a/service/profiles/cs/cs_msg.h b/service/profiles/cs/cs_msg.h new file mode 100644 index 000000000..7ad9ea856 --- /dev/null +++ b/service/profiles/cs/cs_msg.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef __CS_MSG_H__ +#define __CS_MSG_H__ +/**************************************************************************** + * Included Files + ****************************************************************************/ +#include "bt_cs.h" + +typedef enum { + CS_STARTUP, + CS_SHUTDOWN, + START_REQ, + STOP_REQ, + CAPBLITIES_RECEIVED_EVT, + DISCONNECTED_EVT, + CONNECTED_EVT, + CONFIG_DONE_EVT, + SECURITY_DONE_EVT, + PROCEDURE_DONE_EVT, + CS_SUBEVENT_RESULT_EVT, +} cs_msg_id_t; + +typedef struct +{ + bt_address_t bd_addr; + void* data; + void* cb; +} cs_msg_data_t; + +typedef struct +{ + cs_msg_id_t id; + cs_msg_data_t cs_data; +} cs_msg_t; + +cs_msg_t* cs_msg_new(cs_msg_id_t msg, bt_address_t* bd_addr); +void cs_msg_destory(cs_msg_t* cs_msg); + +#endif diff --git a/service/profiles/cs/cs_ras.c b/service/profiles/cs/cs_ras.c new file mode 100644 index 000000000..0735aa8b0 --- /dev/null +++ b/service/profiles/cs/cs_ras.c @@ -0,0 +1,1482 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include +#include "cs_ras.h" +#include "cs_ras_gatts.h" +#include "cs_ras_util.h" +#include "bt_status.h" +#include "utils/log.h" +#include "cs_ras_test.h" + +#ifdef CONFIG_BLUETOOTH_LE_CS + +#ifndef MIN +#define MIN(x, y) (((x) < (y)) ? (x) : (y)) +#endif + +#define CS_CONFIG_ID 0 +#define NUM_MODE_0_STEPS 1 +#define RAS_SEG_HEADER_SIZE 4 + +static ras_srv_env_t* ras_srv; + +static void split_real_time_segment(bt_address_t* addr, uint8_t* buf, int len); +static ssize_t ras_feature_read(bt_address_t* addr, void* buf, uint16_t len, uint16_t offset); +static ras_rang_on_demand_t* ras_rang_on_demand_find_subevent(bt_address_t* addr, uint16_t count); +static bt_status_t ras_data_ready_send(bt_address_t* addr, uint16_t count); +static bt_status_t ras_on_demond_send_cmp_ranging_data_rsp(bt_address_t* addr, uint16_t count); + +static void ras_on_demand_notify_finished(bt_address_t* addr) +{ + cs_node_t* on_deman_pdu = (cs_node_t*)ras_srv->on_deman_curr_node; + + if (!on_deman_pdu) { + BT_LOGD("Compelete segment data sent."); + ras_on_demond_send_cmp_ranging_data_rsp(addr, 10); + return; + } + + ras_segment_t* seg = CS_CONTAINER_OF(on_deman_pdu, ras_segment_t, seg_node); + + if (!seg) { + BT_LOGW("Invalid segment."); + return; + } + + BT_LOGD("seg:%p, seg->data(%d):%s", seg, seg->len, cs_log_to_hex_str(seg->data, seg->len)); + + ras_srv->on_deman_curr_node = on_deman_pdu->next; + + bt_status_t status = BT_GATT_NOTIFY_CB(RAS_ON_DEMAND_CHAR_SEND, addr, seg->data, seg->len); + if (status != 0) { + BT_LOGE("On-demand ranging data notify fail, seg_idx(%d), err(%d).", + seg->seg_idx, status); + return; + } + + return; +} + +static void ras_on_demand_indicate_finished(bt_address_t* addr) +{ + cs_node_t* on_deman_pdu = (cs_node_t*)ras_srv->on_deman_curr_node; + + if (!on_deman_pdu) { + BT_LOGD("Compelete segment data sent."); + ras_on_demond_send_cmp_ranging_data_rsp(addr, 10); + return; + } + + ras_segment_t* seg = CS_CONTAINER_OF(on_deman_pdu, ras_segment_t, seg_node); + + if (!seg) { + BT_LOGW("Invalid segment."); + return; + } + + BT_LOGD("seg:%p, seg->data(%d):%s", seg, seg->len, cs_log_to_hex_str(seg->data, seg->len)); + + ras_srv->on_deman_curr_node = on_deman_pdu->next; + + bt_status_t status = BT_GATT_INDICATE(RAS_ON_DEMAND_CHAR_SEND, addr, seg->data, seg->len); + if (status != 0) { + BT_LOGE("On-demand ranging data indication fail, seg_idx(%d), err(%d).", + seg->seg_idx, status); + return; + } + + return; +} + +static bt_status_t ras_ondemand_send_ranging_data(bt_address_t* addr, uint16_t count) +{ + if (!ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY) && + !ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE)) { + BT_LOGE("RAS haven't enable the on-demond ranging data notify or indication state."); + return BT_STATUS_FAIL; + } + + bt_status_t status = BT_STATUS_SUCCESS; + + ras_rang_on_demand_t* on_demand_sub = ras_rang_on_demand_find_subevent(addr, count); + + if (!on_demand_sub) { + BT_LOGE("Haven't find subevent with count(%d).", count); + return BT_STATUS_PARM_INVALID; + } + + const cs_node_t* on_deman_pdu = cs_list_peek_head(&on_demand_sub->seg_list); + struct ras_segment_t* seg = CS_CONTAINER_OF(on_deman_pdu, ras_segment_t, seg_node); + + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY)) { + BT_LOGD("seg:%p, seg->data(%d):%s", seg, seg->len, cs_log_to_hex_str(seg->data, seg->len)); + + status = BT_GATT_NOTIFY_CB(RAS_ON_DEMAND_CHAR_SEND, addr, seg->data, seg->len); + + if (status != BT_STATUS_SUCCESS) { + BT_LOGE("On-demand ranging data notify fail, status(%d).", status); + return status; + } + + } else if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE)) { + BT_LOGD("seg:%p, seg->data(%d):%s", seg, seg->len, cs_log_to_hex_str(seg->data, seg->len)); + // Set the current node to the next. + ras_srv->on_deman_curr_node = on_deman_pdu->next; + status = BT_GATT_INDICATE(RAS_ON_DEMAND_CHAR_SEND, addr, seg->data, seg->len); + + if (status != 0) { + BT_LOGE("On-demand ranging data indicate fail, err(%d).", status); + return status; + } + } + + return status; +} + +static bt_status_t ras_on_demond_send_cmp_ranging_data_rsp(bt_address_t* addr, uint16_t count) +{ + ras_rang_on_demand_t* on_demand_data = ras_rang_on_demand_find_subevent(addr, count); + + if (!on_demand_data) { + BT_LOGW("Haven't find the demand data with the count(%d).", count); + } + + uint8_t buf[3] = { 0 }; + bt_status_t status; + buf[0] = SAL_LE_RAS_CTL_OP_RSP_CMP_RANG_DATA; + ras_put_uint16_to_ptr(count, &buf[1]); + + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + status = BT_GATT_NOTIFY_CB(RAS_CONTROL_POINT_CHAR_SEND, addr, buf, sizeof(buf)); + if (status != 0) { + BT_LOGE("ranging data Control Point response ACK notify fail, err(%d).", status); + return status; + } + } else if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + status = BT_GATT_INDICATE(RAS_CONTROL_POINT_CHAR_SEND, addr, buf, sizeof(buf)); + if (status != 0) { + BT_LOGE("ranging data Control Point response ACK indicate fail, err(%d).", status); + return status; + } + } else { + BT_LOGE("The Ranging Control Point haven't set to notify or indication state."); + return BT_STATUS_FAIL; + } + + return BT_STATUS_SUCCESS; +} + +static bt_status_t ras_on_demond_send_code_rsp(bt_address_t* addr, uint16_t count) +{ + ras_rang_on_demand_t* on_demand_data = ras_rang_on_demand_find_subevent(addr, count); + + if (!on_demand_data) { + BT_LOGW("Haven't find the demand data with the count(%d).", count); + } + + uint8_t buf[2] = { 0 }; + bt_status_t status; + buf[0] = SAL_LE_RAS_CTL_OP_RSP_CODE; + buf[1] = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; + + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + status = BT_GATT_NOTIFY_CB(RAS_CONTROL_POINT_CHAR_SEND, addr, buf, sizeof(buf)); + if (status != 0) { + BT_LOGE("ranging data Control Point response ACK notify fail, err(%d).", status); + return status; + } + } else if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + status = BT_GATT_INDICATE(RAS_CONTROL_POINT_CHAR_SEND, addr, buf, sizeof(buf)); + if (status != 0) { + BT_LOGE("ranging data Control Point response ACK indicate fail, err(%d).", status); + return status; + } + } + + // Free the on-demand segment list when response the ack to the Client. + ras_segment_t *seg_prev, *seg_next; + CS_LIST_FOR_EACH_CONTAINER_SAFE(&on_demand_data->seg_list, + seg_prev, seg_next, seg_node) + { + cs_list_remove(&on_demand_data->seg_list, NULL, &seg_prev->seg_node); + BT_LOGD("seg_prev:%p", seg_prev); + free(seg_prev); + } + + BT_LOGD("The on-demand data has been sent, Cancel on-demand timer."); + /* The on-demand data has been sent, Cancel on-demand timer */ + service_loop_cancel_timer(on_demand_data->on_demand_timer); + memset(on_demand_data, 0, sizeof(ras_rang_on_demand_t)); + return BT_STATUS_SUCCESS; +} + +static bt_status_t ras_on_demond_send_lost_ranging_data_cmp_rsp(bt_address_t* addr, uint16_t count, uint8_t first, uint8_t last) +{ + uint8_t buf[5] = { 0 }; + + buf[0] = 0x01; + ras_put_uint16_to_ptr(count, &buf[1]); + buf[3] = first; + buf[4] = last; + + bt_status_t status; + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + buf[0] = SAL_LE_RAS_CTL_OP_RSP_CODE; + buf[1] = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; + + status = BT_GATT_NOTIFY_CB(RAS_CONTROL_POINT_CHAR_SEND, addr, buf, sizeof(buf)); + if (status != 0) { + BT_LOGE("ranging data Control Point response ACK notify fail, err(%d).", status); + return status; + } + } else if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_INDICATE)) { + buf[0] = SAL_LE_RAS_CTL_OP_RSP_CODE; + buf[1] = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; + + status = BT_GATT_INDICATE(RAS_CONTROL_POINT_CHAR_SEND, addr, buf, sizeof(buf)); + if (status != 0) { + BT_LOGE("ranging data Control Point response ACK indicate fail, err(%d).", status); + return status; + } + } + + return BT_STATUS_SUCCESS; +} + +static bt_status_t ras_on_demand_retrieve_send_lost_data(bt_address_t* addr, uint16_t count, + uint8_t first_seg, uint8_t last_seg) +{ + ras_rang_on_demand_t* on_demand_data = ras_rang_on_demand_find_subevent(addr, count); + + if (!ras_state_get_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_RSP)) { + BT_LOGW("Invalid on_demand state."); + return -1; + } + + bt_status_t status; + // Free the on-demand segment list when response the ack to the Client. + ras_segment_t *seg_prev, *seg_next; + CS_LIST_FOR_EACH_CONTAINER_SAFE(&on_demand_data->seg_list, + seg_prev, seg_next, seg_node) + { + if (seg_prev && (seg_prev->seg_idx >= first_seg) && (seg_prev->seg_idx <= last_seg)) { + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + status = BT_GATT_NOTIFY_CB(RAS_ON_DEMAND_CHAR_SEND, addr, seg_prev->data, seg_prev->len); + if (status != 0) { + BT_LOGE("On-demand ranging data notify fail, err(%d).", status); + return status; + } + } else if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_INDICATE)) { + status = BT_GATT_INDICATE(RAS_ON_DEMAND_CHAR_SEND, addr, seg_prev->data, seg_prev->len); + if (status != 0) { + BT_LOGE("On-demand ranging data indicate fail, err(%d).", status); + return status; + } + } + } else if (seg_prev && (seg_prev->seg_idx > last_seg)) { + break; + } + } + + ras_on_demond_send_lost_ranging_data_cmp_rsp(addr, count, first_seg, last_seg); + return BT_STATUS_SUCCESS; +} + +static uint8_t ras_check_ranging_mode(bt_address_t* addr) +{ + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_NOTIFY) || + ras_state_get_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_INDICATE)) { + BT_LOGD("The real-time mode has been set."); + return SAL_LE_RAS_RANGING_MODE_REAL_TIME; + } else if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY) || + ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE)) { + BT_LOGD("The on-demand mode has been set."); + return SAL_LE_RAS_RANGING_MODE_ON_DEMAND; + } + + BT_LOGE("No mode have been set."); + return SAL_LE_RAS_RANGING_MODE_UNDEFINED; +} + +bool ras_is_filter_bit_set(uint16_t mode, uint16_t filter_bit) +{ + // Check if the mode is valid (within the range of SAL_LE_RAS_MODE_3_FILTER_MAX) + if (mode >= SAL_LE_RAS_FILTER_MODE_MAX) { + BT_LOGE("Error: Mode %d is out of valid range (0 to %d)", + mode, SAL_LE_RAS_FILTER_MODE_MAX - 1); + return false; + } + + // Retrieve the current filter mask from the atomic array + uint32_t current_filter_mask = __atomic_load_n(&ras_srv->ras_filter[mode], __ATOMIC_SEQ_CST); + + // Check if the specific filter bit is set + // We shift the filter_bit into the correct position and mask it to check + return (current_filter_mask & (1 << filter_bit)) != 0; +} + +static void ras_set_filter(uint16_t filter_value) +{ + // Extract the mode (bits 0-1) + uint16_t mode = filter_value & SAL_LE_RAS_FILTER_MODE_MASK; + + // Extract the filter mask (bits 2-15) + uint16_t filter_mask = filter_value & ~SAL_LE_RAS_FILTER_MODE_MASK; + + // Use the mode directly as an index into ras_filter + if (mode < SAL_LE_RAS_FILTER_MODE_MAX) { + // Atomic operations to modify the filter mask + // Clear the filter bits (2-15) + __atomic_fetch_add(&ras_srv->ras_filter[mode], SAL_LE_RAS_FILTER_BIT_MASK, __ATOMIC_SEQ_CST); + // Set the new filter mask (bits 2-15) + __atomic_fetch_or(&ras_srv->ras_filter[mode], filter_mask, __ATOMIC_SEQ_CST); + } else { + // Handle error case if mode is out of range + BT_LOGE("Error: Mode %d is out of valid range (0 to %d)\n", mode, SAL_LE_RAS_FILTER_MODE_MAX - 1); + } + + return; +} + +void ras_handle_abort_operation(ras_control_point_t* control_point) +{ + if (control_point->is_processing) { + BT_LOGD("Stopping current RAS Control Point processes..."); + control_point->is_processing = 0; // Stop current processing. + + if (control_point->is_data_pending) { + BT_LOGD("Flushing pending Ranging Data segments..."); + control_point->is_data_pending = 0; // Discard pending data + } + + control_point->response_code = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; // response success. + BT_LOGD("Abort Operation completed successfully."); + } else { + control_point->response_code = SAL_LE_RAS_CTL_OP_RSP_CODE_NOT_SUPPORTED; + BT_LOGD("Abort Operation was unsuccessful (no operation to abort)."); + } +} + +void ras_handle_other_operation(ras_control_point_t* control_point) +{ + BT_LOGD("Handling other operations..."); + control_point->response_code = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; // response success. +} + +void ras_write_opcode(ras_control_point_t* control_point, ras_opcode_t op_code) +{ + switch (op_code) { + case ABORT_OPERATION: + BT_LOGD("Received ABORT_OPERATION OpCode."); + ras_handle_abort_operation(control_point); + break; + + case OTHER_OPERATION: + BT_LOGD("Received OTHER_OPERATION OpCode."); + ras_handle_other_operation(control_point); + break; + + default: + control_point->response_code = SAL_LE_RAS_CTL_OP_RSP_CODE_NOT_SUPPORTED; + BT_LOGW("OpCode not supported."); + break; + } +} + +void rs_display_response(ras_control_point_t* control_point) +{ + switch (control_point->response_code) { + case SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS: + BT_LOGD("Response: SUCCESS."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_PROCE_NOT_CMP: + BT_LOGD("Response: PROCEDURE NOT COMPLETED."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_PERSISTED: + BT_LOGD("Response: ABORT UNSUCCESSFUL."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_SERVER_BUSY: + BT_LOGD("Response: SERVER BUSY."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_INVALID_PARAMS: + BT_LOGD("Response: INVALID PARAMETER."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_NOT_SUPPORTED: + BT_LOGD("Response: OP CODE NOT SUPPORTED."); + break; + default: + BT_LOGD("Unknown response code."); + break; + } +} + +ssize_t on_ras_ctr_pt_write_cb(bt_address_t* addr, const void* buf, uint16_t len) +{ + if (!buf || len == 0) { + BT_LOGE("Invalid buf or len."); + return 0; + } + + uint8_t* buf_send = (uint8_t*)buf; + + uint8_t opcode = (uint8_t)buf_send[0]; + BT_LOGD("RAS Control Point cb, opcode(%d)", opcode); + + uint16_t count; + + switch (opcode) { + case SAL_LE_RAS_CTL_OP_CMD_GET_RANG_DATA: { + count = ras_get_uint16_from_ptr((const uint8_t*)&buf_send[1]); + ras_ondemand_send_ranging_data(addr, count); + break; + } + case SAL_LE_RAS_CTL_OP_CMD_ACK_RANG_DATA: { + count = ras_get_uint16_from_ptr((const uint8_t*)&buf_send[1]); + ras_on_demond_send_code_rsp(addr, count); + break; + } + case SAL_LE_RAS_CTL_OP_CMD_RETRIEVE_LOST_RANG_DATA_SEG: { + count = ras_get_uint16_from_ptr((const uint8_t*)&buf_send[1]); + uint8_t first_seg = (uint8_t)buf_send[3]; + uint8_t last_seg = (uint8_t)buf_send[4]; + ras_on_demand_retrieve_send_lost_data(addr, count, first_seg, last_seg); + } + case SAL_LE_RAS_CTL_OP_CMD_ABORT_OPERATION: { + + } + case SAL_LE_RAS_CTL_OP_CMD_SET_FILTER: { + uint16_t filter_params = ras_get_uint16_from_ptr((const uint8_t*)&buf_send[1]); + ras_set_filter(filter_params); + } + default: + break; + } + return len; +} + +static ssize_t ras_feature_read(bt_address_t* addr, void* buf, uint16_t len, uint16_t offset) +{ + BT_LOGD("RAS feature read cb, ras_feature 0x%lx.\n", ras_srv->ras_feature); + BT_LOGD("offset:%d, buf[%d]:%s", offset, len, cs_log_to_hex_str(buf, len)); + return 0; +} + +static void range_rtt_dt_ccc_cfg_changed(bt_address_t* addr, uint16_t value) +{ + if (!ras_srv) { + BT_LOGE("Invalid ras_srv value."); + return; + } + + // The Real time mode and the on-demand mode can't be set together. + if ((value != 0) && (ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY) || + ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE))) { + BT_LOGE("The on-demond mode has been setted, Please clear it before set to real-time mode."); + return; + } + + if (value == 0) { + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_NOTIFY); + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_INDICATE); + } else { + ras_state_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_RTT_DATA_NOTIFY : RAS_RTT_DATA_INDICATE); + } + + BT_LOGD("The range real-time data ccc value is change to (%d)\n", value); + return; +} + +static void range_on_dem_dt_ccc_cfg_changed(bt_address_t* addr, uint16_t value) +{ + if (!ras_srv) { + BT_LOGE("Invalid ras_srv value."); + return; + } + + // The Real time mode and the on-demand mode can't be set together. + if ((value != 0) && (ras_state_get_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_NOTIFY) || + ras_state_get_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_INDICATE))) { + BT_LOGE("The on-demond mode has been setted, please clear it before set to real-time mode."); + return; + } + + if (value == 0) { + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY); + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE); + } else { + ras_state_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_ON_DEMAND_DATA_NOTIFY : RAS_ON_DEMAND_DATA_INDICATE); + } + + BT_LOGD("The range on-dem data ccc value is change to (%d)\n", value); + return; +} + +static void range_ctr_pt_ccc_cfg_changed(bt_address_t* addr, uint16_t value) +{ + if (!ras_srv) { + BT_LOGE("Invalid ras_srv value."); + return; + } + + if (value == 0) { + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY); + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_INDICATE); + } else { + ras_state_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_CONTROL_POINT_NOTIFY : RAS_CONTROL_POINT_INDICATE); + } + + BT_LOGD("The range control point data ccc value is change to (%d)", value); + return; +} + +static void range_dt_rd_ccc_cfg_changed(bt_address_t* addr, uint16_t value) +{ + if (!ras_srv) { + BT_LOGE("Invalid ras_srv value."); + return; + } + + if (value == 0) { + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY); + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_DATA_READY_INDICATE); + } else { + ras_state_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_DATA_READY_NOTIFY : RAS_DATA_READY_INDICATE); + } + + BT_LOGD("The range data ready data ccc value is change to (%d)", value); + return; +} + +static void range_dt_ov_wr_ccc_cfg_changed(bt_address_t* addr, uint16_t value) +{ + if (!ras_srv) { + BT_LOGE("Invalid ras_srv value."); + return; + } + + if (value == 0) { + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_OVER_WRITE_NOTIFY); + ras_state_clear_bit(&ras_srv->char_notify_state, RAS_OVER_WRITE_INDICATE); + } else { + ras_state_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_OVER_WRITE_NOTIFY : RAS_OVER_WRITE_INDICATE); + } + + BT_LOGD("The range over write data ccc value is change to (%d)\n", value); + return; +} + +static void ras_dt_rd_indicate_cb(bt_address_t* addr) +{ + BT_LOGD("Indication finish"); + if (ras_srv->remaining_len) { + split_real_time_segment(addr, &ras_srv->latest_local_steps[ras_srv->ras_seg_offset], ras_srv->remaining_len); + } + + return; +} + +// static void ras_dt_rd_indicate_destroy(struct bt_gatt_indicate_params* params) +// { +// BT_LOGD("Indication complete\n"); +// ras_srv->ras_dt_rd_indicating = 0U; + +// return; +// } + +static void ras_write_bits(uint8_t* buf, int* bit_offset, uint32_t value, int bit_count) +{ + int byte_index; + int bit_index; + for (int i = bit_count - 1; i >= 0; i--) { + byte_index = *bit_offset / 8; + bit_index = 7 - (*bit_offset % 8); + uint8_t bit = (value >> i) & 0x01; + + if (bit) { + buf[byte_index] |= (1 << bit_index); + } else { + buf[byte_index] &= ~(1 << bit_index); + } + + (*bit_offset)++; + } + + return; +} + +static void split_real_time_segment(bt_address_t* addr, uint8_t* buf, int len) +{ + if (ras_srv->remaining_len > ras_srv->ras_mtu - RAS_SEG_HEADER_SIZE - 1) { + int curr_seg_size = ras_srv->ras_mtu - RAS_SEG_HEADER_SIZE - 1; + BT_LOGD("data send: offset:%lu, len:%d\n", ras_srv->ras_seg_offset, curr_seg_size); + + // update offset + ras_srv->ras_seg_offset += curr_seg_size; + uint8_t* send_buf = malloc(curr_seg_size + 1); + + ras_srv->remaining_len -= curr_seg_size; + send_buf[0] = (ras_srv->ras_seg_idx == 0) ? (0x01) : (ras_srv->ras_seg_idx << 2); + if (ras_srv->remaining_len == 0) { + ras_srv->ras_seg_idx = 0; + send_buf[0] |= (0x01 << 1); + ras_srv->remaining_len = 0; + ras_srv->ras_seg_offset = 0; + } + + ras_srv->ras_seg_idx++; + memcpy(&send_buf[1], buf, curr_seg_size); + + if ((send_buf[0] & 0x01) == 0x01) { + BT_LOGD("First seg, data(%d):%s\n", curr_seg_size + 1, cs_log_to_hex_str(send_buf, curr_seg_size + 1)); + } else { + BT_LOGD("The %d seg, data(%d):%s\n", send_buf[0] >> 2, curr_seg_size + 1, cs_log_to_hex_str(send_buf, curr_seg_size + 1)); + } + + if (ras_srv->rt_dt_ccc_cfg == SAL_LE_RAS_GATT_NOTIFY) { + if (BT_GATT_NOTIFY(RAS_REAL_TIME_CHAR_SEND, addr, send_buf, curr_seg_size + 1) == 0) { + if (ras_srv->remaining_len) { + split_real_time_segment(addr, &ras_srv->latest_local_steps[ras_srv->ras_seg_offset], ras_srv->remaining_len); + } + ras_srv->ras_dt_rd_indicating = 1U; + free(send_buf); + } else { + BT_LOGD("ras data ready Notify fail.\n"); + free(send_buf); + return; + } + } else if (ras_srv->rt_dt_ccc_cfg == SAL_LE_RAS_GATT_INDICATION) { + if (BT_GATT_INDICATE(RAS_REAL_TIME_CHAR_SEND, addr, send_buf, curr_seg_size + 1) == 0) { + ras_srv->ras_dt_rd_indicating = 1U; + free(send_buf); + } else { + BT_LOGD("ras data ready Indicate fail.\n"); + free(send_buf); + return; + } + } + } else { + int curr_seg_size = len; + uint8_t* send_buf = malloc(curr_seg_size + 1); + send_buf[0] = (ras_srv->ras_seg_idx == 0) ? (0x01) : (ras_srv->ras_seg_idx << 2); + send_buf[0] |= (0x01 << 1); + memcpy(&send_buf[1], buf, curr_seg_size); + ras_srv->ras_dt_rd_indicating = 0U; + + ras_srv->ras_seg_idx = 0; + ras_srv->ras_dt_rd_indicating = 0U; + ras_srv->remaining_len = 0; + ras_srv->ras_seg_offset = 0; + BT_LOGD("The last(%d) seg, data(%d):%s\n", send_buf[0] >> 2, curr_seg_size + 1, cs_log_to_hex_str(send_buf, curr_seg_size + 1)); + BT_LOGD("ras_dt_rd_indicating:%d\n", ras_srv->ras_dt_rd_indicating); + if (ras_srv->rt_dt_ccc_cfg == SAL_LE_RAS_GATT_NOTIFY) { + if (BT_GATT_NOTIFY(RAS_REAL_TIME_CHAR_SEND, addr, send_buf, curr_seg_size + 1) == 0) { + free(send_buf); + } else { + BT_LOGD("ras data ready Indicate fail.\n"); + free(send_buf); + return; + } + } else if (ras_srv->rt_dt_ccc_cfg == SAL_LE_RAS_GATT_INDICATION) { + if (BT_GATT_INDICATE(RAS_REAL_TIME_CHAR_SEND, addr, send_buf, curr_seg_size + 1) == 0) { + ras_srv->ras_dt_rd_indicating = 1U; + free(send_buf); + } else { + BT_LOGD("ras data ready Indicate fail.\n"); + free(send_buf); + return; + } + } + } + + return; +} + +static void ras_on_demand_data_send_timeout(service_timer_t* timer, void* data) +{ + ras_rang_on_demand_t* on_demand_subevent = (ras_rang_on_demand_t*)data; + BT_LOGD("On-demand data send timeout, remove the data in the list."); + // Free the on-demand segment list when response the ack to the Client. + ras_segment_t *seg_prev, *seg_next; + CS_LIST_FOR_EACH_CONTAINER_SAFE(&on_demand_subevent->seg_list, + seg_prev, seg_next, seg_node) + { + cs_list_remove(&on_demand_subevent->seg_list, NULL, &seg_prev->seg_node); + BT_LOGD("seg_prev:%p", seg_prev); + free(seg_prev); + } + + memset(on_demand_subevent, 0, sizeof(ras_rang_on_demand_t)); + + return; +} + +static void split_on_demand_segment(bt_address_t* addr, uint8_t* buf, int len, + ras_rang_on_demand_t* subevent) +{ + int curr_seg_size = 0; + uint16_t seg_index = 0; + ras_srv->ras_seg_offset = 0; + + do { + if (ras_srv->remaining_len > ras_srv->ras_mtu - RAS_SEG_HEADER_SIZE - 1) { + curr_seg_size = ras_srv->ras_mtu - RAS_SEG_HEADER_SIZE - 1; + BT_LOGD("data send: offset:%lu, len:%d\n", ras_srv->ras_seg_offset, curr_seg_size); + subevent->seg = (ras_segment_t*)malloc(sizeof(ras_segment_t) + curr_seg_size + 1); + + if (!subevent->seg) { + BT_LOGE("Malloc fail."); + return; + } + + memcpy(&subevent->seg->data[1], &buf[ras_srv->ras_seg_offset], curr_seg_size); + // update offset + ras_srv->ras_seg_offset += curr_seg_size; + ras_srv->remaining_len -= curr_seg_size; + + subevent->seg->data[0] = (seg_index == 0) ? (0x01) : (seg_index << 2); + if (ras_srv->remaining_len == 0) { + subevent->seg->data[0] |= (0x01 << 1); + ras_srv->remaining_len = 0; + ras_srv->ras_seg_offset = 0; + } + + } else { + curr_seg_size = ras_srv->remaining_len; + subevent->seg = (ras_segment_t*)malloc(sizeof(ras_segment_t) + curr_seg_size + 1); + + if (!subevent->seg) { + BT_LOGE("Malloc fail."); + return; + } + + memcpy(&subevent->seg->data[1], &buf[ras_srv->ras_seg_offset], curr_seg_size); + + subevent->seg->data[0] = (seg_index == 0) ? (0x01) : (seg_index << 2); + subevent->seg->data[0] |= (0x01 << 1); + ras_srv->remaining_len = 0; + BT_LOGD("ras_dt_rd_indicating:%d\n", ras_srv->ras_dt_rd_indicating); + } + + if ((subevent->seg->data[0] & 0x01) == 0x01) { + BT_LOGD("First seg, data(%d):%s\n", curr_seg_size + 1, cs_log_to_hex_str(subevent->seg->data, curr_seg_size + 1)); + } else { + BT_LOGD("The %d seg, data(%d):%s\n", subevent->seg->data[0] >> 2, curr_seg_size + 1, + cs_log_to_hex_str(subevent->seg->data, curr_seg_size + 1)); + } + + subevent->seg->seg_idx = seg_index++; + subevent->seg->len = curr_seg_size + 1; + cs_list_append(&subevent->seg_list, &subevent->seg->seg_node); + BT_LOGD("subevent seg:%p, seg_node:%p.", subevent->seg, &subevent->seg->seg_node); + } while (ras_srv->remaining_len > 0); + + subevent->on_demand_timer = service_loop_timer(RAS_RSP_TIMEOUT, false, ras_on_demand_data_send_timeout, &subevent); + return; +} + +/** + * Input: data points to the original buffer, data_len is the total length of the input + * Output: buf stores the transformed result, function returns the length of the output buffer + */ +static size_t transform_step_data_to_ras_format_filtered( + uint8_t* data, size_t data_len, + uint8_t* buf, uint32_t* ras_filter, uint8_t role, + uint8_t num_antenna_paths) +{ + size_t in_offset = 0; + size_t out_offset = 0; + + while (in_offset + 3 <= data_len) { + uint8_t step_mode = data[in_offset]; + uint8_t step_data_length = data[in_offset + 2]; + + if (in_offset + 3 + step_data_length > data_len) { + BT_LOGW("Incomplete data, exit early."); + break; + } + + uint8_t* step_data = &data[in_offset + 3]; + uint32_t filter_mask = ras_filter[step_mode]; + + // Write Step_Mode to the output + buf[out_offset++] = step_mode; + + const uint8_t* p = step_data; + size_t remaining = step_data_length; + + switch (step_mode) { + /* + * MODE 0 — Basic CS Packet Mode + * -------------------------------------------------------- + * Initiator: + * 1. Packet_Quality (1) + * 2. Packet_RSSI (1) + * 3. Packet_Antenna (1) + * 4. Measured_Freq_Offset (2) + * --> Total: 5 bytes + * + * Reflector: + * 1. Packet_Quality (1) + * 2. Packet_RSSI (1) + * 3. Packet_Antenna (1) + * --> Total: 3 bytes + * + ************************************************************/ + case SAL_LE_RAS_SUBEVENT_STEP_MODE_0: + if (role == SAL_LE_RAS_ROLE_INITIATOR) { + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_FREQ_OFFSET); + } else { // Reflector + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + } + break; + /* MODE 1 — Time-of-Flight (ToF) Mode + * ------------------------------------------------------------ + * Initiator: + * 1. Packet_Quality (1) + * 2. Packet_NADM (1) + * 3. Packet_RSSI (1) + * 4. ToA_ToD_Initiator (2) + * 5. Packet_Antenna (1) + * 6. Packet_PCT1 (4) + * 7. Packet_PCT2 (4) + * --> Total: 14 bytes + * + * Reflector: + * 1. Packet_Quality (1) + * 2. Packet_NADM (1) + * 3. Packet_RSSI (1) + * 4. ToD_ToA_Reflector (2) + * 5. Packet_Antenna (1) + * 6. Packet_PCT1 (4) + * 7. Packet_PCT2 (4) + * --> Total: 14 bytes + * + **************************************************************/ + case SAL_LE_RAS_SUBEVENT_STEP_MODE_1: + if (role == SAL_LE_RAS_ROLE_REFLECTOR) { + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_NADM); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_TOA_TOD); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT1); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT2); + } else { // Reflector + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_NADM); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_TOD_TOA); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT1); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT2); + } + break; + /***************************************************************** + * MODE 2 — Tone-based Phase Measurement Mode + * ------------------------------------------------------------ + * (Same layout for Initiator and Reflector) + * 1. Antenna_Permutation_Index (1) + * 2. Tone_PCT[k] ((Num_Antenna_Paths + 1) × 3) + * 3. Tone_Quality_Indicator[k] ((Num_Antenna_Paths + 1) × 1) + * --> Example total (Num_Antenna_Paths=3): 17 bytes + * + *******************************************************************/ + case SAL_LE_RAS_SUBEVENT_STEP_MODE_2: + // Initiator and Reflector are the same. + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_ANT_PERM_IDX); + + // Tone_PCT[k] + // Actual size = (Num_Antenna_Paths + 1) × 3 octets + // COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 3, RAS_FILTER_BIT_TONE_PCT); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 3, RAS_FILTER_BIT_TONE_PCT); + // Tone_Quality_Indicator[k] + // COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 1, RAS_FILTER_BIT_TONE_QUALITY); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 1, RAS_FILTER_BIT_TONE_QUALITY); + break; + /* MODE 3 — Combined (ToF + Tone) Mode + * ------------------------------------------------------------ + * Initiator: + * 1. Packet_Quality (1) + * 2. Packet_NADM (1) + * 3. Packet_RSSI (1) + * 4. ToA_ToD_Initiator (2) + * 5. Packet_Antenna (1) + * 6. Packet_PCT1 (4) + * 7. Packet_PCT2 (4) + * 8. Antenna_Permutation_Index (1) + * 9. Tone_PCT[k] ((Num_Antenna_Paths + 1) × 3) + * 10. Tone_Quality_Indicator[k] ((Num_Antenna_Paths + 1) × 1) + * --> Example total (Num_Antenna_Paths=3): 31 bytes + * + * Reflector: + * 1. Packet_Quality (1) + * 2. Packet_NADM (1) + * 3. Packet_RSSI (1) + * 4. ToD_ToA_Reflector (2) + * 5. Packet_Antenna (1) + * 6. Packet_PCT1 (4) + * 7. Packet_PCT2 (4) + * 8. Antenna_Permutation_Index (1) + * 9. Tone_PCT[k] ((Num_Antenna_Paths + 1) × 3) + * 10. Tone_Quality_Indicator[k] ((Num_Antenna_Paths + 1) × 1) + * --> Example total (Num_Antenna_Paths=3): 31 bytes + * + ********************************************************************/ + case SAL_LE_RAS_SUBEVENT_STEP_MODE_3: + if (role == SAL_LE_RAS_ROLE_INITIATOR) { + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_NADM); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_TOA_TOD); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT1); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT2); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_ANT_PERM_IDX); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 3, RAS_FILTER_BIT_TONE_PCT); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 1, RAS_FILTER_BIT_TONE_QUALITY); + } else { // Reflector + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_NADM); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_TOD_TOA); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT1); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT2); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_ANT_PERM_IDX); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 3, RAS_FILTER_BIT_TONE_PCT); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 1, RAS_FILTER_BIT_TONE_QUALITY); + } + break; + default: + BT_LOGW("Unknown mode %d, copying raw data.", step_mode); + memcpy(&buf[out_offset], step_data, step_data_length); + out_offset += step_data_length; + break; + } + + in_offset += 3 + step_data_length; + } + + return out_offset; +} + +static ras_rang_on_demand_t* ras_rang_on_demand_subevent_pool_find(bt_address_t* addr) +{ + if (!ras_srv) { + BT_LOGE("Invalid ras_srv enviranment, init it first."); + return NULL; + } + + for (int i = 0; i < SAL_LE_RAS_STORE_PROCEDURE_NUM_MAX; i++) { + if (ras_srv->subevent[i].proc_used == false) { + memset(&ras_srv->subevent[i], 0, sizeof(ras_rang_on_demand_t)); + ras_srv->subevent[i].proc_used = true; + return &ras_srv->subevent[i]; + } + } + + BT_LOGD("No subvent pool can be used."); + return NULL; +} + +static ras_rang_on_demand_t* ras_rang_on_demand_find_subevent(bt_address_t* addr, uint16_t count) +{ + if (!ras_srv) { + BT_LOGE("Invalid ras_srv enviranment, init it first."); + return NULL; + } + + for (int i = 0; i < SAL_LE_RAS_STORE_PROCEDURE_NUM_MAX; i++) { + if (ras_srv->subevent[i].count == count && ras_srv->subevent[i].proc_used == true) { + return &ras_srv->subevent[i]; + } + } + + BT_LOGD("No subvent find with count(%d).", count); + return NULL; +} + +static uint8_t* ras_subevent_data_conversion(bt_address_t* addr, bt_srv_conn_le_cs_subevent_result_t* result) +{ + memset(ras_srv->latest_local_steps, 0, sizeof(ras_srv->latest_local_steps)); + + if (result->len <= SAL_LE_RAS_STEP_DATA_BUF_LEN) { + memcpy(ras_srv->latest_local_steps, result->step_data_buf, + result->len); + BT_LOGD("step data[%d]:%s\n", result->len, + cs_log_to_hex_str(result->step_data_buf, result->len)); + } else { + BT_LOGD("Not enough memory to store step data. (%d > %d)\n", + result->len, SAL_LE_RAS_STEP_DATA_BUF_LEN); + } + + uint8_t* stream_buf = ras_srv->latest_local_steps; + int bit_offset = 0; + uint16_t count_id = ((result->header.procedure_counter & 0x0FFF) << 12) | (result->header.config_id & 0x0F); + /** + * CS configuration identifier. + * Range: 0 to 3 + * Rangging Counter is lower 12-bits of CS Procedure_Counter Provided by the Core Controller + */ + ras_write_bits(stream_buf, &bit_offset, count_id, 16); + /** + * Transmit power level used for the CS Procedure. + * Range: -127 to 20 + * Units: decibels referenced to 1 milliwatt(dBm) + */ + ras_write_bits(stream_buf, &bit_offset, result->header.reference_power_level, 8); + /** + * Antenna paths that are reported: + * Bit0: 1 if Antenna Path_1 included; 0 if not. + * Bit1: 1 if Antenna Path_2 included; 0 if not. + * Bit2: 1 if Antenna Path_3 included; 0 if not. + * Bit3: 1 if Antenna Path_4 included; 0 if not. + * Bits 4-7: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.num_antenna_paths, 8); + /** + * Starting ACL addrection event count for the results reported in the event. + */ + ras_write_bits(stream_buf, &bit_offset, result->header.start_acl_conn_event, 16); + /** + * Frequency compensation value in units of 0.01 parts permillion(ppm)(15-bit signed integer). + */ + ras_write_bits(stream_buf, &bit_offset, result->header.frequency_compensation, 16); + /** + * 0x0: All results complete for the CS Procedure + * 0x1: Partial results with more to follow for the CS Procedure + * 0xF: All subsequent CS Procedures aborted + * All other values: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.procedure_done_status, 4); + /** + * 0x0: All results complete for the CS Subevent + * 0xF: Current CS Subevent aborted + * All other values: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.subevent_done_status, 4); + /** + * Indicates the abort reason when the Procedure_Done Status received from the Core + * Controller is set to 0xF; otherwise, the value is set to zero. + * 0x0: Report with no abort + * 0x1: Abort because of local Host or remote request + * 0x2: Abort because filtered channel map has less than 15 channels + * 0x3: Abort because the channel map update instant has passed + * 0xF: Abort because of unspecified reasons + * All other values: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.procedure_abort_reason, 4); + /** + * Indicates the abort reason when the Subevent_Done_Status received from the Core Controller + * is set to 0xF; otherwise, the default value is set to zero. + * 0x0: Report with no abort + * 0x1: Abort because of local Host or remote request + * 0x2: Abort because no CS_SYNC(mode 0) was received + * 0x3: Abort because of scheduling conflicts or limited resources + * 0xF: Abort because of unspecified reasons + * All other values: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.subevent_abort_reason, 4); + /** + * Reference power level. + * Range: -127 to 20 + * Units: dBm + */ + ras_write_bits(stream_buf, &bit_offset, result->header.reference_power_level, 8); + /** + * Number of steps in the CS Subevent for which results are reported. If the Subevent is + * aborted, then the Number of Steps Reported can be set to zero. + */ + ras_write_bits(stream_buf, &bit_offset, result->header.num_steps_reported, 8); + + ras_srv->ras_seg_offset = 0; + ras_srv->remaining_len = transform_step_data_to_ras_format_filtered(result->step_data_buf, + result->len, stream_buf + SAL_LE_RAS_SUB_PROCUDURE_HEAD, + ras_srv->ras_filter, ras_srv->ras_role, result->header.num_antenna_paths & 0x0F); + ras_srv->remaining_len += SAL_LE_RAS_SUB_PROCUDURE_HEAD; + BT_LOGD("stream head:%s.", cs_log_to_hex_str(stream_buf, SAL_LE_RAS_SUB_PROCUDURE_HEAD)); + BT_LOGD("stream_buf(%ld):%s.\n", ras_srv->remaining_len, + cs_log_to_hex_str(stream_buf + 12, ras_srv->remaining_len - SAL_LE_RAS_SUB_PROCUDURE_HEAD)); + BT_LOGD("data ready indiacte count(%d)", result->header.procedure_counter); + BT_LOGD("procedure_counter:0x%x, config_id:0x%x, reference_power_level:0x%x", + result->header.procedure_counter, result->header.config_id, + result->header.reference_power_level); + BT_LOGD("num_antenna_paths:0x%x, start_acl_conn_event:0x%x, frequency_compensation:0x%x", + result->header.num_antenna_paths, result->header.start_acl_conn_event, + result->header.frequency_compensation); + BT_LOGD("procedure_done_status:0x%x, subevent_done_status:0x%x, procedure_abort_reason:0x%x", + result->header.procedure_done_status, result->header.subevent_done_status, + result->header.procedure_abort_reason); + BT_LOGD("subevent_abort_reason:0x%x, reference_power_level:0x%x, num_steps_reported:0x%x", + result->header.subevent_abort_reason, result->header.reference_power_level, + result->header.num_steps_reported); + BT_LOGD("mode:0x%x, channel:0x%x, len:0x%x", result->step_data_buf[0], + result->step_data_buf[1], + result->step_data_buf[2]); + return stream_buf; +} + +static void ras_process_real_time_ranging_data(bt_address_t* addr, bt_srv_conn_le_cs_subevent_result_t* result) +{ + uint8_t* stream_buf = ras_subevent_data_conversion(addr, result); + split_real_time_segment(addr, stream_buf, ras_srv->remaining_len); +} + +static void ras_process_on_demand_ranging_data(bt_address_t* addr, bt_srv_conn_le_cs_subevent_result_t* result) +{ + ras_rang_on_demand_t* subevent = ras_rang_on_demand_subevent_pool_find(addr); + + if (!subevent) { + BT_LOGE("No subevent pool found."); + return; + } + + BT_LOGD("procedure counter:%d.", result->header.procedure_counter); + + subevent->count = result->header.procedure_counter; + uint8_t* stream_buf = ras_subevent_data_conversion(addr, result); + split_on_demand_segment(addr, stream_buf, ras_srv->remaining_len, subevent); + if (ras_state_get_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_IDLE)) { + ras_data_ready_send(addr, subevent->count); + // Set the on-demand state to ready. + ras_state_set_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_DATA_READY_INDICATE); + } + + return; +} + +static void subevent_result_cb(bt_address_t* addr, bt_srv_conn_le_cs_subevent_result_t* result) +{ + if (result->header.procedure_done_status == BT_LE_SRV_CS_PROCEDURE_COMPLETE && + ras_check_ranging_mode(addr) == SAL_LE_RAS_RANGING_MODE_REAL_TIME) { + BT_LOGD("Recv the real-time ranging data."); + ras_process_real_time_ranging_data(addr, result); + return; + } + + if (result->header.procedure_done_status == BT_LE_SRV_CS_PROCEDURE_COMPLETE && + ras_check_ranging_mode(addr) == SAL_LE_RAS_RANGING_MODE_ON_DEMAND) { + BT_LOGD("Recv the on-demand ranging data."); + ras_process_on_demand_ranging_data(addr, result); + return; + } + + BT_LOGE("No mode has been set, discard the subevent result."); + return; +} + +static bt_status_t ras_data_ready_send(bt_address_t* addr, uint16_t count) +{ + uint8_t buf[2]; + ras_put_uint16_to_ptr(count, buf); + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY)) { + return BT_GATT_NOTIFY(RAS_DATA_READY_CHAR_SEND, addr, (uint8_t *)&count, sizeof(count)); + } else if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_DATA_READY_INDICATE)) { + return BT_GATT_INDICATE(RAS_DATA_READY_CHAR_SEND, addr, (uint8_t *)&count, sizeof(count)); + } + + BT_LOGE("Invalid data ready char indication state."); + return BT_STATUS_FAIL; +} + +static void ras_mtu_updated_cb(bt_address_t* addr, uint32_t mtu) +{ + if (ras_srv) { + ras_srv->ras_mtu = mtu; + } + + BT_LOGD("Updated MTU, ras_mtu: %lu\n", ras_srv->ras_mtu); + return; +} + +static void ras_gatts_ccc_cfg_cb(bt_address_t* addr, ras_ccc_cfg_change_evt_t event, + const uint8_t* value, uint16_t length) +{ + if (!addr) { + BT_LOGE("Invalid BT address."); + return; + } + + switch(event) { + case RAS_RTT_DATA_CCC_CFG_CHANGE_EVT: { + uint16_t rtt_cfg_val; + + if (length != 2) { + BT_LOGE("Invalid RAS RTT CCC data config value length(%d).", length); + return; + } + + CS_BYTE_STREAM_TO_UINT16(rtt_cfg_val, value); + range_rtt_dt_ccc_cfg_changed(addr, rtt_cfg_val); + } + break; + case RAS_ON_DEMAND_DATA_CCC_CFG_CHANGE_EVT: { + uint16_t on_demand_cfg_val; + + if (length != 2) { + BT_LOGE("Invalid RAS On-demand CCC data config value length(%d).", length); + return; + } + + CS_BYTE_STREAM_TO_UINT16(on_demand_cfg_val, value); + range_on_dem_dt_ccc_cfg_changed(addr, on_demand_cfg_val); + } + break; + case RAS_CTR_PT_CCC_CFG_CHANGE_EVT: { + uint16_t ctr_point_cfg_val; + + if (length != 2) { + BT_LOGE("Invalid RAS Control Point CCC data config value length(%d).", length); + return; + } + + CS_BYTE_STREAM_TO_UINT16(ctr_point_cfg_val, value); + range_ctr_pt_ccc_cfg_changed(addr, ctr_point_cfg_val); + } + break; + case RAS_DATA_READY_CCC_CFG_CHANGE_EVT: { + uint16_t data_ready_cfg_val; + + if (length != 2) { + BT_LOGE("Invalid RAS data ready CCC data config value length(%d).", length); + return; + } + + CS_BYTE_STREAM_TO_UINT16(data_ready_cfg_val, value); + range_dt_rd_ccc_cfg_changed(addr, data_ready_cfg_val); + } + break; + case RAS_OVER_WRITE_CCC_CFG_CHANGE_EVT: { + uint16_t over_write_cfg_val; + + if (length != 2) { + BT_LOGE("Invalid RAS Over Write CCC data config value length(%d).", length); + return; + } + + CS_BYTE_STREAM_TO_UINT16(over_write_cfg_val, value); + range_dt_ov_wr_ccc_cfg_changed(addr, over_write_cfg_val); + } + break; + } + return; +} + +static void ras_gatts_ctr_pt_write_cb(bt_address_t* addr, + const uint8_t* value, uint16_t length) +{ + on_ras_ctr_pt_write_cb(addr, value, length); + return; +} + +static void ras_gatts_feature_read_cb(bt_address_t* addr, uint32_t req_handle) +{ + BT_LOGI("feature read, req_handle:%lu", req_handle); + if (!ras_srv) { + BT_LOGE("RAS haven't init."); + return; + } + + ras_send_feature_read_rsp(addr, ras_srv->ras_feature); + return; +} + +static void ras_notify_cb(bt_address_t* addr, gatt_status_t status, ras_attr_notify_t attr) +{ + if (status != GATT_STATUS_SUCCESS) { + BT_LOGE("Notify fail, status(%d)", status); + return; + } + + BT_LOGI("ras notify cb, attr:%d", attr); + switch(attr) { + case RAS_REAL_TIME_CHAR_SEND: { + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE)) { + ras_dt_rd_indicate_cb(addr); + } + } + break; + case RAS_ON_DEMAND_CHAR_SEND: { + if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY)) { + ras_on_demand_notify_finished(addr); + } else if (ras_state_get_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE)) { + ras_on_demand_indicate_finished(addr); + } + } + break; + default: + break; + } + + return; +} + +static void ras_conn_cb(bt_address_t* addr) +{ + if (ras_srv->addr) { + BT_LOGE("The connection has been created."); + return; + } + + ras_srv->addr = (bt_address_t*)malloc(sizeof(bt_address_t)); + + memcpy(ras_srv->addr, addr, sizeof(bt_address_t)); + BT_LOGD("RAS Conn to address:%s", bt_addr_str(addr)); + return; +} + +static void ras_disconn_cb(bt_address_t* addr) +{ + if (ras_srv->addr == NULL) { + BT_LOGE("The connection has been release."); + return; + } + + BT_LOGD("RAS disconn to address:%s", bt_addr_str(addr)); + free(ras_srv->addr); + ras_srv->addr = NULL; + return; +} + +static const ras_gatts_callbacks_t ras_cb = { + .cfg_cb = ras_gatts_ccc_cfg_cb, + .pt_write_cb = ras_gatts_ctr_pt_write_cb, + .feature_read_cb = ras_gatts_feature_read_cb, + .mtu_updated_cb = ras_mtu_updated_cb, + .notify_cb = ras_notify_cb, + .conn_cb = ras_conn_cb, + .disconn_cb = ras_disconn_cb, +}; + +int bt_cs_ras_enable(void) +{ + BT_LOGD("Enable Channel Sounding."); + + ras_srv = (ras_srv_env_t*)malloc(sizeof(ras_srv_env_t)); + + memset(ras_srv, 0, sizeof(ras_srv_env_t)); + + ras_srv->ras_feature = 0x07000007; + + for (int i = 0; i < SAL_LE_RAS_FILTER_MODE_MAX; i++) { + ras_srv->ras_filter[i] = 0xFFFFFFFF; + } + + bt_cs_ras_gatts_init(&ras_cb); + bt_cs_register_subevent_cb(subevent_result_cb); + + return 0; +} + +int ras_subevent_recv_test(ras_rang_mode_t mode, ras_testcase_t test_case, + bt_address_t* addr, bt_srv_conn_le_cs_subevent_result_t* result) +{ + if (mode == SAL_LE_RAS_RANGING_MODE_REAL_TIME) { + ras_state_set_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_NOTIFY); + BT_LOGD("RAS Server test: set ranging to real-time mode."); + } + + switch (test_case) { + case RAS_TESTCASE_REAL_TIME_NOTIFY_VALID_RANG_DATA_001: + ras_srv->ras_mtu = 253; + ras_srv->rt_dt_ccc_cfg = SAL_LE_RAS_GATT_NOTIFY; + break; + case RAS_TESTCASE_REAL_TIME_INDICATE_VALID_RANG_DATA_002: + ras_srv->ras_mtu = 253; + ras_srv->rt_dt_ccc_cfg = SAL_LE_RAS_GATT_INDICATION; + break; + case RAS_TESTCASE_ON_DEMAND_NOTIFY_VALID_RANG_DATA_003: + ras_srv->ras_mtu = 253; + ras_state_set_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY); + ras_state_set_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY); + ras_state_set_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_IDLE); + ras_state_set_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY); + break; + case RAS_TESTCASE_ON_DEMAND_INDICATE_VALID_RANG_DATA_004: + ras_srv->ras_mtu = 253; + ras_state_set_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE); + ras_state_set_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY); + ras_state_set_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_IDLE); + ras_state_set_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY); + break; + case RAS_TESTCASE_ON_DEMAND_WRITE_RANG_DATA_TIMWOUT_010: + ras_srv->ras_mtu = 253; + ras_state_set_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE); + ras_state_set_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY); + ras_state_set_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_IDLE); + break; + default: + BT_LOGE("Invalid test case number(%d).", test_case); + return -1; + } + + subevent_result_cb(addr, result); + return 0; +} + +int ras_ctrl_point_send_test(bt_address_t* addr, uint8_t* data, uint16_t len) +{ + on_ras_ctr_pt_write_cb(addr, (void*)data, len); + return 0; +} + +void ras_on_demand_notify_finish_test(bt_address_t* addr) +{ + ras_on_demand_notify_finished(addr); +} + +void ras_on_demand_indicate_finish_test(bt_address_t* addr, ras_attr_notify_t attr) +{ + switch(attr) { + case RAS_REAL_TIME_CHAR_SEND: + ras_dt_rd_indicate_cb(addr); + break; + case RAS_ON_DEMAND_CHAR_SEND: + ras_on_demand_indicate_finished(addr); + break; + default: + break; + } +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/service/profiles/cs/cs_ras_gatts.c b/service/profiles/cs/cs_ras_gatts.c new file mode 100644 index 000000000..8462287da --- /dev/null +++ b/service/profiles/cs/cs_ras_gatts.c @@ -0,0 +1,414 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "bt_gatts.h" +#include "bt_profile.h" +#include "bt_status.h" +#include "cs_ras_gatts.h" +#include "gatts_service.h" +#include "cs_ras_util.h" +#include "utils/log.h" +#include "service_manager.h" + +typedef struct { + gatts_handle_t ras_gatts_handle; + bt_address_t addr; + const gatts_interface_t* ras_gatts_interface; + const ras_gatts_callbacks_t* cb; +} ras_gatt_info_t; + +static ras_gatt_info_t* gatts_info = NULL; + +static uint16_t ras_real_time_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset); + +static uint16_t ras_on_demand_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset); + +static uint16_t ras_control_point_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset); + +static uint16_t ras_data_ready_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset); +static uint16_t ras_over_write_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset); +static uint16_t ras_control_point_write_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset); +static uint16_t ras_feature_read_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, uint32_t req_handle); +static void cs_ras_gatts_connected_cb(gatts_handle_t srv_handle, bt_address_t* addr); +static void cs_ras_gatts_disconnected_cb(gatts_handle_t srv_handle, bt_address_t* addr); +static void cs_ras_notify_complete_cb(gatts_handle_t srv_handle, bt_address_t* addr, gatt_status_t status, uint16_t attr_handle); +static void cs_ras_mtu_change_cb(gatts_handle_t srv_handle, bt_address_t* addr, uint32_t mtu); + +static gatt_attr_db_t ras_attr_db[] = { + GATT_H_PRIMARY_SERVICE(BT_UUID_DECLARE_16(BT_UUID_RANGING_VAL), RAS_RANGING_SERVICE_ATTR_ID), + GATT_H_CHARACTERISTIC_USER_RSP(BT_UUID_DECLARE_16(BT_UUID_RANG_FEAT_VAL), + GATT_PROP_READ, GATT_PERM_READ, ras_feature_read_cb, NULL, RAS_RANGING_FEATURE_ATTR_ID), + GATT_H_CHARACTERISTIC_USER_RSP(BT_UUID_DECLARE_16(BT_UUID_RANG_RTT_DT_VAL), + GATT_PROP_NOTIFY | GATT_PROP_INDICATE, GATT_PERM_READ, + NULL, NULL, RAS_RANGING_REAL_TIME_ATTR_ID), + GATT_H_CCCD(GATT_PERM_READ | GATT_PERM_WRITE, ras_real_time_ccc_changed_cb, RAS_RANGING_REAL_TIME_CCC_ID), + GATT_H_CHARACTERISTIC_USER_RSP(BT_UUID_DECLARE_16(BT_UUID_RANG_ON_DEM_DT_VAL), + GATT_PROP_NOTIFY | GATT_PROP_INDICATE, GATT_PERM_READ, NULL, NULL, RAS_RANGING_ON_DEMAND_ATTR_ID), + GATT_H_CCCD(GATT_PERM_READ | GATT_PERM_WRITE, ras_on_demand_ccc_changed_cb, RAS_RANGING_ON_DEMAND_CCC_ID), + GATT_H_CHARACTERISTIC_USER_RSP(BT_UUID_DECLARE_16(BT_UUID_RANG_RAS_CTR_POINT_VAL), + GATT_PROP_NOTIFY | GATT_PROP_INDICATE | GATT_PROP_WRITE_NR, GATT_PERM_READ | GATT_PERM_WRITE, NULL, + ras_control_point_write_cb, RAS_RANGING_CONTROL_POINT_ATTR_ID), + GATT_H_CCCD(GATT_PERM_READ | GATT_PERM_WRITE, ras_control_point_ccc_changed_cb, RAS_RANGING_CONTROL_POINT_CCC_ID), + GATT_H_CHARACTERISTIC_USER_RSP(BT_UUID_DECLARE_16(BT_UUID_RANG_DT_RD_VAL), + GATT_PROP_NOTIFY | GATT_PROP_INDICATE, GATT_PERM_READ, + NULL, NULL, RAS_RANGING_DATA_READY_ATTR_ID), + GATT_H_CCCD(GATT_PERM_READ | GATT_PERM_WRITE, ras_data_ready_ccc_changed_cb, RAS_RANGING_DATA_READY_CCC_ID), + GATT_H_CHARACTERISTIC_USER_RSP(BT_UUID_DECLARE_16(BT_UUID_RANG_DT_OV_WR_VAL), + GATT_PROP_NOTIFY | GATT_PROP_INDICATE, GATT_PERM_READ, + NULL, NULL, RAS_RANGING_DATA_OVER_WRITE_ATTR_ID), + GATT_H_CCCD(GATT_PERM_READ | GATT_PERM_WRITE, ras_over_write_ccc_changed_cb, RAS_RANGING_DATA_OVER_WRITE_CCC_ID), +}; + +static gatt_srv_db_t ras_service_db = { + .attr_db = ras_attr_db, + .attr_num = sizeof(ras_attr_db) / sizeof(gatt_attr_db_t), +}; + +static uint16_t ras_real_time_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return 0; + } + + if (gatts_info && gatts_info->cb->cfg_cb) { + gatts_info->cb->cfg_cb(addr, RAS_RTT_DATA_CCC_CFG_CHANGE_EVT, value, length); + } + + return length; +} + +static uint16_t ras_on_demand_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return 0; + } + + if (gatts_info && gatts_info->cb->cfg_cb) { + gatts_info->cb->cfg_cb(addr, RAS_ON_DEMAND_DATA_CCC_CFG_CHANGE_EVT, value, length); + } + + return length; +} + +static uint16_t ras_control_point_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return 0; + } + + if (gatts_info && gatts_info->cb->cfg_cb) { + gatts_info->cb->cfg_cb(addr, RAS_CTR_PT_CCC_CFG_CHANGE_EVT, value, length); + } + + return length; +} + +static uint16_t ras_data_ready_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return 0; + } + + if (gatts_info && gatts_info->cb->cfg_cb) { + gatts_info->cb->cfg_cb(addr, RAS_DATA_READY_CCC_CFG_CHANGE_EVT, value, length); + } + + return length; +} + +static uint16_t ras_over_write_ccc_changed_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return 0; + } + + if (gatts_info && gatts_info->cb->cfg_cb) { + gatts_info->cb->cfg_cb(addr, RAS_OVER_WRITE_CCC_CFG_CHANGE_EVT, value, length); + } + + return length; +} + +uint16_t ras_control_point_write_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, + const uint8_t* value, uint16_t length, uint16_t offset) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return 0; + } + + if (gatts_info && gatts_info->cb->pt_write_cb) { + gatts_info->cb->pt_write_cb(addr, value, length); + } + + return length; +} + +static uint16_t ras_feature_read_cb(void* srv_handle, bt_address_t* addr, uint16_t attr_handle, uint32_t req_handle) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return 0; + } + + if (gatts_info && gatts_info->cb->cfg_cb) { + gatts_info->cb->feature_read_cb(addr, req_handle); + } + + return 0; +} + +bt_status_t ras_send_feature_read_rsp(bt_address_t* addr, uint32_t feature) +{ + if (gatts_info->ras_gatts_handle == NULL) { + BT_LOGE("Invalid gatts handle."); + return BT_STATUS_FAIL; + } + + uint8_t feature_val[4]; + uint8_t *p = feature_val; + CS_UINT32_TO_BYTE_STREAM(p, feature); + bt_status_t status = bt_gatts_response(gatts_info->ras_gatts_handle, addr, RAS_RANGING_FEATURE_ATTR_ID, + feature_val, sizeof(feature_val)); + + return status; +} + +static gatts_callbacks_t ras_gatts_callbacks = { + .size = sizeof(ras_gatts_callbacks), + .on_connected = cs_ras_gatts_connected_cb, + .on_disconnected = cs_ras_gatts_disconnected_cb, + .on_attr_table_added = NULL, + .on_attr_table_removed = NULL, + .on_notify_complete = cs_ras_notify_complete_cb, + .on_mtu_changed = cs_ras_mtu_change_cb, + .on_phy_read = NULL, + .on_phy_updated = NULL, + .on_conn_param_changed = NULL, +}; + +static void ras_gatts_register(void) +{ + const gatts_interface_t* interface = gatts_info->ras_gatts_interface; + + BT_LOGD("%s", __func__); + + if (gatts_info->ras_gatts_handle) { + BT_LOGW("The RAS gatt Server has been register."); + return; + } + + interface->register_service(NULL, &gatts_info->ras_gatts_handle, &ras_gatts_callbacks); + if (!gatts_info->ras_gatts_handle) { + BT_LOGE("%s, failed to register service", __func__); + return; + } + + if (interface->add_attr_table(gatts_info->ras_gatts_handle, &ras_service_db) + != BT_STATUS_SUCCESS) { + BT_LOGE("%s, failed to add attribute", __func__); + return; + } + + BT_LOGD("%s, wait for service registered", __func__); +} + +static void gatts_unregister(void) +{ + const gatts_interface_t* interface = gatts_info->ras_gatts_interface; + + BT_LOGD("%s", __func__); + + if (gatts_info->ras_gatts_handle) { + interface->unregister_service(gatts_info->ras_gatts_handle); + gatts_info->ras_gatts_handle = NULL; + } +} + +void bt_cs_ras_gatts_init(const ras_gatts_callbacks_t* callback) +{ + BT_LOGD("%s", __func__); + + if (!gatts_info) + gatts_info = zalloc(sizeof(ras_gatt_info_t)); + + gatts_info->ras_gatts_interface = (gatts_interface_t*)service_manager_get_profile(PROFILE_GATTS); + + if (gatts_info->ras_gatts_interface == NULL) { + BT_LOGE("ras gatts interface get fail."); + return; + } + + gatts_info->cb = callback; + + ras_gatts_register(); +} + +void ras_gatts_deinit(void) +{ + BT_LOGD("%s", __func__); + + if (!gatts_info) + return; + + gatts_unregister(); + free(gatts_info); + gatts_info = NULL; +} + +void cs_ras_gatts_connected_cb(gatts_handle_t srv_handle, bt_address_t* addr) +{ + cs_msg_t* msg = cs_msg_new(CONNECTED_EVT, addr); + bt_sal_cs_event_callback(msg); + + if (gatts_info && gatts_info->cb->conn_cb) { + gatts_info->cb->conn_cb(addr); + } + + return; +} + +void cs_ras_gatts_disconnected_cb(gatts_handle_t srv_handle, bt_address_t* addr) +{ + cs_msg_t* msg = cs_msg_new(DISCONNECTED_EVT, addr); + bt_sal_cs_event_callback(msg); + + if (gatts_info && gatts_info->cb->disconn_cb) { + gatts_info->cb->disconn_cb(addr); + } + + return; +} + +static void cs_ras_notify_complete_cb(gatts_handle_t srv_handle, bt_address_t* addr, gatt_status_t status, uint16_t attr_handle) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return; + } + + ras_attr_notify_t attr_ntf; + switch (attr_handle) + { + case RAS_RANGING_REAL_TIME_CCC_ID: + attr_ntf = RAS_REAL_TIME_CHAR_SEND; + break; + case RAS_RANGING_ON_DEMAND_CCC_ID: + attr_ntf = RAS_ON_DEMAND_CHAR_SEND; + break; + case RAS_RANGING_CONTROL_POINT_CCC_ID: + attr_ntf = RAS_CONTROL_POINT_CHAR_SEND; + break; + case RAS_RANGING_DATA_READY_CCC_ID: + attr_ntf = RAS_DATA_READY_CHAR_SEND; + break; + case RAS_RANGING_DATA_OVER_WRITE_CCC_ID: + attr_ntf = RAS_OVER_WRITE_CHAR_SEND; + break; + default: + BT_LOGE("Invalid attr_handle:%d", attr_handle); + return; + } + + if (gatts_info && gatts_info->ras_gatts_handle && gatts_info->cb->notify_cb) { + gatts_info->cb->notify_cb(addr, status, attr_ntf); + } + + return; +} + +static void cs_ras_mtu_change_cb(gatts_handle_t srv_handle, bt_address_t* addr, uint32_t mtu) +{ + if (gatts_info->ras_gatts_handle != srv_handle) { + BT_LOGE("srv_handle(%p) not equal to gatts_handle(%p)", srv_handle, gatts_info->ras_gatts_handle); + return; + } + + if (gatts_info && gatts_info->cb->mtu_updated_cb) { + gatts_info->cb->mtu_updated_cb(addr, mtu); + } + + return; +} + +bt_status_t ras_gatts_data_send_notify(ras_attr_notify_t attr, bt_address_t* addr, + uint8_t* value, uint16_t len, bool is_notify) +{ + if (!gatts_info || !value || !addr) { + BT_LOGE("Invalid params"); + return BT_STATUS_PARM_INVALID; + } + + uint16_t attr_handle; + switch(attr) { + case RAS_REAL_TIME_CHAR_SEND: + attr_handle = RAS_RANGING_REAL_TIME_ATTR_ID; + break; + case RAS_ON_DEMAND_CHAR_SEND: + attr_handle = RAS_RANGING_ON_DEMAND_ATTR_ID; + break; + case RAS_DATA_READY_CHAR_SEND: + attr_handle = RAS_RANGING_DATA_READY_ATTR_ID; + break; + case RAS_CONTROL_POINT_CHAR_SEND: + attr_handle = RAS_RANGING_CONTROL_POINT_ATTR_ID; + break; + case RAS_OVER_WRITE_CHAR_SEND: + attr_handle = RAS_RANGING_DATA_OVER_WRITE_ATTR_ID; + break; + default: + BT_LOGE("Invalid attr_ntf type:%d", attr); + return BT_STATUS_FAIL; + } + + bt_status_t status = (is_notify ? (gatts_info->ras_gatts_interface->notify(gatts_info->ras_gatts_handle, addr, attr_handle, value, len)) :\ + gatts_info->ras_gatts_interface->indicate(gatts_info->ras_gatts_handle, addr, attr_handle, value, len)); + return status; +} + diff --git a/service/profiles/cs/cs_ras_test.c b/service/profiles/cs/cs_ras_test.c new file mode 100644 index 000000000..6f48d33f5 --- /dev/null +++ b/service/profiles/cs/cs_ras_test.c @@ -0,0 +1,598 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#include "cs_ras.h" +#include "cs_ras_test.h" +#include "cs_ras_gatts.h" +#include "cs_service.h" +#include "utils/log.h" + +/** + * @brief LE CS Subevent Result Event + * + * This event is generated when the local Controller has results to report for a CS + * (Channel Sounding) subevent during the CS procedure. Depending on the number of CS steps in the CS + * subevent, the Controller may choose to report complete or partial results. If the number of CS steps exceeds + * the maximum HCI event size, the Controller may report further results using the HCI_LE_CS_Subevent_Result_Continue event. + * + * **General Overview**: + * - When the `Connection_Handle` is set to 0x0FFF, the `Config_ID` and `Start_ACL_Conn_Event_Counter` parameters + * are ignored. + * - The `Start_ACL_Conn_Event_Counter` parameter indicates the starting ACL connection event count to which the CS + * event results reported in this HCI event are anchored. + * - The `Procedure_Counter` parameter indicates the associated CS procedure count for the results reported in this event. + * - The `Frequency_Compensation` parameter is a value used by the initiator device to align the timing of CS steps and + * transmit frequencies. + * - The `Reference_Power_Level` is the power level in dBm used during the CS procedure. If the reference power level + * is not available during a subevent, it is set to `0x7F`. + * - The event can report either complete or partial results, and more results can be sent in continuation events when the + * CS steps exceed the maximum event size. + * - The event also includes various parameters related to the CS steps, such as the number of steps, step modes, channels, + * and specific data for each step. + * + * **Abort Reason**: + * - The `Abort_Reason` field specifies why the CS procedure or subevent was aborted, if applicable. + * + * **Event Parameters**: + * + * - `Subevent_Code` (1 octet) : + * - Value `0x31` indicates this is an HCI_LE_CS_Subevent_Result event. + * + * - `Connection_Handle` (2 octets): + * - `0x0000 to 0x0EFF`: Connection handle associated with the ACL connection. + * - `0x0FFF`: Indicates a CS test; other values are reserved for future use. + * + * - `Config_ID` (1 octet): + * - A configuration identifier for the CS procedure. The value ranges from `0` to `3`. + * + * - `Start_ACL_Conn_Event_Counter` (2 octets): + * - Indicates the starting ACL connection event count for the results in this event. + * + * - `Procedure_Counter` (2 octets): + * - CS procedure count indicating the number of procedures since the CS procedure started. + * - Range: `0x0000` to `0xFFFF`. + * + * - `Frequency_Compensation` (2 octets): + * - Fractional frequency offset (FFO) in units of `0.01 ppm` (signed 15-bit integer). + * - Range: `-100 ppm (0x58F0)` to `+100 ppm (0x2710)`. + * - `0xC000`: Frequency compensation not available (or the role is not initiator). + * + * - `Reference_Power_Level` (1 octet): + * - Reference power level in dBm. + * - Range: `-127 to 20 dBm`. + * - `0x7F`: Reference power level not applicable. + * + * - `Procedure_Done_Status` (1 octet): + * - Indicates the status of the CS procedure. + * - Bits 0-3: + * - `0x0`: All results complete. + * - `0x1`: Partial results, more to follow. + * - `0xF`: Procedure aborted. + * - Bits 4-7: Reserved for future use. + * + * - `Subevent_Done_Status` (1 octet): + * - Indicates the status of the CS subevent. + * - Bits 0-3: + * - `0x0`: All results complete. + * - `0x1`: Partial results, more to follow. + * - `0xF`: Subevent aborted. + * - Bits 4-7: Reserved for future use. + * + * - `Abort_Reason` (1 octet): + * - Specifies the reason for aborting the procedure or subevent, if applicable. + * - Bits 0-3: Abort reason for the procedure. + * - Bits 4-7: Abort reason for the subevent. + * + * - `Num_Antenna_Paths` (1 octet): + * - Indicates the number of antenna paths used during the CS procedure. + * - Range: `0x00` (no phase measurement) to `0x04` (up to four antenna paths). + * + * - `Num_Steps_Reported` (1 octet): + * - Number of CS steps reported in this event. + * - Range: `0x00` to `0xA0` (up to 160 steps). + * + * - `Step_Mode[i]` (Num_Steps_Reported × 1 octet): + * - The CS mode for each CS step. + * - Values: `0x00 to 0x03` (various modes). + * + * - `Step_Channel[i]` (Num_Steps_Reported × 1 octet): + * - The channel used for each CS step. + * - Valid channels: `0x00 to 0x4E` (refer to [Vol 6] Part A, Section 2 for details). + * + * - `Step_Data_Length[i]` (Num_Steps_Reported × 1 octet): + * - The length of mode- and role-specific information for each step. + * - Range: `0x00 to 0xFF`. + * + * - `Step_Data[i]` (Σi(Step_Data_Length[i]) octets): + * - The mode- and role-specific information for each step. + * - The structure of this data varies based on the CS mode and role of the local device. + * + * **Procedure Done and Subevent Done Status Combinations**: + * The valid combinations of `Procedure_Done_Status` and `Subevent_Done_Status` are: + * + * | Procedure_Done_Status (Bits 0 to 3) | Allowed Subevent_Done_Status (Bits 0 to 3) | + * |------------------------------------|-------------------------------------------| + * | `0x0` | `0x0`, `0xF` | + * | `0x1` | `0x0`, `0x1`, `0xF` | + * | `0xF` | `0x0`, `0xF` | + * + * **Mode Role Specific Information**: + * The information reported in `Step_Data` varies based on the CS mode and the role of the device. + * + * - **Mode 0** (Initiator): + * - `Packet_Quality`, `Packet_RSSI`, `Packet_Antenna`, `Measured_Freq_Offset` + * - **Mode 0** (Reflector): + * - `Packet_Quality`, `Packet_RSSI`, `Packet_Antenna` + * - **Mode 1** (Initiator): + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `ToA_ToD_Initiator`, `Packet_Antenna` + * - **Mode 1** (Reflector): + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `ToD_ToA_Reflector`, `Packet_Antenna` + * - **Mode 2** (Initiator or Reflector): + * - `Antenna_Permutation_Index`, `Tone_PCT[k]`, `Tone_Quality_Indicator[k]` + * - **Mode 3** (Initiator): + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `ToA_ToD_Initiator`, `Packet_Antenna`, + * `Antenna_Permutation_Index`, `Tone_PCT[k]`, `Tone_Quality_Indicator[k]` + * - **Mode 3** (Reflector): + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `ToD_ToA_Reflector`, `Packet_Antenna`, + * `Antenna_Permutation_Index`, `Tone_PCT[k]`, `Tone_Quality_Indicator[k]` + * + * **Mode and Role Specific Information Parameters**: + * For each CS mode, specific parameters like `Packet_Quality`, `Packet_RSSI`, `Packet_Antenna`, etc., are included. + * + * **Tone Quality Indicator**: + * - Bits 0-3: Tone quality values (`0x0` to `0x3`). + * - Bits 4-7: Tone extension slot information (`0x0` to `0x2`). + * + * **Other Parameters**: + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `Packet_Antenna`: These parameters represent the quality of the communication packet, + * the NADM (Normalized Antenna Diversity Measure), received signal strength indication (RSSI), and the antenna used for the transmission. + * + * **Example of Event Data**: + * - If the number of reported CS steps exceeds the maximum HCI event size, multiple continuation events may follow. + */ +static uint8_t ras_sub_data_test[] = { + 0x00, 0x0c, 0x03, 0x00, 0xda, 0x01, 0x00, 0x14, 0x03, 0x00, 0xd9, 0x01, 0x02, 0x1a, 0x09, 0x00, 0xdf, 0xe2, 0x11, 0x00, + 0xb4, 0x72, 0x17, 0x20, 0x02, 0x1f, 0x09, 0x00, 0x44, 0xd2, 0x1e, 0x00, 0xe7, 0x91, 0x23, 0x20, 0x02, 0x25, 0x09, 0x00, 0x3e, + 0xbe, 0x12, 0x00, 0x23, 0x1e, 0x10, 0x20, 0x02, 0x45, 0x09, 0x00, 0xdb, 0xff, 0x13, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x31, + 0x09, 0x00, 0xa3, 0xf1, 0x11, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x4c, 0x09, 0x00, 0xa5, 0x10, 0x01, 0x00, 0xa1, 0xe0, + 0x02, 0x20, 0x02, 0x29, 0x09, 0x00, 0x3a, 0x01, 0xeb, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x32, 0x09, 0x00, 0x9e, 0x61, 0x18, + 0x00, 0x74, 0xb1, 0x1a, 0x20, 0x02, 0x0e, 0x09, 0x00, 0x4a, 0xc0, 0x48, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x44, 0x09, + 0x00, 0xb4, 0x50, 0x12, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x04, 0x09, 0x00, 0x77, 0xea, 0x03, 0x00, 0x74, 0xba, 0xfa, 0x20, + 0x02, 0x13, 0x09, 0x00, 0x1f, 0x6d, 0xce, 0x00, 0xc6, 0x0d, 0xc7, 0x20, 0x02, 0x4b, 0x09, 0x00, 0x8f, 0xdf, 0xf4, 0x00, + 0xb2, 0x9f, 0xf3, 0x20, 0x02, 0x39, 0x09, 0x00, 0x16, 0x63, 0xf4, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x2f, 0x09, 0x00, + 0x26, 0x41, 0x11, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x10, 0x09, 0x00, 0xab, 0x01, 0xbf, 0x00, 0x46, 0x42, 0xc3, 0x20, 0x02, + 0x22, 0x09, 0x00, 0x27, 0x42, 0x07, 0x00, 0x12, 0xe2, 0x0b, 0x20, 0x02, 0x20, 0x09, 0x00, 0xaa, 0x8e, 0x25, 0x00, 0x4f, 0xde, + 0x21, 0x20, 0x02, 0x48, 0x09, 0x00, 0xe1, 0x7e, 0xfd, 0x00, 0xf1, 0xce, 0xf9, 0x20, 0x02, 0x27, 0x09, 0x00, 0x87, 0xcf, + 0x1d, 0x01, 0x0f, 0x0f, 0x1a, 0x21, 0x02, 0x3e, 0x09, 0x00, 0x45, 0xef, 0x28, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x42, 0x09, + 0x00, 0x29, 0x81, 0xed, 0x00, 0x5c, 0xd1, 0xf0, 0x20, 0x02, 0x33, 0x09, 0x00, 0x6c, 0xf2, 0x0c, 0x00, 0x00, 0x00, 0x00, + 0x12, 0x02, 0x02, 0x09, 0x00, 0x5a, 0x2a, 0xd6, 0x00, 0xcd, 0x8a, 0xc9, 0x20, 0x02, 0x1b, 0x09, 0x00, 0x01, 0xbf, 0xd2, 0x00, + 0xa8, 0xaf, 0xd0, 0x20, 0x02, 0x12, 0x09, 0x00, 0x5d, 0x1e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x0b, 0x09, 0x00, + 0x1b, 0xd2, 0x44, 0x00, 0x8d, 0x91, 0x47, 0x21, 0x02, 0x3f, 0x09, 0x00, 0x2d, 0xa2, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, + 0x3c, 0x09, 0x00, 0x1d, 0x0e, 0x29, 0x00, 0xb2, 0x6d, 0x23, 0x20, 0x02, 0x30, 0x09, 0x00, 0x3e, 0xce, 0x05, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x06, 0x09, 0x00, 0xa4, 0xb1, 0xb2, 0x00, 0x6a, 0x62, 0xb8, 0x20, 0x02, 0x40, 0x09, 0x00, 0xb2, 0x5e, + 0xe5, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x2e, 0x09, 0x00, 0x61, 0x30, 0x17, 0x00, 0x30, 0x60, 0x17, 0x20, 0x02, 0x37, + 0x09, 0x00, 0x92, 0x11, 0xd4, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x2d, 0x09, 0x00, 0x2e, 0xbf, 0xec, 0x00, 0x4f, 0xaf, 0xeb, + 0x20, 0x02, 0x05, 0x09, 0x00, 0x54, 0xf5, 0x0d, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x49, 0x09, 0x00, 0xe4, 0xce, 0xfb, + 0x01, 0xed, 0x6e, 0xf9, 0x20, 0x02, 0x21, 0x09, 0x00, 0xda, 0xef, 0x25, 0x00, 0x76, 0x7f, 0x24, 0x20, 0x02, 0x26, 0x09, 0x00, + 0x60, 0xff, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x41, 0x09, 0x00, 0xda, 0x60, 0xe6, 0x00, 0x0d, 0x51, 0xe9, 0x21, + 0x02, 0x43, 0x09, 0x00, 0xb5, 0x10, 0xec, 0x00, 0xe7, 0xc0, 0xee, 0x21, 0x02, 0x2a, 0x09, 0x00, 0xd0, 0x9e, 0xef, 0x01, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x3b, 0x09, 0x00, 0xff, 0xae, 0x32, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x4a, 0x09, 0x00, 0x31, + 0xdf, 0xf4, 0x00, 0x45, 0x9f, 0xf3, 0x20, 0x02, 0x14, 0x09, 0x00, 0x8e, 0x9c, 0xe2, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x47, + 0x09, 0x00, 0x8a, 0xb0, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x07, 0x09, 0x00, 0x4f, 0x81, 0xb5, 0x00, 0xcb, 0x71, + 0xb8, 0x20, 0x02, 0x09, 0x09, 0x00, 0x4e, 0x44, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x35, 0x09, 0x00, 0xf0, 0x40, 0xd1, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x0c, 0x09, 0x00, 0xfd, 0x4f, 0x4b, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x03, 0x09, + 0x00, 0x74, 0x33, 0xb5, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x1c, 0x09, 0x00, 0x88, 0x81, 0x28, 0x00, 0x3a, 0x61, 0x2b, 0x20, + 0x02, 0x08, 0x09, 0x00, 0x0d, 0xed, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x24, 0x09, 0x00, 0x6b, 0x7e, 0xec, 0x00, + 0x00, 0x00, 0x00, 0x12, 0x02, 0x16, 0x09, 0x00, 0xdf, 0x9f, 0x36, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x28, 0x09, 0x00, 0x38, + 0x1e, 0x03, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x3a, 0x09, 0x00, 0xa0, 0xae, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, + 0x11, 0x09, 0x00, 0x84, 0x9d, 0xc8, 0x00, 0xd4, 0xbd, 0xc5, 0x20, 0x02, 0x1d, 0x09, 0x00, 0x8d, 0x50, 0xd1, 0x00, 0x00, 0x00, + 0x00, 0x12, 0x02, 0x34, 0x09, 0x00, 0x2f, 0x3d, 0x08, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x2c, 0x09, 0x00, 0x78, 0x1f, + 0x16, 0x00, 0x4f, 0xaf, 0x14, 0x20, 0x02, 0x0f, 0x09, 0x00, 0xcc, 0xff, 0xb9, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x0a, 0x09, + 0x00, 0xdf, 0x32, 0x3a, 0x00, 0x8e, 0xa2, 0x3d, 0x20, 0x02, 0x38, 0x09, 0x00, 0x7c, 0x10, 0xc9, 0x02, 0x6f, 0x30, 0xcc, + 0x22, 0x02, 0x15, 0x09, 0x00, 0x9b, 0x6d, 0x2b, 0x00, 0x6b, 0xbd, 0x28, 0x21, 0x02, 0x2b, 0x09, 0x00, 0x12, 0xa1, 0xee, 0x00, + 0x42, 0x61, 0xf2, 0x20, 0x02, 0x3d, 0x09, 0x00, 0x43, 0xb2, 0x20, 0x02, 0x00, 0x00, 0x00, 0x12, 0x02, 0x23, 0x09, 0x00, + 0x3f, 0x41, 0x18, 0x00, 0x02, 0x01, 0x1b, 0x20, 0x02, 0x46, 0x09, 0x00, 0x12, 0xc1, 0xf9, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, + 0x0d, 0x09, 0x00, 0x30, 0xee, 0x42, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x1e, 0x09, 0x00, 0xd1, 0xc1, 0xdc, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x36, 0x09, 0x00, 0x32, 0x0d, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x12 +}; + +static uint8_t ras_recv_first_seg_data_test[] = { + 0x01, + 0xa0, + 0x00, + 0x00, + 0x01, + 0x00, + 0x03, + 0xc0, + 0x00, + 0x00, + 0x00, + 0x00, + 0x4a, + 0x00, + 0x00, + 0xda, + 0x01, + 0x00, + 0x00, + 0xd9, + 0x01, + 0x02, + 0x00, + 0xdf, + 0xe2, + 0x11, + 0x00, + 0xb4, + 0x72, + 0x17, + 0x20, + 0x02, + 0x00, + 0x44, + 0xd2, + 0x1e, + 0x00, + 0xe7, + 0x91, + 0x23, + 0x20, + 0x02, + 0x00, + 0x3e, + 0xbe, + 0x12, + 0x00, + 0x23, + 0x1e, + 0x10, + 0x20, + 0x02, + 0x00, + 0xdb, + 0xff, + 0x13, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0xa3, + 0xf1, + 0x11, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0xa5, + 0x10, + 0x01, + 0x00, + 0xa1, + 0xe0, + 0x02, + 0x20, + 0x02, + 0x00, + 0x3a, + 0x01, + 0xeb, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0x9e, + 0x61, + 0x18, + 0x00, + 0x74, + 0xb1, + 0x1a, + 0x20, + 0x02, + 0x00, + 0x4a, + 0xc0, + 0x48, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0xb4, + 0x50, + 0x12, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0x77, + 0xea, + 0x03, + 0x00, + 0x74, + 0xba, + 0xfa, + 0x20, + 0x02, + 0x00, + 0x1f, + 0x6d, + 0xce, + 0x00, + 0xc6, + 0x0d, + 0xc7, + 0x20, + 0x02, + 0x00, + 0x8f, + 0xdf, + 0xf4, + 0x00, + 0xb2, + 0x9f, + 0xf3, + 0x20, + 0x02, + 0x00, + 0x16, + 0x63, + 0xf4, + 0x01, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0x26, + 0x41, + 0x11, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0xab, + 0x01, + 0xbf, + 0x00, + 0x46, + 0x42, + 0xc3, + 0x20, + 0x02, + 0x00, + 0x27, + 0x42, + 0x07, + 0x00, + 0x12, + 0xe2, + 0x0b, + 0x20, + 0x02, + 0x00, + 0xaa, + 0x8e, + 0x25, + 0x00, + 0x4f, + 0xde, + 0x21, + 0x20, + 0x02, + 0x00, + 0xe1, + 0x7e, + 0xfd, + 0x00, + 0xf1, + 0xce, + 0xf9, + 0x20, + 0x02, + 0x00, + 0x87, + 0xcf, + 0x1d, + 0x01, + 0x0f, + 0x0f, + 0x1a, + 0x21, + 0x02, + 0x00, + 0x45, + 0xef, + 0x28, + 0x01, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0x29, + 0x81, + 0xed, + 0x00, + 0x5c, + 0xd1, + 0xf0, + 0x20, + 0x02, + 0x00, + 0x6c, + 0xf2, + 0x0c, + 0x00, + 0x00, + 0x00, +}; + +static uint8_t ras_recv_second_seg_data_test[] = { + 0x04, 0x00, 0x12, 0x02, 0x00, 0x5a, 0x2a, 0xd6, 0x00, 0xcd, 0x8a, 0xc9, 0x20, 0x02, 0x00, 0x01, 0xbf, 0xd2, 0x00, 0xa8, + 0xaf, 0xd0, 0x20, 0x02, 0x00, 0x5d, 0x1e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x1b, 0xd2, 0x44, 0x00, 0x8d, + 0x91, 0x47, 0x21, 0x02, 0x00, 0x2d, 0xa2, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x1d, 0x0e, 0x29, 0x00, 0xb2, + 0x6d, 0x23, 0x20, 0x02, 0x00, 0x3e, 0xce, 0x05, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xa4, 0xb1, 0xb2, 0x00, 0x6a, + 0x62, 0xb8, 0x20, 0x02, 0x00, 0xb2, 0x5e, 0xe5, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x61, 0x30, 0x17, 0x00, 0x30, + 0x60, 0x17, 0x20, 0x02, 0x00, 0x92, 0x11, 0xd4, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x2e, 0xbf, 0xec, 0x00, 0x4f, + 0xaf, 0xeb, 0x20, 0x02, 0x00, 0x54, 0xf5, 0x0d, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xe4, 0xce, 0xfb, 0x01, 0xed, + 0x6e, 0xf9, 0x20, 0x02, 0x00, 0xda, 0xef, 0x25, 0x00, 0x76, 0x7f, 0x24, 0x20, 0x02, 0x00, 0x60, 0xff, 0x1b, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x00, 0xda, 0x60, 0xe6, 0x00, 0x0d, 0x51, 0xe9, 0x21, 0x02, 0x00, 0xb5, 0x10, 0xec, 0x00, 0xe7, + 0xc0, 0xee, 0x21, 0x02, 0x00, 0xd0, 0x9e, 0xef, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xff, 0xae, 0x32, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x00, 0x31, 0xdf, 0xf4, 0x00, 0x45, 0x9f, 0xf3, 0x20, 0x02, 0x00, 0x8e, 0x9c, 0xe2, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x00, 0x8a, 0xb0, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x4f, 0x81, 0xb5, 0x00, 0xcb, + 0x71, 0xb8, 0x20, 0x02, 0x00, 0x4e, 0x44, 0x1b, 0x00 +}; + +static uint8_t ras_recv_last_seg_data_test[] = { + 0x0a, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xf0, 0x40, 0xd1, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xfd, 0x4f, 0x4b, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x74, 0x33, 0xb5, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x88, 0x81, 0x28, + 0x00, 0x3a, 0x61, 0x2b, 0x20, 0x02, 0x00, 0x0d, 0xed, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x6b, 0x7e, 0xec, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xdf, 0x9f, 0x36, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x38, 0x1e, 0x03, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xa0, 0xae, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x84, 0x9d, 0xc8, + 0x00, 0xd4, 0xbd, 0xc5, 0x20, 0x02, 0x00, 0x8d, 0x50, 0xd1, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x2f, 0x3d, 0x08, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x78, 0x1f, 0x16, 0x00, 0x4f, 0xaf, 0x14, 0x20, 0x02, 0x00, 0xcc, 0xff, 0xb9, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xdf, 0x32, 0x3a, 0x00, 0x8e, 0xa2, 0x3d, 0x20, 0x02, 0x00, 0x7c, 0x10, 0xc9, + 0x02, 0x6f, 0x30, 0xcc, 0x22, 0x02, 0x00, 0x9b, 0x6d, 0x2b, 0x00, 0x6b, 0xbd, 0x28, 0x21, 0x02, 0x00, 0x12, 0xa1, 0xee, + 0x00, 0x42, 0x61, 0xf2, 0x20, 0x02, 0x00, 0x43, 0xb2, 0x20, 0x02, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x3f, 0x41, 0x18, + 0x00, 0x02, 0x01, 0x1b, 0x20, 0x02, 0x00, 0x12, 0xc1, 0xf9, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x30, 0xee, 0x42, + 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xd1, 0xc1, 0xdc, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x32, 0x0d, 0x1b, + 0x00, 0x00, 0x00, 0x00, 0x12 +}; + +static ras_testcase_t test_case = RAS_TESTCASE_ON_DEMAND_WRITE_RANG_DATA_TIMWOUT_010; + +static uint8_t test_status = RAS_TEST_SUCESS; + +int bt_gatt_notify_cb_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len) +{ + switch (test_case) { + case RAS_TESTCASE_ON_DEMAND_NOTIFY_VALID_RANG_DATA_003: + case RAS_TESTCASE_ON_DEMAND_INDICATE_VALID_RANG_DATA_004: { + if (attr == RAS_ON_DEMAND_CHAR_SEND) { + ras_on_demand_notify_finish_test(addr); + } else if (attr == RAS_CONTROL_POINT_CHAR_SEND) { + uint8_t opcode = *(uint8_t*)value; + if (opcode == SAL_LE_RAS_CTL_OP_RSP_CMP_RANG_DATA) { + uint8_t buf[3] = { 0 }; + buf[0] = SAL_LE_RAS_CTL_OP_CMD_ACK_RANG_DATA; + memcpy(&buf[1], (uint8_t*)value + 1, 2); + ras_ctrl_point_send_test(NULL, buf, sizeof(buf)); + } + } + break; + } + default: + break; + } + return 0; +} + +ssize_t bt_gatt_attr_read_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len) +{ + return 0; +} + +int bt_gatt_notify_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len) +{ + switch (test_case) { + case RAS_TESTCASE_REAL_TIME_NOTIFY_VALID_RANG_DATA_001: + if (memcmp(value, ras_recv_first_seg_data_test, sizeof(ras_recv_first_seg_data_test)) != 0 && + memcmp(value, ras_recv_second_seg_data_test, sizeof(ras_recv_second_seg_data_test)) != 0 && + memcmp(value, ras_recv_last_seg_data_test, sizeof(ras_recv_last_seg_data_test)) != 0) { + test_status = RAS_TEST_FAIL; + BT_LOGE("---- No segment match the test data. -----"); + return -1; + } + + break; + case RAS_TESTCASE_ON_DEMAND_NOTIFY_VALID_RANG_DATA_003: + case RAS_TESTCASE_ON_DEMAND_INDICATE_VALID_RANG_DATA_004: { + if (attr == RAS_CONTROL_POINT_CHAR_SEND) { + uint8_t buf[3] = { 0 }; + uint16_t count = 10; + buf[0] = SAL_LE_RAS_CTL_OP_CMD_GET_RANG_DATA; + ras_put_uint16_to_ptr(count, &buf[1]); + ras_ctrl_point_send_test(NULL, buf, sizeof(buf)); + } + + break; + } + default: + break; + } + return 0; +} + +int bt_gatt_indicate_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len) +{ + ras_on_demand_indicate_finish_test(addr, attr); + return 0; +} + +int cs_ras_subevent_recv_test(void* data, uint16_t len) +{ + bt_srv_conn_le_cs_subevent_result_t* result = (bt_srv_conn_le_cs_subevent_result_t*)malloc( + sizeof(bt_srv_conn_le_cs_subevent_result_t) + sizeof(ras_sub_data_test)); + + result->header.config_id = 0; + result->header.start_acl_conn_event = 0x03; + result->header.procedure_counter = 0xa; + result->header.frequency_compensation = 0xc000; + result->header.reference_power_level = 0x00; + result->header.procedure_done_status = 0x00; + result->header.subevent_done_status = 0x00; + result->header.procedure_abort_reason = 0x00; + result->header.subevent_abort_reason = 0x00; + result->header.num_antenna_paths = 0x01; + result->header.abort_step = 0x00; + result->len = sizeof(ras_sub_data_test); + memcpy(result->step_data_buf, ras_sub_data_test, sizeof(ras_sub_data_test)); + + ras_rang_mode_t mode = SAL_LE_RAS_RANGING_MODE_ON_DEMAND; + + test_status = RAS_TEST_SUCESS; + + int err = ras_subevent_recv_test(mode, test_case, NULL, result); + + if (err != 0 || test_status == RAS_TEST_FAIL) { + BT_LOGE("-------- CS TEST Fail, case(%d), err:%d -------", test_case, err); + return -1; + } + + BT_LOGE("-------- CS TEST Success, case(%d) -------", test_case); + return err; +} diff --git a/service/profiles/cs/cs_ras_util.c b/service/profiles/cs/cs_ras_util.c new file mode 100644 index 000000000..cc8194213 --- /dev/null +++ b/service/profiles/cs/cs_ras_util.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + #include "cs_ras_util.h" + + const char *cs_log_to_hex_str(const void *buf, size_t len) +{ + static const char hex[] = "0123456789abcdef"; + static char str[1024]; + const uint8_t *b = buf; + size_t i; + + len = MIN(len, (sizeof(str) - 1) / 2); + + for (i = 0; i < len; i++) { + str[i * 2] = hex[b[i] >> 4]; + str[i * 2 + 1] = hex[b[i] & 0xf]; + } + + str[i * 2] = '\0'; + + return str; +} \ No newline at end of file diff --git a/service/profiles/cs/cs_service.c b/service/profiles/cs/cs_service.c new file mode 100644 index 000000000..8abfca769 --- /dev/null +++ b/service/profiles/cs/cs_service.c @@ -0,0 +1,417 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#define LOG_TAG "cs_service" + +#include +#include +#include +#include + +#include "adapter_internel.h" +#include "bt_addr.h" +#include "bt_cs.h" +#include "bt_list.h" +#include "callbacks_list.h" +#include "cs_msg.h" +#include "cs_ras.h" +#include "cs_ras_gatts.h" +#include "cs_ras_test.h" +#include "cs_service.h" +#include "cs_state_machine.h" +#include "gatts_service.h" +#include "power_manager.h" +#include "service_loop.h" +#include "service_manager.h" +#include "utils/log.h" + +#ifdef CONFIG_BLUETOOTH_LE_CS + +#define CS_CALLBACK_FOREACH(_list, _cback, ...) \ + BT_CALLBACK_FOREACH(_list, cs_callbacks_t, _cback, ##__VA_ARGS__) + +typedef void (*subevent_result_cb_t)(bt_address_t* addr, bt_srv_conn_le_cs_subevent_result_t* result); + +static gatts_handle_t g_cs_handle = NULL; +typedef struct { + struct list_node list; + callbacks_list_t* callbacks; +} cs_servie_t; + +static cs_servie_t g_cs_service = { 0 }; +static subevent_result_cb_t result_cb = NULL; + +static void service_startup(profile_on_startup_t cb); +static void service_shutdown(profile_on_shutdown_t cb); +static const void* get_cs_profile_interface(void); + +static cs_device_t* cs_device_new(void* ctx, bt_address_t* bd_addr) +{ + cs_device_t* device; + cs_state_machine_t* cs_sm; + + device = (cs_device_t*)malloc(sizeof(cs_device_t)); + if (!device) + return NULL; + + memcpy(&device->bd_addr, bd_addr, sizeof(bt_address_t)); + cs_sm = cs_state_machine_new(ctx, bd_addr); + if (!cs_sm) { + BT_LOGE("Create state machine failed"); + free(device); + return NULL; + } + + device->cs_sm = cs_sm; + + return device; +} + +cs_device_t* find_cs_device_by_addr(struct list_node* list, bt_address_t* bd_addr) +{ + cs_device_t* device; + struct list_node* node; + + list_for_every(list, node) + { + device = (cs_device_t*)node; + if (memcmp(&device->bd_addr, bd_addr, sizeof(bt_address_t)) == 0) + return device; + } + + return NULL; +} + +static cs_device_t* find_or_create_device(bt_address_t* bd_addr) +{ + cs_device_t* device = find_cs_device_by_addr(&g_cs_service.list, bd_addr); + if (device) + return device; + + device = cs_device_new(&g_cs_service, bd_addr); + if (!device) { + BT_LOGE("CS new device alloc failed"); + return NULL; + } + list_add_tail(&g_cs_service.list, &device->node); + + return device; +} + +static cs_state_machine_t* get_state_machine(bt_address_t* bd_addr) +{ + cs_device_t* device = find_or_create_device(bd_addr); + + if (!device) + return NULL; + + return device->cs_sm; +} + +static void cs_service_handle_event(void* data) +{ + cs_msg_t* msg = (cs_msg_t*)data; + + switch (msg->id) { + case CS_STARTUP: + service_startup((profile_on_startup_t)msg->cs_data.cb); + break; + case CS_SHUTDOWN: + service_shutdown((profile_on_shutdown_t)msg->cs_data.cb); + break; + case CS_SUBEVENT_RESULT_EVT: + const bt_cs_interface_t* cs_if = (const bt_cs_interface_t*)get_cs_profile_interface(); + if (cs_if->subevent_result_callbacks) { + cs_if->subevent_result_callbacks(&msg->cs_data.bd_addr, msg->cs_data.data); + } + + free(msg->cs_data.data); + break; + default: { + cs_state_machine_t* cs_sm; + cs_sm = get_state_machine(&msg->cs_data.bd_addr); + if (!cs_sm) { + break; + } + + cs_state_machine_handle_event(cs_sm, msg); + break; + } break; + } + + cs_msg_destory(msg); +} + +static void do_in_cs_service(cs_msg_t* msg) +{ + if (msg == NULL) + return; + + do_in_service_loop(cs_service_handle_event, msg); +} + +// static void connect_callback(void* srv_handle, bt_address_t* addr) +// { +// cs_msg_t* msg = cs_msg_new(CONNECTED_EVT, NULL); +// msg->data.data = addr; +// do_in_cs_service(msg); + +// return BT_STATUS_SUCCESS; +// BT_LOGD("gatts_connect_callback, addr:%s", addr); +// } + +// static void disconnect_callback(void* srv_handle, bt_address_t* addr) +// { +// BT_LOGD("gatts_disconnect_callback, addr:%s", addr); +// } + +// static void attr_table_added_callback(void* srv_handle, gatt_status_t status, uint16_t attr_handle) +// { +// BT_LOGD("gatts add attribute table complete, handle 0x%" PRIx16 ", status:%d", attr_handle, status); +// } + +// static void attr_table_removed_callback(void* srv_handle, gatt_status_t status, uint16_t attr_handle) +// { +// BT_LOGD("gatts remove attribute table complete, handle 0x%" PRIx16 ", status:%d", attr_handle, status); +// } + +// static void notify_complete_callback(void* srv_handle, bt_address_t* addr, gatt_status_t status, uint16_t attr_handle) +// { +// if (status != GATT_STATUS_SUCCESS) { +// BT_LOGD("gatts service notify failed, addr:%s, handle 0x%" PRIx16 ", status:%d", addr, attr_handle, status); +// return; +// } +// } + +// static void mtu_changed_callback(void* srv_handle, bt_address_t* addr, uint32_t mtu) +// { +// BT_LOGD("gatts_mtu_changed_callback, addr:%s, mtu:%" PRIu32, addr, mtu); +// } + +// static void phy_read_callback(void* srv_handle, bt_address_t* addr, ble_phy_type_t tx_phy, ble_phy_type_t rx_phy) +// { +// BT_LOGD("gatts read phy complete, addr:%s, tx:%d, rx:%d", addr, tx_phy, rx_phy); +// } + +// static void phy_updated_callback(void* srv_handle, bt_address_t* addr, gatt_status_t status, ble_phy_type_t tx_phy, ble_phy_type_t rx_phy) +// { +// BT_LOGD("gatts phy updated, addr:%s, status:%d, tx:%d, rx:%d", addr, status, tx_phy, rx_phy); +// } + +// static void conn_param_changed_callback(void* srv_handle, bt_address_t* addr, uint16_t connection_interval, +// uint16_t peripheral_latency, uint16_t supervision_timeout) +// { +// BT_LOGD("gatts_conn_param_changed_callback, addr:%s, interval:%" PRIu16 ", latency:%" PRIu16 ", timeout:%" PRIu16, +// addr, connection_interval, peripheral_latency, supervision_timeout); +// } + +// static gatts_callbacks_t gatts_cbs = { +// sizeof(gatts_cbs), +// connect_callback, +// disconnect_callback, +// attr_table_added_callback, +// attr_table_removed_callback, +// notify_complete_callback, +// mtu_changed_callback, +// phy_read_callback, +// phy_updated_callback, +// conn_param_changed_callback, +// }; + +static bt_status_t cs_init(void) +{ + g_cs_service.callbacks = bt_callbacks_list_new(CONFIG_BLUETOOTH_MAX_REGISTER_NUM); + + if (g_cs_handle) { + BT_LOGD("has registed, please unregister then try again"); + return BT_STATUS_FAIL; + } + + list_initialize(&g_cs_service.list); + + return BT_STATUS_SUCCESS; +} + +static void cs_cleanup(void) +{ + bt_callbacks_list_free(g_cs_service.callbacks); + g_cs_service.callbacks = NULL; +} + +static void service_startup(profile_on_startup_t cb) +{ + bt_cs_ras_enable(); + cb(PROFILE_CS, true); +} + +static void service_shutdown(profile_on_shutdown_t cb) +{ + cb(PROFILE_CS, true); +} + +static bt_status_t cs_service_startup(profile_on_startup_t cb) +{ + cs_msg_t* msg = cs_msg_new(CS_STARTUP, NULL); + msg->cs_data.cb = cb; + do_in_cs_service(msg); + + return BT_STATUS_SUCCESS; +} + +static bt_status_t cs_service_shutdown(profile_on_shutdown_t cb) +{ + cs_msg_t* msg = cs_msg_new(CS_SHUTDOWN, NULL); + msg->cs_data.cb = cb; + do_in_cs_service(msg); + + return BT_STATUS_SUCCESS; +} + +static void* cs_register_callbacks(void* remote, const cs_callbacks_t* callbacks) +{ + return bt_remote_callbacks_register(g_cs_service.callbacks, remote, (void*)callbacks); +} + +static bool cs_unregister_callbacks(void** remote, void* cookie) +{ + return bt_remote_callbacks_unregister(g_cs_service.callbacks, remote, cookie); +} + +static bt_status_t cs_start_distance_measurement(bt_distance_measurement_params_t* params) +{ + BT_LOGD("cs_start_distance_measurement"); + switch (params->method) { + case METHOD_AUTO: + case METHOD_RSSI: + BT_LOGD("not supported method"); + break; + case METHOD_CS: { + cs_msg_t* msg = cs_msg_new(START_REQ, ¶ms->addr); + msg->cs_data.data = params; + do_in_cs_service(msg); + break; + } + + default: + break; + } + + return BT_STATUS_SUCCESS; +} + +static bt_status_t cs_stop_distance_measurement(bt_address_t* addr, int method, bool timeout) +{ + BT_LOGD("cs_stop_distance_measurement"); + switch (method) { + case METHOD_AUTO: + case METHOD_RSSI: + BT_LOGD("not supported method"); + break; + case METHOD_CS: { + cs_msg_t* msg = cs_msg_new(STOP_REQ, addr); + do_in_cs_service(msg); + break; + } + + default: + break; + } + + return BT_STATUS_SUCCESS; +} + +static bt_status_t cs_subevent_result_callbacks(bt_address_t* addr, void* data) +{ + if (result_cb == NULL) { + BT_LOGW("The subevent result callbacks haven't registers."); + return BT_STATUS_PARM_INVALID; + } + + result_cb(addr, (bt_srv_conn_le_cs_subevent_result_t*)data); + return BT_STATUS_SUCCESS; +} + +static bt_status_t cs_test(void* data, uint16_t len) +{ + int err = cs_ras_subevent_recv_test(data, len); + return (err == 0) ? BT_STATUS_SUCCESS : BT_STATUS_FAIL; +} + +static const bt_cs_interface_t cs_interface = { + .size = sizeof(cs_interface), + .register_callbacks = cs_register_callbacks, + .unregister_callbacks = cs_unregister_callbacks, + .start_distance_measurement = cs_start_distance_measurement, + .stop_distance_measurement = cs_stop_distance_measurement, + .subevent_result_callbacks = cs_subevent_result_callbacks, + .cs_test = cs_test, +}; + +static const void* get_cs_profile_interface(void) +{ + return (void*)&cs_interface; +} + +static int cs_dump(void) +{ + return 0; +} + +static void cs_process_msg(profile_msg_t* msg) +{ + switch (msg->event) { + default: + break; + } +} + +static int cs_get_state(void) +{ + return 1; +} + +static const profile_service_t cs_service = { + .auto_start = true, + .name = PROFILE_CS_NAME, + .id = PROFILE_CS, + .transport = BT_TRANSPORT_BLE, + .uuid = { BT_UUID128_TYPE, { 0 } }, + .init = cs_init, + .startup = cs_service_startup, + .shutdown = cs_service_shutdown, + .process_msg = cs_process_msg, + .get_state = cs_get_state, + .get_profile_interface = get_cs_profile_interface, + .cleanup = cs_cleanup, + .dump = cs_dump, +}; + +void bt_sal_cs_event_callback(cs_msg_t* msg) +{ + do_in_cs_service(msg); +} + +void bt_register_cs_service(void) +{ + register_service(&cs_service); +} + +void bt_cs_register_subevent_cb(subevent_result_cb_t* cb) +{ + result_cb = cb; + return; +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/service/profiles/cs/cs_state_machine.c b/service/profiles/cs/cs_state_machine.c new file mode 100644 index 000000000..f2cfa74a3 --- /dev/null +++ b/service/profiles/cs/cs_state_machine.c @@ -0,0 +1,486 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#define LOG_TAG "cs_stm" +#include +#include +#include +#include + +#include "adapter_internel.h" +#include "bt_addr.h" +#include "bt_cs.h" +#include "bt_list.h" +#include "bt_utils.h" +#include "callbacks_list.h" +#include "cs_msg.h" +#include "cs_service.h" +#include "cs_state_machine.h" +#include "gatts_service.h" +#include "power_manager.h" +#include "sal_le_cs_interface.h" +#include "service_loop.h" +#include "service_manager.h" +#include "state_machine.h" +#include "utils/log.h" + +#ifdef CONFIG_BLUETOOTH_LE_CS + +static char* stack_event_to_string(cs_msg_id_t msg_id); + +#define CS_TRANS_DBG(_sm, _addr, _action) \ + do { \ + char __addr_str[BT_ADDR_STR_LENGTH] = { 0 }; \ + bt_addr_ba2str(_addr, __addr_str); \ + BT_LOGD("%s State=%s, Peer=[%s]", _action, hsm_get_current_state_name(sm), __addr_str); \ + } while (0); + +#define CS_DBG_ENTER(__sm, __addr) CS_TRANS_DBG(__sm, __addr, "Enter") +#define CS_DBG_EXIT(__sm, __addr) CS_TRANS_DBG(__sm, __addr, "Exit ") +#define CS_DBG_EVENT(__sm, __addr, __event) \ + do { \ + char __addr_str[BT_ADDR_STR_LENGTH] = { 0 }; \ + bt_addr_ba2str(__addr, __addr_str); \ + BT_LOGD("ProcessEvent, State=%s, Peer=[%s], Event=%s", hsm_get_current_state_name(__sm), \ + __addr_str, stack_event_to_string(__event)); \ + } while (0); + +static char* stack_event_to_string(cs_msg_id_t msg_id) +{ + switch (msg_id) { + CASE_RETURN_STR(CS_STARTUP) + CASE_RETURN_STR(CS_SHUTDOWN) + CASE_RETURN_STR(START_REQ) + CASE_RETURN_STR(STOP_REQ) + CASE_RETURN_STR(CAPBLITIES_RECEIVED_EVT) + CASE_RETURN_STR(DISCONNECTED_EVT) + CASE_RETURN_STR(CONNECTED_EVT) + CASE_RETURN_STR(CONFIG_DONE_EVT) + CASE_RETURN_STR(SECURITY_DONE_EVT) + default: + return "UNKNOWN_EVENT"; + } +} +typedef struct _cs_state_machine { + state_machine_t sm; + void* service; + bt_address_t addr; + bool is_capbilities_exchanged; + bt_distance_measurement_params_t params; +} cs_state_machine_t; + +static bt_le_srv_cs_set_default_settings_param_t g_default_settings = {}; + +static void stopped_enter(state_machine_t* sm); +static void stopped_exit(state_machine_t* sm); +// static void init_enter(state_machine_t* sm); +// static void init_exit(state_machine_t* sm); +static void connected_enter(state_machine_t* sm); +static void connected_exit(state_machine_t* sm); +static void wait_for_config_complete_enter(state_machine_t* sm); +static void wait_for_config_complete_exit(state_machine_t* sm); +static void wait_for_security_complete_enter(state_machine_t* sm); +static void wait_for_security_complete_exit(state_machine_t* sm); +static void wait_for_procedure_complete_enter(state_machine_t* sm); +static void wait_for_procedure_complete_exit(state_machine_t* sm); +static void started_enter(state_machine_t* sm); +static void started_exit(state_machine_t* sm); + +static bool stopped_process_event(state_machine_t* sm, uint32_t event, void* p_data); +// static bool init_process_event(state_machine_t* sm, uint32_t event, void* p_data); +static bool connected_process_event(state_machine_t* sm, uint32_t event, void* p_data); +static bool wait_for_config_complete_process_event(state_machine_t* sm, uint32_t event, void* p_data); +static bool wait_for_security_complete_process_event(state_machine_t* sm, uint32_t event, void* p_data); +static bool wait_for_procedure_complete_process_event(state_machine_t* sm, uint32_t event, void* p_data); +static bool started_process_event(state_machine_t* sm, uint32_t event, void* p_data); + +static const state_t stopped_state = { + .state_name = "Stopped", + .state_value = CS_STATE_STOPPED, + .enter = stopped_enter, + .exit = stopped_exit, + .process_event = stopped_process_event, +}; + +// static const state_t init_state = { +// .state_name = "Init", +// .state_value = CS_STATE_INIT, +// .enter = init_enter, +// .exit = init_exit, +// .process_event = init_process_event, +// }; + +static const state_t connected_state = { + .state_name = "Connected", + .state_value = CS_STATE_CONNECTED, + .enter = connected_enter, + .exit = connected_exit, + .process_event = connected_process_event, +}; + +static const state_t wait_for_config_complete_state = { + .state_name = "Wait_for_config_complete", + .state_value = CS_STATE_WAIT_FOR_CONFIG_COMPLETE, + .enter = wait_for_config_complete_enter, + .exit = wait_for_config_complete_exit, + .process_event = wait_for_config_complete_process_event, +}; + +static const state_t wait_for_security_complete_state = { + .state_name = "Wait_for_security_complete", + .state_value = CS_STATE_WAIT_FOR_SECURITY_COMPLETE, + .enter = wait_for_security_complete_enter, + .exit = wait_for_security_complete_exit, + .process_event = wait_for_security_complete_process_event, +}; + +static const state_t wait_for_procedure_complete_state = { + .state_name = "Wait_for_procedure_complete", + .state_value = CS_STATE_WAIT_FOR_PROCEDURE_COMPLETE, + .enter = wait_for_procedure_complete_enter, + .exit = wait_for_procedure_complete_exit, + .process_event = wait_for_procedure_complete_process_event, +}; + +static const state_t started_state = { + .state_name = "Start", + .state_value = CS_STATE_START, + .enter = started_enter, + .exit = started_exit, + .process_event = started_process_event, +}; + +static void stopped_enter(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // const state_t* prev_state = hsm_get_previous_state(sm); + + CS_DBG_ENTER(sm, &cs_sm->addr); +} + +static void stopped_exit(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + + CS_DBG_EXIT(sm, &cs_sm->addr); +} + +static bool stopped_process_event(state_machine_t* sm, uint32_t event, void* p_data) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // cs_msg_t* data = (cs_msg_t*)p_data; + + CS_DBG_EVENT(sm, &cs_sm->addr, event); + switch (event) { + case CONNECTED_EVT: + hsm_transition_to(sm, &connected_state); + break; + default: + break; + } + + return true; +} + +// static void init_enter(state_machine_t* sm) +// { +// cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; +// // const state_t* prev_state = hsm_get_previous_state(sm); + +// CS_DBG_ENTER(sm, &cs_sm->addr); +// } + +// static void init_exit(state_machine_t* sm) +// { +// cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + +// CS_DBG_EXIT(sm, &cs_sm->addr); +// } + +// static bool init_process_event(state_machine_t* sm, uint32_t event, void* p_data) +// { +// cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; +// // cs_msg_t* data = (cs_msg_t*)p_data; + +// CS_DBG_EVENT(sm, &cs_sm->addr, event); +// switch (event) { +// case CONNECTED_EVT: +// hsm_transition_to(sm, &connected_state); +// break; +// case DISCONNECTED_EVT: +// hsm_transition_to(sm, &stopped_state); +// break; +// default: +// break; +// } + +// return true; +// } + +static void connected_enter(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // const state_t* prev_state = hsm_get_previous_state(sm); + + CS_DBG_ENTER(sm, &cs_sm->addr); +} + +static void connected_exit(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + + CS_DBG_EXIT(sm, &cs_sm->addr); +} + +static bool connected_process_event(state_machine_t* sm, uint32_t event, void* p_data) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + cs_msg_data_t* data = (cs_msg_data_t*)p_data; + + CS_DBG_EVENT(sm, &cs_sm->addr, event); + switch (event) { + case START_REQ: + bt_distance_measurement_params_t* params = data->data; + + g_default_settings.enable_initiator_role = params->role == CS_BT_SRV_CONN_LE_CS_ROLE_INITIATOR ? true : false; + g_default_settings.enable_reflector_role = params->role == CS_BT_SRV_CONN_LE_CS_ROLE_REFLECTOR ? true : false; + g_default_settings.cs_sync_antenna_selection = params->antenna_paths_mask; + cs_sm->params = *params; + + if (cs_sm->is_capbilities_exchanged) { + bt_sal_cs_set_default_settings(PRIMARY_ADAPTER, &(data->bd_addr), &g_default_settings); + hsm_transition_to(sm, &wait_for_config_complete_state); + } else { + bt_sal_cs_read_remote_supported_capabilities(PRIMARY_ADAPTER, &(data->bd_addr)); + } + break; + case DISCONNECTED_EVT: + hsm_transition_to(sm, &stopped_state); + break; + case CAPBLITIES_RECEIVED_EVT: + cs_sm->is_capbilities_exchanged = true; + bt_sal_cs_set_default_settings(PRIMARY_ADAPTER, &(data->bd_addr), &g_default_settings); + hsm_transition_to(sm, &wait_for_config_complete_state); + break; + default: + break; + } + + return true; +} + +static void wait_for_config_complete_enter(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // const state_t* prev_state = hsm_get_previous_state(sm); + + CS_DBG_ENTER(sm, &cs_sm->addr); +} + +static void wait_for_config_complete_exit(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + + CS_DBG_EXIT(sm, &cs_sm->addr); +} + +static bool wait_for_config_complete_process_event(state_machine_t* sm, uint32_t event, void* p_data) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // cs_msg_data_t* data = (cs_msg_data_t*)p_data; + + CS_DBG_EVENT(sm, &cs_sm->addr, event); + switch (event) { + case CONNECTED_EVT: + hsm_transition_to(sm, &connected_state); + break; + case DISCONNECTED_EVT: + hsm_transition_to(sm, &stopped_state); + break; + case CONFIG_DONE_EVT: + hsm_transition_to(sm, &wait_for_security_complete_state); + break; + default: + break; + } + + return true; +} + +static void wait_for_security_complete_enter(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // const state_t* prev_state = hsm_get_previous_state(sm); + + CS_DBG_ENTER(sm, &cs_sm->addr); +} + +static void wait_for_security_complete_exit(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + + CS_DBG_EXIT(sm, &cs_sm->addr); +} + +static bool wait_for_security_complete_process_event(state_machine_t* sm, uint32_t event, void* p_data) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // cs_msg_t* data = (cs_msg_t*)p_data; + + CS_DBG_EVENT(sm, &cs_sm->addr, event); + switch (event) { + case CONNECTED_EVT: + hsm_transition_to(sm, &connected_state); + break; + case DISCONNECTED_EVT: + hsm_transition_to(sm, &stopped_state); + break; + case CONFIG_DONE_EVT: + hsm_transition_to(sm, &wait_for_security_complete_state); + break; + case SECURITY_DONE_EVT: + hsm_transition_to(sm, &wait_for_procedure_complete_state); + break; + default: + break; + } + + return true; +} + +static void wait_for_procedure_complete_enter(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // const state_t* prev_state = hsm_get_previous_state(sm); + + CS_DBG_ENTER(sm, &cs_sm->addr); +} + +static void wait_for_procedure_complete_exit(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + + CS_DBG_EXIT(sm, &cs_sm->addr); +} + +static bool wait_for_procedure_complete_process_event(state_machine_t* sm, uint32_t event, void* p_data) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + cs_msg_t* data = (cs_msg_t*)p_data; + + CS_DBG_EVENT(sm, &cs_sm->addr, event); + switch (event) { + case CONNECTED_EVT: + hsm_transition_to(sm, &connected_state); + break; + case DISCONNECTED_EVT: + hsm_transition_to(sm, &stopped_state); + break; + case CONFIG_DONE_EVT: + hsm_transition_to(sm, &wait_for_security_complete_state); + if (data->cs_data.data) { + free(data->cs_data.data); + } + break; + case SECURITY_DONE_EVT: + hsm_transition_to(sm, &wait_for_procedure_complete_state); + if (data->cs_data.data) { + free(data->cs_data.data); + } + break; + case PROCEDURE_DONE_EVT: + hsm_transition_to(sm, &started_state); + if (data->cs_data.data) { + free(data->cs_data.data); + } + break; + default: + break; + } + + return true; +} + +static void started_enter(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // const state_t* prev_state = hsm_get_previous_state(sm); + + CS_DBG_ENTER(sm, &cs_sm->addr); +} + +static void started_exit(state_machine_t* sm) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + + CS_DBG_EXIT(sm, &cs_sm->addr); +} + +static bool started_process_event(state_machine_t* sm, uint32_t event, void* p_data) +{ + cs_state_machine_t* cs_sm = (cs_state_machine_t*)sm; + // cs_msg_t* data = (cs_msg_t*)p_data; + + CS_DBG_EVENT(sm, &cs_sm->addr, event); + switch (event) { + case CONNECTED_EVT: + hsm_transition_to(sm, &connected_state); + break; + case DISCONNECTED_EVT: + hsm_transition_to(sm, &stopped_state); + break; + case CONFIG_DONE_EVT: + hsm_transition_to(sm, &wait_for_security_complete_state); + break; + case SECURITY_DONE_EVT: + hsm_transition_to(sm, &wait_for_procedure_complete_state); + break; + default: + break; + } + + return true; +} + +static void cs_state_machine_event_dispatch(cs_state_machine_t* sm, cs_msg_t* msg) +{ + if (!msg || !sm) + return; + + hsm_dispatch_event(&sm->sm, msg->id, &msg->cs_data); +} + +void cs_state_machine_handle_event(cs_state_machine_t* sm, cs_msg_t* msg) +{ + cs_state_machine_event_dispatch(sm, msg); +} + +cs_state_machine_t* cs_state_machine_new(void* context, bt_address_t* bd_addr) +{ + cs_state_machine_t* cs_sm; + + cs_sm = (cs_state_machine_t*)malloc(sizeof(cs_state_machine_t)); + if (!cs_sm) + return NULL; + + memset(cs_sm, 0, sizeof(cs_state_machine_t)); + cs_sm->service = context; + hsm_ctor(&cs_sm->sm, (state_t*)&stopped_state); + memcpy(&cs_sm->addr, bd_addr, sizeof(bt_address_t)); + + return cs_sm; +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/service/profiles/cs/cs_state_machine.h b/service/profiles/cs/cs_state_machine.h new file mode 100644 index 000000000..b9a3d6065 --- /dev/null +++ b/service/profiles/cs/cs_state_machine.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef __CS_STATE_MACHINE_H__ +#define __CS_STATE_MACHINE_H__ + +#include "bt_device.h" + +typedef enum { + CS_STATE_STOPPED, + CS_STATE_INIT, + CS_STATE_CONNECTED, + CS_STATE_WAIT_FOR_CONFIG_COMPLETE, + CS_STATE_WAIT_FOR_SECURITY_COMPLETE, + CS_STATE_WAIT_FOR_PROCEDURE_COMPLETE, + CS_STATE_START +} cs_state_t; + +typedef enum { + CS_CONNECT, + CS_DISCONNECT, +} cs_event_t; + +typedef struct _cs_state_machine cs_state_machine_t; + +typedef struct { + struct list_node node; + cs_state_machine_t* cs_sm; + bt_address_t bd_addr; +} cs_device_t; + +cs_state_machine_t* cs_state_machine_new(void* context, bt_address_t* bd_addr); +void cs_state_machine_handle_event(cs_state_machine_t* sm, cs_msg_t* msg); +#endif \ No newline at end of file diff --git a/service/profiles/include/cs_msg.h b/service/profiles/include/cs_msg.h new file mode 100644 index 000000000..7ad9ea856 --- /dev/null +++ b/service/profiles/include/cs_msg.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef __CS_MSG_H__ +#define __CS_MSG_H__ +/**************************************************************************** + * Included Files + ****************************************************************************/ +#include "bt_cs.h" + +typedef enum { + CS_STARTUP, + CS_SHUTDOWN, + START_REQ, + STOP_REQ, + CAPBLITIES_RECEIVED_EVT, + DISCONNECTED_EVT, + CONNECTED_EVT, + CONFIG_DONE_EVT, + SECURITY_DONE_EVT, + PROCEDURE_DONE_EVT, + CS_SUBEVENT_RESULT_EVT, +} cs_msg_id_t; + +typedef struct +{ + bt_address_t bd_addr; + void* data; + void* cb; +} cs_msg_data_t; + +typedef struct +{ + cs_msg_id_t id; + cs_msg_data_t cs_data; +} cs_msg_t; + +cs_msg_t* cs_msg_new(cs_msg_id_t msg, bt_address_t* bd_addr); +void cs_msg_destory(cs_msg_t* cs_msg); + +#endif diff --git a/service/profiles/include/cs_ras.h b/service/profiles/include/cs_ras.h new file mode 100644 index 000000000..6820c1d73 --- /dev/null +++ b/service/profiles/include/cs_ras.h @@ -0,0 +1,846 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef _CS_RAS_H_ +#define _CS_RAS_H_ + +#include "service_loop.h" +#include "cs_ras_util.h" +#include "bt_addr.h" +#include "cs_ras_gatts.h" +#include "bt_gatt_defs.h" +#include "cs_ras_test.h" +#include "cs_service.h" + +#define CONFIG_BT_CS_TEST 1 + +#ifndef BIT +#define BIT(n) (1 << n) +#endif /* BIT */ + +/** + * @brief Response timeout for Ranging Service (RAS) operations. + * + * This macro defines the timeout duration for Ranging Service (RAS) + * responses, set to 5 seconds. + */ +#define RAS_RSP_TIMEOUT (5 * 1000) + +/** + * @brief Empty array indicator. + * + * This macro represents an empty array or zero-length data structure + * used in Ranging Service contexts. + */ +#define RAS_EMPTY_ARRAY (0) + +/** + * @brief RAS role: Initiator. + * + * This macro defines the Ranging Service (RAS) role as the Initiator, + * which starts the ranging process by sending ranging requests. + */ +#define SAL_LE_RAS_ROLE_INITIATOR (0) + +/** + * @brief RAS role: Reflector. + * + * This macro defines the Ranging Service (RAS) role as the Reflector, + * which responds to ranging requests from the Initiator. + */ +#define SAL_LE_RAS_ROLE_REFLECTOR (1) + +/** + * @brief RAS subevent step mode 0. + * + * This macro defines subevent step mode 0 for the Ranging Service (RAS). + * The meaning of this mode depends on the specific RAS configuration or + * ranging algorithm being used. + */ +#define SAL_LE_RAS_SUBEVENT_STEP_MODE_0 (0) + +/** + * @brief RAS subevent step mode 1. + * + * This macro defines subevent step mode 1 for the Ranging Service (RAS). + */ +#define SAL_LE_RAS_SUBEVENT_STEP_MODE_1 (1) + +/** + * @brief RAS subevent step mode 2. + * + * This macro defines subevent step mode 2 for the Ranging Service (RAS). + */ +#define SAL_LE_RAS_SUBEVENT_STEP_MODE_2 (2) + +/** + * @brief RAS subevent step mode 3. + * + * This macro defines subevent step mode 3 for the Ranging Service (RAS). + */ +#define SAL_LE_RAS_SUBEVENT_STEP_MODE_3 (3) + +/** + * @brief GATT notification type for Ranging Service (RAS). + * + * This macro defines the use of GATT Notification in the Ranging Service (RAS) + * for transmitting ranging-related data or events to a connected peer device. + * Notifications are sent without requiring acknowledgment from the receiver. + */ +#define SAL_LE_RAS_GATT_NOTIFY (1) + +/** + * @brief GATT indication type for Ranging Service (RAS). + * + * This macro defines the use of GATT Indication in the Ranging Service (RAS) + * for transmitting ranging-related data or events to a connected peer device. + * Indications require acknowledgment (confirmation) from the receiver. + */ +#define SAL_LE_RAS_GATT_INDICATION (2) + +/** + * @brief RAS sub-procedure header identifier. + * + * This macro defines the identifier value (12) used for the Ranging Service (RAS) + * sub-procedure header. It marks the beginning of the RAS sub-procedure data block. + * + * The RAS Sub-Procedure Header includes the following fields: + * + * | Field | Size (bits) | Description | + * |----------------------------|-------------|--------------------------------------------------------------------------------------------------------------------------| + * | Ranging Counter | 12 | Lower 12 bits of `CS_Procedure_Counter` (see *Vol 4, Part E, Sec 7.7.65.44* in [1]) provided by the Core Controller. | + * | Configuration ID | 4 | CS configuration identifier. Range: 0–3. | + * | Selected TX Power | 8 | Transmit power level used for the CS Procedure. Range: -127 to 20 dBm (referenced to 1 mW). | + * | Antenna Paths Mask | 8 | Indicates which antenna paths are reported:
• Bit0: Path 1
• Bit1: Path 2
• Bit2: + * Path 3
• Bit3: Path 4
• Bits 4–7: RFU | + * | Subevent Header | — | — | + * | Start ACL Connection Event | 16 | Starting ACL connection event count for results reported in the event. | + * | Frequency Compensation | 16 | Frequency compensation value in units of 0.01 ppm (15-bit signed integer). | + * | Ranging Done Status | 4 | Completion state for the CS Procedure:
• 0x0 – All results complete
• 0x1 – Partial results, more to + * follow
• 0xF – All subsequent CS Procedures aborted
• Others – RFU | + * | Subevent Done Status | 4 | Completion state for the CS Subevent:
• 0x0 – All results complete
• 0xF – Subevent aborted
• Others – RFU | + * | Ranging Abort Reason | 4 | Abort reason when `Procedure_Done_Status` = 0xF; otherwise 0.
• 0x0 – No abort
• 0x1 – Local/remote abort + * request
• 0x2 – Filtered channel map < 15 channels
• 0x3 – Channel map update instant passed
• 0xF – Unspecified
• Others – RFU | + * | Subevent Abort Reason | 4 | Abort reason when `Subevent_Done_Status` = 0xF; otherwise 0.
• 0x0 – No abort
• 0x1 – Local/remote abort request
• 0x2 – + * No CS_SYNC (mode 0) received
• 0x3 – Scheduling/resource conflict
• 0xF – Unspecified
• Others – RFU | + * | Reference Power Level | 8 | Reference power level. Range: –127 to 20 dBm. | + * | Number of Steps Reported | 8 | Number of steps in the CS Subevent for which results are reported. If aborted, can be set to 0. | + * + * @note Refer to Bluetooth Core Specification Volume 4, Part E, Section 7.7.65.44 for field definitions. + */ +#define SAL_LE_RAS_SUB_PROCUDURE_HEAD (12) + +/** + * @brief CCCD "Not Implemented Configuration" error code for RAS. + * + * This macro defines the error code (0xFD) used in the Ranging Service (RAS) + * to indicate that the Client Characteristic Configuration Descriptor (CCCD) + * write request refers to a configuration that is not implemented or supported. + * + * Typically returned when a peer attempts to enable notifications or indications + * on a characteristic that does not support the requested configuration. + */ +#define SAL_LE_RAS_CCCD_NOT_IMPR_CONFIG_ERR (0xFD) + +/** + * @brief CCCD write request rejected error code for RAS. + * + * This macro defines the error code (0xFC) used in the Ranging Service (RAS) + * to indicate that the Client Characteristic Configuration Descriptor (CCCD) + * write request was explicitly rejected by the server. + * + * This may occur due to invalid parameters, insufficient permissions, + * or internal service state preventing configuration changes. + */ +#define SAL_LE_RAS_CCCD_WR_REQ_REJECT (0xFC) + +/** + * @brief Ranging mode definitions for the Ranging Service (RAS). + * + * RAS enables the client to read Ranging Data from a RAS Server. RAS is implemented + * on a device that can generate Ranging Data using the Channel Sounding (CS) feature + * of the local Core Controller. RAS distinguishes between two modes of Ranging Data exchange, + * each represented by a service characteristic: + * + * • Real-time Ranging Data: Data received from the local Core Controller and + * communicated immediately by the RAS Server while connected to the client. + * • On-demand Ranging Data: Data received from the local Core Controller and + * stored on the RAS Server for on-demand retrieval by the client. + * + * The client enables one of these modes by subscribing to either notifications or + * indications of the corresponding characteristic (Real-time or On-demand) via the + * Client Characteristic Configuration Descriptor (CCCD). The RAS Server shall operate + * in either Real-time or On-demand mode, but **not both simultaneously**. A client can + * switch modes by first disabling the active mode and then enabling the other mode. + * + * If a client attempts to enable more than one mode, the RAS Server shall reject the + * Write Characteristic Descriptor request and return a `SAL_LE_RAS_CCCD_NOT_IMPR_CONFIG_ERR` + * (0xFD). If the RAS Server does not support notifications for a characteristic and + * the client enables notifications, the server shall reject the request and return + * `SAL_LE_RAS_CCCD_WR_REQ_REJECT` (0xFC). + * + * A RAS Server shall transfer Ranging Data in chronological order, with the oldest + * data sent first. This applies to both On-demand Ranging Data (transferred via + * RAS Control Point procedure) and Real-time Ranging Data. If the ACL connection with + * the client is lost or terminated, any pending Ranging Data segments shall be flushed. + * + * The RAS Ranging Modes are defined as follows: + * + * | **Macro** | **Value** | **Description** | + * |---------------------------------------|------------|---------------------------------------------------------------------------------| + * | **SAL_LE_RAS_RANGING_MODE_REAL_TIME** | 0x01 | Real-time ranging mode. Measurements are performed continuously or periodically in real time. | + * | **SAL_LE_RAS_RANGING_MODE_ON_DEMAND** | 0x02 | On-demand ranging mode. Measurements are performed only when explicitly requested by the client. | + * | **SAL_LE_RAS_RANGING_MODE_UNDEFINED** | 0xFF | Undefined or invalid mode. Used as default or error value. | + * + * @note These values are typically exchanged via GATT characteristics as part of + * RAS control or configuration procedures. + */ +#define SAL_LE_RAS_RANGING_MODE_REAL_TIME (0x01) +#define SAL_LE_RAS_RANGING_MODE_ON_DEMAND (0x02) +#define SAL_LE_RAS_RANGING_MODE_UNDEFINED (0xFF) +typedef uint8_t ras_rang_mode_t; + +/** + * @brief Maximum number of Ranging Service (RAS) procedures that can be stored. + * + * This macro defines the upper limit on the number of RAS procedures that the + * RAS Server can store simultaneously. Each stored procedure may contain + * multiple steps or subevents of Ranging Data generated via the Channel Sounding (CS) + * feature of the local Core Controller. + * + * The value 10 indicates that the server can hold up to 10 procedures in memory. + * If additional procedures are generated beyond this limit, the server may + * reject new procedures or overwrite older ones depending on the implementation. + * + * @note This value helps the RAS Server manage memory and resources efficiently, + * preventing overflow when multiple Ranging procedures are active. + */ +#define SAL_LE_RAS_STORE_PROCEDURE_NUM_MAX (10) + +/** + * RAS Features format + * The RAS Features characteristic bit formats are listed in + * +--------+-----------------------------------------------+ + * | Bit(s) | Definition | + * +--------+-----------------------------------------------+ + * | 0 | Real-time Ranging Data | + * | 1 | Retrieve Lost Ranging Data Segments | + * | 2 | Abort Operation | + * | 3 | Filter Ranging Data | + * | 4–31 | Reserved for Future Use (RFU) | + * +--------+-----------------------------------------------+ + */ +#define SAL_LE_RAS_REAL_TIME_RANG_DATA_SUPPROTED (1 << 0) +#define SAL_LE_RAS_RETRI_LOST_RANG_DATA_SEG (1 << 1) +#define SAL_LE_RAS_ABORT_OPRATION (1 << 2) +#define SAL_LE_RAS_FILTER_RANG_DATA (1 << 3) + +/** + * @brief RAS Control Point Operation Codes (Op Codes) and Parameters. + * + * The Ranging Service (RAS) Control Point allows clients to issue commands + * to a RAS Server to control and retrieve Ranging Data. Each operation has + * a defined Op Code and associated parameters. Requirements are summarized + * in Tables 3.10 and 3.11 of the specification. + * + * | **Operation** | **Op Code** | **Requirement** | **Parameter #1** | **Parameter #2** | **Parameter #3** | + * |----------------------------------------|------------|----------------|----------------------------|---------------------------|---------------------------| + * | **Get_Ranging_Data** | 0x00 | Mandatory (M) | uint16 Ranging Counter | – | – | + * | **ACK_Ranging_Data** | 0x01 | Mandatory (M) | uint16 Ranging Counter | – | – | + * | **Retrieve_Lost_Ranging_Data_Segments**| 0x02 | Optional (O) | uint16 Ranging Counter | uint8 First Segment Index | uint8 Last Segment Index | + * | **Abort_Operation** | 0x03 | Optional (O) | No Parameter used | – | – | + * | **Set_Filter** | 0x04 | Optional (O) | uint16 Filter Configuration:
• Bits 0-1: Mode
• Bits 2-15: Filter bit mask (see Section 3.3.2.4) | – | – | + * + * @note The client uses these Op Codes via the RAS Control Point characteristic + * to perform operations on the RAS Server. The server shall respond according + * to the Op Code requirements, returning results or acknowledgements as defined. + */ +#define SAL_LE_RAS_CTL_OP_CMD_GET_RANG_DATA (0x00) +#define SAL_LE_RAS_CTL_OP_CMD_ACK_RANG_DATA (0x01) +#define SAL_LE_RAS_CTL_OP_CMD_RETRIEVE_LOST_RANG_DATA_SEG (0x02) +#define SAL_LE_RAS_CTL_OP_CMD_ABORT_OPERATION (0x03) +#define SAL_LE_RAS_CTL_OP_CMD_SET_FILTER (0x04) + +/** + * @brief RAS Control Point Response Op Codes and Parameters. + * + * The Ranging Service (RAS) Control Point characteristic also defines responses + * from the RAS Server to the client. Each response has an Op Code and associated + * parameters. Requirements are summarized in Table 3.11 of the specification. + * + * | **Response** | **Op Code** | **Requirement** | **Parameter #1** | **Parameter #2** | **Parameter #3** | + * |-------------------------------------------- |------------|----------------|----------------------------|---------------------------|---------------------------| + * | **Complete Ranging Data Response** | 0x00 | Mandatory (M) | uint16 Ranging Counter | – | – | + * | **Complete Lost Ranging Data Segment Response** | 0x01 | Conditional (C.1) | uint16 Ranging Counter | uint8 First Segment Index | uint8 Last Segment Index | + * | **Response Code** | 0x02 | Mandatory (M) | Response Code Value | – | – | + * + * @note C.1: If the Retrieve Lost Ranging Data Segments procedure is supported, + * then this Op Code is considered Mandatory. + * + * @note These Op Codes are used by the RAS Server to report the results of + * operations initiated by the client via the RAS Control Point characteristic. + */ +#define SAL_LE_RAS_CTL_OP_RSP_CMP_RANG_DATA (0x00) +#define SAL_LE_RAS_CTL_OP_RSP_CMP_LOST_RANG_DATA_SEG (0x01) +#define SAL_LE_RAS_CTL_OP_RSP_CODE (0x02) + +/** + * @brief RAS Control Point Response Code Values (for Op Code 0x02). + * + * These values are used by the RAS Server to indicate the result of an operation + * requested by the client via the RAS Control Point characteristic. + * Requirements are summarized in Table 3.12 of the specification. + * + * | **Response Code Value** | **Definition** | **Description** | + * |------------------------|-------------------------|-------------------------------------------------------------------------------| + * | 0x00 | Reserved for Future Use | N/A | + * | 0x01 | Success | Normal response for a successful operation | + * | 0x02 | Op Code Not Supported | Normal response if an unsupported Op Code is received | + * | 0x03 | Invalid Parameter | Normal response if the received parameter does not meet service requirements | + * | 0x04 | Success/Persisted | Normal response for a successful write operation where values are persisted | + * | 0x05 | Abort Unsuccessful | Normal response if a request for Abort is unsuccessful | + * | 0x06 | Procedure Not Completed | Normal response if unable to complete a procedure for any reason | + * | 0x07 | Server Busy | Normal response if the Server is still busy with other requests | + * | 0x08 | No Records Found | Normal response if the requested Ranging Counter is not found | + * | 0x09-0xFF | Reserved for Future Use | N/A | + * + * @note These codes are specifically returned in response to Control Point + * operations with Op Code 0x02 (Response Code Op). + */ +#define SAL_LE_RAS_CTL_OP_RSP_CODE_RESERVED (0x00) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS (0x01) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_NOT_SUPPORTED (0x02) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_INVALID_PARAMS (0x03) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_PERSISTED (0x04) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_ABORT (0x05) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_PROCE_NOT_CMP (0x06) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_SERVER_BUSY (0x07) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_NO_RECORD_FOUND (0x08) + +/** + * @brief RAS Filter Mode and Filter Bit Masks. + * + * These macros define the maximum filter mode, and masks for extracting + * the mode and filter bits from a 16-bit Filter Configuration field used + * in the Ranging Service (RAS) Control Point. + */ + +/** Maximum valid RAS filter mode value (0x00 to 0x03). */ +#define SAL_LE_RAS_FILTER_MODE_MAX (0x04) + +/** Mask for the Mode bits (bits 0-1) in the Filter Configuration field. */ +#define SAL_LE_RAS_FILTER_MODE_MASK (0x03) // Mask for Mode bits (0-1) + +/** Mask for the Filter bits (bits 2-15) in the Filter Configuration field. */ +#define SAL_LE_RAS_FILTER_BIT_MASK (0xFFFC) // Mask for Filter bits (2-15) + +/** + * @brief Maximum buffer length for RAS Step Data. + * + * This macro defines the maximum length of the buffer used to store + * step data for the Ranging Service (RAS) GATT characteristic. + * + * @note The length is set according to the maximum GATT characteristic + * length supported by the Bluetooth stack. + */ +#define SAL_LE_RAS_STEP_DATA_BUF_LEN 2048 +#define SAL_LE_RAS_SWAP_ADD_SUB(a, b) \ + do { \ + (a) = (a) + (b); \ + (b) = (a) - (b); \ + (a) = (a) - (b); \ + } while (0) + +/** + * @brief Copy a field to the output buffer if the corresponding filter bit is enabled. + * + * This macro conditionally copies a field of data from a source pointer `p` + * to an output buffer `buf` depending on the `filter_mask`. It also updates + * offsets and tracks remaining bytes. + * + * @param _size Size of the field in bytes. + * @param _filter_bit Bitmask corresponding to the field in the filter_mask. + * + * @details + * - The field is only copied if: + * 1. There are enough bytes remaining (`remaining >= _size`), and + * 2. The corresponding bit in `filter_mask` is set (`filter_mask & _filter_bit`). + * - After copying, the output offset (`out_offset`) is incremented by `_size`. + * - Regardless of copying, the source pointer `p` is advanced by `_size`, + * and `remaining` bytes are decreased by `_size`. + * - If there are not enough bytes remaining to copy the field, a warning is logged + * and `remaining` is set to 0 to prevent further copying. + * + * @note This macro is typically used when serializing data fields conditionally + * based on a filter mask in Ranging Service (RAS) or similar protocols. + */ +#define COPY_FIELD_IF_ENABLED(_size, _filter_bit) \ + do { \ + if (remaining >= (_size)) { \ + if (filter_mask & (_filter_bit)) { \ + memcpy(&buf[out_offset], p, (_size)); \ + out_offset += (_size); \ + } \ + p += (_size); \ + remaining -= (_size); \ + } else { \ + BT_LOGW("Field truncated, size=%d", (int)(_size)); \ + remaining = 0; \ + } \ + } while (0) + +/** + * @brief Bits used to enable notifications or indications for RAS characteristics. + * + * This enumeration defines the bit positions for enabling notifications or + * indications for various Ranging Service (RAS) GATT characteristics. + * These bits are typically used in a client configuration mask to control + * which types of RAS data are sent from the server to the client. + */ +enum cs_ras_notify_enable_bit { + /** Enable notification for Real-time Ranging (RTT) Data characteristic. */ + RAS_RTT_DATA_NOTIFY, + /** Enable indication for Real-time Ranging (RTT) Data characteristic. */ + RAS_RTT_DATA_INDICATE, + /** Enable notification for RAS Control Point characteristic. */ + RAS_CONTROL_POINT_NOTIFY, + /** Enable indication for RAS Control Point characteristic. */ + RAS_CONTROL_POINT_INDICATE, + /** Enable notification for On-demand Ranging Data characteristic. */ + RAS_ON_DEMAND_DATA_NOTIFY, + /** Enable indication for On-demand Ranging Data characteristic. */ + RAS_ON_DEMAND_DATA_INDICATE, + /** Enable notification for Data Ready characteristic. */ + RAS_DATA_READY_NOTIFY, + /** Enable indication for Data Ready characteristic. */ + RAS_DATA_READY_INDICATE, + /** Enable notification for Overwrite event characteristic. */ + RAS_OVER_WRITE_NOTIFY, + /** Enable indication for Overwrite event characteristic. */ + RAS_OVER_WRITE_INDICATE +}; + +/** + * @brief RAS GATT Attribute Table Indexes. + * + * This enumeration defines indexes for the Ranging Service (RAS) GATT attributes. + * These indexes are used to reference characteristics, their values, and + * Client Characteristic Configuration Descriptors (CCCDs) in the attribute table. + */ +enum { + /** RAS Service Index. */ + SAL_LE_RAS_SVC_IDX, + + /** Feature Characteristic Index. */ + SAL_LE_RAS_FEAT_CHAR_IDX, + /** Feature Characteristic Value Index. */ + SAL_LE_RAS_FEAT_CHAR_VAL_IDX, + + /** Real-time Data Characteristic Index. */ + SAL_LE_RAS_RT_DT_CHAR_IDX, + /** Real-time Data Characteristic Value Index. */ + SAL_LE_RAS_RT_DT_CHAR_VAL_IDX, + /** Real-time Data Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_RT_DT_CCC_CFG_IDX, + + /** On-demand Data Characteristic Index. */ + SAL_LE_RAS_ON_DEM_CHAR_IDX, + /** On-demand Data Characteristic Value Index. */ + SAL_LE_RAS_ON_DEM_CHAR_VAL_IDX, + /** On-demand Data Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_ON_DEM_CCC_CFG_IDX, + + /** Control Point Characteristic Index. */ + SAL_LE_RAS_CTR_PT_CHAR_IDX, + /** Control Point Characteristic Value Index. */ + SAL_LE_RAS_CTR_PT_CHAR_VAL_IDX, + /** Control Point Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_CTR_PT_CCC_CFG_IDX, + + /** Data Ready Characteristic Index. */ + SAL_LE_RAS_DT_RD_CHAR_IDX, + /** Data Ready Characteristic Value Index. */ + SAL_LE_RAS_DT_RD_CHAR_VAL_IDX, + /** Data Ready Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_DT_RD_CCC_CFG_IDX, + + /** Data Overwrite Characteristic Index. */ + SAL_LE_RAS_DT_OV_WR_CHAR_IDX, + /** Data Overwrite Characteristic Value Index. */ + SAL_LE_RAS_DT_OV_WR_CHAR_VAL_IDX, + /** Data Overwrite Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_DT_OV_WR_CCC_CFG_IDX, + + /** Total number of RAS attribute table entries. */ + RAS_IDX_MAX +}; + +/******************************************************************************************* + * + * FILTER BIT MAPPING TABLE + * ------------------------------------------------------------ + * Each field can be individually filtered out using the + * uint32_t ras_filter[SAL_LE_RAS_FILTER_MODE_MAX] bitmask. + * + * +----------------------------+-------------------------------+ + * | Field Name | Bit Definition | + * +----------------------------+-------------------------------+ + * | Packet_Quality | RAS_FILTER_BIT_PKT_QUALITY (BIT(0)) | + * | Packet_NADM | RAS_FILTER_BIT_PKT_NADM (BIT(1)) | + * | Packet_RSSI | RAS_FILTER_BIT_PKT_RSSI (BIT(2)) | + * | Packet_Antenna | RAS_FILTER_BIT_PKT_ANTENNA (BIT(3)) | + * | Packet_PCT1 | RAS_FILTER_BIT_PKT_PCT1 (BIT(4)) | + * | Packet_PCT2 | RAS_FILTER_BIT_PKT_PCT2 (BIT(5)) | + * | Measured_Freq_Offset | RAS_FILTER_BIT_FREQ_OFFSET (BIT(6)) | + * | ToA_ToD_Initiator | RAS_FILTER_BIT_TOA_TOD (BIT(7)) | + * | ToD_ToA_Reflector | RAS_FILTER_BIT_TOD_TOA (BIT(8)) | + * | Antenna_Permutation_Index | RAS_FILTER_BIT_ANT_PERM_IDX (BIT(9)) | + * | Tone_PCT[k] | RAS_FILTER_BIT_TONE_PCT (BIT(10)) | + * | Tone_Quality_Indicator[k] | RAS_FILTER_BIT_TONE_QUALITY (BIT(11)) | + * +----------------------------+-------------------------------+ + * + *******************************************************************************************/ +typedef enum { + RAS_FILTER_BIT_PKT_QUALITY = BIT(0), + RAS_FILTER_BIT_PKT_NADM = BIT(1), + RAS_FILTER_BIT_PKT_RSSI = BIT(2), + RAS_FILTER_BIT_PKT_ANTENNA = BIT(3), + RAS_FILTER_BIT_PKT_PCT1 = BIT(4), + RAS_FILTER_BIT_PKT_PCT2 = BIT(5), + RAS_FILTER_BIT_FREQ_OFFSET = BIT(6), + RAS_FILTER_BIT_TOA_TOD = BIT(7), + RAS_FILTER_BIT_TOD_TOA = BIT(8), + RAS_FILTER_BIT_ANT_PERM_IDX = BIT(9), + RAS_FILTER_BIT_TONE_PCT = BIT(10), + RAS_FILTER_BIT_TONE_QUALITY = BIT(11), +} ras_filter_bits_t; + +/** + * @brief RAS Mode 0 Filter Bit Definitions. + * + * These enumeration values define the filter bit positions for RAS Mode 0. + * Each bit indicates whether a particular type of packet or measurement data + * should be included in the Ranging Service procedure. + */ +enum { + SAL_LE_RAS_MODE_0_FILTER_PACKET_QUALITY, + SAL_LE_RAS_MODE_0_FILTER_PACKET_RSSI, + SAL_LE_RAS_MODE_0_FILTER_PACKET_ANTENNA, + SAL_LE_RAS_MODE_0_FILTER_MEASURED_FREQ_OFFSET, + SAL_LE_RAS_MODE_0_FILTER_MAX, +}; + +/** + * @brief RAS Mode 1 Filter Bit Definitions. + * + * Filter bit positions for RAS Mode 1, specifying which packet or measurement + * fields are included in the Ranging Service procedure. + */ +enum { + SAL_LE_RAS_MODE_1_FILTER_PACKET_QUALITY, + SAL_LE_RAS_MODE_1_FILTER_PACKET_NADM, + SAL_LE_RAS_MODE_1_FILTER_PACKET_RSSI, + SAL_LE_RAS_MODE_1_FILTER_TOD_TOA, + SAL_LE_RAS_MODE_1_FILTER_PACKET_ANTENNA, + SAL_LE_RAS_MODE_1_FILTER_PACKET_PCT_1, + SAL_LE_RAS_MODE_1_FILTER_PACKET_PCT_2, + SAL_LE_RAS_MODE_1_FILTER_MAX, +}; + +/** + * @brief RAS Mode 2 Filter Bit Definitions. + * + * Filter bit positions for RAS Mode 2, primarily used for antenna permutation + * and tone analysis in the Ranging Service procedure. + */ +enum { + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PERMUTATION_INDEX, + SAL_LE_RAS_MODE_2_FILTER_TONE_PCT, + SAL_LE_RAS_MODE_2_FILTER_TONE_QUALITY_INDICATOR, + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PATH_1, + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PATH_2, + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PATH_3, + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PATH_4, + SAL_LE_RAS_MODE_2_FILTER_MAX, +}; + +/** + * @brief RAS Mode 3 Filter Bit Definitions. + * + * Filter bit positions for RAS Mode 3. Combines Mode 1 and Mode 2 filters + * to enable comprehensive packet and antenna/tone analysis. + */ +enum { + SAL_LE_RAS_MODE_3_FILTER_PACKET_QUALITY, + SAL_LE_RAS_MODE_3_FILTER_PACKET_NADM, + SAL_LE_RAS_MODE_3_FILTER_PACKET_RSSI, + SAL_LE_RAS_MODE_3_FILTER_TOD_TOA, + SAL_LE_RAS_MODE_3_FILTER_PACKET_ANTENNA, + SAL_LE_RAS_MODE_3_FILTER_PACKET_PCT_1, + SAL_LE_RAS_MODE_3_FILTER_PACKET_PCT_2, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PERMUTATION_INDEX, + SAL_LE_RAS_MODE_3_FILTER_TONE_PCT, + SAL_LE_RAS_MODE_3_FILTER_TONE_QUALITY_INDICATOR, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PATH_1, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PATH_2, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PATH_3, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PATH_4, + SAL_LE_RAS_MODE_3_FILTER_MAX, +}; + +/** + * @brief On-demand RAS Service State Machine. + * + * These enumeration values represent the different states of the On-demand + * Ranging Service procedure. The state machine controls the sequence of + * data ready notifications, data retrieval, sending, acknowledgment, and + * completion. + */ +enum { + /** Idle state. No operation in progress. */ + SAL_LE_RAS_ON_DEMAND_STATE_IDLE, + + /** Data ready indication has been sent to client. */ + SAL_LE_RAS_ON_DEMAND_STATE_DATA_READY_INDICATE, + + /** Waiting for Get Ranging Data response from client. */ + SAL_LE_RAS_ON_DEMAND_STATE_GET_RANGING_DATA_RSP, + + /** Ranging Data is being sent to client. */ + SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_SENDING, + + /** All Ranging Data segments have been sent. */ + SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_COMPLETE_SEND, + + /** Waiting for acknowledgment from client for Ranging Data. */ + SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_ACK_RECV, + + /** Waiting for Ranging Data response. */ + SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_RSP, + + /** Retrieving lost Ranging Data segments received from client. */ + SAL_LE_RAS_ON_DEMAND_STATE_RETRIEVE_LOST_RANGING_DATA_RECV, + + /** Lost Ranging Data segments response being sent. */ + SAL_LE_RAS_ON_DEMAND_STATE_RETRIEVE_LOST_RANGING_DATA_RSP, + + /** Completion of retrieving lost Ranging Data segments. */ + SAL_LE_RAS_ON_DEMAND_STATE_RETRIEVE_LOST_RANGING_DATA_COMPLETE, + + /** Waiting for acknowledgment of lost Ranging Data segments. */ + SAL_LE_RAS_ON_DEMAND_STATE_RETRIEVE_LOST_RANGING_DATA_ACK, + + /** Number of On-demand states. */ + SAL_LE_RAS_ON_DEMAND_STATE_NUMS, +}; + +/** + * @brief RAS Control Point Operation Codes. + * + * Enumeration of possible opcodes for RAS control point operations. + */ +typedef enum { + ABORT_OPERATION = 0, /**< Abort the ongoing Ranging operation. */ + OTHER_OPERATION = 1 /**< Placeholder for other operations. */ +} ras_opcode_t; + +/** + * @brief RAS Data Segment Structure. + * + * Represents a single segment of Ranging Data for On-demand transfers. + */ +typedef struct ras_segment_t { + cs_node_t seg_node; /**< Node for linked list of segments. */ + uint16_t seg_idx; /**< Segment index in the sequence. */ + uint16_t len; /**< Length of the segment data. */ + uint8_t data[RAS_EMPTY_ARRAY]; /**< Flexible array member for segment payload. */ +} ras_segment_t; + +/** + * @brief RAS On-demand Ranging Data Structure. + * + * Tracks the state of On-demand Ranging Data procedure for a client. + */ +typedef struct ras_rang_on_demand_t { + bool proc_used; /**< Indicates if this procedure slot is in use. */ + uint16_t count; /**< Count of segments or data items. */ + service_timer_t* on_demand_timer; /**< Timer/work item for response timeout. */ + cs_list_t seg_list; /**< Linked list of Ranging Data segments. */ + ras_segment_t* seg; /**< Pointer to the current segment being processed. */ +} ras_rang_on_demand_t; + +/** + * @brief RAS Control Point Status. + * + * Tracks the current operation and processing state for the RAS Control Point. + */ +typedef struct { + ras_opcode_t opcode; /**< Current opcode being processed. */ + int is_processing; /**< 1 if operation is in progress, 0 otherwise. */ + int is_data_pending; /**< 1 if data is pending to send, 0 otherwise. */ + uint8_t response_code; /**< Last response code sent to client. */ +} ras_control_point_t; + +/** + * @brief RAS Service Environment. + * + * Maintains all runtime state and data for the RAS BLE Service instance. + */ +typedef struct { + uint16_t step_data_attr_handle; /**< GATT handle of Step Data characteristic. */ + bt_address_t* addr; /**< Current BLE address reference. */ + uint8_t latest_local_steps[SAL_LE_RAS_STEP_DATA_BUF_LEN]; /**< Buffer for step or ranging data. */ + uint8_t rt_dt_ccc_cfg; /**< CCC configuration for Real-time Data characteristic. */ + uint8_t ras_dt_rd_indicating; /**< Flag indicating Ranging Data indication state. */ + uint8_t ras_role; /**< RAS role (Server/Client). */ + uint32_t ras_mtu; /**< Maximum Transfer Unit for RAS GATT operations. */ + uint32_t ras_seg_offset; /**< Offset in the current Ranging Data segment. */ + uint32_t remaining_len; /**< Remaining bytes to send in current operation. */ + uint8_t ras_seg_idx; /**< Index of the current Ranging Data segment. */ + uint32_t ras_feature; /**< Bitfield indicating RAS feature support. */ + uint32_t char_notify_state; /**< Bitfield tracking characteristic notification/indication state. */ + uint32_t ras_filter[SAL_LE_RAS_FILTER_MODE_MAX]; /**< Filter settings per RAS mode (0-3). */ + uint32_t on_demand_state; /**< Current state of the On-demand RAS procedure. */ + ras_rang_on_demand_t subevent[SAL_LE_RAS_STORE_PROCEDURE_NUM_MAX]; /**< Array of On-demand procedure slots. */ + ras_control_point_t control_point; /**< Control Point status for current operation. */ + cs_node_t* on_deman_curr_node; /**< Pointer to current node in On-demand segment list. */ +} ras_srv_env_t; + +/** + * @brief Write the step data for the CS Reflector. + * + * This function is used to send the latest ranging step data + * to the reflector device. + * + * @return 0 on success, or a negative error code on failure. + */ +int write_cs_reflector_step_data(void); + +/** + * @brief Enable the Channel Sounding (CS) feature. + * + * This function initializes and enables the local Core Controller + * to generate Ranging Data for RAS (Ranging Service) operations. + * + * @return 0 on success, or a negative error code on failure. + */ +int bt_cs_ras_enable(void); + +#ifdef CONFIG_BT_CS_TEST + +/** + * @brief Notify callback wrapper for test mode. + * + * Redirects the GATT notification callback to the test implementation. + */ +#define BT_GATT_NOTIFY_CB(attr, addr, value, len) bt_gatt_notify_cb_test(attr, addr, value, len) + +/** + * @brief Attribute read wrapper for test mode. + * + * Redirects the GATT attribute read operation to the test implementation. + */ +#define BT_GATT_ATTR_READ(addr, attr, buf, buf_len, offset, value, value_len) //bt_gatt_attr_read_test(addr, attr, buf, buf_len, offset, value, value_len) + +/** + * @brief GATT notify wrapper for test mode. + * + * Redirects the GATT notification operation to the test implementation. + */ +#define BT_GATT_NOTIFY(attr, addr, value, len) bt_gatt_notify_test(attr, addr, value, len) + +/** + * @brief GATT indicate wrapper for test mode. + * + * Redirects the GATT indication operation to the test implementation. + */ +#define BT_GATT_INDICATE(attr, addr, value, len) bt_gatt_indicate_test(attr, addr, value, len) + +#else + +/** + * @brief Notify callback wrapper for normal mode. + * + * Uses the standard GATT notification callback. + */ +#define BT_GATT_NOTIFY_CB(attr, addr, value, len) ras_gatts_data_send_notify(attr, addr, value, len, true) + +/** + * @brief GATT notify wrapper for normal mode. + * + * Uses the standard GATT notification function. + */ +#define BT_GATT_NOTIFY(attr, addr, value, len) ras_gatts_data_send_notify(attr, addr, value, len, true) + +/** + * @brief GATT indicate wrapper for normal mode. + * + * Uses the standard GATT indication function. + */ +#define BT_GATT_INDICATE(attr, addr, value, len) ras_gatts_data_send_notify(attr, addr, value, len, false) + +#endif /* CONFIG_BT_CS_TEST */ + +/** + * @brief Test helper: simulate receiving a RAS subevent. + * + * This function is used for test purposes to inject a subevent + * result into the RAS stack. + * + * @param mode The Ranging mode used for the test (e.g., Real-time or On-demand). + * @param test_case The specific test case scenario to simulate. + * @param addr Pointer to the bluetooth address associated with this subevent. + * @param result Pointer to the subevent result data to inject. + * + * @return 0 on success, or a negative error code on failure. + */ +int ras_subevent_recv_test(ras_rang_mode_t mode, ras_testcase_t test_case, + bt_address_t* addr, bt_srv_conn_le_cs_subevent_result_t* result); + +/** + * @brief Test helper: simulate sending a Control Point response. + * + * This function is used for testing the RAS Control Point procedure. + * + * @param addr Pointer to the bluetooth address to send the response to. + * @param data Pointer to the data to send. + * @param len Length of the data in bytes. + * + * @return 0 on success, or a negative error code on failure. + */ +int ras_ctrl_point_send_test(bt_address_t* addr, uint8_t* data, uint16_t len); + +void ras_on_demand_notify_finish_test(bt_address_t* addr); + +void ras_on_demand_indicate_finish_test(bt_address_t* addr, ras_attr_notify_t attr); +#endif /* _CS_RAS_H_ */ \ No newline at end of file diff --git a/service/profiles/include/cs_ras_common.h b/service/profiles/include/cs_ras_common.h new file mode 100644 index 000000000..66f2e0b6f --- /dev/null +++ b/service/profiles/include/cs_ras_common.h @@ -0,0 +1,33 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#ifndef _CS_RAS_COMMON_H_ +#define _CS_RAS_COMMON_H_ + +#include +#include + +#ifndef BIT +#define BIT(n) (1 << n) +#endif /* BIT */ + +#define RAS_FEATURE_REALTIME_RANG_DATA BIT(0) +#define RAS_FEATURE_RET_LOST_RANG_DATA_SEG BIT(1) +#define RAS_FEATURE_ABORT_OPERATION BIT(2) +#define RAS_FEATURE_FILTER_RANG_DATA BIT(3) + +#endif /* _CS_RAS_COMMON_H_ */ + diff --git a/service/profiles/include/cs_ras_gatts.h b/service/profiles/include/cs_ras_gatts.h new file mode 100644 index 000000000..a032b608e --- /dev/null +++ b/service/profiles/include/cs_ras_gatts.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef _CS_RAS_GATTS_H_ +#define _CS_RAS_GATTS_H_ + +#include "bt_addr.h" +#include "bt_gatt_defs.h" +#include "bt_status.h" + +#define BT_UUID_RANGING_VAL 0x185B +#define BT_UUID_RANG_FEAT_VAL 0x2C14 +#define BT_UUID_RANG_RTT_DT_VAL 0x2C15 +#define BT_UUID_RANG_ON_DEM_DT_VAL 0x2C16 +#define BT_UUID_RANG_RAS_CTR_POINT_VAL 0x2C17 +#define BT_UUID_RANG_DT_RD_VAL 0x2C18 +#define BT_UUID_RANG_DT_OV_WR_VAL 0x2C19 + +#define RAS_RTT_DATA_CCC_CFG_CHANGE_EVT (0) +#define RAS_ON_DEMAND_DATA_CCC_CFG_CHANGE_EVT (1) +#define RAS_CTR_PT_CCC_CFG_CHANGE_EVT (2) +#define RAS_DATA_READY_CCC_CFG_CHANGE_EVT (3) +#define RAS_OVER_WRITE_CCC_CFG_CHANGE_EVT (4) +typedef uint8_t ras_ccc_cfg_change_evt_t; + + +#define RAS_REAL_TIME_CHAR_SEND (0) +#define RAS_ON_DEMAND_CHAR_SEND (1) +#define RAS_DATA_READY_CHAR_SEND (2) +#define RAS_CONTROL_POINT_CHAR_SEND (3) +#define RAS_OVER_WRITE_CHAR_SEND (4) +typedef uint8_t ras_attr_notify_t; + +enum { + RAS_RANGING_SERVICE_ATTR_ID = 1, + RAS_RANGING_FEATURE_ATTR_ID, + RAS_RANGING_REAL_TIME_ATTR_ID, + RAS_RANGING_REAL_TIME_CCC_ID, + RAS_RANGING_ON_DEMAND_ATTR_ID, + RAS_RANGING_ON_DEMAND_CCC_ID, + RAS_RANGING_CONTROL_POINT_ATTR_ID, + RAS_RANGING_CONTROL_POINT_CCC_ID, + RAS_RANGING_DATA_READY_ATTR_ID, + RAS_RANGING_DATA_READY_CCC_ID, + RAS_RANGING_DATA_OVER_WRITE_ATTR_ID, + RAS_RANGING_DATA_OVER_WRITE_CCC_ID +}; + +typedef void (*ras_ccc_cfg_cb_t)(bt_address_t* addr, ras_ccc_cfg_change_evt_t event, + const uint8_t* value, uint16_t length); + +typedef void (*ras_ctr_pt_write_cb_t)(bt_address_t* addr, + const uint8_t* value, uint16_t length); + +typedef void (*ras_feature_read_cb_t)(bt_address_t* addr, uint32_t req_handle); + +typedef void (*ras_data_notify_cb_t)(ras_attr_notify_t attr, bt_address_t* addr); + +typedef void (*ras_mtu_updated_cb_t)(bt_address_t* addr, uint32_t mtu); + + typedef void (*ras_gatts_notify_complete_cb_t)(bt_address_t* addr, gatt_status_t status, + ras_attr_notify_t attr); + +typedef void (*ras_gatts_conn_cb_t)(bt_address_t* addr); +typedef void (*ras_gatts_disconn_cb_t)(bt_address_t* addr); + +typedef struct { + ras_ccc_cfg_cb_t cfg_cb; + ras_ctr_pt_write_cb_t pt_write_cb; + ras_feature_read_cb_t feature_read_cb; + ras_mtu_updated_cb_t mtu_updated_cb; + ras_gatts_notify_complete_cb_t notify_cb; + ras_gatts_conn_cb_t conn_cb; + ras_gatts_disconn_cb_t disconn_cb; +} ras_gatts_callbacks_t; + +bt_status_t ras_gatts_data_send_notify(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, + uint16_t len, bool is_notify); +void ras_gatts_set_feature_value(uint32_t feature); +void bt_cs_ras_gatts_init(const ras_gatts_callbacks_t* callback); +bt_status_t ras_send_feature_read_rsp(bt_address_t* addr, uint32_t feature); +void ras_gatts_deinit(void); + +#endif /* _CS_RAS_GATTS_H_ */ \ No newline at end of file diff --git a/service/profiles/include/cs_ras_test.h b/service/profiles/include/cs_ras_test.h new file mode 100644 index 000000000..1be42b92f --- /dev/null +++ b/service/profiles/include/cs_ras_test.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * Copyright (C) 2024 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#ifndef _CS_RAS_TEST_H_ +#define _CS_RAS_TEST_H_ + +#include "cs_ras_gatts.h" +#include "bt_addr.h" + +#define RAS_TEST_SUCESS (0) +#define RAS_TEST_FAIL (1) + +#define RAS_TESTCASE_REAL_TIME_NOTIFY_VALID_RANG_DATA_001 (0) +#define RAS_TESTCASE_REAL_TIME_INDICATE_VALID_RANG_DATA_002 (1) +#define RAS_TESTCASE_ON_DEMAND_NOTIFY_VALID_RANG_DATA_003 (3) +#define RAS_TESTCASE_ON_DEMAND_INDICATE_VALID_RANG_DATA_004 (4) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_1_005 (5) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_2_006 (6) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_3_007 (7) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_4_008 (8) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_5_009 (9) +#define RAS_TESTCASE_ON_DEMAND_WRITE_RANG_DATA_TIMWOUT_010 (10) + +typedef uint16_t ras_testcase_t; + +int bt_gatt_notify_cb_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len); + +ssize_t bt_gatt_attr_read_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len); + +int bt_gatt_notify_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len); + +int bt_gatt_indicate_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len); + +int cs_ras_subevent_recv_test(void* data, uint16_t len); +#endif /* _CS_RAS_TEST_H_ */ \ No newline at end of file diff --git a/service/profiles/include/cs_ras_util.h b/service/profiles/include/cs_ras_util.h new file mode 100644 index 000000000..647ff4b97 --- /dev/null +++ b/service/profiles/include/cs_ras_util.h @@ -0,0 +1,342 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef _CS_RAS_UTIL_H_ +#define _CS_RAS_UTIL_H_ + +#include +#include +#include + +#ifndef MIN +#define MIN(a, b) (a) < (b) ? (a) : (b) +#endif /* MIN */ + +#define CS_CONTAINER_OF(ptr, type, member) ({ \ + const typeof(((type *)0)->member) *__mptr = (ptr); \ + (type *)((char *)__mptr - offsetof(type, member)); \ +}) + +#define CS_UINT32_TO_BYTE_STREAM(p, u32) \ + { \ + *(p)++ = (uint8_t)((u32) >> 24); \ + *(p)++ = (uint8_t)((u32) >> 16); \ + *(p)++ = (uint8_t)((u32) >> 8); \ + *(p)++ = (uint8_t)(u32); \ + } + +#define CS_UINT16_TO_BYTE_STREAM(p, u16) \ + { \ + *(p)++ = (uint8_t)((u16) >> 8); \ + *(p)++ = (uint8_t)(u16); \ + } + +#define CS_UINT8_TO_BYTE_STREAM(p, u8) \ + { \ + *(p)++ = (uint8_t)(u8); \ + } + +#define CS_INT8_TO_BYTE_STREAM(p, u8) \ + { \ + *(p)++ = (int8_t)(u8); \ + } + +#define CS_ARRAY16_TO_BYTE_STREAM(p, a) \ + { \ + int ijk; \ + for (ijk = 0; ijk < 16; ijk++) \ + *(p)++ = (uint8_t)(a)[15 - ijk]; \ + } + +#define CS_ARRAY8_TO_BYTE_STREAM(p, a) \ + { \ + int ijk; \ + for (ijk = 0; ijk < 8; ijk++) \ + *(p)++ = (uint8_t)(a)[7 - ijk]; \ + } + +#define CS_ARRAY_TO_BYTE_STREAM(p, a, len) \ + { \ + int ijk; \ + for (ijk = 0; ijk < (len); ijk++) \ + *(p)++ = (uint8_t)(a)[ijk]; \ + } + +#define CS_BYTE_STREAM_TO_INT8(u8, p) \ + { \ + (u8) = (*((int8_t*)(p))); \ + (p) += 1; \ + } + +#define CS_BYTE_STREAM_TO_UINT8(u8, p) \ + { \ + (u8) = (uint8_t)(*(p)); \ + (p) += 1; \ + } + +#define CS_BYTE_STREAM_TO_UINT16(u16, p) \ + { \ + (u16) = ((uint16_t)(*((p) + 1)) + (((uint16_t)(*(p))) << 8)); \ + (p) += 2; \ + } + +#define CS_BYTE_STREAM_TO_UINT32(u32, p) \ + { \ + (u32) = (((uint32_t)(*((p) + 3))) + ((((uint32_t)(*((p) + 2)))) << 8) \ + + ((((uint32_t)(*((p) + 1)))) << 16) + ((((uint32_t)(*(p))) << 24))); \ + (p) += 4; \ + } + +#define CS_BYTE_STREAM_TO_ARRAY16(a, p) \ + { \ + int ijk; \ + uint8_t* _pa = (uint8_t*)(a) + 15; \ + for (ijk = 0; ijk < 16; ijk++) \ + *_pa-- = *(p)++; \ + } + +#define CS_BYTE_STREAM_TO_ARRAY8(a, p) \ + { \ + int ijk; \ + uint8_t* _pa = (uint8_t*)(a) + 7; \ + for (ijk = 0; ijk < 8; ijk++) \ + *_pa-- = *(p)++; \ + } + +#define CS_BYTE_STREAM_TO_ARRAY(a, p, len) \ + { \ + int ijk; \ + for (ijk = 0; ijk < (len); ijk++) \ + ((uint8_t*)(a))[ijk] = *(p)++; \ + } + +/** @cond INTERNAL_HIDDEN */ +struct _cs_node { + struct _cs_node *next; +}; +/** @endcond */ + +/** Single-linked list node structure. */ +typedef struct _cs_node cs_node_t; + +/** @cond INTERNAL_HIDDEN */ +struct _cs_list { + cs_node_t *head; + cs_node_t *tail; +}; +/** @endcond */ + +/** Single-linked list structure. */ +typedef struct _cs_list cs_list_t; + +/** + * @brief Initialize a list + * + * @param list A pointer on the list to initialize + */ +static inline void cs_list_init(cs_list_t *list) +{ + list->head = NULL; + list->tail = NULL; +} + +static inline cs_node_t *cs_node_next_peek(cs_node_t *node) +{ + return node->next; +} + +static inline void cs_node_next_set(cs_node_t *parent, cs_node_t *child) +{ + parent->next = child; +} + +static inline void cs_list_head_set(cs_list_t *list, cs_node_t *node) +{ + list->head = node; +} + +static inline void cs_list_tail_set(cs_list_t *list, cs_node_t *node) +{ + list->tail = node; +} + +/** + * @brief Peek the first node from the list + * + * @param list A point on the list to peek the first node from + * + * @return A pointer on the first node of the list (or NULL if none) + */ +static inline cs_node_t *cs_list_peek_head(cs_list_t *list) +{ + return list->head; +} + +/** + * @brief Peek the last node from the list + * + * @param list A point on the list to peek the last node from + * + * @return A pointer on the last node of the list (or NULL if none) + */ +static inline cs_node_t *cs_list_peek_tail(cs_list_t *list) +{ + return list->tail; +} + +static inline cs_node_t *cs_list_peek_next_no_check(cs_node_t *node) +{ + return cs_node_next_peek(node); +} + +static inline cs_node_t *cs_list_peek_next(cs_node_t *node) +{ + return (node != NULL) ? cs_list_peek_next_no_check(node) : NULL; +} + +/** + * @brief Remove a node + * + * This and other sys_list_*() functions are not thread safe. + * + * @param list A pointer on the list to affect + * @param prev_node A pointer on the previous node + * (can be NULL, which means the node is the list's head) + * @param node A pointer on the node to remove + */ +static inline void cs_list_remove(cs_list_t *list, + cs_node_t *prev_node, + cs_node_t *node) +{ + if (prev_node == NULL) { + cs_list_head_set(list, cs_node_next_peek(node)); + if (cs_list_peek_tail(list) == node) { + cs_list_tail_set(list, cs_list_peek_head(list)); + } + } else { + cs_node_next_set(prev_node, cs_node_next_peek(node)); + if (cs_list_peek_tail(list) == node) { + cs_list_tail_set(list, prev_node); + } + } + + cs_node_next_set(node, NULL); +} + +static inline void cs_list_append(cs_list_t *list, cs_node_t *node) +{ + cs_node_next_set(node, NULL); + + if (cs_list_peek_tail(list) == NULL) { + cs_list_tail_set(list, node); + cs_list_head_set(list, node); + } else { + cs_node_next_set(cs_list_peek_tail(list), node); + cs_list_tail_set(list, node); + } +} + +#define CS_GENLIST_FOR_EACH_NODE(__lname, __l, __sn) \ + for ((__sn) = cs_ ## __lname ## _peek_head(__l); (__sn) != NULL; \ + (__sn) = cs_ ## __lname ## _peek_next(__sn)) + +#define CS_GENLIST_ITERATE_FROM_NODE(__lname, __l, __sn) \ + for ((__sn) = (__sn) ? cs_ ## __lname ## _peek_next_no_check(__sn) \ + : cs_ ## __lname ## _peek_head(__l); \ + (__sn) != NULL; \ + (__sn) = cs_ ## __lname ## _peek_next(__sn)) + +#define CS_GENLIST_FOR_EACH_NODE_SAFE(__lname, __l, __sn, __sns) \ + for ((__sn) = cs_ ## __lname ## _peek_head(__l), \ + (__sns) = cs_ ## __lname ## _peek_next(__sn); \ + (__sn) != NULL ; (__sn) = (__sns), \ + (__sns) = cs_ ## __lname ## _peek_next(__sn)) + +#define CS_GENLIST_CONTAINER(__ln, __cn, __n) \ + ((__ln) ? CS_CONTAINER_OF((__ln), __typeof__(*(__cn)), __n) : NULL) + +#define CS_GENLIST_PEEK_HEAD_CONTAINER(__lname, __l, __cn, __n) \ + CS_GENLIST_CONTAINER(cs_ ## __lname ## _peek_head(__l), __cn, __n) + +#define CS_GENLIST_PEEK_TAIL_CONTAINER(__lname, __l, __cn, __n) \ + CS_GENLIST_CONTAINER(cs_ ## __lname ## _peek_tail(__l), __cn, __n) + +#define CS_GENLIST_PEEK_NEXT_CONTAINER(__lname, __cn, __n) \ + ((__cn) ? CS_GENLIST_CONTAINER( \ + cs_ ## __lname ## _peek_next(&((__cn)->__n)), \ + __cn, __n) : NULL) + +#define CS_GENLIST_FOR_EACH_CONTAINER(__lname, __l, __cn, __n) \ + for ((__cn) = CS_GENLIST_PEEK_HEAD_CONTAINER(__lname, __l, __cn, \ + __n); \ + (__cn) != NULL; \ + (__cn) = CS_GENLIST_PEEK_NEXT_CONTAINER(__lname, __cn, __n)) + +#define CS_GENLIST_FOR_EACH_CONTAINER_SAFE(__lname, __l, __cn, __cns, __n) \ + for ((__cn) = CS_GENLIST_PEEK_HEAD_CONTAINER(__lname, __l, __cn, __n), \ + (__cns) = CS_GENLIST_PEEK_NEXT_CONTAINER(__lname, __cn, __n); \ + (__cn) != NULL; (__cn) = (__cns), \ + (__cns) = CS_GENLIST_PEEK_NEXT_CONTAINER(__lname, __cn, __n)) + +#define CS_LIST_FOR_EACH_CONTAINER_SAFE(__sl, __cn, __cns, __n) \ + CS_GENLIST_FOR_EACH_CONTAINER_SAFE(list, __sl, __cn, __cns, __n) + +static inline void ras_state_set_bit(uint32_t *state, uint8_t bit) +{ + *state |= (1U << bit); +} + +static inline void ras_state_clear_bit(uint32_t *state, uint8_t bit) +{ + *state &= ~(1U << bit); +} + +static inline bool ras_state_get_bit(uint32_t *state, uint8_t bit) +{ + return ((*state >> bit) & 1U); +} + +static inline uint16_t ras_get_uint16_from_ptr(const uint8_t src[2]) +{ + return ((uint16_t)src[1] << 8) | src[0]; +} + +static inline void ras_put_uint16_to_ptr(uint16_t val, uint8_t dst[2]) +{ + dst[0] = val; + dst[1] = val >> 8; +} + +const char *cs_log_to_hex_str(const void *buf, size_t len); + +#endif /* _CS_RAS_GATTS_H_ */ \ No newline at end of file diff --git a/service/profiles/include/cs_service.h b/service/profiles/include/cs_service.h new file mode 100644 index 000000000..e3bc81dff --- /dev/null +++ b/service/profiles/include/cs_service.h @@ -0,0 +1,677 @@ +/**************************************************************************** + * Copyright (C) 2022 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#ifndef __CS_SERVICE_H__ +#define __CS_SERVICE_H__ + +/**************************************************************************** + * Included Files + ****************************************************************************/ +#include "bt_cs.h" +#include "cs_msg.h" + +typedef enum { + CS_BT_SRV_CONN_LE_CS_PROCEDURES_DISABLED, + CS_BT_SRV_CONN_LE_CS_PROCEDURES_ENABLED, +} bt_srv_conn_le_cs_procedure_enable_state_t; + +/** CS Test Tone Antennna Config Selection. + * + * These enum values are indices in the following table, where N_AP is the maximum + * number of antenna paths (in the range [1, 4]). + * + * +--------------+-------------+-------------------+-------------------+--------+ + * | Config Index | Total Paths | Dev A: # Antennas | Dev B: # Antennas | Config | + * +--------------+-------------+-------------------+-------------------+--------+ + * | 0 | 1 | 1 | 1 | 1:1 | + * | 1 | 2 | 2 | 1 | N_AP:1 | + * | 2 | 3 | 3 | 1 | N_AP:1 | + * | 3 | 4 | 4 | 1 | N_AP:1 | + * | 4 | 2 | 1 | 2 | 1:N_AP | + * | 5 | 3 | 1 | 3 | 1:N_AP | + * | 6 | 4 | 1 | 4 | 1:N_AP | + * | 7 | 4 | 2 | 2 | 2:2 | + * +--------------+-------------+-------------------+-------------------+--------+ + * + * There are therefore four groups of possible antenna configurations: + * + * - 1:1 configuration, where both A and B support 1 antenna each + * - 1:N_AP configuration, where A supports 1 antenna, B supports N_AP antennas, and + * N_AP is a value in the range [2, 4] + * - N_AP:1 configuration, where A supports N_AP antennas, B supports 1 antenna, and + * N_AP is a value in the range [2, 4] + * - 2:2 configuration, where both A and B support 2 antennas and N_AP = 4 + */ +typedef enum { + CS_BT_SRV_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_ONE, + CS_BT_SRV_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_TWO, + CS_BT_SRV_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_THREE, + CS_BT_SRV_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_FOUR, + CS_BT_SRV_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_FIVE, + CS_BT_SRV_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_SIX, + CS_BT_SRV_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_SEVEN, + CS_BT_SRV_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_EIGHT, +} bt_srv_conn_le_cs_tone_antenna_config_selection_t; + +/** Channel sounding main mode */ +typedef enum { + /** Mode-1 (RTT) */ + CS_BT_SRV_CONN_LE_CS_MAIN_MODE_1 = 0x01, + /** Mode-2 (PBR) */ + CS_BT_SRV_CONN_LE_CS_MAIN_MODE_2, + /** Mode-3 (RTT and PBR) */ + CS_BT_SRV_CONN_LE_CS_MAIN_MODE_3, +} bt_srv_conn_le_cs_main_mode_t; + +/** Channel sounding sub mode */ +typedef enum { + /** Mode-1 (RTT) */ + CS_BT_SRV_CONN_LE_CS_SUB_MODE_1 = 0x01, + /** Mode-2 (PBR) */ + CS_BT_SRV_CONN_LE_CS_SUB_MODE_2, + /** Mode-3 (RTT and PBR) */ + CS_BT_SRV_CONN_LE_CS_SUB_MODE_3, + /** Unused */ + CS_BT_SRV_CONN_LE_CS_SUB_MODE_UNUSED = 0xFF, +} bt_srv_conn_le_cs_sub_mode_t; + +/** Channel sounding role */ +typedef enum { + /** CS initiator role */ + CS_BT_SRV_CONN_LE_CS_ROLE_INITIATOR, + /** CS reflector role */ + CS_BT_SRV_CONN_LE_CS_ROLE_REFLECTOR, +} bt_srv_conn_le_cs_role_t; + +/** Channel sounding RTT type */ +typedef enum { + /** RTT AA only */ + CS_BT_SRV_CONN_LE_CS_RTT_TYPE_AA_ONLY, + /** RTT with 32-bit sounding sequence */ + CS_BT_SRV_CONN_LE_CS_RTT_TYPE_32_BIT_SOUNDING, + /** RTT with 96-bit sounding sequence */ + CS_BT_SRV_CONN_LE_CS_RTT_TYPE_96_BIT_SOUNDING, + /** RTT with 32-bit random sequence */ + CS_BT_SRV_CONN_LE_CS_RTT_TYPE_32_BIT_RANDOM, + /** RTT with 64-bit random sequence */ + CS_BT_SRV_CONN_LE_CS_RTT_TYPE_64_BIT_RANDOM, + /** RTT with 96-bit random sequence */ + CS_BT_SRV_CONN_LE_CS_RTT_TYPE_96_BIT_RANDOM, + /** RTT with 128-bit random sequence */ + CS_BT_SRV_CONN_LE_CS_RTT_TYPE_128_BIT_RANDOM, +} bt_srv_conn_le_cs_rtt_type_t; + +/** Channel sounding PHY used for CS sync */ +typedef enum { + /** LE 1M PHY */ + CS_BT_SRV_CONN_LE_CS_SYNC_1M_PHY = 0x01, + /** LE 2M PHY */ + CS_BT_SRV_CONN_LE_CS_SYNC_2M_PHY, + /** LE 2M 2BT PHY */ + CS_BT_SRV_CONN_LE_CS_SYNC_2M_2BT_PHY, +} bt_srv_conn_le_cs_sync_phy_t; + +/** Channel sounding channel selection type */ +typedef enum { + /** Use Channel Selection Algorithm #3b for non-mode-0 CS steps */ + CS_BT_SRV_CONN_LE_CS_CHSEL_TYPE_3B, + /** Use Channel Selection Algorithm #3c for non-mode-0 CS steps */ + CS_BT_SRV_CONN_LE_CS_CHSEL_TYPE_3C, +} bt_srv_conn_le_cs_chsel_type_t; + +/** Channel sounding channel sequence shape */ +typedef enum { + /** Use Hat shape for user-specified channel sequence */ + CS_BT_SRV_CONN_LE_CS_CH3C_SHAPE_HAT, + /** Use X shape for user-specified channel sequence */ + CS_BT_SRV_CONN_LE_CS_CH3C_SHAPE_X, +} bt_srv_conn_le_cs_ch3c_shape_t; + +/** Supported AA-Only RTT precision. */ +typedef enum { + /** AA-Only RTT variant is not supported. */ + CS_BT_SRV_CONN_LE_CS_RTT_AA_ONLY_NOT_SUPP = 0, + /** 10ns time-of-flight accuracy. */ + CS_BT_SRV_CONN_LE_CS_RTT_AA_ONLY_10NS, + /** 150ns time-of-flight accuracy. */ + CS_BT_SRV_CONN_LE_CS_RTT_AA_ONLY_150NS, +} bt_srv_conn_le_cs_capability_rtt_aa_only_t; + +/** Supported Sounding Sequence RTT precision. */ +typedef enum { + /** Sounding Sequence RTT variant is not supported. */ + CS_BT_SRV_CONN_LE_CS_RTT_SOUNDING_NOT_SUPP = 0, + /** 10ns time-of-flight accuracy. */ + CS_BT_SRV_CONN_LE_CS_RTT_SOUNDING_10NS, + /** 150ns time-of-flight accuracy. */ + CS_BT_SRV_CONN_LE_CS_RTT_SOUNDING_150NS, +} bt_srv_conn_le_cs_capability_rtt_sounding_t; + +/** Supported Random Payload RTT precision. */ +typedef enum { + /** Random Payload RTT variant is not supported. */ + CS_BT_SRV_CONN_LE_CS_RTT_RANDOM_PAYLOAD_NOT_SUPP = 0, + /** 10ns time-of-flight accuracy. */ + CS_BT_SRV_CONN_LE_CS_RTT_RANDOM_PAYLOAD_10NS, + /** 150ns time-of-flight accuracy. */ + CS_BT_SRV_CONN_LE_CS_RTT_RANDOM_PAYLOAD_150NS, +} bt_srv_conn_le_cs_capability_rtt_random_payload_t; + +typedef enum { + /** Use antenna identifier 1 for CS_SYNC packets. */ + BT_LE_SRV_CS_ANTENNA_SELECTION_OPT_ONE = 0x01, + /** Use antenna identifier 2 for CS_SYNC packets. */ + BT_LE_SRV_CS_ANTENNA_SELECTION_OPT_TWO = 0x02, + /** Use antenna identifier 3 for CS_SYNC packets. */ + BT_LE_SRV_CS_ANTENNA_SELECTION_OPT_THREE = 0x03, + /** Use antenna identifier 4 for CS_SYNC packets. */ + BT_LE_SRV_CS_ANTENNA_SELECTION_OPT_FOUR = 0x04, + /** Use antennas in repetitive order from 1 to 4 for CS_SYNC packets. */ + BT_LE_SRV_CS_ANTENNA_SELECTION_OPT_REPETITIVE = 0xFE, + /** No recommendation for local controller antenna selection. */ + BT_LE_SRV_CS_ANTENNA_SELECTION_OPT_NO_RECOMMENDATION = 0xFF, +} bt_le_srv_cs_sync_antenna_selection_opt_t; + +/** CS Test Initiator SNR control options */ +typedef enum { + BT_LE_SRV_CS_INITIATOR_SNR_CONTROL_18dB = 0x01, + BT_LE_SRV_CS_INITIATOR_SNR_CONTROL_21dB, + BT_LE_SRV_CS_INITIATOR_SNR_CONTROL_24dB, + BT_LE_SRV_CS_INITIATOR_SNR_CONTROL_27dB, + BT_LE_SRV_CS_INITIATOR_SNR_CONTROL_30dB, + BT_LE_SRV_CS_INITIATOR_SNR_CONTROL_NOT_USED = 0xFF, +} bt_le_srv_cs_initiator_snr_control_t; + +typedef enum { + BT_LE_SRV_CS_PROCEDURE_PHY_1M = 0x01, + BT_LE_SRV_CS_PROCEDURE_PHY_2M, + BT_LE_SRV_CS_PROCEDURE_PHY_CODED_S8, + BT_LE_SRV_CS_PROCEDURE_PHY_CODED_S2, +} bt_le_srv_cs_procedure_phy_t; + +/** CS config creation context */ +typedef enum { + /** Write CS configuration in local Controller only */ + BT_LE_SRV_CS_CREATE_CONFIG_CONTEXT_LOCAL_ONLY, + /** Write CS configuration in both local and remote Controller using Channel Sounding + * Configuration procedure + */ + BT_LE_SRV_CS_CREATE_CONFIG_CONTEXT_LOCAL_AND_REMOTE, +} bt_le_srv_cs_create_config_context_t; + +/** Procedure done status */ +typedef enum { + BT_LE_SRV_CS_PROCEDURE_COMPLETE = 0x0, + BT_LE_SRV_CS_PROCEDURE_INCOMPLETE = 0x1, + BT_LE_SRV_CS_PROCEDURE_ABORTED = 0xF, +} bt_le_srv_cs_procedure_done_status_t; + +/** Subevent done status */ +typedef enum { + BT_LE_SRV_CS_SUBEVENT_COMPLETE = 0x1, + BT_LE_SRV_CS_SUBEVENT_ABORTED = 0xF, +} bt_le_srv_le_cs_subevent_done_status_t; + +/** Procedure abort reason */ +typedef enum { + BT_LE_SRV_CS_PROCEDURE_NOT_ABORTED = 0x0, + BT_LE_SRV_CS_PROCEDURE_ABORT_REQUESTED = 0x1, + BT_LE_SRV_CS_PROCEDURE_ABORT_TOO_FEW_CHANNELS = 0x2, + BT_LE_SRV_CS_PROCEDURE_ABORT_CHMAP_INSTANT_PASSED = 0x3, + BT_LE_SRV_CS_PROCEDURE_ABORT_UNSPECIFIED = 0xF, +} bt_le_srv_cs_procedure_abort_reason_t; + +/** Subevent abort reason */ +typedef enum { + BT_LE_SRV_CS_SUBEVENT_NOT_ABORTED = 0x0, + BT_LE_SRV_CS_SUBEVENT_ABORT_REQUESTED = 0x1, + BT_LE_SRV_CS_SUBEVENT_ABORT_NO_CS_SYNC = 0x2, + BT_LE_SRV_CS_SUBEVENT_ABORT_SCHED_CONFLICT = 0x3, + BT_LE_SRV_CS_SUBEVENT_ABORT_UNSPECIFIED = 0xF, +} bt_le_srv_cs_subevent_abort_reason_t; + +typedef struct { + /** CS configuration ID */ + uint8_t id; + /** Main CS mode type */ + bt_srv_conn_le_cs_main_mode_t main_mode_type; + /** Sub CS mode type */ + bt_srv_conn_le_cs_sub_mode_t sub_mode_type; + /** Minimum number of CS main mode steps to be executed before a submode step is executed */ + uint8_t min_main_mode_steps; + /** Maximum number of CS main mode steps to be executed before a submode step is executed */ + uint8_t max_main_mode_steps; + /** Number of main mode steps taken from the end of the last CS subevent to be repeated + * at the beginning of the current CS subevent directly after the last mode-0 step of that + * event + */ + uint8_t main_mode_repetition; + /** Number of CS mode-0 steps to be included at the beginning of each CS subevent */ + uint8_t mode_0_steps; + /** CS role */ + bt_srv_conn_le_cs_role_t role; + /** RTT type */ + bt_srv_conn_le_cs_rtt_type_t rtt_type; + /** CS Sync PHY */ + bt_srv_conn_le_cs_sync_phy_t cs_sync_phy; + /** The number of times the Channel_Map field will be cycled through for non-mode-0 steps + * within a CS procedure + */ + uint8_t channel_map_repetition; + /** Channel selection type */ + bt_srv_conn_le_cs_chsel_type_t channel_selection_type; + /** User-specified channel sequence shape */ + bt_srv_conn_le_cs_ch3c_shape_t ch3c_shape; + /** Number of channels skipped in each rising and falling sequence */ + uint8_t ch3c_jump; + /** Channel map used for CS procedure + * Channels n = 0, 1, 23, 24, 25, 77, and 78 are not allowed and shall be set to zero. + * Channel 79 is reserved for future use and shall be set to zero. + * At least 15 channels shall be enabled. + */ + uint8_t channel_map[10]; +} bt_le_srv_cs_create_config_params_t; + +typedef struct { + uint8_t config_id; + uint8_t enable; +} bt_le_srv_cs_procedure_enable_param_t; + +typedef struct { + /* The ID associated with the desired configuration (0 to 3) */ + uint8_t config_id; + + /* Max. duration for each CS procedure, where T = N * 0.625 ms (0x0001 to 0xFFFF) */ + uint16_t max_procedure_len; + + /* Min. number of connection events between consecutive CS procedures (0x0001 to 0xFFFF) */ + uint16_t min_procedure_interval; + + /* Max. number of connection events between consecutive CS procedures (0x0001 to 0xFFFF) */ + uint16_t max_procedure_interval; + + /* Max. number of procedures to be scheduled (0x0000 for no limit; otherwise 0x0001 + * to 0xFFFF) + */ + uint16_t max_procedure_count; + + /* Min. suggested duration for each CS subevent in microseconds (1250 us to 4 s) */ + uint32_t min_subevent_len; + + /* Max. suggested duration for each CS subevent in microseconds (1250 us to 4 s) */ + uint32_t max_subevent_len; + + /* Antenna configuration index */ + bt_srv_conn_le_cs_tone_antenna_config_selection_t tone_antenna_config_selection; + + /* Phy */ + bt_le_srv_cs_procedure_phy_t phy; + + /* Transmit power delta, in signed dB, to indicate the recommended difference between the + * remote device's power level for the CS tones and RTT packets and the existing power + * level for the Phy indicated by the Phy parameter (0x80 for no recommendation) + */ + int8_t tx_power_delta; + + /* Preferred peer antenna (Bitmask of BT_LE_CS_PROCEDURE_PREFERRED_PEER_ANTENNA_*) */ + uint8_t preferred_peer_antenna; + + /* Initiator SNR control adjustment */ + bt_le_srv_cs_initiator_snr_control_t snr_control_initiator; + + /* Reflector SNR control adjustment */ + bt_le_srv_cs_initiator_snr_control_t snr_control_reflector; +} bt_le_srv_cs_set_procedure_parameters_param_t; + +/** Default CS settings in the local Controller */ +typedef struct { + /** Enable CS initiator role. */ + bool enable_initiator_role; + /** Enable CS reflector role. */ + bool enable_reflector_role; + /** Antenna identifier to be used for CS_SYNC packets by the local controller. + */ + bt_le_srv_cs_sync_antenna_selection_opt_t cs_sync_antenna_selection; + /** Maximum output power (Effective Isotropic Radiated Power) to be used + * for all CS transmissions. + * + * Value range is @ref BT_HCI_OP_LE_CS_MIN_MAX_TX_POWER to + * @ref BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER. + */ + uint8_t max_tx_power; +} bt_le_srv_cs_set_default_settings_param_t; + +/** Remote channel sounding capabilities for LE connections supporting CS */ +typedef struct { + /** Number of CS configurations */ + uint8_t num_config_supported; + /** Maximum number of consecutive CS procedures. + * + * When set to zero, indicates support for both fixed and indefinite + * numbers of CS procedures before termination. + */ + uint16_t max_consecutive_procedures_supported; + /** Number of antennas. */ + uint8_t num_antennas_supported; + /** Maximum number of antenna paths. */ + uint8_t max_antenna_paths_supported; + /** Initiator role. */ + bool initiator_supported; + /** Reflector role. */ + bool reflector_supported; + /** Mode-3 */ + bool mode_3_supported; + /** RTT AA-Only */ + bt_srv_conn_le_cs_capability_rtt_aa_only_t rtt_aa_only_precision; + /** RTT Sounding */ + bt_srv_conn_le_cs_capability_rtt_sounding_t rtt_sounding_precision; + /** RTT Random Payload */ + bt_srv_conn_le_cs_capability_rtt_random_payload_t rtt_random_payload_precision; + /** Number of CS steps needed to achieve the + * accuracy requirements for RTT AA Only. + * + * Set to 0 if RTT AA Only isn't supported. + */ + uint8_t rtt_aa_only_n; + /** Number of CS steps needed to achieve the + * accuracy requirements for RTT Sounding. + * + * Set to 0 if RTT Sounding isn't supported + */ + uint8_t rtt_sounding_n; + /** Number of CS steps needed to achieve the + * accuracy requirements for RTT Random Payload. + * + * Set to 0 if RTT Random Payload isn't supported. + */ + uint8_t rtt_random_payload_n; + /** Phase-based normalized attack detector metric + * when a CS_SYNC with sounding sequence is received. + */ + bool phase_based_nadm_sounding_supported; + /** Phase-based normalized attack detector metric + * when a CS_SYNC with random sequence is received. + */ + bool phase_based_nadm_random_supported; + /** CS_SYNC LE 2M PHY. */ + bool cs_sync_2m_phy_supported; + /** CS_SYNC LE 2M 2BT PHY. */ + bool cs_sync_2m_2bt_phy_supported; + /** Subfeature: CS with no Frequency Actuation Error. */ + bool cs_without_fae_supported; + /** Subfeature: Channel Selection Algorithm #3c */ + bool chsel_alg_3c_supported; + /** Subfeature: Phase-based Ranging from RTT sounding sequence. */ + bool pbr_from_rtt_sounding_seq_supported; + /** Optional T_IP1 time durations during CS steps. + * + * - Bit 0: 10 us + * - Bit 1: 20 us + * - Bit 2: 30 us + * - Bit 3: 40 us + * - Bit 4: 50 us + * - Bit 5: 60 us + * - Bit 6: 80 us + */ + uint16_t t_ip1_times_supported; + /** Optional T_IP2 time durations during CS steps. + * + * - Bit 0: 10 us + * - Bit 1: 20 us + * - Bit 2: 30 us + * - Bit 3: 40 us + * - Bit 4: 50 us + * - Bit 5: 60 us + * - Bit 6: 80 us + */ + uint16_t t_ip2_times_supported; + /** Optional T_FCS time durations during CS steps. + * + * - Bit 0: 15 us + * - Bit 1: 20 us + * - Bit 2: 30 us + * - Bit 3: 40 us + * - Bit 4: 50 us + * - Bit 5: 60 us + * - Bit 6: 80 us + * - Bit 7: 100 us + * - Bit 8: 120 us + */ + uint16_t t_fcs_times_supported; + /** Optional T_PM time durations during CS steps. + * + * - Bit 0: 10 us + * - Bit 1: 20 us + */ + uint16_t t_pm_times_supported; + /** Time in microseconds for the antenna switch period of the CS tones. */ + uint8_t t_sw_time; + /** Supported SNR levels used in RTT packets. + * + * - Bit 0: 18dB + * - Bit 1: 21dB + * - Bit 2: 24dB + * - Bit 3: 27dB + * - Bit 4: 30dB + */ + uint8_t tx_snr_capability; +} bt_srv_conn_le_cs_capabilities_t; + +typedef struct { + /** CS configuration ID */ + uint8_t id; + /** Main CS mode type */ + bt_srv_conn_le_cs_main_mode_t main_mode_type; + /** Sub CS mode type */ + bt_srv_conn_le_cs_sub_mode_t sub_mode_type; + /** Minimum number of CS main mode steps to be executed before a submode step is executed */ + uint8_t min_main_mode_steps; + /** Maximum number of CS main mode steps to be executed before a submode step is executed */ + uint8_t max_main_mode_steps; + /** Number of main mode steps taken from the end of the last CS subevent to be repeated + * at the beginning of the current CS subevent directly after the last mode-0 step of that + * event + */ + uint8_t main_mode_repetition; + /** Number of CS mode-0 steps to be included at the beginning of each CS subevent */ + uint8_t mode_0_steps; + /** CS role */ + bt_srv_conn_le_cs_role_t role; + /** RTT type */ + bt_srv_conn_le_cs_rtt_type_t rtt_type; + /** CS Sync PHY */ + bt_srv_conn_le_cs_sync_phy_t cs_sync_phy; + /** The number of times the Channel_Map field will be cycled through for non-mode-0 steps + * within a CS procedure + */ + uint8_t channel_map_repetition; + /** Channel selection type */ + bt_srv_conn_le_cs_chsel_type_t channel_selection_type; + /** User-specified channel sequence shape */ + bt_srv_conn_le_cs_ch3c_shape_t ch3c_shape; + /** Number of channels skipped in each rising and falling sequence */ + uint8_t ch3c_jump; + /** Interlude time in microseconds between the RTT packets */ + uint8_t t_ip1_time_us; + /** Interlude time in microseconds between the CS tones */ + uint8_t t_ip2_time_us; + /** Time in microseconds for frequency changes */ + uint8_t t_fcs_time_us; + /** Time in microseconds for the phase measurement period of the CS tones */ + uint8_t t_pm_time_us; + /** Channel map used for CS procedure + * Channels n = 0, 1, 23, 24, 25, 77, and 78 are not allowed and shall be set to zero. + * Channel 79 is reserved for future use and shall be set to zero. + * At least 15 channels shall be enabled. + */ + uint8_t channel_map[10]; +} bt_srv_conn_le_cs_config_t; + +typedef struct { + /* The ID associated with the desired configuration (0 to 3) */ + uint8_t config_id; + + /* State of the CS procedure */ + bt_srv_conn_le_cs_procedure_enable_state_t state; + + /* Antenna configuration index */ + bt_srv_conn_le_cs_tone_antenna_config_selection_t tone_antenna_config_selection; + + /* Transmit power level used for CS procedures (-127 to 20 dB; 0x7F if unavailable) */ + int8_t selected_tx_power; + + /* Duration of each CS subevent in microseconds (1250 us to 4 s) */ + uint32_t subevent_len; + + /* Number of CS subevents anchored off the same ACL connection event (0x01 to 0x20) */ + uint8_t subevents_per_event; + + /* Time between consecutive CS subevents anchored off the same ACL connection event in + * units of 0.625 ms + */ + uint16_t subevent_interval; + + /* Number of ACL connection events between consecutive CS event anchor points */ + uint16_t event_interval; + + /* Number of ACL connection events between consecutive CS procedure anchor points */ + uint16_t procedure_interval; + + /* Number of CS procedures to be scheduled (0 if procedures to continue until disabled) */ + uint16_t procedure_count; + + /* Maximum duration for each procedure in units of 0.625 ms (0x0001 to 0xFFFF) */ + uint16_t max_procedure_len; +} bt_srv_conn_le_cs_procedure_enable_complete_t; + +typedef struct { + struct { + /** CS configuration identifier. + * + * Range: 0 to 3 + * + * If these results were generated by a CS Test, + * this value will be set to 0 and has no meaning. + */ + uint8_t config_id; + /** Starting ACL connection event counter. + * + * If these results were generated by a CS Test, + * this value will be set to 0 and has no meaning. + */ + uint16_t start_acl_conn_event; + /** CS procedure count associated with these results. + * + * This is the CS procedure count since the completion of + * the Channel Sounding Security Start procedure. + */ + uint16_t procedure_counter; + /** Frequency compensation value in units of 0.01 ppm. + * + * This is a 15-bit signed integer in the range [-100, 100] ppm. + * + * indicates that the role is not the initiator, or that the + * frequency compensation value is unavailable. + */ + uint16_t frequency_compensation; + /** Reference power level in dBm. + * + * Range: -127 to 20 + * + * that the reference power level was not available during a subevent. + */ + int8_t reference_power_level; + /** Procedure status. */ + bt_le_srv_cs_procedure_done_status_t procedure_done_status; + /** Subevent status + * + * For aborted subevents, + * and abort_step will contain the step number on which the subevent was aborted. + * Consider the following example: + * + * subevent_done_status = @ref SAL_LE_CS_SUBEVENT_ABORTED + * num_steps_reported = 160 + * abort_step = 100 + * + * this would mean that steps from 0 to 99 are complete and steps from 100 to 159 + * are aborted. + */ + bt_le_srv_le_cs_subevent_done_status_t subevent_done_status; + /** Abort reason. + * + * If the procedure status is + * @ref SAL_LE_CS_PROCEDURE_ABORTED, this field will + * specify the reason for the abortion. + */ + bt_le_srv_cs_procedure_abort_reason_t procedure_abort_reason; + /** Abort reason. + * + * If the subevent status is + * @ref SAL_LE_CS_SUBEVENT_ABORTED, this field will + * specify the reason for the abortion. + */ + bt_le_srv_cs_subevent_abort_reason_t subevent_abort_reason; + /** Number of antenna paths used during the phase measurement stage. + */ + uint8_t num_antenna_paths; + /** Number of CS steps in the subevent. + */ + uint8_t num_steps_reported; + /** Step number, on which the subevent was aborted + * if subevent_done_status is @ref SAL_LE_CS_SUBEVENT_COMPLETE + * then abort_step will be unused and set to 255 + */ + uint8_t abort_step; + } header; + + uint16_t len; + uint8_t step_data_buf[0]; +} bt_srv_conn_le_cs_subevent_result_t; + +typedef struct { + size_t size; + + /** + * @brief Register the cs event callback + * @param[in] callbacks cs event callback function. + */ + void* (*register_callbacks)(void* remote, const cs_callbacks_t* callbacks); + + /** + * @brief Unregister the cs event callback + */ + bool (*unregister_callbacks)(void** remote, void* cookie); + bt_status_t (*start_distance_measurement)(bt_distance_measurement_params_t* params); + + bt_status_t (*stop_distance_measurement)(bt_address_t* addr, int method, bool timeout); + + bt_status_t (*subevent_result_callbacks)(bt_address_t* addr, void* data); + + bt_status_t (*cs_test)(void* data, uint16_t len); + +} bt_cs_interface_t; + +typedef void (*subevent_result_cb_t)(bt_address_t* addr, bt_srv_conn_le_cs_subevent_result_t* result); + +/* + * register profile to service manager + */ +void bt_register_cs_service(void); + +void bt_sal_cs_event_callback(cs_msg_t* msg); + +void bt_cs_register_subevent_cb(subevent_result_cb_t* cb); + +#endif /* __CS_SERVICE_H__ */ diff --git a/service/profiles/include/cs_state_machine.h b/service/profiles/include/cs_state_machine.h new file mode 100644 index 000000000..b9a3d6065 --- /dev/null +++ b/service/profiles/include/cs_state_machine.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (C) 2025 Xiaomi InC. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef __CS_STATE_MACHINE_H__ +#define __CS_STATE_MACHINE_H__ + +#include "bt_device.h" + +typedef enum { + CS_STATE_STOPPED, + CS_STATE_INIT, + CS_STATE_CONNECTED, + CS_STATE_WAIT_FOR_CONFIG_COMPLETE, + CS_STATE_WAIT_FOR_SECURITY_COMPLETE, + CS_STATE_WAIT_FOR_PROCEDURE_COMPLETE, + CS_STATE_START +} cs_state_t; + +typedef enum { + CS_CONNECT, + CS_DISCONNECT, +} cs_event_t; + +typedef struct _cs_state_machine cs_state_machine_t; + +typedef struct { + struct list_node node; + cs_state_machine_t* cs_sm; + bt_address_t bd_addr; +} cs_device_t; + +cs_state_machine_t* cs_state_machine_new(void* context, bt_address_t* bd_addr); +void cs_state_machine_handle_event(cs_state_machine_t* sm, cs_msg_t* msg); +#endif \ No newline at end of file diff --git a/service/profiles/include/gatts_event.h b/service/profiles/include/gatts_event.h index 9a9ce1549..cf4d90159 100644 --- a/service/profiles/include/gatts_event.h +++ b/service/profiles/include/gatts_event.h @@ -28,6 +28,37 @@ * Pre-processor Definitions ****************************************************************************/ + /** Procedure done status */ +typedef enum { + GATTS_LE_CS_PROCEDURE_COMPLETE = 0x0, + GATTS_LE_CS_PROCEDURE_INCOMPLETE = 0x1, + GATTS_LE_CS_PROCEDURE_ABORTED = 0xF, +} gatts_le_cs_procedure_done_status_t; + +/** Subevent done status */ +typedef enum { + GATTS_LE_CS_SUBEVENT_COMPLETE = 0x1, + GATTS_LE_CS_SUBEVENT_ABORTED = 0xF, +} gatts_le_cs_subevent_done_status_t; + +/** Procedure abort reason */ +typedef enum { + GATTS_LE_CS_PROCEDURE_NOT_ABORTED = 0x0, + GATTS_LE_CS_PROCEDURE_ABORT_REQUESTED = 0x1, + GATTS_LE_CS_PROCEDURE_ABORT_TOO_FEW_CHANNELS = 0x2, + GATTS_LE_CS_PROCEDURE_ABORT_CHMAP_INSTANT_PASSED = 0x3, + GATTS_LE_CS_PROCEDURE_ABORT_UNSPECIFIED = 0xF, +} gatts_le_cs_procedure_abort_reason_t; + +/** Subevent abort reason */ +typedef enum { + GATTS_LE_CS_SUBEVENT_NOT_ABORTED = 0x0, + GATTS_LE_CS_SUBEVENT_ABORT_REQUESTED = 0x1, + GATTS_LE_CS_SUBEVENT_ABORT_NO_CS_SYNC = 0x2, + GATTS_LE_CS_SUBEVENT_ABORT_SCHED_CONFLICT = 0x3, + GATTS_LE_CS_SUBEVENT_ABORT_UNSPECIFIED = 0xF, +} gatts_le_cs_subevent_abort_reason_t; + typedef enum { GATTS_EVENT_ATTR_TABLE_ADDED, GATTS_EVENT_ATTR_TABLE_REMOVED, diff --git a/service/profiles/include/gatts_service.h b/service/profiles/include/gatts_service.h index 0ea4dcd1f..c9b9766fe 100644 --- a/service/profiles/include/gatts_service.h +++ b/service/profiles/include/gatts_service.h @@ -23,6 +23,7 @@ #include "bt_gatt_defs.h" #include "bt_gatts.h" #include "gatt_define.h" +#include "sal_le_cs_interface.h" /* * sal callback diff --git a/service/profiles/service_manager.h b/service/profiles/service_manager.h index 08ba261b9..f9557a9d0 100644 --- a/service/profiles/service_manager.h +++ b/service/profiles/service_manager.h @@ -15,6 +15,7 @@ ***************************************************************************/ #ifndef _BT_SERVICE_MANAGER_H__ #define _BT_SERVICE_MANAGER_H__ +#include #include "bt_profile.h" #include "bt_status.h" diff --git a/service/src/btservice.c b/service/src/btservice.c index 42b5a78bb..686a0464c 100644 --- a/service/src/btservice.c +++ b/service/src/btservice.c @@ -20,6 +20,7 @@ #include "stack_manager.h" #include "state_machine.h" #include "storage.h" +#include "cs_service.h" #ifdef CONFIG_BLUETOOTH_HFP_HF #include "hfp_hf_service.h" @@ -180,6 +181,9 @@ void bt_profile_init(void) #ifdef CONFIG_BLUETOOTH_LEAUDIO_VMICP register_lea_vmicp_service(); #endif +#ifdef CONFIG_BLUETOOTH_LE_CS + bt_register_cs_service(); +#endif /* CONFIG_BLUETOOTH_LE_CS */ } static int create_bt_folder(void) diff --git a/service/stacks/include/sal_le_cs_interface.h b/service/stacks/include/sal_le_cs_interface.h new file mode 100644 index 000000000..5335956c6 --- /dev/null +++ b/service/stacks/include/sal_le_cs_interface.h @@ -0,0 +1,124 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#ifndef __SAL_LE_CS_INTERFACE_H__ +#define __SAL_LE_CS_INTERFACE_H__ + +#include "bluetooth.h" +#include +#include + +typedef struct { + struct { + /** CS configuration identifier. + * + * Range: 0 to 3 + * + * If these results were generated by a CS Test, + * this value will be set to 0 and has no meaning. + */ + uint8_t config_id; + /** Starting ACL connection event counter. + * + * If these results were generated by a CS Test, + * this value will be set to 0 and has no meaning. + */ + uint16_t start_acl_conn_event; + /** CS procedure count associated with these results. + * + * This is the CS procedure count since the completion of + * the Channel Sounding Security Start procedure. + */ + uint16_t procedure_counter; + /** Frequency compensation value in units of 0.01 ppm. + * + * This is a 15-bit signed integer in the range [-100, 100] ppm. + * + * indicates that the role is not the initiator, or that the + * frequency compensation value is unavailable. + */ + uint16_t frequency_compensation; + /** Reference power level in dBm. + * + * Range: -127 to 20 + * + * that the reference power level was not available during a subevent. + */ + int8_t reference_power_level; + /** Procedure status. */ + bt_le_srv_cs_procedure_done_status_t procedure_done_status; + /** Subevent status + * + * For aborted subevents, + * and abort_step will contain the step number on which the subevent was aborted. + * Consider the following example: + * + * subevent_done_status = @ref SAL_LE_CS_SUBEVENT_ABORTED + * num_steps_reported = 160 + * abort_step = 100 + * + * this would mean that steps from 0 to 99 are complete and steps from 100 to 159 + * are aborted. + */ + bt_le_srv_le_cs_subevent_done_status_t subevent_done_status; + /** Abort reason. + * + * If the procedure status is + * @ref SAL_LE_CS_PROCEDURE_ABORTED, this field will + * specify the reason for the abortion. + */ + bt_le_srv_cs_procedure_abort_reason_t procedure_abort_reason; + /** Abort reason. + * + * If the subevent status is + * @ref SAL_LE_CS_SUBEVENT_ABORTED, this field will + * specify the reason for the abortion. + */ + bt_le_srv_cs_subevent_abort_reason_t subevent_abort_reason; + /** Number of antenna paths used during the phase measurement stage. + */ + uint8_t num_antenna_paths; + /** Number of CS steps in the subevent. + */ + uint8_t num_steps_reported; + /** Step number, on which the subevent was aborted + * if subevent_done_status is @ref SAL_LE_CS_SUBEVENT_COMPLETE + * then abort_step will be unused and set to 255 + */ + uint8_t abort_step; + } header; + + uint16_t len; + uint8_t *step_data_buf; +} sal_cs_subevent_result_t; + +bt_status_t bt_sal_cs_read_remote_supported_capabilities(bt_controller_id_t id, bt_address_t* addr); +bt_status_t bt_sal_cs_set_default_settings(bt_controller_id_t id, bt_address_t* addr, + bt_le_srv_cs_set_default_settings_param_t* params); +bt_status_t bt_sal_cs_read_remote_fae_table(bt_controller_id_t id, bt_address_t* addr); +bt_status_t bt_sal_cs_create_config(bt_controller_id_t id, bt_address_t* addr, + bt_le_srv_cs_create_config_params_t* params, + bt_le_srv_cs_create_config_context_t context); +bt_status_t bt_sal_cs_security_enable(bt_controller_id_t id, bt_address_t* addr); +bt_status_t bt_sal_cs_procedure_enable(bt_address_t* addr, const bt_le_srv_cs_procedure_enable_param_t* params); +bt_status_t bt_sal_cs_remove_config(bt_controller_id_t id, bt_address_t* addr, uint8_t config_id); +bt_status_t bt_sal_cs_set_procedure_parameters(bt_controller_id_t id, bt_address_t* addr, + const bt_le_srv_cs_set_procedure_parameters_param_t* params); +bt_status_t bt_sal_cs_set_channel_classification(uint8_t channel_classification[10], bt_address_t* addr); +bt_status_t bt_sal_cs_read_local_supported_capabilities(bt_srv_conn_le_cs_capabilities_t* params, bt_address_t* addr); +bt_status_t bt_sal_cs_write_cached_remote_supported_capabilities(bt_srv_conn_le_cs_capabilities_t* params, bt_address_t* addr); + +#endif //__SAL_LE_CS_INTERFACE_H__ \ No newline at end of file diff --git a/service/stacks/zephyr/include/le_cs_manager.h b/service/stacks/zephyr/include/le_cs_manager.h new file mode 100644 index 000000000..2160e8fba --- /dev/null +++ b/service/stacks/zephyr/include/le_cs_manager.h @@ -0,0 +1,28 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#ifndef __LE_CS_MANAGER_H__ +#define __LE_CS_MANAGER_H__ + +#include +#include + + void bt_le_cs_run_distance_estimation(uint8_t *local_step_data, uint16_t local_data_len, + uint8_t *peer_step_data, uint16_t peer_data_len, + uint8_t antenna_count, enum bt_conn_le_cs_role device_role); + + +#endif //__LE_CS_MANAGER_H__ \ No newline at end of file diff --git a/service/stacks/zephyr/le_cs_manager.c b/service/stacks/zephyr/le_cs_manager.c new file mode 100644 index 000000000..500d537ce --- /dev/null +++ b/service/stacks/zephyr/le_cs_manager.c @@ -0,0 +1,310 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + + #include + #include + #include "le_cs_manager.h" + + #ifdef CONFIG_BLUETOOTH_LE_CS + +#define LE_CS_FREQUENCY_MHZ(ch) (2402u + 1u * (ch)) +#define LE_CS_FREQUENCY_HZ(ch) (LE_CS_FREQUENCY_MHZ(ch) * 1000000.0f) +#define LE_CS_SPEED_OF_LIGHT_M_PER_S (299792458.0f) +#define LE_CS_SPEED_OF_LIGHT_NM_PER_S (LE_CS_SPEED_OF_LIGHT_M_PER_S / 1000000000.0f) +#define PI 3.14159265358979323846f +#define LE_CS_MAX_NUM_SAMPLES 256 + +struct iq_sample_and_channel { + bool failed; + uint8_t channel; + uint8_t antenna_permutation; + struct bt_le_cs_iq_sample local_iq_sample; + struct bt_le_cs_iq_sample peer_iq_sample; +}; + +struct rtt_timing { + bool failed; + int16_t initiator_toa_tod; + int16_t reflector_tod_toa; +}; + +static struct iq_sample_and_channel mode_2_data[LE_CS_MAX_NUM_SAMPLES]; +static struct rtt_timing tof_data_records[LE_CS_MAX_NUM_SAMPLES]; + +struct cs_processing_state { + bool processing_local_data; + uint8_t mode_1_data_index; + uint8_t mode_2_data_index; + uint8_t n_ap; + enum bt_conn_le_cs_role role; +}; + +static void le_cs_compute_complex_mul(int32_t real1, int32_t imag1, + int32_t real2, int32_t imag2, + int32_t *result_real, int32_t *result_imag) +{ + *result_real = real1 * real2 - imag1 * imag2; + *result_imag = real1 * imag2 + imag1 * real2; +} + +static float le_cs_perform_linear_fit(float *x, float *y, uint8_t count) +{ + if (count == 0) { + return 0.0f; + } + + float mean_x = 0.0f, mean_y = 0.0f; + + for (uint8_t i = 0; i < count; i++) { + mean_x += (x[i] - mean_x) / (i + 1); + mean_y += (y[i] - mean_y) / (i + 1); + } + + float numerator = 0.0f, denominator = 0.0f; + + for (uint8_t i = 0; i < count; i++) { + float dx = x[i] - mean_x; + float dy = y[i] - mean_y; + numerator += dx * dy; + denominator += dx * dx; + } + + return numerator / denominator; +} + +static void le_cs_sort_by_reference_array(float *keys, float *values, uint16_t size) +{ + for (uint16_t i = 0; i < size - 1; i++) { + bool swapped = false; + for (uint16_t j = 0; j < size - i - 1; j++) { + if (keys[j] > keys[j + 1]) { + float tmp_key = keys[j]; + float tmp_val = values[j]; + keys[j] = keys[j + 1]; + values[j] = values[j + 1]; + keys[j + 1] = tmp_key; + values[j + 1] = tmp_val; + swapped = true; + } + } + if (!swapped) { + break; + } + } +} + +static float le_cs_compute_distance_from_phase_gradient(struct iq_sample_and_channel *samples, + uint8_t sample_count) +{ + int32_t i_combined, q_combined; + uint16_t valid_count = 0; + static float phase_array[LE_CS_MAX_NUM_SAMPLES]; + static float freq_array[LE_CS_MAX_NUM_SAMPLES]; + + for (uint8_t idx = 0; idx < sample_count; idx++) { + if (!samples[idx].failed) { + le_cs_compute_complex_mul(samples[idx].local_iq_sample.i, samples[idx].local_iq_sample.q, + samples[idx].peer_iq_sample.i, samples[idx].peer_iq_sample.q, + &i_combined, &q_combined); + + phase_array[valid_count] = atan2f((float)q_combined, (float)i_combined); + freq_array[valid_count] = (float)LE_CS_FREQUENCY_MHZ(samples[idx].channel); + valid_count++; + } + } + + if (valid_count < 2) { + return 0.0f; + } + + sort_by_reference_array(freq_array, phase_array, valid_count); + + for (uint8_t i = 1; i < valid_count; i++) { + float delta = phase_array[i] - phase_array[i - 1]; + if (delta > PI) { + for (uint8_t j = i; j < valid_count; j++) { + phase_array[j] -= 2.0f * PI; + } + } else if (delta < -PI) { + for (uint8_t j = i; j < valid_count; j++) { + phase_array[j] += 2.0f * PI; + } + } + } + + float slope = le_cs_perform_linear_fit(freq_array, phase_array, valid_count); + float estimated_distance = -slope * (LE_CS_SPEED_OF_LIGHT_NM_PER_S / (4.0f * PI)); + + return estimated_distance / 1000000.0f; // Convert to meters +} + +static float calculate_distance_via_tof(uint8_t sample_count) +{ + float time_diff; + float avg_tof = 0.0f; + + // Cumulative Moving Average (CMA) + for (uint8_t index = 0; index < sample_count; index++) { + if (!tof_data_records[index].is_invalid) { + time_diff = (tof_data_records[index].initiator_toa_tod - + tof_data_records[index].reflector_tod_toa) / 2.0f; + + avg_tof += (time_diff - avg_tof) / (index + 1); + } + } + + float avg_tof_ns = avg_tof / 2.0f; + + return avg_tof_ns * LE_CS_SPEED_OF_LIGHT_NM_PER_S; +} + +static bool handle_subevent_step_data(struct bt_le_cs_subevent_step *subevent_step, void *arg) +{ + struct cs_processing_state *state = (struct cs_processing_state *)arg; + + if (subevent_step->mode == BT_CONN_LE_CS_MAIN_MODE_2) { + struct bt_hci_le_cs_step_data_mode_2 *mode2_raw = + (struct bt_hci_le_cs_step_data_mode_2 *)subevent_step->data; + + if (state->processing_local_data) { + for (uint8_t idx = 0; idx < (state->antenna_count + 1); idx++) { + if (mode2_raw->tone_info[idx].extension_indicator != + BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) { + continue; + } + + phase_data_mode2[state->index_mode2].channel = subevent_step->channel; + phase_data_mode2[state->index_mode2].antenna_permutation = + mode2_raw->antenna_permutation_index; + phase_data_mode2[state->index_mode2].local_iq = + bt_le_cs_parse_pct(mode2_raw->tone_info[idx].phase_correction_term); + + if (mode2_raw->tone_info[idx].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_LOW || + mode2_raw->tone_info[idx].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) { + phase_data_mode2[state->index_mode2].invalid = true; + } + + state->index_mode2++; + } + } else { + for (uint8_t idx = 0; idx < (state->antenna_count + 1); idx++) { + if (mode2_raw->tone_info[idx].extension_indicator != + BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) { + continue; + } + + phase_data_mode2[state->index_mode2].peer_iq = + bt_le_cs_parse_pct(mode2_raw->tone_info[idx].phase_correction_term); + + if (mode2_raw->tone_info[idx].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_LOW || + mode2_raw->tone_info[idx].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) { + phase_data_mode2[state->index_mode2].invalid = true; + } + + state->index_mode2++; + } + } + } else if (subevent_step->mode == BT_HCI_OP_LE_CS_MAIN_MODE_1) { + struct bt_hci_le_cs_step_data_mode_1 *mode1_raw = + (struct bt_hci_le_cs_step_data_mode_1 *)subevent_step->data; + + if (mode1_raw->packet_quality_aa_check != + BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL || + mode1_raw->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE || + mode1_raw->tod_toa_reflector == BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE) { + tof_data_mode1[state->index_mode1].invalid = true; + } + + if (state->processing_local_data) { + if (state->role == BT_CONN_LE_CS_ROLE_INITIATOR) { + tof_data_mode1[state->index_mode1].initiator_toa_tod = + mode1_raw->toa_tod_initiator; + } else if (state->role == BT_CONN_LE_CS_ROLE_REFLECTOR) { + tof_data_mode1[state->index_mode1].reflector_tod_toa = + mode1_raw->tod_toa_reflector; + } + } else { + if (state->role == BT_CONN_LE_CS_ROLE_INITIATOR) { + tof_data_mode1[state->index_mode1].reflector_tod_toa = + mode1_raw->tod_toa_reflector; + } else if (state->role == BT_CONN_LE_CS_ROLE_REFLECTOR) { + tof_data_mode1[state->index_mode1].initiator_toa_tod = + mode1_raw->toa_tod_initiator; + } + } + + state->index_mode1++; + } + + return true; +} + +void bt_le_cs_run_distance_estimation(uint8_t *local_step_data, uint16_t local_data_len, + uint8_t *peer_step_data, uint16_t peer_data_len, + uint8_t antenna_count, enum bt_conn_le_cs_role device_role) +{ + struct net_buf_simple buffer; + + struct cs_processing_state state = { + .processing_local_data = true, + .index_mode_1 = 0, + .index_mode_2 = 0, + .antenna_count = antenna_count, + .role = device_role, + }; + + memset(tof_data_mode1, 0, sizeof(tof_data_mode1)); + memset(phase_data_mode2, 0, sizeof(phase_data_mode2)); + + net_buf_simple_init_with_data(&buffer, local_step_data, local_data_len); + bt_le_cs_step_data_parse(&buffer, handle_subevent_step_data, &state); + + state.index_mode_1 = 0; + state.index_mode_2 = 0; + state.processing_local_data = false; + + net_buf_simple_init_with_data(&buffer, peer_step_data, peer_data_len); + bt_le_cs_step_data_parse(&buffer, handle_subevent_step_data, &state); + + float distance_by_phase = compute_distance_from_phase_gradient( + phase_data_mode2, state.index_mode_2); + + float distance_by_tof = calculate_distance_via_tof(state.index_mode_1); + + if (distance_by_tof == 0.0f && distance_by_phase == 0.0f) { + BT_LOGI("A reliable distance estimate could not be computed."); + } else { + BT_LOGI("Estimated distance to reflector:"); + } + + if (distance_by_tof != 0.0f) { + BT_LOGI("- Round-Trip Timing method: %f meters (derived from %d samples)\n", + (double)distance_by_tof, state.index_mode_1); + } + + if (distance_by_phase != 0.0f) { + BT_LOGI("- Phase-Based Ranging method: %f meters (derived from %d samples)\n", + (double)distance_by_phase, state.index_mode_2); + } + + return; +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/service/stacks/zephyr/profile/cs_ras_test.c b/service/stacks/zephyr/profile/cs_ras_test.c new file mode 100644 index 000000000..1a39f5245 --- /dev/null +++ b/service/stacks/zephyr/profile/cs_ras_test.c @@ -0,0 +1,596 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#include "cs_ras.h" +#include "cs_ras_test.h" +#include "cs_ras_gatts.h" + +/** + * @brief LE CS Subevent Result Event + * + * This event is generated when the local Controller has results to report for a CS + * (Channel Sounding) subevent during the CS procedure. Depending on the number of CS steps in the CS + * subevent, the Controller may choose to report complete or partial results. If the number of CS steps exceeds + * the maximum HCI event size, the Controller may report further results using the HCI_LE_CS_Subevent_Result_Continue event. + * + * **General Overview**: + * - When the `Connection_Handle` is set to 0x0FFF, the `Config_ID` and `Start_ACL_Conn_Event_Counter` parameters + * are ignored. + * - The `Start_ACL_Conn_Event_Counter` parameter indicates the starting ACL connection event count to which the CS + * event results reported in this HCI event are anchored. + * - The `Procedure_Counter` parameter indicates the associated CS procedure count for the results reported in this event. + * - The `Frequency_Compensation` parameter is a value used by the initiator device to align the timing of CS steps and + * transmit frequencies. + * - The `Reference_Power_Level` is the power level in dBm used during the CS procedure. If the reference power level + * is not available during a subevent, it is set to `0x7F`. + * - The event can report either complete or partial results, and more results can be sent in continuation events when the + * CS steps exceed the maximum event size. + * - The event also includes various parameters related to the CS steps, such as the number of steps, step modes, channels, + * and specific data for each step. + * + * **Abort Reason**: + * - The `Abort_Reason` field specifies why the CS procedure or subevent was aborted, if applicable. + * + * **Event Parameters**: + * + * - `Subevent_Code` (1 octet) : + * - Value `0x31` indicates this is an HCI_LE_CS_Subevent_Result event. + * + * - `Connection_Handle` (2 octets): + * - `0x0000 to 0x0EFF`: Connection handle associated with the ACL connection. + * - `0x0FFF`: Indicates a CS test; other values are reserved for future use. + * + * - `Config_ID` (1 octet): + * - A configuration identifier for the CS procedure. The value ranges from `0` to `3`. + * + * - `Start_ACL_Conn_Event_Counter` (2 octets): + * - Indicates the starting ACL connection event count for the results in this event. + * + * - `Procedure_Counter` (2 octets): + * - CS procedure count indicating the number of procedures since the CS procedure started. + * - Range: `0x0000` to `0xFFFF`. + * + * - `Frequency_Compensation` (2 octets): + * - Fractional frequency offset (FFO) in units of `0.01 ppm` (signed 15-bit integer). + * - Range: `-100 ppm (0x58F0)` to `+100 ppm (0x2710)`. + * - `0xC000`: Frequency compensation not available (or the role is not initiator). + * + * - `Reference_Power_Level` (1 octet): + * - Reference power level in dBm. + * - Range: `-127 to 20 dBm`. + * - `0x7F`: Reference power level not applicable. + * + * - `Procedure_Done_Status` (1 octet): + * - Indicates the status of the CS procedure. + * - Bits 0-3: + * - `0x0`: All results complete. + * - `0x1`: Partial results, more to follow. + * - `0xF`: Procedure aborted. + * - Bits 4-7: Reserved for future use. + * + * - `Subevent_Done_Status` (1 octet): + * - Indicates the status of the CS subevent. + * - Bits 0-3: + * - `0x0`: All results complete. + * - `0x1`: Partial results, more to follow. + * - `0xF`: Subevent aborted. + * - Bits 4-7: Reserved for future use. + * + * - `Abort_Reason` (1 octet): + * - Specifies the reason for aborting the procedure or subevent, if applicable. + * - Bits 0-3: Abort reason for the procedure. + * - Bits 4-7: Abort reason for the subevent. + * + * - `Num_Antenna_Paths` (1 octet): + * - Indicates the number of antenna paths used during the CS procedure. + * - Range: `0x00` (no phase measurement) to `0x04` (up to four antenna paths). + * + * - `Num_Steps_Reported` (1 octet): + * - Number of CS steps reported in this event. + * - Range: `0x00` to `0xA0` (up to 160 steps). + * + * - `Step_Mode[i]` (Num_Steps_Reported × 1 octet): + * - The CS mode for each CS step. + * - Values: `0x00 to 0x03` (various modes). + * + * - `Step_Channel[i]` (Num_Steps_Reported × 1 octet): + * - The channel used for each CS step. + * - Valid channels: `0x00 to 0x4E` (refer to [Vol 6] Part A, Section 2 for details). + * + * - `Step_Data_Length[i]` (Num_Steps_Reported × 1 octet): + * - The length of mode- and role-specific information for each step. + * - Range: `0x00 to 0xFF`. + * + * - `Step_Data[i]` (Σi(Step_Data_Length[i]) octets): + * - The mode- and role-specific information for each step. + * - The structure of this data varies based on the CS mode and role of the local device. + * + * **Procedure Done and Subevent Done Status Combinations**: + * The valid combinations of `Procedure_Done_Status` and `Subevent_Done_Status` are: + * + * | Procedure_Done_Status (Bits 0 to 3) | Allowed Subevent_Done_Status (Bits 0 to 3) | + * |------------------------------------|-------------------------------------------| + * | `0x0` | `0x0`, `0xF` | + * | `0x1` | `0x0`, `0x1`, `0xF` | + * | `0xF` | `0x0`, `0xF` | + * + * **Mode Role Specific Information**: + * The information reported in `Step_Data` varies based on the CS mode and the role of the device. + * + * - **Mode 0** (Initiator): + * - `Packet_Quality`, `Packet_RSSI`, `Packet_Antenna`, `Measured_Freq_Offset` + * - **Mode 0** (Reflector): + * - `Packet_Quality`, `Packet_RSSI`, `Packet_Antenna` + * - **Mode 1** (Initiator): + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `ToA_ToD_Initiator`, `Packet_Antenna` + * - **Mode 1** (Reflector): + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `ToD_ToA_Reflector`, `Packet_Antenna` + * - **Mode 2** (Initiator or Reflector): + * - `Antenna_Permutation_Index`, `Tone_PCT[k]`, `Tone_Quality_Indicator[k]` + * - **Mode 3** (Initiator): + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `ToA_ToD_Initiator`, `Packet_Antenna`, + * `Antenna_Permutation_Index`, `Tone_PCT[k]`, `Tone_Quality_Indicator[k]` + * - **Mode 3** (Reflector): + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `ToD_ToA_Reflector`, `Packet_Antenna`, + * `Antenna_Permutation_Index`, `Tone_PCT[k]`, `Tone_Quality_Indicator[k]` + * + * **Mode and Role Specific Information Parameters**: + * For each CS mode, specific parameters like `Packet_Quality`, `Packet_RSSI`, `Packet_Antenna`, etc., are included. + * + * **Tone Quality Indicator**: + * - Bits 0-3: Tone quality values (`0x0` to `0x3`). + * - Bits 4-7: Tone extension slot information (`0x0` to `0x2`). + * + * **Other Parameters**: + * - `Packet_Quality`, `Packet_NADM`, `Packet_RSSI`, `Packet_Antenna`: These parameters represent the quality of the communication packet, + * the NADM (Normalized Antenna Diversity Measure), received signal strength indication (RSSI), and the antenna used for the transmission. + * + * **Example of Event Data**: + * - If the number of reported CS steps exceeds the maximum HCI event size, multiple continuation events may follow. + */ +static uint8_t ras_sub_data_test[] = { + 0x00, 0x0c, 0x03, 0x00, 0xda, 0x01, 0x00, 0x14, 0x03, 0x00, 0xd9, 0x01, 0x02, 0x1a, 0x09, 0x00, 0xdf, 0xe2, 0x11, 0x00, + 0xb4, 0x72, 0x17, 0x20, 0x02, 0x1f, 0x09, 0x00, 0x44, 0xd2, 0x1e, 0x00, 0xe7, 0x91, 0x23, 0x20, 0x02, 0x25, 0x09, 0x00, 0x3e, + 0xbe, 0x12, 0x00, 0x23, 0x1e, 0x10, 0x20, 0x02, 0x45, 0x09, 0x00, 0xdb, 0xff, 0x13, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x31, + 0x09, 0x00, 0xa3, 0xf1, 0x11, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x4c, 0x09, 0x00, 0xa5, 0x10, 0x01, 0x00, 0xa1, 0xe0, + 0x02, 0x20, 0x02, 0x29, 0x09, 0x00, 0x3a, 0x01, 0xeb, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x32, 0x09, 0x00, 0x9e, 0x61, 0x18, + 0x00, 0x74, 0xb1, 0x1a, 0x20, 0x02, 0x0e, 0x09, 0x00, 0x4a, 0xc0, 0x48, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x44, 0x09, + 0x00, 0xb4, 0x50, 0x12, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x04, 0x09, 0x00, 0x77, 0xea, 0x03, 0x00, 0x74, 0xba, 0xfa, 0x20, + 0x02, 0x13, 0x09, 0x00, 0x1f, 0x6d, 0xce, 0x00, 0xc6, 0x0d, 0xc7, 0x20, 0x02, 0x4b, 0x09, 0x00, 0x8f, 0xdf, 0xf4, 0x00, + 0xb2, 0x9f, 0xf3, 0x20, 0x02, 0x39, 0x09, 0x00, 0x16, 0x63, 0xf4, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x2f, 0x09, 0x00, + 0x26, 0x41, 0x11, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x10, 0x09, 0x00, 0xab, 0x01, 0xbf, 0x00, 0x46, 0x42, 0xc3, 0x20, 0x02, + 0x22, 0x09, 0x00, 0x27, 0x42, 0x07, 0x00, 0x12, 0xe2, 0x0b, 0x20, 0x02, 0x20, 0x09, 0x00, 0xaa, 0x8e, 0x25, 0x00, 0x4f, 0xde, + 0x21, 0x20, 0x02, 0x48, 0x09, 0x00, 0xe1, 0x7e, 0xfd, 0x00, 0xf1, 0xce, 0xf9, 0x20, 0x02, 0x27, 0x09, 0x00, 0x87, 0xcf, + 0x1d, 0x01, 0x0f, 0x0f, 0x1a, 0x21, 0x02, 0x3e, 0x09, 0x00, 0x45, 0xef, 0x28, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x42, 0x09, + 0x00, 0x29, 0x81, 0xed, 0x00, 0x5c, 0xd1, 0xf0, 0x20, 0x02, 0x33, 0x09, 0x00, 0x6c, 0xf2, 0x0c, 0x00, 0x00, 0x00, 0x00, + 0x12, 0x02, 0x02, 0x09, 0x00, 0x5a, 0x2a, 0xd6, 0x00, 0xcd, 0x8a, 0xc9, 0x20, 0x02, 0x1b, 0x09, 0x00, 0x01, 0xbf, 0xd2, 0x00, + 0xa8, 0xaf, 0xd0, 0x20, 0x02, 0x12, 0x09, 0x00, 0x5d, 0x1e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x0b, 0x09, 0x00, + 0x1b, 0xd2, 0x44, 0x00, 0x8d, 0x91, 0x47, 0x21, 0x02, 0x3f, 0x09, 0x00, 0x2d, 0xa2, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, + 0x3c, 0x09, 0x00, 0x1d, 0x0e, 0x29, 0x00, 0xb2, 0x6d, 0x23, 0x20, 0x02, 0x30, 0x09, 0x00, 0x3e, 0xce, 0x05, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x06, 0x09, 0x00, 0xa4, 0xb1, 0xb2, 0x00, 0x6a, 0x62, 0xb8, 0x20, 0x02, 0x40, 0x09, 0x00, 0xb2, 0x5e, + 0xe5, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x2e, 0x09, 0x00, 0x61, 0x30, 0x17, 0x00, 0x30, 0x60, 0x17, 0x20, 0x02, 0x37, + 0x09, 0x00, 0x92, 0x11, 0xd4, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x2d, 0x09, 0x00, 0x2e, 0xbf, 0xec, 0x00, 0x4f, 0xaf, 0xeb, + 0x20, 0x02, 0x05, 0x09, 0x00, 0x54, 0xf5, 0x0d, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x49, 0x09, 0x00, 0xe4, 0xce, 0xfb, + 0x01, 0xed, 0x6e, 0xf9, 0x20, 0x02, 0x21, 0x09, 0x00, 0xda, 0xef, 0x25, 0x00, 0x76, 0x7f, 0x24, 0x20, 0x02, 0x26, 0x09, 0x00, + 0x60, 0xff, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x41, 0x09, 0x00, 0xda, 0x60, 0xe6, 0x00, 0x0d, 0x51, 0xe9, 0x21, + 0x02, 0x43, 0x09, 0x00, 0xb5, 0x10, 0xec, 0x00, 0xe7, 0xc0, 0xee, 0x21, 0x02, 0x2a, 0x09, 0x00, 0xd0, 0x9e, 0xef, 0x01, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x3b, 0x09, 0x00, 0xff, 0xae, 0x32, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x4a, 0x09, 0x00, 0x31, + 0xdf, 0xf4, 0x00, 0x45, 0x9f, 0xf3, 0x20, 0x02, 0x14, 0x09, 0x00, 0x8e, 0x9c, 0xe2, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x47, + 0x09, 0x00, 0x8a, 0xb0, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x07, 0x09, 0x00, 0x4f, 0x81, 0xb5, 0x00, 0xcb, 0x71, + 0xb8, 0x20, 0x02, 0x09, 0x09, 0x00, 0x4e, 0x44, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x35, 0x09, 0x00, 0xf0, 0x40, 0xd1, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x0c, 0x09, 0x00, 0xfd, 0x4f, 0x4b, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x03, 0x09, + 0x00, 0x74, 0x33, 0xb5, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x1c, 0x09, 0x00, 0x88, 0x81, 0x28, 0x00, 0x3a, 0x61, 0x2b, 0x20, + 0x02, 0x08, 0x09, 0x00, 0x0d, 0xed, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x24, 0x09, 0x00, 0x6b, 0x7e, 0xec, 0x00, + 0x00, 0x00, 0x00, 0x12, 0x02, 0x16, 0x09, 0x00, 0xdf, 0x9f, 0x36, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x28, 0x09, 0x00, 0x38, + 0x1e, 0x03, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x3a, 0x09, 0x00, 0xa0, 0xae, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, + 0x11, 0x09, 0x00, 0x84, 0x9d, 0xc8, 0x00, 0xd4, 0xbd, 0xc5, 0x20, 0x02, 0x1d, 0x09, 0x00, 0x8d, 0x50, 0xd1, 0x00, 0x00, 0x00, + 0x00, 0x12, 0x02, 0x34, 0x09, 0x00, 0x2f, 0x3d, 0x08, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x2c, 0x09, 0x00, 0x78, 0x1f, + 0x16, 0x00, 0x4f, 0xaf, 0x14, 0x20, 0x02, 0x0f, 0x09, 0x00, 0xcc, 0xff, 0xb9, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x0a, 0x09, + 0x00, 0xdf, 0x32, 0x3a, 0x00, 0x8e, 0xa2, 0x3d, 0x20, 0x02, 0x38, 0x09, 0x00, 0x7c, 0x10, 0xc9, 0x02, 0x6f, 0x30, 0xcc, + 0x22, 0x02, 0x15, 0x09, 0x00, 0x9b, 0x6d, 0x2b, 0x00, 0x6b, 0xbd, 0x28, 0x21, 0x02, 0x2b, 0x09, 0x00, 0x12, 0xa1, 0xee, 0x00, + 0x42, 0x61, 0xf2, 0x20, 0x02, 0x3d, 0x09, 0x00, 0x43, 0xb2, 0x20, 0x02, 0x00, 0x00, 0x00, 0x12, 0x02, 0x23, 0x09, 0x00, + 0x3f, 0x41, 0x18, 0x00, 0x02, 0x01, 0x1b, 0x20, 0x02, 0x46, 0x09, 0x00, 0x12, 0xc1, 0xf9, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, + 0x0d, 0x09, 0x00, 0x30, 0xee, 0x42, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x1e, 0x09, 0x00, 0xd1, 0xc1, 0xdc, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x36, 0x09, 0x00, 0x32, 0x0d, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x12 +}; + +static uint8_t ras_recv_first_seg_data_test[] = { + 0x01, + 0xa0, + 0x00, + 0x00, + 0x01, + 0x00, + 0x03, + 0xc0, + 0x00, + 0x00, + 0x00, + 0x00, + 0x4a, + 0x00, + 0x00, + 0xda, + 0x01, + 0x00, + 0x00, + 0xd9, + 0x01, + 0x02, + 0x00, + 0xdf, + 0xe2, + 0x11, + 0x00, + 0xb4, + 0x72, + 0x17, + 0x20, + 0x02, + 0x00, + 0x44, + 0xd2, + 0x1e, + 0x00, + 0xe7, + 0x91, + 0x23, + 0x20, + 0x02, + 0x00, + 0x3e, + 0xbe, + 0x12, + 0x00, + 0x23, + 0x1e, + 0x10, + 0x20, + 0x02, + 0x00, + 0xdb, + 0xff, + 0x13, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0xa3, + 0xf1, + 0x11, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0xa5, + 0x10, + 0x01, + 0x00, + 0xa1, + 0xe0, + 0x02, + 0x20, + 0x02, + 0x00, + 0x3a, + 0x01, + 0xeb, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0x9e, + 0x61, + 0x18, + 0x00, + 0x74, + 0xb1, + 0x1a, + 0x20, + 0x02, + 0x00, + 0x4a, + 0xc0, + 0x48, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0xb4, + 0x50, + 0x12, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0x77, + 0xea, + 0x03, + 0x00, + 0x74, + 0xba, + 0xfa, + 0x20, + 0x02, + 0x00, + 0x1f, + 0x6d, + 0xce, + 0x00, + 0xc6, + 0x0d, + 0xc7, + 0x20, + 0x02, + 0x00, + 0x8f, + 0xdf, + 0xf4, + 0x00, + 0xb2, + 0x9f, + 0xf3, + 0x20, + 0x02, + 0x00, + 0x16, + 0x63, + 0xf4, + 0x01, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0x26, + 0x41, + 0x11, + 0x00, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0xab, + 0x01, + 0xbf, + 0x00, + 0x46, + 0x42, + 0xc3, + 0x20, + 0x02, + 0x00, + 0x27, + 0x42, + 0x07, + 0x00, + 0x12, + 0xe2, + 0x0b, + 0x20, + 0x02, + 0x00, + 0xaa, + 0x8e, + 0x25, + 0x00, + 0x4f, + 0xde, + 0x21, + 0x20, + 0x02, + 0x00, + 0xe1, + 0x7e, + 0xfd, + 0x00, + 0xf1, + 0xce, + 0xf9, + 0x20, + 0x02, + 0x00, + 0x87, + 0xcf, + 0x1d, + 0x01, + 0x0f, + 0x0f, + 0x1a, + 0x21, + 0x02, + 0x00, + 0x45, + 0xef, + 0x28, + 0x01, + 0x00, + 0x00, + 0x00, + 0x12, + 0x02, + 0x00, + 0x29, + 0x81, + 0xed, + 0x00, + 0x5c, + 0xd1, + 0xf0, + 0x20, + 0x02, + 0x00, + 0x6c, + 0xf2, + 0x0c, + 0x00, + 0x00, + 0x00, +}; + +static uint8_t ras_recv_second_seg_data_test[] = { + 0x04, 0x00, 0x12, 0x02, 0x00, 0x5a, 0x2a, 0xd6, 0x00, 0xcd, 0x8a, 0xc9, 0x20, 0x02, 0x00, 0x01, 0xbf, 0xd2, 0x00, 0xa8, + 0xaf, 0xd0, 0x20, 0x02, 0x00, 0x5d, 0x1e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x1b, 0xd2, 0x44, 0x00, 0x8d, + 0x91, 0x47, 0x21, 0x02, 0x00, 0x2d, 0xa2, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x1d, 0x0e, 0x29, 0x00, 0xb2, + 0x6d, 0x23, 0x20, 0x02, 0x00, 0x3e, 0xce, 0x05, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xa4, 0xb1, 0xb2, 0x00, 0x6a, + 0x62, 0xb8, 0x20, 0x02, 0x00, 0xb2, 0x5e, 0xe5, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x61, 0x30, 0x17, 0x00, 0x30, + 0x60, 0x17, 0x20, 0x02, 0x00, 0x92, 0x11, 0xd4, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x2e, 0xbf, 0xec, 0x00, 0x4f, + 0xaf, 0xeb, 0x20, 0x02, 0x00, 0x54, 0xf5, 0x0d, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xe4, 0xce, 0xfb, 0x01, 0xed, + 0x6e, 0xf9, 0x20, 0x02, 0x00, 0xda, 0xef, 0x25, 0x00, 0x76, 0x7f, 0x24, 0x20, 0x02, 0x00, 0x60, 0xff, 0x1b, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x00, 0xda, 0x60, 0xe6, 0x00, 0x0d, 0x51, 0xe9, 0x21, 0x02, 0x00, 0xb5, 0x10, 0xec, 0x00, 0xe7, + 0xc0, 0xee, 0x21, 0x02, 0x00, 0xd0, 0x9e, 0xef, 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xff, 0xae, 0x32, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x00, 0x31, 0xdf, 0xf4, 0x00, 0x45, 0x9f, 0xf3, 0x20, 0x02, 0x00, 0x8e, 0x9c, 0xe2, 0x00, 0x00, + 0x00, 0x00, 0x12, 0x02, 0x00, 0x8a, 0xb0, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x4f, 0x81, 0xb5, 0x00, 0xcb, + 0x71, 0xb8, 0x20, 0x02, 0x00, 0x4e, 0x44, 0x1b, 0x00 +}; + +static uint8_t ras_recv_last_seg_data_test[] = { + 0x0a, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xf0, 0x40, 0xd1, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xfd, 0x4f, 0x4b, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x74, 0x33, 0xb5, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x88, 0x81, 0x28, + 0x00, 0x3a, 0x61, 0x2b, 0x20, 0x02, 0x00, 0x0d, 0xed, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x6b, 0x7e, 0xec, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xdf, 0x9f, 0x36, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x38, 0x1e, 0x03, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xa0, 0xae, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x84, 0x9d, 0xc8, + 0x00, 0xd4, 0xbd, 0xc5, 0x20, 0x02, 0x00, 0x8d, 0x50, 0xd1, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x2f, 0x3d, 0x08, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x78, 0x1f, 0x16, 0x00, 0x4f, 0xaf, 0x14, 0x20, 0x02, 0x00, 0xcc, 0xff, 0xb9, + 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xdf, 0x32, 0x3a, 0x00, 0x8e, 0xa2, 0x3d, 0x20, 0x02, 0x00, 0x7c, 0x10, 0xc9, + 0x02, 0x6f, 0x30, 0xcc, 0x22, 0x02, 0x00, 0x9b, 0x6d, 0x2b, 0x00, 0x6b, 0xbd, 0x28, 0x21, 0x02, 0x00, 0x12, 0xa1, 0xee, + 0x00, 0x42, 0x61, 0xf2, 0x20, 0x02, 0x00, 0x43, 0xb2, 0x20, 0x02, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x3f, 0x41, 0x18, + 0x00, 0x02, 0x01, 0x1b, 0x20, 0x02, 0x00, 0x12, 0xc1, 0xf9, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x30, 0xee, 0x42, + 0x01, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0xd1, 0xc1, 0xdc, 0x00, 0x00, 0x00, 0x00, 0x12, 0x02, 0x00, 0x32, 0x0d, 0x1b, + 0x00, 0x00, 0x00, 0x00, 0x12 +}; + +static ras_testcase_t test_case = RAS_TESTCASE_ON_DEMAND_WRITE_RANG_DATA_TIMWOUT_010; + +static uint8_t test_status = RAS_TEST_SUCESS; + +int bt_gatt_notify_cb_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len) +{ + switch (test_case) { + case RAS_TESTCASE_ON_DEMAND_NOTIFY_VALID_RANG_DATA_003: + case RAS_TESTCASE_ON_DEMAND_INDICATE_VALID_RANG_DATA_004: { + if (attr == RAS_ON_DEMAND_CHAR_SEND) { + ras_on_demand_notify_finish_test(addr); + } else if (attr == RAS_CONTROL_POINT_CHAR_SEND) { + uint8_t opcode = *(uint8_t*)value; + if (opcode == SAL_LE_RAS_CTL_OP_RSP_CMP_RANG_DATA) { + uint8_t buf[3] = { 0 }; + buf[0] = SAL_LE_RAS_CTL_OP_CMD_ACK_RANG_DATA; + memcpy(&buf[1], (uint8_t*)value + 1, 2); + ras_ctrl_point_send_test(NULL, &attr_svc[SAL_LE_RAS_CTR_PT_CHAR_IDX], buf, sizeof(buf), 0, 0); + } + } + break; + } + default: + break; + } + return 0; +} + +ssize_t bt_gatt_attr_read_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len) +{ + return 0; +} + +int bt_gatt_notify_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len) +{ + switch (test_case) { + case RAS_TESTCASE_REAL_TIME_NOTIFY_VALID_RANG_DATA_001: + if (memcmp(value, ras_recv_first_seg_data_test, sizeof(ras_recv_first_seg_data_test)) != 0 && + memcmp(value, ras_recv_second_seg_data_test, sizeof(ras_recv_second_seg_data_test)) != 0 && + memcmp(value, ras_recv_last_seg_data_test, sizeof(ras_recv_last_seg_data_test)) != 0) { + test_status = RAS_TEST_FAIL; + LOG_ERR("---- No segment match the test data. -----"); + return -1; + } + + break; + case RAS_TESTCASE_ON_DEMAND_NOTIFY_VALID_RANG_DATA_003: + case RAS_TESTCASE_ON_DEMAND_INDICATE_VALID_RANG_DATA_004: { + if (attr == RAS_CONTROL_POINT_CHAR_SEND) { + uint8_t buf[3] = { 0 }; + uint16_t count = 10; + buf[0] = SAL_LE_RAS_CTL_OP_CMD_GET_RANG_DATA; + ras_put_uint16_to_ptr(count, &buf[1]); + ras_ctrl_point_send_test(NULL, &attr_svc[SAL_LE_RAS_CTR_PT_CHAR_IDX], buf, sizeof(buf), 0, 0); + } + + break; + } + default: + break; + } + return 0; +} + +int bt_gatt_indicate_test(ras_attr_notify_t attr, bt_address_t* addr, uint8_t* value, uint16_t len) +{ + ras_on_demand_indicate_finish_test(attr, addr); + return 0; +} + +int cs_ras_subevent_recv_test(void* data, uint16_t len) +{ + bt_srv_conn_le_cs_subevent_result_t* result = (bt_srv_conn_le_cs_subevent_result_t*)malloc( + sizeof(bt_srv_conn_le_cs_subevent_result_t) + sizeof(ras_sub_data_test)); + + result->header.config_id = 0; + result->header.start_acl_conn_event = 0x03; + result->header.procedure_counter = 0xa; + result->header.frequency_compensation = 0xc000; + result->header.reference_power_level = 0x00; + result->header.procedure_done_status = 0x00; + result->header.subevent_done_status = 0x00; + result->header.procedure_abort_reason = 0x00; + result->header.subevent_abort_reason = 0x00; + result->header.num_antenna_paths = 0x01; + result->header.abort_step = 0x00; + result->len = sizeof(ras_sub_data_test); + memcpy(result->step_data_buf, ras_sub_data_test, sizeof(ras_sub_data_test)); + + ras_rang_mode_t mode = SAL_LE_RAS_RANGING_MODE_ON_DEMAND; + + test_status = RAS_TEST_SUCESS; + + int err = ras_subevent_recv_test(mode, test_case, NULL, result); + + if (err != 0 || test_status == RAS_TEST_FAIL) { + BT_LOGE("-------- CS TEST Fail, case(%d), err:%d -------", test_case, err); + return -1; + } + + BT_LOGE("-------- CS TEST Success, case(%d) -------", test_case); + return err; +} diff --git a/service/stacks/zephyr/profile/include/cs_ras_client.h b/service/stacks/zephyr/profile/include/cs_ras_client.h new file mode 100644 index 000000000..d07c293d6 --- /dev/null +++ b/service/stacks/zephyr/profile/include/cs_ras_client.h @@ -0,0 +1,171 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#ifndef _CS_RAS_CLIENT_H_ +#include +#include + +// Maximum number of segments (derived from Segment Index range in Segmentation Header) +#define MAX_SEGMENTS 64 +#define MAX_SUBEVENT_RESULT 10 + +#define SAL_LE_RAP_STEP_DATA_BUF_LEN 2048 + +typedef struct { + /** CS configuration identifier. + * + * Range: 0 to 3 + * + * If these results were generated by a CS Test, + * this value will be set to 0 and has no meaning. + */ + uint8_t config_id; + /** Starting ACL connection event counter. + * + * If these results were generated by a CS Test, + * this value will be set to 0 and has no meaning. + */ + uint16_t start_acl_conn_event; + /** CS procedure count associated with these results. + * + * This is the CS procedure count since the completion of + * the Channel Sounding Security Start procedure. + */ + uint16_t procedure_counter; + /** Frequency compensation value in units of 0.01 ppm. + * + * This is a 15-bit signed integer in the range [-100, 100] ppm. + * + * A value of @ref BT_HCI_LE_CS_SUBEVENT_RESULT_FREQ_COMPENSATION_NOT_AVAILABLE + * indicates that the role is not the initiator, or that the + * frequency compensation value is unavailable. + */ + uint16_t frequency_compensation; + /** Reference power level in dBm. + * + * Range: -127 to 20 + * + * A value of @ref BT_HCI_LE_CS_REF_POWER_LEVEL_UNAVAILABLE indicates + * that the reference power level was not available during a subevent. + */ + int8_t reference_power_level; + /** Procedure status. */ + enum bt_conn_le_cs_procedure_done_status procedure_done_status; + /** Subevent status + * + * For aborted subevents, this will be set to @ref BT_CONN_LE_CS_SUBEVENT_ABORTED + * and abort_step will contain the step number on which the subevent was aborted. + * Consider the following example: + * + * subevent_done_status = @ref BT_CONN_LE_CS_SUBEVENT_ABORTED + * num_steps_reported = 160 + * abort_step = 100 + * + * this would mean that steps from 0 to 99 are complete and steps from 100 to 159 + * are aborted. + */ + enum bt_conn_le_cs_subevent_done_status subevent_done_status; + /** Abort reason. + * + * If the procedure status is + * @ref BT_CONN_LE_CS_PROCEDURE_ABORTED, this field will + * specify the reason for the abortion. + */ + enum bt_conn_le_cs_procedure_abort_reason procedure_abort_reason; + /** Abort reason. + * + * If the subevent status is + * @ref BT_CONN_LE_CS_SUBEVENT_ABORTED, this field will + * specify the reason for the abortion. + */ + enum bt_conn_le_cs_subevent_abort_reason subevent_abort_reason; + /** Number of antenna paths used during the phase measurement stage. + */ + uint8_t num_antenna_paths; + /** Number of CS steps in the subevent. + */ + uint8_t num_steps_reported; + /** Step number, on which the subevent was aborted + * if subevent_done_status is @ref BT_CONN_LE_CS_SUBEVENT_COMPLETE + * then abort_step will be unused and set to 255 + */ + uint8_t abort_step; +} rap_subevent_header_t; + +// Define the segmentation header structure with rap_ prefix +typedef struct { + unsigned char first_segment : 1; // Flag indicating if it's the first segment + unsigned char last_segment : 1; // Flag indicating if it's the last segment + unsigned char segment_index : 6; // Segment index (0-63) +} rap_segmentation_header_t; + +// Define the structure for the received data segment with rap_ prefix +typedef struct { + rap_segmentation_header_t header; // Segmentation header + uint8_t *data; // The data content + size_t data_size; // The size of the data +} rap_ranging_data_segment_t; + +// Define the complete Ranging Data structure with rap_ prefix +typedef struct { + uint8_t *data; // The complete data + size_t total_size; // The total size of the data + size_t received_size; // The size of data received so far + bool *segment_received; // Flag for each segment, indicating if it's been received +} rap_ranging_data_body_t; + +enum ras_svc_disc_state_bit { + DISC_RAS_RANG_RT_DT, + DISC_RAS_NAME, + DISC_RAS_TYPE, + DISC_RAS_SIZE, + DISC_RAS_ID, +}; + +typedef struct { + uint16_t rang_realtime_data_handle; + uint16_t rang_feature_handle; + uint16_t rang_on_demond_data_handle; + uint16_t rang_ras_ctr_point_handle; + uint16_t rang_data_ready_handle; + uint16_t rang_data_ov_wr_handle; + uint16_t step_data_attr_handle; +} ras_char_handle_t; + +typedef struct { + rap_subevent_header_t header; + bool is_used; + uint8_t *local_steps; +} rap_subevent_result_t; + +typedef struct { + ras_char_handle_t char_handles; + struct bt_conn *connection; + uint32_t ras_mtu; + uint32_t ras_seg_offset; + uint32_t remaining_len; + uint8_t ras_seg_idx; + uint32_t feature; + rap_subevent_result_t result_pool[MAX_SUBEVENT_RESULT]; + struct bt_gatt_indicate_params ras_dt_rd_ind_params; + struct bt_gatt_read_params read_feature; + uint32_t ras_feature; + atomic_t disc_state; + uint8_t *ras_svc_data; + uint8_t *ras_cli_data; +} sal_le_ras_cli_env_t; + +#endif /* _CS_RAS_CLIENT_H_ */ \ No newline at end of file diff --git a/service/stacks/zephyr/profile/include/cs_ras_common.h b/service/stacks/zephyr/profile/include/cs_ras_common.h new file mode 100644 index 000000000..66f2e0b6f --- /dev/null +++ b/service/stacks/zephyr/profile/include/cs_ras_common.h @@ -0,0 +1,33 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#ifndef _CS_RAS_COMMON_H_ +#define _CS_RAS_COMMON_H_ + +#include +#include + +#ifndef BIT +#define BIT(n) (1 << n) +#endif /* BIT */ + +#define RAS_FEATURE_REALTIME_RANG_DATA BIT(0) +#define RAS_FEATURE_RET_LOST_RANG_DATA_SEG BIT(1) +#define RAS_FEATURE_ABORT_OPERATION BIT(2) +#define RAS_FEATURE_FILTER_RANG_DATA BIT(3) + +#endif /* _CS_RAS_COMMON_H_ */ + diff --git a/service/stacks/zephyr/profile/include/cs_ras_server.h b/service/stacks/zephyr/profile/include/cs_ras_server.h new file mode 100644 index 000000000..f4cb9f531 --- /dev/null +++ b/service/stacks/zephyr/profile/include/cs_ras_server.h @@ -0,0 +1,864 @@ +/**************************************************************************** + * Copyright (C) 2024 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#ifndef _CS_RAS_SERVER_H_ +#define _CS_RAS_SERVER_H_ + +#include "cs_ras_test.h" +#include +#include +#include + +#define CONFIG_BT_CS_TEST 1 + +#ifndef BIT +#define BIT(n) (1 << n) +#endif /* BIT */ + +/** + * @brief Response timeout for Ranging Service (RAS) operations. + * + * This macro defines the timeout duration for Ranging Service (RAS) + * responses, set to 5 seconds. + */ +#define RAS_RSP_TIMEOUT K_SECONDS(5) + +/** + * @brief Empty array indicator. + * + * This macro represents an empty array or zero-length data structure + * used in Ranging Service contexts. + */ +#define RAS_EMPTY_ARRAY (0) + +/** + * @brief RAS role: Initiator. + * + * This macro defines the Ranging Service (RAS) role as the Initiator, + * which starts the ranging process by sending ranging requests. + */ +#define SAL_LE_RAS_ROLE_INITIATOR (0) + +/** + * @brief RAS role: Reflector. + * + * This macro defines the Ranging Service (RAS) role as the Reflector, + * which responds to ranging requests from the Initiator. + */ +#define SAL_LE_RAS_ROLE_REFLECTOR (1) + +/** + * @brief RAS subevent step mode 0. + * + * This macro defines subevent step mode 0 for the Ranging Service (RAS). + * The meaning of this mode depends on the specific RAS configuration or + * ranging algorithm being used. + */ +#define SAL_LE_RAS_SUBEVENT_STEP_MODE_0 (0) + +/** + * @brief RAS subevent step mode 1. + * + * This macro defines subevent step mode 1 for the Ranging Service (RAS). + */ +#define SAL_LE_RAS_SUBEVENT_STEP_MODE_1 (1) + +/** + * @brief RAS subevent step mode 2. + * + * This macro defines subevent step mode 2 for the Ranging Service (RAS). + */ +#define SAL_LE_RAS_SUBEVENT_STEP_MODE_2 (2) + +/** + * @brief RAS subevent step mode 3. + * + * This macro defines subevent step mode 3 for the Ranging Service (RAS). + */ +#define SAL_LE_RAS_SUBEVENT_STEP_MODE_3 (3) + +/** + * @brief GATT notification type for Ranging Service (RAS). + * + * This macro defines the use of GATT Notification in the Ranging Service (RAS) + * for transmitting ranging-related data or events to a connected peer device. + * Notifications are sent without requiring acknowledgment from the receiver. + */ +#define SAL_LE_RAS_GATT_NOTIFY (1) + +/** + * @brief GATT indication type for Ranging Service (RAS). + * + * This macro defines the use of GATT Indication in the Ranging Service (RAS) + * for transmitting ranging-related data or events to a connected peer device. + * Indications require acknowledgment (confirmation) from the receiver. + */ +#define SAL_LE_RAS_GATT_INDICATION (2) + +/** + * @brief RAS sub-procedure header identifier. + * + * This macro defines the identifier value (12) used for the Ranging Service (RAS) + * sub-procedure header. It marks the beginning of the RAS sub-procedure data block. + * + * The RAS Sub-Procedure Header includes the following fields: + * + * | Field | Size (bits) | Description | + * |----------------------------|-------------|--------------------------------------------------------------------------------------------------------------------------| + * | Ranging Counter | 12 | Lower 12 bits of `CS_Procedure_Counter` (see *Vol 4, Part E, Sec 7.7.65.44* in [1]) provided by the Core Controller. | + * | Configuration ID | 4 | CS configuration identifier. Range: 0–3. | + * | Selected TX Power | 8 | Transmit power level used for the CS Procedure. Range: -127 to 20 dBm (referenced to 1 mW). | + * | Antenna Paths Mask | 8 | Indicates which antenna paths are reported:
• Bit0: Path 1
• Bit1: Path 2
• Bit2: + * Path 3
• Bit3: Path 4
• Bits 4–7: RFU | + * | Subevent Header | — | — | + * | Start ACL Connection Event | 16 | Starting ACL connection event count for results reported in the event. | + * | Frequency Compensation | 16 | Frequency compensation value in units of 0.01 ppm (15-bit signed integer). | + * | Ranging Done Status | 4 | Completion state for the CS Procedure:
• 0x0 – All results complete
• 0x1 – Partial results, more to + * follow
• 0xF – All subsequent CS Procedures aborted
• Others – RFU | + * | Subevent Done Status | 4 | Completion state for the CS Subevent:
• 0x0 – All results complete
• 0xF – Subevent aborted
• Others – RFU | + * | Ranging Abort Reason | 4 | Abort reason when `Procedure_Done_Status` = 0xF; otherwise 0.
• 0x0 – No abort
• 0x1 – Local/remote abort + * request
• 0x2 – Filtered channel map < 15 channels
• 0x3 – Channel map update instant passed
• 0xF – Unspecified
• Others – RFU | + * | Subevent Abort Reason | 4 | Abort reason when `Subevent_Done_Status` = 0xF; otherwise 0.
• 0x0 – No abort
• 0x1 – Local/remote abort request
• 0x2 – + * No CS_SYNC (mode 0) received
• 0x3 – Scheduling/resource conflict
• 0xF – Unspecified
• Others – RFU | + * | Reference Power Level | 8 | Reference power level. Range: –127 to 20 dBm. | + * | Number of Steps Reported | 8 | Number of steps in the CS Subevent for which results are reported. If aborted, can be set to 0. | + * + * @note Refer to Bluetooth Core Specification Volume 4, Part E, Section 7.7.65.44 for field definitions. + */ +#define SAL_LE_RAS_SUB_PROCUDURE_HEAD (12) + +/** + * @brief CCCD "Not Implemented Configuration" error code for RAS. + * + * This macro defines the error code (0xFD) used in the Ranging Service (RAS) + * to indicate that the Client Characteristic Configuration Descriptor (CCCD) + * write request refers to a configuration that is not implemented or supported. + * + * Typically returned when a peer attempts to enable notifications or indications + * on a characteristic that does not support the requested configuration. + */ +#define SAL_LE_RAS_CCCD_NOT_IMPR_CONFIG_ERR (0xFD) + +/** + * @brief CCCD write request rejected error code for RAS. + * + * This macro defines the error code (0xFC) used in the Ranging Service (RAS) + * to indicate that the Client Characteristic Configuration Descriptor (CCCD) + * write request was explicitly rejected by the server. + * + * This may occur due to invalid parameters, insufficient permissions, + * or internal service state preventing configuration changes. + */ +#define SAL_LE_RAS_CCCD_WR_REQ_REJECT (0xFC) + +/** + * @brief Ranging mode definitions for the Ranging Service (RAS). + * + * RAS enables the client to read Ranging Data from a RAS Server. RAS is implemented + * on a device that can generate Ranging Data using the Channel Sounding (CS) feature + * of the local Core Controller. RAS distinguishes between two modes of Ranging Data exchange, + * each represented by a service characteristic: + * + * • Real-time Ranging Data: Data received from the local Core Controller and + * communicated immediately by the RAS Server while connected to the client. + * • On-demand Ranging Data: Data received from the local Core Controller and + * stored on the RAS Server for on-demand retrieval by the client. + * + * The client enables one of these modes by subscribing to either notifications or + * indications of the corresponding characteristic (Real-time or On-demand) via the + * Client Characteristic Configuration Descriptor (CCCD). The RAS Server shall operate + * in either Real-time or On-demand mode, but **not both simultaneously**. A client can + * switch modes by first disabling the active mode and then enabling the other mode. + * + * If a client attempts to enable more than one mode, the RAS Server shall reject the + * Write Characteristic Descriptor request and return a `SAL_LE_RAS_CCCD_NOT_IMPR_CONFIG_ERR` + * (0xFD). If the RAS Server does not support notifications for a characteristic and + * the client enables notifications, the server shall reject the request and return + * `SAL_LE_RAS_CCCD_WR_REQ_REJECT` (0xFC). + * + * A RAS Server shall transfer Ranging Data in chronological order, with the oldest + * data sent first. This applies to both On-demand Ranging Data (transferred via + * RAS Control Point procedure) and Real-time Ranging Data. If the ACL connection with + * the client is lost or terminated, any pending Ranging Data segments shall be flushed. + * + * The RAS Ranging Modes are defined as follows: + * + * | **Macro** | **Value** | **Description** | + * |---------------------------------------|------------|---------------------------------------------------------------------------------| + * | **SAL_LE_RAS_RANGING_MODE_REAL_TIME** | 0x01 | Real-time ranging mode. Measurements are performed continuously or periodically in real time. | + * | **SAL_LE_RAS_RANGING_MODE_ON_DEMAND** | 0x02 | On-demand ranging mode. Measurements are performed only when explicitly requested by the client. | + * | **SAL_LE_RAS_RANGING_MODE_UNDEFINED** | 0xFF | Undefined or invalid mode. Used as default or error value. | + * + * @note These values are typically exchanged via GATT characteristics as part of + * RAS control or configuration procedures. + */ +#define SAL_LE_RAS_RANGING_MODE_REAL_TIME (0x01) +#define SAL_LE_RAS_RANGING_MODE_ON_DEMAND (0x02) +#define SAL_LE_RAS_RANGING_MODE_UNDEFINED (0xFF) +typedef uint8_t ras_rang_mode_t; + +/** + * @brief Maximum number of Ranging Service (RAS) procedures that can be stored. + * + * This macro defines the upper limit on the number of RAS procedures that the + * RAS Server can store simultaneously. Each stored procedure may contain + * multiple steps or subevents of Ranging Data generated via the Channel Sounding (CS) + * feature of the local Core Controller. + * + * The value 10 indicates that the server can hold up to 10 procedures in memory. + * If additional procedures are generated beyond this limit, the server may + * reject new procedures or overwrite older ones depending on the implementation. + * + * @note This value helps the RAS Server manage memory and resources efficiently, + * preventing overflow when multiple Ranging procedures are active. + */ +#define SAL_LE_RAS_STORE_PROCEDURE_NUM_MAX (10) + +/** + * RAS Features format + * The RAS Features characteristic bit formats are listed in + * +--------+-----------------------------------------------+ + * | Bit(s) | Definition | + * +--------+-----------------------------------------------+ + * | 0 | Real-time Ranging Data | + * | 1 | Retrieve Lost Ranging Data Segments | + * | 2 | Abort Operation | + * | 3 | Filter Ranging Data | + * | 4–31 | Reserved for Future Use (RFU) | + * +--------+-----------------------------------------------+ + */ +#define SAL_LE_RAS_REAL_TIME_RANG_DATA_SUPPROTED (1 << 0) +#define SAL_LE_RAS_RETRI_LOST_RANG_DATA_SEG (1 << 1) +#define SAL_LE_RAS_ABORT_OPRATION (1 << 2) +#define SAL_LE_RAS_FILTER_RANG_DATA (1 << 3) + +/** + * @brief RAS Control Point Operation Codes (Op Codes) and Parameters. + * + * The Ranging Service (RAS) Control Point allows clients to issue commands + * to a RAS Server to control and retrieve Ranging Data. Each operation has + * a defined Op Code and associated parameters. Requirements are summarized + * in Tables 3.10 and 3.11 of the specification. + * + * | **Operation** | **Op Code** | **Requirement** | **Parameter #1** | **Parameter #2** | **Parameter #3** | + * |----------------------------------------|------------|----------------|----------------------------|---------------------------|---------------------------| + * | **Get_Ranging_Data** | 0x00 | Mandatory (M) | uint16 Ranging Counter | – | – | + * | **ACK_Ranging_Data** | 0x01 | Mandatory (M) | uint16 Ranging Counter | – | – | + * | **Retrieve_Lost_Ranging_Data_Segments**| 0x02 | Optional (O) | uint16 Ranging Counter | uint8 First Segment Index | uint8 Last Segment Index | + * | **Abort_Operation** | 0x03 | Optional (O) | No Parameter used | – | – | + * | **Set_Filter** | 0x04 | Optional (O) | uint16 Filter Configuration:
• Bits 0-1: Mode
• Bits 2-15: Filter bit mask (see Section 3.3.2.4) | – | – | + * + * @note The client uses these Op Codes via the RAS Control Point characteristic + * to perform operations on the RAS Server. The server shall respond according + * to the Op Code requirements, returning results or acknowledgements as defined. + */ +#define SAL_LE_RAS_CTL_OP_CMD_GET_RANG_DATA (0x00) +#define SAL_LE_RAS_CTL_OP_CMD_ACK_RANG_DATA (0x01) +#define SAL_LE_RAS_CTL_OP_CMD_RETRIEVE_LOST_RANG_DATA_SEG (0x02) +#define SAL_LE_RAS_CTL_OP_CMD_ABORT_OPERATION (0x03) +#define SAL_LE_RAS_CTL_OP_CMD_SET_FILTER (0x04) + +/** + * @brief RAS Control Point Response Op Codes and Parameters. + * + * The Ranging Service (RAS) Control Point characteristic also defines responses + * from the RAS Server to the client. Each response has an Op Code and associated + * parameters. Requirements are summarized in Table 3.11 of the specification. + * + * | **Response** | **Op Code** | **Requirement** | **Parameter #1** | **Parameter #2** | **Parameter #3** | + * |-------------------------------------------- |------------|----------------|----------------------------|---------------------------|---------------------------| + * | **Complete Ranging Data Response** | 0x00 | Mandatory (M) | uint16 Ranging Counter | – | – | + * | **Complete Lost Ranging Data Segment Response** | 0x01 | Conditional (C.1) | uint16 Ranging Counter | uint8 First Segment Index | uint8 Last Segment Index | + * | **Response Code** | 0x02 | Mandatory (M) | Response Code Value | – | – | + * + * @note C.1: If the Retrieve Lost Ranging Data Segments procedure is supported, + * then this Op Code is considered Mandatory. + * + * @note These Op Codes are used by the RAS Server to report the results of + * operations initiated by the client via the RAS Control Point characteristic. + */ +#define SAL_LE_RAS_CTL_OP_RSP_CMP_RANG_DATA (0x00) +#define SAL_LE_RAS_CTL_OP_RSP_CMP_LOST_RANG_DATA_SEG (0x01) +#define SAL_LE_RAS_CTL_OP_RSP_CODE (0x02) + +/** + * @brief RAS Control Point Response Code Values (for Op Code 0x02). + * + * These values are used by the RAS Server to indicate the result of an operation + * requested by the client via the RAS Control Point characteristic. + * Requirements are summarized in Table 3.12 of the specification. + * + * | **Response Code Value** | **Definition** | **Description** | + * |------------------------|-------------------------|-------------------------------------------------------------------------------| + * | 0x00 | Reserved for Future Use | N/A | + * | 0x01 | Success | Normal response for a successful operation | + * | 0x02 | Op Code Not Supported | Normal response if an unsupported Op Code is received | + * | 0x03 | Invalid Parameter | Normal response if the received parameter does not meet service requirements | + * | 0x04 | Success/Persisted | Normal response for a successful write operation where values are persisted | + * | 0x05 | Abort Unsuccessful | Normal response if a request for Abort is unsuccessful | + * | 0x06 | Procedure Not Completed | Normal response if unable to complete a procedure for any reason | + * | 0x07 | Server Busy | Normal response if the Server is still busy with other requests | + * | 0x08 | No Records Found | Normal response if the requested Ranging Counter is not found | + * | 0x09-0xFF | Reserved for Future Use | N/A | + * + * @note These codes are specifically returned in response to Control Point + * operations with Op Code 0x02 (Response Code Op). + */ +#define SAL_LE_RAS_CTL_OP_RSP_CODE_RESERVED (0x00) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS (0x01) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_NOT_SUPPORTED (0x02) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_INVALID_PARAMS (0x03) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_PERSISTED (0x04) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_ABORT (0x05) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_PROCE_NOT_CMP (0x06) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_SERVER_BUSY (0x07) +#define SAL_LE_RAS_CTL_OP_RSP_CODE_NO_RECORD_FOUND (0x08) + +/** + * @brief RAS Filter Mode and Filter Bit Masks. + * + * These macros define the maximum filter mode, and masks for extracting + * the mode and filter bits from a 16-bit Filter Configuration field used + * in the Ranging Service (RAS) Control Point. + */ + +/** Maximum valid RAS filter mode value (0x00 to 0x03). */ +#define SAL_LE_RAS_FILTER_MODE_MAX (0x04) + +/** Mask for the Mode bits (bits 0-1) in the Filter Configuration field. */ +#define SAL_LE_RAS_FILTER_MODE_MASK (0x03) // Mask for Mode bits (0-1) + +/** Mask for the Filter bits (bits 2-15) in the Filter Configuration field. */ +#define SAL_LE_RAS_FILTER_BIT_MASK (0xFFFC) // Mask for Filter bits (2-15) + +/** + * @brief Maximum buffer length for RAS Step Data. + * + * This macro defines the maximum length of the buffer used to store + * step data for the Ranging Service (RAS) GATT characteristic. + * + * @note The length is set according to the maximum GATT characteristic + * length supported by the Bluetooth stack. + */ +#define SAL_LE_RAS_STEP_DATA_BUF_LEN 2048 +#define SAL_LE_RAS_SWAP_ADD_SUB(a, b) \ + do { \ + (a) = (a) + (b); \ + (b) = (a) - (b); \ + (a) = (a) - (b); \ + } while (0) + +/** + * @brief Copy a field to the output buffer if the corresponding filter bit is enabled. + * + * This macro conditionally copies a field of data from a source pointer `p` + * to an output buffer `buf` depending on the `filter_mask`. It also updates + * offsets and tracks remaining bytes. + * + * @param _size Size of the field in bytes. + * @param _filter_bit Bitmask corresponding to the field in the filter_mask. + * + * @details + * - The field is only copied if: + * 1. There are enough bytes remaining (`remaining >= _size`), and + * 2. The corresponding bit in `filter_mask` is set (`filter_mask & _filter_bit`). + * - After copying, the output offset (`out_offset`) is incremented by `_size`. + * - Regardless of copying, the source pointer `p` is advanced by `_size`, + * and `remaining` bytes are decreased by `_size`. + * - If there are not enough bytes remaining to copy the field, a warning is logged + * and `remaining` is set to 0 to prevent further copying. + * + * @note This macro is typically used when serializing data fields conditionally + * based on a filter mask in Ranging Service (RAS) or similar protocols. + */ +#define COPY_FIELD_IF_ENABLED(_size, _filter_bit) \ + do { \ + if (remaining >= (_size)) { \ + if (filter_mask & (_filter_bit)) { \ + memcpy(&buf[out_offset], p, (_size)); \ + out_offset += (_size); \ + } \ + p += (_size); \ + remaining -= (_size); \ + } else { \ + LOG_WRN("Field truncated, size=%d", (int)(_size)); \ + remaining = 0; \ + } \ + } while (0) + +/** + * @brief Bits used to enable notifications or indications for RAS characteristics. + * + * This enumeration defines the bit positions for enabling notifications or + * indications for various Ranging Service (RAS) GATT characteristics. + * These bits are typically used in a client configuration mask to control + * which types of RAS data are sent from the server to the client. + */ +enum cs_ras_notify_enable_bit { + /** Enable notification for Real-time Ranging (RTT) Data characteristic. */ + RAS_RTT_DATA_NOTIFY, + /** Enable indication for Real-time Ranging (RTT) Data characteristic. */ + RAS_RTT_DATA_INDICATE, + /** Enable notification for RAS Control Point characteristic. */ + RAS_CONTROL_POINT_NOTIFY, + /** Enable indication for RAS Control Point characteristic. */ + RAS_CONTROL_POINT_INDICATE, + /** Enable notification for On-demand Ranging Data characteristic. */ + RAS_ON_DEMAND_DATA_NOTIFY, + /** Enable indication for On-demand Ranging Data characteristic. */ + RAS_ON_DEMAND_DATA_INDICATE, + /** Enable notification for Data Ready characteristic. */ + RAS_DATA_READY_NOTIFY, + /** Enable indication for Data Ready characteristic. */ + RAS_DATA_READY_INDICATE, + /** Enable notification for Overwrite event characteristic. */ + RAS_OVER_WRITE_NOTIFY, + /** Enable indication for Overwrite event characteristic. */ + RAS_OVER_WRITE_INDICATE +}; + +/** + * @brief RAS GATT Attribute Table Indexes. + * + * This enumeration defines indexes for the Ranging Service (RAS) GATT attributes. + * These indexes are used to reference characteristics, their values, and + * Client Characteristic Configuration Descriptors (CCCDs) in the attribute table. + */ +enum { + /** RAS Service Index. */ + SAL_LE_RAS_SVC_IDX, + + /** Feature Characteristic Index. */ + SAL_LE_RAS_FEAT_CHAR_IDX, + /** Feature Characteristic Value Index. */ + SAL_LE_RAS_FEAT_CHAR_VAL_IDX, + + /** Real-time Data Characteristic Index. */ + SAL_LE_RAS_RT_DT_CHAR_IDX, + /** Real-time Data Characteristic Value Index. */ + SAL_LE_RAS_RT_DT_CHAR_VAL_IDX, + /** Real-time Data Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_RT_DT_CCC_CFG_IDX, + + /** On-demand Data Characteristic Index. */ + SAL_LE_RAS_ON_DEM_CHAR_IDX, + /** On-demand Data Characteristic Value Index. */ + SAL_LE_RAS_ON_DEM_CHAR_VAL_IDX, + /** On-demand Data Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_ON_DEM_CCC_CFG_IDX, + + /** Control Point Characteristic Index. */ + SAL_LE_RAS_CTR_PT_CHAR_IDX, + /** Control Point Characteristic Value Index. */ + SAL_LE_RAS_CTR_PT_CHAR_VAL_IDX, + /** Control Point Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_CTR_PT_CCC_CFG_IDX, + + /** Data Ready Characteristic Index. */ + SAL_LE_RAS_DT_RD_CHAR_IDX, + /** Data Ready Characteristic Value Index. */ + SAL_LE_RAS_DT_RD_CHAR_VAL_IDX, + /** Data Ready Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_DT_RD_CCC_CFG_IDX, + + /** Data Overwrite Characteristic Index. */ + SAL_LE_RAS_DT_OV_WR_CHAR_IDX, + /** Data Overwrite Characteristic Value Index. */ + SAL_LE_RAS_DT_OV_WR_CHAR_VAL_IDX, + /** Data Overwrite Characteristic CCC Descriptor Index. */ + SAL_LE_RAS_DT_OV_WR_CCC_CFG_IDX, + + /** Total number of RAS attribute table entries. */ + RAS_IDX_MAX +}; + +/******************************************************************************************* + * + * FILTER BIT MAPPING TABLE + * ------------------------------------------------------------ + * Each field can be individually filtered out using the + * atomic_t ras_filter[SAL_LE_RAS_FILTER_MODE_MAX] bitmask. + * + * +----------------------------+-------------------------------+ + * | Field Name | Bit Definition | + * +----------------------------+-------------------------------+ + * | Packet_Quality | RAS_FILTER_BIT_PKT_QUALITY (BIT(0)) | + * | Packet_NADM | RAS_FILTER_BIT_PKT_NADM (BIT(1)) | + * | Packet_RSSI | RAS_FILTER_BIT_PKT_RSSI (BIT(2)) | + * | Packet_Antenna | RAS_FILTER_BIT_PKT_ANTENNA (BIT(3)) | + * | Packet_PCT1 | RAS_FILTER_BIT_PKT_PCT1 (BIT(4)) | + * | Packet_PCT2 | RAS_FILTER_BIT_PKT_PCT2 (BIT(5)) | + * | Measured_Freq_Offset | RAS_FILTER_BIT_FREQ_OFFSET (BIT(6)) | + * | ToA_ToD_Initiator | RAS_FILTER_BIT_TOA_TOD (BIT(7)) | + * | ToD_ToA_Reflector | RAS_FILTER_BIT_TOD_TOA (BIT(8)) | + * | Antenna_Permutation_Index | RAS_FILTER_BIT_ANT_PERM_IDX (BIT(9)) | + * | Tone_PCT[k] | RAS_FILTER_BIT_TONE_PCT (BIT(10)) | + * | Tone_Quality_Indicator[k] | RAS_FILTER_BIT_TONE_QUALITY (BIT(11)) | + * +----------------------------+-------------------------------+ + * + *******************************************************************************************/ +typedef enum { + RAS_FILTER_BIT_PKT_QUALITY = BIT(0), + RAS_FILTER_BIT_PKT_NADM = BIT(1), + RAS_FILTER_BIT_PKT_RSSI = BIT(2), + RAS_FILTER_BIT_PKT_ANTENNA = BIT(3), + RAS_FILTER_BIT_PKT_PCT1 = BIT(4), + RAS_FILTER_BIT_PKT_PCT2 = BIT(5), + RAS_FILTER_BIT_FREQ_OFFSET = BIT(6), + RAS_FILTER_BIT_TOA_TOD = BIT(7), + RAS_FILTER_BIT_TOD_TOA = BIT(8), + RAS_FILTER_BIT_ANT_PERM_IDX = BIT(9), + RAS_FILTER_BIT_TONE_PCT = BIT(10), + RAS_FILTER_BIT_TONE_QUALITY = BIT(11), +} ras_filter_bits_t; + +/** + * @brief RAS Mode 0 Filter Bit Definitions. + * + * These enumeration values define the filter bit positions for RAS Mode 0. + * Each bit indicates whether a particular type of packet or measurement data + * should be included in the Ranging Service procedure. + */ +enum { + SAL_LE_RAS_MODE_0_FILTER_PACKET_QUALITY, + SAL_LE_RAS_MODE_0_FILTER_PACKET_RSSI, + SAL_LE_RAS_MODE_0_FILTER_PACKET_ANTENNA, + SAL_LE_RAS_MODE_0_FILTER_MEASURED_FREQ_OFFSET, + SAL_LE_RAS_MODE_0_FILTER_MAX, +}; + +/** + * @brief RAS Mode 1 Filter Bit Definitions. + * + * Filter bit positions for RAS Mode 1, specifying which packet or measurement + * fields are included in the Ranging Service procedure. + */ +enum { + SAL_LE_RAS_MODE_1_FILTER_PACKET_QUALITY, + SAL_LE_RAS_MODE_1_FILTER_PACKET_NADM, + SAL_LE_RAS_MODE_1_FILTER_PACKET_RSSI, + SAL_LE_RAS_MODE_1_FILTER_TOD_TOA, + SAL_LE_RAS_MODE_1_FILTER_PACKET_ANTENNA, + SAL_LE_RAS_MODE_1_FILTER_PACKET_PCT_1, + SAL_LE_RAS_MODE_1_FILTER_PACKET_PCT_2, + SAL_LE_RAS_MODE_1_FILTER_MAX, +}; + +/** + * @brief RAS Mode 2 Filter Bit Definitions. + * + * Filter bit positions for RAS Mode 2, primarily used for antenna permutation + * and tone analysis in the Ranging Service procedure. + */ +enum { + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PERMUTATION_INDEX, + SAL_LE_RAS_MODE_2_FILTER_TONE_PCT, + SAL_LE_RAS_MODE_2_FILTER_TONE_QUALITY_INDICATOR, + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PATH_1, + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PATH_2, + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PATH_3, + SAL_LE_RAS_MODE_2_FILTER_ANTENNA_PATH_4, + SAL_LE_RAS_MODE_2_FILTER_MAX, +}; + +/** + * @brief RAS Mode 3 Filter Bit Definitions. + * + * Filter bit positions for RAS Mode 3. Combines Mode 1 and Mode 2 filters + * to enable comprehensive packet and antenna/tone analysis. + */ +enum { + SAL_LE_RAS_MODE_3_FILTER_PACKET_QUALITY, + SAL_LE_RAS_MODE_3_FILTER_PACKET_NADM, + SAL_LE_RAS_MODE_3_FILTER_PACKET_RSSI, + SAL_LE_RAS_MODE_3_FILTER_TOD_TOA, + SAL_LE_RAS_MODE_3_FILTER_PACKET_ANTENNA, + SAL_LE_RAS_MODE_3_FILTER_PACKET_PCT_1, + SAL_LE_RAS_MODE_3_FILTER_PACKET_PCT_2, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PERMUTATION_INDEX, + SAL_LE_RAS_MODE_3_FILTER_TONE_PCT, + SAL_LE_RAS_MODE_3_FILTER_TONE_QUALITY_INDICATOR, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PATH_1, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PATH_2, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PATH_3, + SAL_LE_RAS_MODE_3_FILTER_ANTENNA_PATH_4, + SAL_LE_RAS_MODE_3_FILTER_MAX, +}; + +/** + * @brief On-demand RAS Service State Machine. + * + * These enumeration values represent the different states of the On-demand + * Ranging Service procedure. The state machine controls the sequence of + * data ready notifications, data retrieval, sending, acknowledgment, and + * completion. + */ +enum { + /** Idle state. No operation in progress. */ + SAL_LE_RAS_ON_DEMAND_STATE_IDLE, + + /** Data ready indication has been sent to client. */ + SAL_LE_RAS_ON_DEMAND_STATE_DATA_READY_INDICATE, + + /** Waiting for Get Ranging Data response from client. */ + SAL_LE_RAS_ON_DEMAND_STATE_GET_RANGING_DATA_RSP, + + /** Ranging Data is being sent to client. */ + SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_SENDING, + + /** All Ranging Data segments have been sent. */ + SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_COMPLETE_SEND, + + /** Waiting for acknowledgment from client for Ranging Data. */ + SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_ACK_RECV, + + /** Waiting for Ranging Data response. */ + SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_RSP, + + /** Retrieving lost Ranging Data segments received from client. */ + SAL_LE_RAS_ON_DEMAND_STATE_RETRIEVE_LOST_RANGING_DATA_RECV, + + /** Lost Ranging Data segments response being sent. */ + SAL_LE_RAS_ON_DEMAND_STATE_RETRIEVE_LOST_RANGING_DATA_RSP, + + /** Completion of retrieving lost Ranging Data segments. */ + SAL_LE_RAS_ON_DEMAND_STATE_RETRIEVE_LOST_RANGING_DATA_COMPLETE, + + /** Waiting for acknowledgment of lost Ranging Data segments. */ + SAL_LE_RAS_ON_DEMAND_STATE_RETRIEVE_LOST_RANGING_DATA_ACK, + + /** Number of On-demand states. */ + SAL_LE_RAS_ON_DEMAND_STATE_NUMS, +}; + +/** + * @brief RAS Control Point Operation Codes. + * + * Enumeration of possible opcodes for RAS control point operations. + */ +typedef enum { + ABORT_OPERATION = 0, /**< Abort the ongoing Ranging operation. */ + OTHER_OPERATION = 1 /**< Placeholder for other operations. */ +} ras_opcode_t; + +/** + * @brief RAS Data Segment Structure. + * + * Represents a single segment of Ranging Data for On-demand transfers. + */ +typedef struct ras_segment_t { + sys_snode_t seg_node; /**< Node for linked list of segments. */ + uint16_t seg_idx; /**< Segment index in the sequence. */ + uint16_t len; /**< Length of the segment data. */ + uint8_t data[RAS_EMPTY_ARRAY]; /**< Flexible array member for segment payload. */ +} ras_segment_t; + +/** + * @brief RAS On-demand Ranging Data Structure. + * + * Tracks the state of On-demand Ranging Data procedure for a client. + */ +typedef struct ras_rang_on_demand_t { + bool proc_used; /**< Indicates if this procedure slot is in use. */ + uint16_t count; /**< Count of segments or data items. */ + struct k_work_delayable on_demand_work; /**< Timer/work item for response timeout. */ + sys_slist_t seg_list; /**< Linked list of Ranging Data segments. */ + ras_segment_t* seg; /**< Pointer to the current segment being processed. */ +} ras_rang_on_demand_t; + +/** + * @brief RAS Control Point Status. + * + * Tracks the current operation and processing state for the RAS Control Point. + */ +typedef struct { + ras_opcode_t opcode; /**< Current opcode being processed. */ + int is_processing; /**< 1 if operation is in progress, 0 otherwise. */ + int is_data_pending; /**< 1 if data is pending to send, 0 otherwise. */ + uint8_t response_code; /**< Last response code sent to client. */ +} ras_control_point_t; + +/** + * @brief RAS Service Environment. + * + * Maintains all runtime state and data for the RAS BLE Service instance. + */ +typedef struct { + uint16_t step_data_attr_handle; /**< GATT handle of Step Data characteristic. */ + struct bt_conn* connection; /**< Current BLE connection reference. */ + uint8_t latest_local_steps[SAL_LE_RAS_STEP_DATA_BUF_LEN]; /**< Buffer for step or ranging data. */ + uint8_t rt_dt_ccc_cfg; /**< CCC configuration for Real-time Data characteristic. */ + uint8_t ras_dt_rd_indicating; /**< Flag indicating Ranging Data indication state. */ + uint8_t ras_role; /**< RAS role (Server/Client). */ + uint32_t ras_mtu; /**< Maximum Transfer Unit for RAS GATT operations. */ + uint32_t ras_seg_offset; /**< Offset in the current Ranging Data segment. */ + uint32_t remaining_len; /**< Remaining bytes to send in current operation. */ + uint8_t ras_seg_idx; /**< Index of the current Ranging Data segment. */ + struct bt_gatt_indicate_params ras_dt_rd_ind_params; /**< Parameters for GATT indication. */ + uint32_t ras_feature; /**< Bitfield indicating RAS feature support. */ + atomic_t char_notify_state; /**< Bitfield tracking characteristic notification/indication state. */ + atomic_t ras_filter[SAL_LE_RAS_FILTER_MODE_MAX]; /**< Filter settings per RAS mode (0-3). */ + atomic_t on_demand_state; /**< Current state of the On-demand RAS procedure. */ + ras_rang_on_demand_t subevent[SAL_LE_RAS_STORE_PROCEDURE_NUM_MAX]; /**< Array of On-demand procedure slots. */ + ras_control_point_t control_point; /**< Control Point status for current operation. */ + sys_snode_t* on_deman_curr_node; /**< Pointer to current node in On-demand segment list. */ +} sal_le_ras_srv_env_t; + +/** + * @brief Retrieve the ras_rang_on_demand_t pointer from a k_work_delayable work item. + * + * This macro converts a pointer to a k_work structure (from a delayed work callback) + * into a pointer to the enclosing ras_rang_on_demand_t structure. + * + * @param _w Pointer to k_work (from delayed work handler). + * + * @return Pointer to the ras_rang_on_demand_t instance that contains this work item. + * + * @note This is used in the On-demand Ranging Data procedure to find the procedure + * slot associated with a delayed work timeout callback. + */ +#define RAS_ON_DEMAND_WORK_PICK(_w) CONTAINER_OF(k_work_delayable_from_work(_w), \ + struct ras_rang_on_demand_t, on_demand_work) + +/** + * @brief Write the step data for the CS Reflector. + * + * This function is used to send the latest ranging step data + * to the reflector device. + * + * @return 0 on success, or a negative error code on failure. + */ +int write_cs_reflector_step_data(void); + +/** + * @brief Enable the Channel Sounding (CS) feature. + * + * This function initializes and enables the local Core Controller + * to generate Ranging Data for RAS (Ranging Service) operations. + * + * @return 0 on success, or a negative error code on failure. + */ +int le_cs_enable(void); + +#ifdef CONFIG_BT_CS_TEST + +/** + * @brief Notify callback wrapper for test mode. + * + * Redirects the GATT notification callback to the test implementation. + */ +#define BT_GATT_NOTIFY_CB(conn, params) bt_gatt_notify_cb_test(conn, params) + +/** + * @brief Attribute read wrapper for test mode. + * + * Redirects the GATT attribute read operation to the test implementation. + */ +#define BT_GATT_ATTR_READ(conn, attr, buf, buf_len, offset, value, value_len) \ + bt_gatt_attr_read_test(conn, attr, buf, buf_len, offset, value, value_len) + +/** + * @brief GATT notify wrapper for test mode. + * + * Redirects the GATT notification operation to the test implementation. + */ +#define BT_GATT_NOTIFY(conn, attr, data, len) bt_gatt_notify_test(conn, attr, data, len) + +/** + * @brief GATT indicate wrapper for test mode. + * + * Redirects the GATT indication operation to the test implementation. + */ +#define BT_GATT_INDICATE(conn, params) bt_gatt_indicate_test(conn, params) + +#else + +/** + * @brief Notify callback wrapper for normal mode. + * + * Uses the standard GATT notification callback. + */ +#define BT_GATT_NOTIFY_CB(conn, params) bt_gatt_notify_cb(conn, params) + +/** + * @brief Attribute read wrapper for normal mode. + * + * Uses the standard GATT attribute read function. + */ +#define BT_GATT_ATTR_READ(conn, attr, buf, buf_len, offset, value, value_len) \ + bt_gatt_attr_read(conn, attr, buf, buf_len, offset, value, value_len) + +/** + * @brief GATT notify wrapper for normal mode. + * + * Uses the standard GATT notification function. + */ +#define BT_GATT_NOTIFY(conn, attr, data, len) bt_gatt_notify(conn, attr, data, len) + +/** + * @brief GATT indicate wrapper for normal mode. + * + * Uses the standard GATT indication function. + */ +#define BT_GATT_INDICATE(conn, params) bt_gatt_indicate(conn, params) + +#endif /* CONFIG_BT_CS_TEST */ + +/** + * @brief Get the GATT attribute for the RAS service. + * + * This function returns a pointer to the primary GATT attribute + * of the Ranging Service (RAS). It can be used to access the + * service characteristics and descriptors. + * + * @return Pointer to the RAS GATT attribute. + */ +struct bt_gatt_attr* ras_get_gatt_attr(void); + +/** + * @brief Test helper: simulate receiving a RAS subevent. + * + * This function is used for test purposes to inject a subevent + * result into the RAS stack. + * + * @param mode The Ranging mode used for the test (e.g., Real-time or On-demand). + * @param test_case The specific test case scenario to simulate. + * @param conn Pointer to the connection associated with this subevent. + * @param result Pointer to the subevent result data to inject. + * + * @return 0 on success, or a negative error code on failure. + */ +int ras_subevent_recv_test(ras_rang_mode_t mode, ras_testcase_t test_case, + struct bt_conn* conn, struct bt_conn_le_cs_subevent_result* result); + +/** + * @brief Test helper: simulate sending a Control Point response. + * + * This function is used for testing the RAS Control Point procedure. + * + * @param conn Pointer to the connection to send the response to. + * @param attr Pointer to the GATT attribute corresponding to the Control Point characteristic. + * @param data Pointer to the data to send. + * @param len Length of the data in bytes. + * @param offset Offset in the characteristic to start writing. + * @param flags GATT-specific flags (e.g., for reliable writes or prepare writes). + * + * @return 0 on success, or a negative error code on failure. + */ +int ras_ctrl_point_send_test(struct bt_conn* conn, struct bt_gatt_attr* attr, + uint8_t* data, uint16_t len, uint16_t offset, + uint8_t flags); + +#endif /* _CS_RAS_SERVER_H_ */ diff --git a/service/stacks/zephyr/profile/include/cs_ras_test.h b/service/stacks/zephyr/profile/include/cs_ras_test.h new file mode 100644 index 000000000..93821f8f2 --- /dev/null +++ b/service/stacks/zephyr/profile/include/cs_ras_test.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * Copyright (C) 2024 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#ifndef _CS_RAS_TEST_H_ +#define _CS_RAS_TEST_H_ + +#include +#include + +#define RAS_TEST_SUCESS (0) +#define RAS_TEST_FAIL (1) + +#define RAS_TESTCASE_REAL_TIME_NOTIFY_VALID_RANG_DATA_001 (0) +#define RAS_TESTCASE_REAL_TIME_INDICATE_VALID_RANG_DATA_002 (1) +#define RAS_TESTCASE_ON_DEMAND_NOTIFY_VALID_RANG_DATA_003 (3) +#define RAS_TESTCASE_ON_DEMAND_INDICATE_VALID_RANG_DATA_004 (4) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_1_005 (5) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_2_006 (6) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_3_007 (7) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_4_008 (8) +#define RAS_TESTCASE_ON_DEMAND_OVERWRITE_VALID_RANG_DATA_5_009 (9) +#define RAS_TESTCASE_ON_DEMAND_WRITE_RANG_DATA_TIMWOUT_010 (10) + +typedef uint16_t ras_testcase_t; + +int bt_gatt_notify_cb_test(struct bt_conn* conn, + struct bt_gatt_notify_params* params); + +ssize_t bt_gatt_attr_read_test(struct bt_conn* conn, const struct bt_gatt_attr* attr, + void* buf, uint16_t buf_len, uint16_t offset, + const void* value, uint16_t value_len); + +int bt_gatt_notify_test(struct bt_conn* conn, + const struct bt_gatt_attr* attr, + const void* data, uint16_t len); + +int bt_gatt_indicate_test(struct bt_conn* conn, + struct bt_gatt_indicate_params* params); + +int cs_ras_subevent_recv_test(void* data, uint16_t len); +#endif /* _CS_RAS_TEST_H_ */ \ No newline at end of file diff --git a/service/stacks/zephyr/profile/include/distance_estimation.h b/service/stacks/zephyr/profile/include/distance_estimation.h new file mode 100644 index 000000000..218e58be9 --- /dev/null +++ b/service/stacks/zephyr/profile/include/distance_estimation.h @@ -0,0 +1,15 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ +#ifndef _DISTANCE_ESTIMATION_H_ +#define _DISTANCE_ESTIMATION_H_ + +#include +#include + +void estimate_distance(uint8_t *local_steps, uint16_t local_steps_len, uint8_t *peer_steps, + uint16_t peer_steps_len, uint8_t n_ap, enum bt_conn_le_cs_role role); + +#endif /* _DISTANCE_ESTIMATION_H_ */ \ No newline at end of file diff --git a/service/stacks/zephyr/profile/ras_client/cs_ras_client.c b/service/stacks/zephyr/profile/ras_client/cs_ras_client.c new file mode 100644 index 000000000..e490ed89b --- /dev/null +++ b/service/stacks/zephyr/profile/ras_client/cs_ras_client.c @@ -0,0 +1,415 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#include "cs_ras_client.h" + +static struct bt_uuid_16 discover_uuid = BT_UUID_INIT_16(0); + +sal_le_ras_cli_env_t *ras_cli; + +static rap_subevent_result_t* rap_alloc_subevent_pool(void) +{ + if (!ras_cli) { + LOG_ERR("RAS Client haven't init."); + return NULL; + } + + for (int i = 0; i < MAX_SUBEVENT_RESULT; i++) { + if (ras_cli->result_pool[i].is_used == false) { + ras_cli->result_pool[i].is_used = true; + return &ras_cli->result_pool[i]; + } + } + + return NULL; +} + +void rap_free_subevent_pool(rap_subevent_result_t *result) +{ + if (!result) { + LOG_ERR("Invalid params."); + return; + } + + memset(result, 0, sizeof(rap_subevent_result_t)); + return; +} + +static bool is_ras_cli_discovery_complete(void) +{ + return (atomic_test_bit(&discovery_state, DISC_OTS_FEATURE) && + atomic_test_bit(&discovery_state, DISC_OTS_NAME) && + atomic_test_bit(&discovery_state, DISC_OTS_TYPE) && + atomic_test_bit(&discovery_state, DISC_OTS_SIZE) && + atomic_test_bit(&discovery_state, DISC_OTS_ID) && + atomic_test_bit(&discovery_state, DISC_OTS_PROPERTIES) && + atomic_test_bit(&discovery_state, DISC_OTS_ACTION_CP) && + atomic_test_bit(&discovery_state, DISC_OTS_LIST_CP)); +} + +static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result) +{ + // if (result->step_data_buf) { + // if (result->step_data_buf->len <= SAL_LE_RAP_STEP_DATA_BUF_LEN) { + // memcpy(ras_cli->latest_local_steps, result->step_data_buf->data, + // result->step_data_buf->len); + // } else { + // LOG_ERR("Not enough memory to store step data. (%d > %d)\n", + // result->step_data_buf->len, SAL_LE_RAP_STEP_DATA_BUF_LEN); + // } + // } + + if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) { + rap_subevent_result_t* sub_result = rap_alloc_subevent_pool(); + if (!result) { + LOG_ERR("No free subvent pool."); + return; + } + + memcpy(&sub_result->header, &result->header, sizeof(rap_subevent_header_t)); + sub_result->local_steps = (uint8_t *)malloc(result->step_data_buf->len); + memcpy(sub_result->local_steps, result->step_data_buf->data, + result->step_data_buf->len); + } +} + +static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err, + struct bt_gatt_exchange_params *params) +{ + LOG_INF("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn)); +} + +static void connected_cb(struct bt_conn *conn, uint8_t err) +{ + char addr[BT_ADDR_LE_STR_LEN]; + + (void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + LOG_INF("Connected to %s (err 0x%02X)\n", addr, err); + + __ASSERT(ras_cli->connection == conn, "Unexpected connected callback"); + + if (err) { + bt_conn_unref(conn); + ras_cli->connection = NULL; + } + + ras_cli->connection = bt_conn_ref(conn); + + static struct bt_gatt_exchange_params mtu_exchange_params = {.func = mtu_exchange_cb}; + + err = bt_gatt_exchange_mtu(ras_cli->connection, &mtu_exchange_params); + if (err) { + LOG_INF("%s: MTU exchange failed (err %d)\n", __func__, err); + } +} + +rap_ranging_data_body_t *rap_ranging_data_body; + +// Initialize the Ranging Data Body structure with rap_ prefix +void rap_init_ranging_data_body(rap_ranging_data_body_t *body) +{ + body->data = NULL; + body->total_size = 0; + body->received_size = 0; + body->segment_received = (bool *)malloc(MAX_SEGMENTS * sizeof(bool)); + memset(body->segment_received, 0, MAX_SEGMENTS * sizeof(bool)); // Initialize all to false +} + +// Append the segment data to the Ranging Data Body with rap_ prefix +void rap_append_segment_to_body(rap_ranging_data_body_t *body, rap_ranging_data_segment_t *segment) +{ + // Ensure there is enough space to store all the data + if (body->data == NULL) { + body->total_size = MAX_MTU * MAX_SEGMENTS; // Assuming up to MAX_SEGMENTS segments + body->data = (uint8_t *)malloc(body->total_size); + if (body->data == NULL) { + LOG_ERR("Memory allocation failed!"); + return; + } + } + + // Copy the segment data into the Ranging Data Body + memcpy(body->data + body->received_size, segment->data, segment->data_size); + body->received_size += segment->data_size; + body->segment_received[segment->header.segment_index] = true; // Mark the segment as received +} + +// Check if all segments have been received with rap_ prefix +bool rap_is_ranging_data_complete(rap_ranging_data_body_t *body) +{ + // Check if all segments have been received + for (int i = 0; i < MAX_SEGMENTS; i++) { + if (!body->segment_received[i]) { + return false; + } + } + return body->received_size == body->total_size; +} + +uint8_t ras_cli_real_time_data_notify_cb(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) +{ + if (!conn || !params || params->value_handle != ras_cli->rang_realtime_data_handle) { + LOG_ERR("Invalid params. conn(%p), params(%p).", conn, params); + return 0; + } + + +} + +uint8_t ras_cli_on_demand_data_notify_cb(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) +{ + if (!conn || !params || params->value_handle != ras_cli->rang_realtime_data_handle) { + LOG_ERR("Invalid params. conn(%p), params(%p).", conn, params); + return 0; + } + + +} + +static void disconnected_cb(struct bt_conn *conn, uint8_t reason) +{ + LOG_INF("Disconnected (reason 0x%02X)\n", reason); + + bt_conn_unref(conn); + ras_cli->connection = NULL; +} + +static void remote_capabilities_cb(struct bt_conn *conn, struct bt_conn_le_cs_capabilities *params) +{ + ARG_UNUSED(params); + LOG_INF("CS capability exchange completed.\n"); +} + +static void config_created_cb(struct bt_conn *conn, struct bt_conn_le_cs_config *config) +{ + LOG_INF("CS config creation complete. ID: %d\n", config->id); +} + +static void security_enabled_cb(struct bt_conn *conn) +{ + LOG_INF("CS security enabled.\n"); +} + +static void procedure_enabled_cb(struct bt_conn *conn, + struct bt_conn_le_cs_procedure_enable_complete *params) +{ + if (params->state == 1) { + LOG_INF("CS procedures enabled.\n"); + } else { + LOG_INF("CS procedures disabled.\n"); + } +} + +static uint8_t read_feature_cb(struct bt_conn *conn, uint8_t err, + struct bt_gatt_read_params *params, + const void *data, uint16_t length) +{ + +} + +static void rap_read_feature(struct bt_conn *conn) +{ + + ras_cli->read_feature.func = read_feature_cb; + ras_cli->read_feature.handle_count = 1; + ras_cli->read_feature.handle = ras_cli->char_handles.rang_feature_handle; + ras_cli->read_feature.offset = 0U; + int err = bt_gatt_read(conn, &ras_cli->read_feature); + if (err) { + LOG_ERR("Client read feature fail(%d).", err); + return; + } +} + +static uint8_t rap_cli_indicate_handler(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) +{ + if (!conn || !params || !data) { + LOG_ERR("Invalid params."); + return BT_GATT_ITER_STOP; + } + + +} + +static int rap_od_demand_subscribe_func(struct bt_conn *conn) +{ + int ret; + + LOG_INF("Subscribe OACP and OLCP Indication\n"); + oacp_sub_params = &otc.oacp_sub_params; + oacp_sub_params->disc_params = &otc.oacp_sub_disc_params; + if (oacp_sub_params) { + oacp_sub_params->ccc_handle = BT_GATT_AUTO_DISCOVER_CCC_HANDLE; + oacp_sub_params->end_handle = otc.end_handle; + oacp_sub_params->value = BT_GATT_CCC_INDICATE; + oacp_sub_params->value_handle = otc.oacp_handle; + oacp_sub_params->notify = rap_cli_indicate_handler; + ret = bt_gatt_subscribe(default_conn, oacp_sub_params); + + if (ret != 0) { + LOG_INF("Subscribe OACP failed %d\n", ret); + return ret; + } + } + + olcp_sub_params = &otc.olcp_sub_params; + olcp_sub_params->disc_params = &otc.olcp_sub_disc_params; + if (olcp_sub_params) { + olcp_sub_params->ccc_handle = BT_GATT_AUTO_DISCOVER_CCC_HANDLE; + olcp_sub_params->end_handle = otc.end_handle; + olcp_sub_params->value = BT_GATT_CCC_INDICATE; + olcp_sub_params->value_handle = otc.olcp_handle; + olcp_sub_params->notify = bt_ots_client_indicate_handler; + ret = bt_gatt_subscribe(default_conn, olcp_sub_params); + + if (ret != 0) { + LOG_INF("Subscribe OLCP failed %d\n", ret); + return ret; + } + } + + return ret; +} + +static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr, + struct bt_gatt_discover_params *params) +{ + struct bt_gatt_chrc *chrc; + char str[BT_UUID_STR_LEN]; + + LOG_INF("Discovery: attr %p\n", attr); + + if (!attr) { + LOG_INF("Discover complete\n"); + (void)memset(params, 0, sizeof(*params)); + return BT_GATT_ITER_STOP; + } + + chrc = (struct bt_gatt_chrc *)attr->user_data; + + bt_uuid_to_str(chrc->uuid, str, sizeof(str)); + LOG_INF("UUID %s\n", str); + + if (bt_uuid_cmp(discover_params.uuid, BT_UUID_RANGING) == 0) { + (void)memcpy(&discover_uuid, BT_UUID_RANG_RT_DT, sizeof(discover_uuid)); + discover_params.uuid = &discover_uuid.uuid; + discover_params.start_handle = attr->handle + 1; + discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + err = bt_gatt_discover(conn, &discover_params); + + if (err != 0) { + LOG_ERR("Discover failed (err %d)\n", err); + } + } else if (bt_uuid_cmp(discover_params.uuid, BT_UUID_RANG_RT_DT) == 0 ) { + atomic_set_bit(&discovery_state, BT_UUID_RANG_RT_DT); + otc.feature_handle = bt_gatt_attr_value_handle(attr); + (void)memcpy(&discover_uuid, BT_UUID_RANG_ON_DEM_DT, sizeof(discover_uuid)); + discover_params.uuid = &discover_uuid.uuid; + discover_params.start_handle = attr->handle + 1; + discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + + err = bt_gatt_discover(conn, &discover_params); + if (err != 0) { + LOG_ERR("Discover failed (err %d)\n", err); + } + } else if (bt_uuid_cmp(discover_params.uuid, BT_UUID_RANG_ON_DEM_DT) == 0) { + atomic_set_bit(&discovery_state, BT_UUID_RANG_ON_DEM_DT); + otc.feature_handle = bt_gatt_attr_value_handle(attr); + (void)memcpy(&discover_uuid, BT_UUID_RANG_RAS_CTR_POINT, sizeof(discover_uuid)); + discover_params.uuid = &discover_uuid.uuid; + discover_params.start_handle = attr->handle + 1; + discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + + err = bt_gatt_discover(conn, &discover_params); + if (err != 0) { + LOG_ERR("Discover failed (err %d)\n", err); + } + } else if (bt_uuid_cmp(discover_params.uuid, BT_UUID_RANG_RAS_CTR_POINT) == 0) { + atomic_set_bit(&discovery_state, BT_UUID_RANG_RAS_CTR_POINT); + otc.obj_name_handle = bt_gatt_attr_value_handle(attr); + (void)memcpy(&discover_uuid, BT_UUID_RANG_DT_RD, sizeof(discover_uuid)); + discover_params.uuid = &discover_uuid.uuid; + discover_params.start_handle = attr->handle + 1; + discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + + err = bt_gatt_discover(conn, &discover_params); + if (err != 0) { + LOG_ERR("Discover failed (err %d)\n", err); + } + + } else if (bt_uuid_cmp(discover_params.uuid, BT_UUID_RANG_DT_RD) == 0) { + atomic_set_bit(&discovery_state, BT_UUID_RANG_DT_RD); + otc.obj_type_handle = bt_gatt_attr_value_handle(attr); + (void)memcpy(&discover_uuid, BT_UUID_RANG_DT_OV_WR, sizeof(discover_uuid)); + discover_params.uuid = &discover_uuid.uuid; + discover_params.start_handle = attr->handle + 1; + discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + + err = bt_gatt_discover(conn, &discover_params); + if (err != 0) { + LOG_ERR("Discover failed (err %d)\n", err); + } + } else if (bt_uuid_cmp(discover_params.uuid, BT_UUID_RANG_DT_OV_WR) == 0) { + atomic_set_bit(&discovery_state, BT_UUID_RANG_DT_OV_WR); + otc.obj_size_handle = bt_gatt_attr_value_handle(attr); + (void)memcpy(&discover_uuid, BT_UUID_OTS_ID, sizeof(discover_uuid)); + discover_params.uuid = &discover_uuid.uuid; + discover_params.start_handle = attr->handle + 1; + discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + + err = bt_gatt_discover(conn, &discover_params); + if (err != 0) { + LOG_ERR("Discover failed (err %d)\n", err); + } + + } else { + return BT_GATT_ITER_STOP; + } + + return BT_GATT_ITER_STOP; +} + +static void write_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params) +{ + if (err) { + LOG_ERR("Write failed (err %d)\n", err); + return; + } +} + +BT_CONN_CB_DEFINE(conn_cb) = { + .connected = connected_cb, + .disconnected = disconnected_cb, + .le_cs_remote_capabilities_available = remote_capabilities_cb, + .le_cs_config_created = config_created_cb, + .le_cs_security_enabled = security_enabled_cb, + .le_cs_procedure_enabled = procedure_enabled_cb, + .le_cs_subevent_data_available = subevent_result_cb, +}; + +void ras_client_init(void) +{ + ras_cli = (sal_le_ras_cli_env_t *)malloc(sizeof(sal_le_ras_cli_env_t)); + __ASSERT(ras_cli != NULL); + return; +} + diff --git a/service/stacks/zephyr/profile/ras_client/distance_estimation.c b/service/stacks/zephyr/profile/ras_client/distance_estimation.c new file mode 100644 index 000000000..670889ec0 --- /dev/null +++ b/service/stacks/zephyr/profile/ras_client/distance_estimation.c @@ -0,0 +1,319 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#include +#include +#include "distance_estimation.h" + +#define CS_FREQUENCY_MHZ(ch) (2402u + 1u * (ch)) +#define CS_FREQUENCY_HZ(ch) (CS_FREQUENCY_MHZ(ch) * 1000000.0f) +#define SPEED_OF_LIGHT_M_PER_S (299792458.0f) +#define SPEED_OF_LIGHT_NM_PER_S (SPEED_OF_LIGHT_M_PER_S / 1000000000.0f) +#define PI 3.14159265358979323846f +#define MAX_NUM_SAMPLES 256 + +struct iq_sample_and_channel { + bool failed; + uint8_t channel; + uint8_t antenna_permutation; + struct bt_le_cs_iq_sample local_iq_sample; + struct bt_le_cs_iq_sample peer_iq_sample; +}; + +struct rtt_timing { + bool failed; + int16_t toa_tod_initiator; + int16_t tod_toa_reflector; +}; + +static struct iq_sample_and_channel mode_2_data[MAX_NUM_SAMPLES]; +static struct rtt_timing mode_1_data[MAX_NUM_SAMPLES]; + +struct processing_context { + bool local_steps; + uint8_t mode_1_data_index; + uint8_t mode_2_data_index; + uint8_t n_ap; + enum bt_conn_le_cs_role role; +}; + +static void calc_complex_product(int32_t z_a_real, int32_t z_a_imag, int32_t z_b_real, + int32_t z_b_imag, int32_t *z_out_real, int32_t *z_out_imag) +{ + *z_out_real = z_a_real * z_b_real - z_a_imag * z_b_imag; + *z_out_imag = z_a_real * z_b_imag + z_a_imag * z_b_real; +} + +static float linear_regression(float *x_values, float *y_values, uint8_t n_samples) +{ + if (n_samples == 0) { + return 0.0; + } + + /* Estimates b in y = a + b x */ + + float y_mean = 0.0; + float x_mean = 0.0; + + for (uint8_t i = 0; i < n_samples; i++) { + y_mean += (y_values[i] - y_mean) / (i + 1); + x_mean += (x_values[i] - x_mean) / (i + 1); + } + + float b_est_upper = 0.0; + float b_est_lower = 0.0; + + for (uint8_t i = 0; i < n_samples; i++) { + b_est_upper += (x_values[i] - x_mean) * (y_values[i] - y_mean); + b_est_lower += (x_values[i] - x_mean) * (x_values[i] - x_mean); + } + + return b_est_upper / b_est_lower; +} + +static void bubblesort_2(float *array1, float *array2, uint16_t len) +{ + bool swapped; + float temp; + + for (uint16_t i = 0; i < len - 1; i++) { + swapped = false; + for (uint16_t j = 0; j < len - i - 1; j++) { + if (array1[j] > array1[j + 1]) { + temp = array1[j]; + array1[j] = array1[j + 1]; + array1[j + 1] = temp; + temp = array2[j]; + array2[j] = array2[j + 1]; + array2[j + 1] = temp; + swapped = true; + } + } + + if (!swapped) { + break; + } + } +} + +static float estimate_distance_using_phase_slope(struct iq_sample_and_channel *data, uint8_t len) +{ + int32_t combined_i; + int32_t combined_q; + uint16_t num_angles = 0; + static float theta[MAX_NUM_SAMPLES]; + static float frequencies[MAX_NUM_SAMPLES]; + + for (uint8_t i = 0; i < len; i++) { + if (!data[i].failed) { + calc_complex_product(data[i].local_iq_sample.i, data[i].local_iq_sample.q, + data[i].peer_iq_sample.i, data[i].peer_iq_sample.q, + &combined_i, &combined_q); + + theta[num_angles] = atan2(1.0 * combined_q, 1.0 * combined_i); + frequencies[num_angles] = 1.0 * CS_FREQUENCY_MHZ(data[i].channel); + num_angles++; + } + } + + if (num_angles < 2) { + return 0.0; + } + + /* Sort phases by tone frequency */ + bubblesort_2(frequencies, theta, num_angles); + + /* One-dimensional phase unwrapping */ + for (uint8_t i = 1; i < num_angles; i++) { + float difference = theta[i] - theta[i - 1]; + + if (difference > PI) { + for (uint8_t j = i; j < num_angles; j++) { + theta[j] -= 2.0f * PI; + } + } else if (difference < -PI) { + for (uint8_t j = i; j < num_angles; j++) { + theta[j] += 2.0f * PI; + } + } + } + + float phase_slope = linear_regression(frequencies, theta, num_angles); + + float distance = -phase_slope * (SPEED_OF_LIGHT_M_PER_S / (4 * PI)); + + return distance / 1000000.0f; /* Scale to meters. */ +} + +static float estimate_distance_using_time_of_flight(uint8_t n_samples) +{ + float tof; + float tof_mean = 0.0; + + /* Cumulative Moving Average */ + for (uint8_t i = 0; i < n_samples; i++) { + if (!mode_1_data[i].failed) { + tof = (mode_1_data[i].toa_tod_initiator - + mode_1_data[i].tod_toa_reflector) / + 2; + tof_mean += (tof - tof_mean) / (i + 1); + } + } + + float tof_mean_ns = tof_mean / 2.0f; + + return tof_mean_ns * SPEED_OF_LIGHT_NM_PER_S; +} + +static bool process_step_data(struct bt_le_cs_subevent_step *step, void *user_data) +{ + struct processing_context *context = (struct processing_context *)user_data; + + if (step->mode == BT_CONN_LE_CS_MAIN_MODE_2) { + struct bt_hci_le_cs_step_data_mode_2 *step_data = + (struct bt_hci_le_cs_step_data_mode_2 *)step->data; + + if (context->local_steps) { + for (uint8_t i = 0; i < (context->n_ap + 1); i++) { + if (step_data->tone_info[i].extension_indicator != + BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) { + continue; + } + + mode_2_data[context->mode_2_data_index].channel = step->channel; + mode_2_data[context->mode_2_data_index].antenna_permutation = + step_data->antenna_permutation_index; + mode_2_data[context->mode_2_data_index].local_iq_sample = + bt_le_cs_parse_pct( + step_data->tone_info[i].phase_correction_term); + if (step_data->tone_info[i].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_LOW || + step_data->tone_info[i].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) { + mode_2_data[context->mode_2_data_index].failed = true; + } + + context->mode_2_data_index++; + } + } else { + for (uint8_t i = 0; i < (context->n_ap + 1); i++) { + if (step_data->tone_info[i].extension_indicator != + BT_HCI_LE_CS_NOT_TONE_EXT_SLOT) { + continue; + } + + mode_2_data[context->mode_2_data_index].peer_iq_sample = + bt_le_cs_parse_pct( + step_data->tone_info[i].phase_correction_term); + if (step_data->tone_info[i].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_LOW || + step_data->tone_info[i].quality_indicator == + BT_HCI_LE_CS_TONE_QUALITY_UNAVAILABLE) { + mode_2_data[context->mode_2_data_index].failed = true; + } + + context->mode_2_data_index++; + } + } + } else if (step->mode == BT_HCI_OP_LE_CS_MAIN_MODE_1) { + struct bt_hci_le_cs_step_data_mode_1 *step_data = + (struct bt_hci_le_cs_step_data_mode_1 *)step->data; + + if (step_data->packet_quality_aa_check != + BT_HCI_LE_CS_PACKET_QUALITY_AA_CHECK_SUCCESSFUL || + step_data->packet_rssi == BT_HCI_LE_CS_PACKET_RSSI_NOT_AVAILABLE || + step_data->tod_toa_reflector == BT_HCI_LE_CS_TIME_DIFFERENCE_NOT_AVAILABLE) { + mode_1_data[context->mode_1_data_index].failed = true; + } + + if (context->local_steps) { + if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) { + mode_1_data[context->mode_1_data_index].toa_tod_initiator = + step_data->toa_tod_initiator; + } else if (context->role == BT_CONN_LE_CS_ROLE_REFLECTOR) { + mode_1_data[context->mode_1_data_index].tod_toa_reflector = + step_data->tod_toa_reflector; + } + } else { + if (context->role == BT_CONN_LE_CS_ROLE_INITIATOR) { + mode_1_data[context->mode_1_data_index].tod_toa_reflector = + step_data->tod_toa_reflector; + } else if (context->role == BT_CONN_LE_CS_ROLE_REFLECTOR) { + mode_1_data[context->mode_1_data_index].toa_tod_initiator = + step_data->toa_tod_initiator; + } + } + + context->mode_1_data_index++; + } + + return true; +} + +void estimate_distance(uint8_t *local_steps, uint16_t local_steps_len, uint8_t *peer_steps, + uint16_t peer_steps_len, uint8_t n_ap, enum bt_conn_le_cs_role role) +{ + struct net_buf_simple buf; + + struct processing_context context = { + .local_steps = true, + .mode_1_data_index = 0, + .mode_2_data_index = 0, + .n_ap = n_ap, + .role = role, + }; + + memset(mode_1_data, 0, sizeof(mode_1_data)); + memset(mode_2_data, 0, sizeof(mode_2_data)); + + net_buf_simple_init_with_data(&buf, local_steps, local_steps_len); + + bt_le_cs_step_data_parse(&buf, process_step_data, &context); + + context.mode_1_data_index = 0; + context.mode_2_data_index = 0; + context.local_steps = false; + + net_buf_simple_init_with_data(&buf, peer_steps, peer_steps_len); + + bt_le_cs_step_data_parse(&buf, process_step_data, &context); + + float phase_slope_based_distance = + estimate_distance_using_phase_slope(mode_2_data, context.mode_2_data_index); + + float rtt_based_distance = + estimate_distance_using_time_of_flight(context.mode_1_data_index); + + if (rtt_based_distance == 0.0f && phase_slope_based_distance == 0.0f) { + LOG_INF("A reliable distance estimate could not be computed.\n"); + } else { + LOG_INF("Estimated distance to reflector:\n"); + } + + if (rtt_based_distance != 0.0f) { + LOG_INF("- Round-Trip Timing method: %f meters (derived from %d samples)\n", + (double)rtt_based_distance, context.mode_1_data_index); + } + if (phase_slope_based_distance != 0.0f) { + LOG_INF("- Phase-Based Ranging method: %f meters (derived from %d samples)\n", + (double)phase_slope_based_distance, context.mode_2_data_index); + } +} diff --git a/service/stacks/zephyr/profile/ras_server/cs_ras_server.c b/service/stacks/zephyr/profile/ras_server/cs_ras_server.c new file mode 100644 index 000000000..f842d57b4 --- /dev/null +++ b/service/stacks/zephyr/profile/ras_server/cs_ras_server.c @@ -0,0 +1,1736 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#include "cs_ras_server.h" +#include "bt_addr.h" +#include "cs_service.h" +#include "profiles/cs/cs_msg.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef CONFIG_BLUETOOTH_LE_CS + +#ifndef MIN +#define MIN(x, y) (((x) < (y)) ? (x) : (y)) +#endif + +#define CS_CONFIG_ID 0 +#define NUM_MODE_0_STEPS 1 +#define RAS_SEG_HEADER_SIZE 4 + +static const char sample_str[] = "CS Sample111"; +static const struct bt_data ad[] = { + BT_DATA(BT_DATA_NAME_COMPLETE, "CS Sample111", sizeof(sample_str) - 1), + BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)), + BT_DATA_BYTES(BT_DATA_UUID16_SOME, BT_UUID_16_ENCODE(0x185B)), +}; + +static sal_le_ras_srv_env_t* ras_srv; + +/** @brief LE Audio Attribute User Data. */ +struct bt_ras_attr_user_data { + /** Attribute read callback */ + ssize_t (*read)(struct bt_conn* conn, const struct bt_gatt_attr* attr, + void* buf, uint16_t len, uint16_t offset); + + /** Attribute write callback */ + ssize_t (*write)(struct bt_conn* conn, const struct bt_gatt_attr* attr, + const void* buf, uint16_t len, uint16_t offset, + uint8_t flags); + + /** Attribute user data */ + void* user_data; +}; + +static void split_real_time_segment(struct bt_conn* conn, uint8_t* buf, int len); +static ssize_t ras_feature_read(struct bt_conn* conn, const struct bt_gatt_attr* attr, + void* buf, uint16_t len, uint16_t offset); +static void range_rt_dt_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value); +static void range_on_dem_dt_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value); +ssize_t on_ras_ctr_pt_write_cb(struct bt_conn* conn, const struct bt_gatt_attr* attr, + const void* buf, uint16_t len, uint16_t offset, + uint8_t flags); +static void range_dt_rd_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value); +static void range_dt_ov_wr_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value); +static void range_ctr_pt_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value); +static ras_rang_on_demand_t* ras_rang_on_demand_find_subevent(struct bt_conn* conn, uint16_t count); +static int ras_data_ready_send(struct bt_conn* conn, uint16_t count); +static int ras_on_demond_send_cmp_ranging_data_rsp(struct bt_conn* conn, uint16_t count); + +#define BT_RAS_ATTR_USER_DATA_INIT(_read, _write, _user_data) \ + { \ + .read = _read, \ + .write = _write, \ + .user_data = _user_data, \ + } + +/** Helper to define LE RAS characteristic. */ +#define BT_RAS_CHRC(_uuid, _props, _perm, _read, _write, _user_data) \ + BT_GATT_CHARACTERISTIC(_uuid, _props, _perm, _read, _write, \ + ((struct bt_ras_attr_user_data[]) { \ + BT_RAS_ATTR_USER_DATA_INIT(_read, _write, _user_data), \ + })) + +#define BT_RAS_CHRC_USER_DATA(_attr) \ + (((struct bt_ras_attr_user_data*)(_attr)->user_data)->user_data) + +/** Helper to define LE Audio CCC descriptor. */ +#define BT_RAS_CCC(_changed) \ + BT_GATT_CCC_MANAGED(((struct _bt_gatt_ccc[]) { BT_GATT_CCC_INITIALIZER(_changed, NULL, NULL) }), \ + (BT_GATT_PERM_READ)) + +#define BT_RAS_SERVICE_DEFINITION() \ + { \ + BT_GATT_PRIMARY_SERVICE(BT_UUID_RANGING), \ + BT_RAS_CHRC(BT_UUID_RANG_FEAT, \ + BT_GATT_CHRC_READ, \ + BT_GATT_PERM_READ, \ + ras_feature_read, NULL, NULL), \ + BT_RAS_CHRC(BT_UUID_RANG_RT_DT, \ + (BT_GATT_CHRC_NOTIFY | BT_GATT_CHRC_INDICATE), \ + BT_GATT_PERM_READ, \ + NULL, NULL, NULL), \ + BT_GATT_CCC(range_rt_dt_ccc_cfg_changed, (BT_GATT_PERM_READ | BT_GATT_PERM_WRITE)), \ + BT_RAS_CHRC(BT_UUID_RANG_ON_DEM_DT, \ + (BT_GATT_CHRC_NOTIFY | BT_GATT_CHRC_INDICATE), \ + BT_GATT_PERM_READ, \ + NULL, NULL, NULL), \ + BT_GATT_CCC(range_on_dem_dt_ccc_cfg_changed, (BT_GATT_PERM_READ | BT_GATT_PERM_WRITE)), \ + BT_RAS_CHRC(BT_UUID_RANG_RAS_CTR_POINT, \ + (BT_GATT_CHRC_WRITE_WITHOUT_RESP | BT_GATT_CHRC_INDICATE), \ + (BT_GATT_PERM_READ | BT_GATT_PERM_WRITE), \ + NULL, on_ras_ctr_pt_write_cb, NULL), \ + BT_GATT_CCC(range_ctr_pt_ccc_cfg_changed, (BT_GATT_PERM_READ | BT_GATT_PERM_WRITE)), \ + BT_RAS_CHRC(BT_UUID_RANG_DT_RD, \ + BT_GATT_CHRC_INDICATE, \ + BT_GATT_PERM_READ, \ + NULL, NULL, NULL), \ + BT_GATT_CCC(range_dt_rd_ccc_cfg_changed, (BT_GATT_PERM_READ | BT_GATT_PERM_WRITE)), \ + BT_RAS_CHRC(BT_UUID_RANG_DT_OV_WR, \ + BT_GATT_CHRC_INDICATE, \ + BT_GATT_PERM_READ, \ + NULL, NULL, NULL), \ + BT_GATT_CCC(range_dt_ov_wr_ccc_cfg_changed, (BT_GATT_PERM_READ | BT_GATT_PERM_WRITE)), \ + } + +static struct bt_gatt_attr ras_attrs[RAS_IDX_MAX] = BT_RAS_SERVICE_DEFINITION(); +static struct bt_gatt_service ras_svc = (struct bt_gatt_service)BT_GATT_SERVICE(ras_attrs); + +static void ras_on_demand_notify_finished(struct bt_conn* conn, void* user_data) +{ + sys_snode_t* on_deman_pdu = (sys_snode_t*)user_data; + + if (!on_deman_pdu) { + LOG_INF("Compelete segment data sent."); + ras_on_demond_send_cmp_ranging_data_rsp(conn, 10); + return; + } + + ras_segment_t* seg = CONTAINER_OF(on_deman_pdu, ras_segment_t, seg_node); + + if (!seg) { + LOG_WRN("Invalid segment."); + return; + } + + LOG_INF("seg:%p, seg->data(%d):%s", seg, seg->len, bt_hex(seg->data, seg->len)); + + struct bt_gatt_notify_params notify_param = { + .attr = &ras_attrs[SAL_LE_RAS_ON_DEM_CHAR_IDX], + .func = ras_on_demand_notify_finished, + .user_data = (void*)on_deman_pdu->next, + .data = seg->data, + .len = seg->len, + }; + + int err = BT_GATT_NOTIFY_CB(conn, ¬ify_param); + if (err != 0) { + LOG_ERR("On-demand ranging data notify fail, seg_idx(%d), err(%d).", + seg->seg_idx, err); + return; + } + + return; +} + +static void ras_on_demand_indicate_finished(struct bt_conn* conn, + struct bt_gatt_indicate_params* params, + uint8_t err) +{ + sys_snode_t* on_deman_pdu = (sys_snode_t*)ras_srv->on_deman_curr_node; + + if (!on_deman_pdu) { + LOG_INF("Compelete segment data sent."); + ras_on_demond_send_cmp_ranging_data_rsp(conn, 10); + return; + } + + ras_segment_t* seg = CONTAINER_OF(on_deman_pdu, ras_segment_t, seg_node); + + if (!seg) { + LOG_WRN("Invalid segment."); + return; + } + + LOG_INF("seg:%p, seg->data(%d):%s", seg, seg->len, bt_hex(seg->data, seg->len)); + + struct bt_gatt_indicate_params ind_params = { + .attr = &ras_attrs[SAL_LE_RAS_ON_DEM_CHAR_IDX], + .data = seg->data, + .len = seg->len, + .func = ras_on_demand_indicate_finished, + }; + + ras_srv->on_deman_curr_node = on_deman_pdu->next; + + err = BT_GATT_INDICATE(conn, &ind_params); + if (err != 0) { + LOG_ERR("On-demand ranging data indication fail, seg_idx(%d), err(%d).", + seg->seg_idx, err); + return; + } + + return; +} + +static int ras_ondemand_send_ranging_data(struct bt_conn* conn, uint16_t count) +{ + if (!atomic_test_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY) && !atomic_test_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE)) { + LOG_ERR("RAS haven't enable the on-demond ranging data notify or indication state."); + return -1; + } + + int err = 0; + + ras_rang_on_demand_t* on_demand_sub = ras_rang_on_demand_find_subevent(conn, count); + + if (!on_demand_sub) { + LOG_ERR("Haven't find subevent with count(%d).", count); + return -1; + } + + const sys_snode_t* on_deman_pdu = sys_slist_peek_head(&on_demand_sub->seg_list); + struct ras_segment_t* seg = CONTAINER_OF(on_deman_pdu, ras_segment_t, seg_node); + + if (atomic_test_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY)) { + struct bt_gatt_notify_params notify_param = { + .attr = &ras_attrs[SAL_LE_RAS_ON_DEM_CHAR_IDX], + .func = ras_on_demand_notify_finished, + .user_data = (void*)on_deman_pdu->next, + .data = seg->data, + .len = seg->len, + }; + + LOG_INF("seg:%p, seg->data(%d):%s", seg, seg->len, bt_hex(seg->data, seg->len)); + + err = BT_GATT_NOTIFY_CB(conn, ¬ify_param); + if (err != 0) { + LOG_ERR("On-demand ranging data notify fail, err(%d).", err); + return err; + } + } else if (atomic_test_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE)) { + struct bt_gatt_indicate_params ind_params = { + .attr = &ras_attrs[SAL_LE_RAS_ON_DEM_CHAR_IDX], + .data = seg->data, + .len = seg->len, + .func = ras_on_demand_indicate_finished, + }; + + LOG_INF("seg:%p, seg->data(%d):%s", seg, seg->len, bt_hex(seg->data, seg->len)); + // Set the current node to the next. + ras_srv->on_deman_curr_node = on_deman_pdu->next; + err = BT_GATT_INDICATE(conn, &ind_params); + if (err != 0) { + LOG_ERR("On-demand ranging data indicate fail, err(%d).", err); + return err; + } + } + + return err; +} + +static int ras_on_demond_send_cmp_ranging_data_rsp(struct bt_conn* conn, uint16_t count) +{ + ras_rang_on_demand_t* on_demand_data = ras_rang_on_demand_find_subevent(conn, count); + + if (!on_demand_data) { + LOG_WRN("Haven't find the demand data with the count(%d).", count); + } + + uint8_t buf[3] = { 0 }; + int err; + buf[0] = SAL_LE_RAS_CTL_OP_RSP_CMP_RANG_DATA; + sys_put_le16(count, &buf[1]); + + if (atomic_test_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + struct bt_gatt_notify_params notify_param = { + .attr = &ras_attrs[SAL_LE_RAS_CTR_PT_CHAR_IDX], + .func = NULL, + .user_data = NULL, + .data = buf, + .len = sizeof(buf), + }; + + err = BT_GATT_NOTIFY_CB(conn, ¬ify_param); + if (err != 0) { + LOG_ERR("ranging data Control Point response ACK notify fail, err(%d).", err); + return err; + } + } else if (atomic_test_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + struct bt_gatt_indicate_params ind_params = { + .attr = &ras_attrs[SAL_LE_RAS_CTR_PT_CHAR_IDX], + .data = buf, + .len = sizeof(buf), + .func = NULL, + }; + + err = BT_GATT_INDICATE(conn, &ind_params); + if (err != 0) { + LOG_ERR("ranging data Control Point response ACK indicate fail, err(%d).", err); + return err; + } + } else { + LOG_ERR("The Ranging Control Point haven't set to notify or indication state."); + return -1; + } + + return 0; +} + +static int ras_on_demond_send_code_rsp(struct bt_conn* conn, uint16_t count) +{ + ras_rang_on_demand_t* on_demand_data = ras_rang_on_demand_find_subevent(conn, count); + + if (!on_demand_data) { + LOG_WRN("Haven't find the demand data with the count(%d).", count); + } + + uint8_t buf[2] = { 0 }; + int err; + buf[0] = SAL_LE_RAS_CTL_OP_RSP_CODE; + buf[1] = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; + + if (atomic_test_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + + struct bt_gatt_notify_params notify_param = { + .attr = &ras_attrs[SAL_LE_RAS_CTR_PT_CHAR_IDX], + .func = NULL, + .user_data = NULL, + .data = buf, + .len = sizeof(buf), + }; + + err = BT_GATT_NOTIFY_CB(conn, ¬ify_param); + if (err != 0) { + LOG_ERR("ranging data Control Point response ACK notify fail, err(%d).", err); + return err; + } + } else if (atomic_test_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + struct bt_gatt_indicate_params ind_params = { + .attr = &ras_attrs[SAL_LE_RAS_CTR_PT_CHAR_IDX], + .data = buf, + .len = sizeof(buf), + .func = NULL, + }; + + err = BT_GATT_INDICATE(conn, &ind_params); + if (err != 0) { + LOG_ERR("ranging data Control Point response ACK indicate fail, err(%d).", err); + return err; + } + } + + // Free the on-demand segment list when response the ack to the Client. + ras_segment_t *seg_prev, *seg_next; + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&on_demand_data->seg_list, + seg_prev, seg_next, seg_node) + { + sys_slist_remove(&on_demand_data->seg_list, NULL, &seg_prev->seg_node); + LOG_INF("seg_prev:%p", seg_prev); + free(seg_prev); + } + + LOG_INF("The on-demand data has been sent, Cancel on-demand timer."); + /* The on-demand data has been sent, Cancel on-demand timer */ + k_work_cancel_delayable(&on_demand_data->on_demand_work); + memset(on_demand_data, 0, sizeof(ras_rang_on_demand_t)); + return 0; +} + +static int ras_on_demond_send_lost_ranging_data_cmp_rsp(struct bt_conn* conn, uint16_t count, uint8_t first, uint8_t last) +{ + uint8_t buf[5] = { 0 }; + + buf[0] = 0x01; + sys_put_le16(count, &buf[1]); + buf[3] = first; + buf[4] = last; + + int err; + if (atomic_test_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + buf[0] = SAL_LE_RAS_CTL_OP_RSP_CODE; + buf[1] = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; + struct bt_gatt_notify_params notify_param = { + .attr = &ras_attrs[SAL_LE_RAS_CTR_PT_CHAR_IDX], + .func = NULL, + .user_data = NULL, + .data = buf, + .len = sizeof(buf), + }; + + err = BT_GATT_NOTIFY_CB(conn, ¬ify_param); + if (err != 0) { + LOG_ERR("ranging data Control Point response ACK notify fail, err(%d).", err); + return err; + } + } else if (atomic_test_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_INDICATE)) { + struct bt_gatt_indicate_params ind_params = { + .attr = &ras_attrs[SAL_LE_RAS_CTR_PT_CHAR_IDX], + .data = buf, + .len = sizeof(buf), + .func = NULL, + }; + + err = BT_GATT_INDICATE(conn, &ind_params); + if (err != 0) { + LOG_ERR("ranging data Control Point response ACK indicate fail, err(%d).", err); + return err; + } + } + + return 0; +} + +static int ras_on_demand_retrieve_send_lost_data(struct bt_conn* conn, uint16_t count, + uint8_t first_seg, uint8_t last_seg) +{ + ras_rang_on_demand_t* on_demand_data = ras_rang_on_demand_find_subevent(conn, count); + + if (!atomic_test_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_RANGING_DATA_RSP)) { + LOG_WRN("Invalid on_demand state."); + return -1; + } + + int err; + // Free the on-demand segment list when response the ack to the Client. + ras_segment_t *seg_prev, *seg_next; + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&on_demand_data->seg_list, + seg_prev, seg_next, seg_node) + { + if (seg_prev && (seg_prev->seg_idx >= first_seg) && (seg_prev->seg_idx <= last_seg)) { + if (atomic_test_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY)) { + struct bt_gatt_notify_params notify_param = { + .attr = &ras_attrs[SAL_LE_RAS_ON_DEM_CHAR_IDX], + .func = ras_on_demand_notify_finished, + .user_data = (void*)seg_prev->seg_node.next, + .data = seg_prev->data, + .len = seg_prev->len, + }; + + err = BT_GATT_NOTIFY_CB(conn, ¬ify_param); + if (err != 0) { + LOG_ERR("On-demand ranging data notify fail, err(%d).", err); + return err; + } + } else if (atomic_test_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_INDICATE)) { + struct bt_gatt_indicate_params ind_params = { + .attr = &ras_attrs[SAL_LE_RAS_ON_DEM_CHAR_IDX], + .data = seg_prev->data, + .len = seg_prev->len, + .func = ras_on_demand_indicate_finished, + }; + + err = BT_GATT_INDICATE(conn, &ind_params); + if (err != 0) { + LOG_ERR("On-demand ranging data indicate fail, err(%d).", err); + return err; + } + } + } else if (seg_prev && (seg_prev->seg_idx > last_seg)) { + break; + } + } + + ras_on_demond_send_lost_ranging_data_cmp_rsp(conn, count, first_seg, last_seg); + return 0; +} + +static uint8_t ras_check_ranging_mode(struct bt_conn* conn) +{ + if (atomic_test_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_NOTIFY) || atomic_test_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_INDICATE)) { + LOG_INF("The real-time mode has been set."); + return SAL_LE_RAS_RANGING_MODE_REAL_TIME; + } else if (atomic_test_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY) || atomic_test_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE)) { + LOG_INF("The on-demand mode has been set."); + return SAL_LE_RAS_RANGING_MODE_ON_DEMAND; + } + + LOG_ERR("No mode have been set."); + return SAL_LE_RAS_RANGING_MODE_UNDEFINED; +} + +bool ras_is_filter_bit_set(uint16_t mode, uint16_t filter_bit) +{ + // Check if the mode is valid (within the range of SAL_LE_RAS_MODE_3_FILTER_MAX) + if (mode >= SAL_LE_RAS_FILTER_MODE_MAX) { + LOG_ERR("Error: Mode %d is out of valid range (0 to %d)\n", + mode, SAL_LE_RAS_FILTER_MODE_MAX - 1); + return false; + } + + // Retrieve the current filter mask from the atomic array + atomic_val_t current_filter_mask = atomic_get(&ras_srv->ras_filter[mode]); + + // Check if the specific filter bit is set + // We shift the filter_bit into the correct position and mask it to check + return (current_filter_mask & (1 << filter_bit)) != 0; +} + +static void ras_set_filter(uint16_t filter_value) +{ + // Extract the mode (bits 0-1) + uint16_t mode = filter_value & SAL_LE_RAS_FILTER_MODE_MASK; + + // Extract the filter mask (bits 2-15) + uint16_t filter_mask = filter_value & ~SAL_LE_RAS_FILTER_MODE_MASK; + + // Use the mode directly as an index into ras_filter + if (mode < SAL_LE_RAS_FILTER_MODE_MAX) { + // Atomic operations to modify the filter mask + atomic_and(&ras_srv->ras_filter[mode], SAL_LE_RAS_FILTER_BIT_MASK); // Clear the filter bits (2-15) + atomic_or(&ras_srv->ras_filter[mode], filter_mask); // Set the new filter mask (bits 2-15) + } else { + // Handle error case if mode is out of range + LOG_ERR("Error: Mode %d is out of valid range (0 to %d)\n", mode, SAL_LE_RAS_FILTER_MODE_MAX - 1); + } + + return; +} + +void ras_handle_abort_operation(ras_control_point_t* control_point) +{ + if (control_point->is_processing) { + LOG_DBG("Stopping current RAS Control Point processes..."); + control_point->is_processing = 0; // Stop current processing. + + if (control_point->is_data_pending) { + LOG_DBG("Flushing pending Ranging Data segments..."); + control_point->is_data_pending = 0; // Discard pending data + } + + control_point->response_code = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; // response success. + LOG_DBG("Abort Operation completed successfully."); + } else { + control_point->response_code = SAL_LE_RAS_CTL_OP_RSP_CODE_NOT_SUPPORTED; + LOG_DBG("Abort Operation was unsuccessful (no operation to abort)."); + } +} + +void ras_handle_other_operation(ras_control_point_t* control_point) +{ + LOG_DBG("Handling other operations..."); + control_point->response_code = SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS; // response success. +} + +void ras_write_opcode(ras_control_point_t* control_point, ras_opcode_t op_code) +{ + switch (op_code) { + case ABORT_OPERATION: + LOG_DBG("Received ABORT_OPERATION OpCode."); + ras_handle_abort_operation(control_point); + break; + + case OTHER_OPERATION: + LOG_DBG("Received OTHER_OPERATION OpCode."); + ras_handle_other_operation(control_point); + break; + + default: + control_point->response_code = SAL_LE_RAS_CTL_OP_RSP_CODE_NOT_SUPPORTED; + LOG_WRN("OpCode not supported."); + break; + } +} + +void rs_display_response(ras_control_point_t* control_point) +{ + switch (control_point->response_code) { + case SAL_LE_RAS_CTL_OP_RSP_CODE_SUCCESS: + LOG_INF("Response: SUCCESS."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_PROCE_NOT_CMP: + LOG_INF("Response: PROCEDURE NOT COMPLETED."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_PERSISTED: + LOG_INF("Response: ABORT UNSUCCESSFUL."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_SERVER_BUSY: + LOG_INF("Response: SERVER BUSY."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_INVALID_PARAMS: + LOG_INF("Response: INVALID PARAMETER."); + break; + case SAL_LE_RAS_CTL_OP_RSP_CODE_NOT_SUPPORTED: + LOG_INF("Response: OP CODE NOT SUPPORTED."); + break; + default: + LOG_INF("Unknown response code."); + break; + } +} + +ssize_t on_ras_ctr_pt_write_cb(struct bt_conn* conn, const struct bt_gatt_attr* attr, + const void* buf, uint16_t len, uint16_t offset, + uint8_t flags) +{ + if (!buf || len == 0) { + LOG_ERR("Invalid buf or len."); + return 0; + } + + uint8_t* buf_send = (uint8_t*)buf; + + uint8_t opcode = (uint8_t)buf_send[0]; + LOG_INF("RAS Control Point cb, opcode(%d)\n", opcode); + // LOG_INF("opcode:%d, offset:%d, flags:%d, buf_send[%d]:%s", + // opcode, offset, flags, len, bt_hex(buf_send, len)); + uint16_t count; + + switch (opcode) { + case SAL_LE_RAS_CTL_OP_CMD_GET_RANG_DATA: { + count = sys_get_le16((const uint8_t*)&buf_send[1]); + ras_ondemand_send_ranging_data(conn, count); + break; + } + case SAL_LE_RAS_CTL_OP_CMD_ACK_RANG_DATA: { + count = sys_get_le16((const uint8_t*)&buf_send[1]); + ras_on_demond_send_code_rsp(conn, count); + break; + } + case SAL_LE_RAS_CTL_OP_CMD_RETRIEVE_LOST_RANG_DATA_SEG: { + count = sys_get_le16((const uint8_t*)&buf_send[1]); + uint8_t first_seg = (uint8_t)buf_send[3]; + uint8_t last_seg = (uint8_t)buf_send[4]; + ras_on_demand_retrieve_send_lost_data(conn, count, first_seg, last_seg); + } + case SAL_LE_RAS_CTL_OP_CMD_ABORT_OPERATION: { + // TODO: + } + case SAL_LE_RAS_CTL_OP_CMD_SET_FILTER: { + uint16_t filter_params = sys_get_le16((const uint8_t*)&buf_send[1]); + ras_set_filter(filter_params); + } + default: + break; + } + return len; +} + +static ssize_t ras_feature_read(struct bt_conn* conn, const struct bt_gatt_attr* attr, + void* buf, uint16_t len, uint16_t offset) +{ + LOG_INF("RAS feature read cb, ras_feature 0x%lx.\n", ras_srv->ras_feature); + LOG_INF("offset:%d, buf[%d]:%s", offset, len, bt_hex(buf, len)); + return BT_GATT_ATTR_READ(conn, attr, buf, len, offset, (uint8_t*)&ras_srv->ras_feature, + sizeof(ras_srv->ras_feature)); +} + +static void range_rt_dt_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value) +{ + if (!ras_srv) { + LOG_ERR("Invalid ras_srv value."); + return; + } + + // The Real time mode and the on-demand mode can't be set together. + if ((value != 0) && (atomic_test_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY) || atomic_test_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE))) { + LOG_ERR("The on-demond mode has been setted, Please clear it before set to real-time mode."); + return; + } + + if (value == 0) { + atomic_clear_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_NOTIFY); + atomic_clear_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_INDICATE); + } else { + atomic_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_RTT_DATA_NOTIFY : RAS_RTT_DATA_INDICATE); + } + + LOG_INF("The range real-time data ccc value is change to (%d)\n", value); + return; +} + +static void range_on_dem_dt_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value) +{ + if (!ras_srv) { + LOG_ERR("Invalid ras_srv value."); + return; + } + + // The Real time mode and the on-demand mode can't be set together. + if ((value != 0) && (atomic_test_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_NOTIFY) || atomic_test_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_INDICATE))) { + LOG_ERR("The on-demond mode has been setted, Please clear it before set to real-time mode."); + return; + } + + if (value == 0) { + atomic_clear_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY); + atomic_clear_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE); + } else { + atomic_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_ON_DEMAND_DATA_NOTIFY : RAS_ON_DEMAND_DATA_INDICATE); + } + + LOG_INF("The range on-dem data ccc value is change to (%d)\n", value); + return; +} + +static void range_ctr_pt_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value) +{ + if (!ras_srv) { + LOG_ERR("Invalid ras_srv value."); + return; + } + + if (value == 0) { + atomic_clear_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY); + atomic_clear_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_INDICATE); + } else { + atomic_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_CONTROL_POINT_NOTIFY : RAS_CONTROL_POINT_INDICATE); + } + + LOG_INF("The range control point data ccc value is change to (%d)\n", value); + return; +} + +static void range_dt_rd_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value) +{ + if (!ras_srv) { + LOG_ERR("Invalid ras_srv value."); + return; + } + + if (value == 0) { + atomic_clear_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY); + atomic_clear_bit(&ras_srv->char_notify_state, RAS_DATA_READY_INDICATE); + } else { + atomic_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_DATA_READY_NOTIFY : RAS_DATA_READY_INDICATE); + } + + LOG_INF("The range data ready data ccc value is change to (%d)\n", value); + return; +} + +static void range_dt_ov_wr_ccc_cfg_changed(const struct bt_gatt_attr* attr, uint16_t value) +{ + if (!ras_srv) { + LOG_ERR("Invalid ras_srv value."); + return; + } + + if (value == 0) { + atomic_clear_bit(&ras_srv->char_notify_state, RAS_OVER_WRITE_NOTIFY); + atomic_clear_bit(&ras_srv->char_notify_state, RAS_OVER_WRITE_INDICATE); + } else { + atomic_set_bit(&ras_srv->char_notify_state, + (value == SAL_LE_RAS_GATT_NOTIFY) ? RAS_OVER_WRITE_NOTIFY : RAS_OVER_WRITE_INDICATE); + } + + LOG_INF("The range over write data ccc value is change to (%d)\n", value); + return; +} + +static void ras_dt_rd_indicate_cb(struct bt_conn* conn, + struct bt_gatt_indicate_params* params, uint8_t err) +{ + LOG_INF("Indication %s\n", err != 0U ? "fail" : "success"); + if (ras_srv->remaining_len) { + split_real_time_segment(conn, &ras_srv->latest_local_steps[ras_srv->ras_seg_offset], ras_srv->remaining_len); + } + + return; +} + +static void ras_dt_rd_indicate_destroy(struct bt_gatt_indicate_params* params) +{ + LOG_INF("Indication complete\n"); + ras_srv->ras_dt_rd_indicating = 0U; + + return; +} + +static void ras_write_bits(uint8_t* buf, int* bit_offset, uint32_t value, int bit_count) +{ + int byte_index; + int bit_index; + for (int i = bit_count - 1; i >= 0; i--) { + byte_index = *bit_offset / 8; + bit_index = 7 - (*bit_offset % 8); + uint8_t bit = (value >> i) & 0x01; + + if (bit) { + buf[byte_index] |= (1 << bit_index); + } else { + buf[byte_index] &= ~(1 << bit_index); + } + + (*bit_offset)++; + } + + return; +} + +static void split_real_time_segment(struct bt_conn* conn, uint8_t* buf, int len) +{ + if (ras_srv->remaining_len > ras_srv->ras_mtu - RAS_SEG_HEADER_SIZE - 1) { + int curr_seg_size = ras_srv->ras_mtu - RAS_SEG_HEADER_SIZE - 1; + LOG_INF("data send: offset:%lu, len:%d\n", ras_srv->ras_seg_offset, curr_seg_size); + + // update offset + ras_srv->ras_seg_offset += curr_seg_size; + uint8_t* send_buf = malloc(curr_seg_size + 1); + + ras_srv->remaining_len -= curr_seg_size; + send_buf[0] = (ras_srv->ras_seg_idx == 0) ? (0x01) : (ras_srv->ras_seg_idx << 2); + if (ras_srv->remaining_len == 0) { + ras_srv->ras_seg_idx = 0; + send_buf[0] |= (0x01 << 1); + ras_srv->remaining_len = 0; + ras_srv->ras_seg_offset = 0; + } + + ras_srv->ras_seg_idx++; + memcpy(&send_buf[1], buf, curr_seg_size); + ras_srv->ras_dt_rd_ind_params.attr = &ras_attrs[SAL_LE_RAS_RT_DT_CHAR_IDX]; + ras_srv->ras_dt_rd_ind_params.func = ras_dt_rd_indicate_cb; + ras_srv->ras_dt_rd_ind_params.destroy = ras_dt_rd_indicate_destroy; + ras_srv->ras_dt_rd_ind_params.data = send_buf; + ras_srv->ras_dt_rd_ind_params.len = curr_seg_size + 1; + + if ((send_buf[0] & 0x01) == 0x01) { + LOG_INF("First seg, data(%d):%s\n", curr_seg_size + 1, bt_hex(send_buf, curr_seg_size + 1)); + } else { + LOG_INF("The %d seg, data(%d):%s\n", send_buf[0] >> 2, curr_seg_size + 1, bt_hex(send_buf, curr_seg_size + 1)); + } + + if (ras_srv->rt_dt_ccc_cfg == SAL_LE_RAS_GATT_NOTIFY) { + if (BT_GATT_NOTIFY(conn, &ras_attrs[SAL_LE_RAS_RT_DT_CHAR_IDX], send_buf, curr_seg_size + 1) == 0) { + if (ras_srv->remaining_len) { + split_real_time_segment(conn, &ras_srv->latest_local_steps[ras_srv->ras_seg_offset], ras_srv->remaining_len); + } + ras_srv->ras_dt_rd_indicating = 1U; + free(send_buf); + } else { + LOG_INF("ras data ready Notify fail.\n"); + free(send_buf); + return; + } + } else if (ras_srv->rt_dt_ccc_cfg == SAL_LE_RAS_GATT_INDICATION) { + if (BT_GATT_INDICATE(conn, &ras_srv->ras_dt_rd_ind_params) == 0) { + ras_srv->ras_dt_rd_indicating = 1U; + free(send_buf); + } else { + LOG_INF("ras data ready Indicate fail.\n"); + free(send_buf); + return; + } + } + } else { + int curr_seg_size = len; + uint8_t* send_buf = malloc(curr_seg_size + 1); + send_buf[0] = (ras_srv->ras_seg_idx == 0) ? (0x01) : (ras_srv->ras_seg_idx << 2); + send_buf[0] |= (0x01 << 1); + memcpy(&send_buf[1], buf, curr_seg_size); + ras_srv->ras_dt_rd_ind_params.attr = &ras_attrs[SAL_LE_RAS_RT_DT_CHAR_IDX]; + ras_srv->ras_dt_rd_ind_params.func = ras_dt_rd_indicate_cb; + ras_srv->ras_dt_rd_ind_params.destroy = ras_dt_rd_indicate_destroy; + ras_srv->ras_dt_rd_ind_params.data = send_buf; + ras_srv->ras_dt_rd_ind_params.len = curr_seg_size + 1; + ras_srv->ras_dt_rd_indicating = 0U; + + ras_srv->ras_seg_idx = 0; + ras_srv->ras_dt_rd_indicating = 0U; + ras_srv->remaining_len = 0; + ras_srv->ras_seg_offset = 0; + LOG_INF("The last(%d) seg, data(%d):%s\n", send_buf[0] >> 2, curr_seg_size + 1, bt_hex(send_buf, curr_seg_size + 1)); + LOG_INF("ras_dt_rd_indicating:%d\n", ras_srv->ras_dt_rd_indicating); + if (ras_srv->rt_dt_ccc_cfg == SAL_LE_RAS_GATT_NOTIFY) { + if (BT_GATT_NOTIFY(conn, &ras_attrs[SAL_LE_RAS_RT_DT_CHAR_IDX], send_buf, curr_seg_size + 1) == 0) { + free(send_buf); + } else { + LOG_INF("ras data ready Indicate fail.\n"); + free(send_buf); + return; + } + } else if (ras_srv->rt_dt_ccc_cfg == SAL_LE_RAS_GATT_INDICATION) { + if (BT_GATT_INDICATE(conn, &ras_srv->ras_dt_rd_ind_params) == 0) { + ras_srv->ras_dt_rd_indicating = 1U; + free(send_buf); + } else { + LOG_INF("ras data ready Indicate fail.\n"); + free(send_buf); + return; + } + } + } + + return; +} + +static void ras_on_demand_data_send_timeout(struct k_work* work) +{ + ras_rang_on_demand_t* on_demand_subevent = RAS_ON_DEMAND_WORK_PICK(work); + LOG_INF("On-demand data send timeout, remove the data in the list."); + // Free the on-demand segment list when response the ack to the Client. + ras_segment_t *seg_prev, *seg_next; + SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&on_demand_subevent->seg_list, + seg_prev, seg_next, seg_node) + { + sys_slist_remove(&on_demand_subevent->seg_list, NULL, &seg_prev->seg_node); + LOG_INF("seg_prev:%p", seg_prev); + free(seg_prev); + } + + memset(on_demand_subevent, 0, sizeof(ras_rang_on_demand_t)); + + return; +} + +static void split_on_demand_segment(struct bt_conn* conn, uint8_t* buf, int len, + ras_rang_on_demand_t* subevent) +{ + int curr_seg_size = 0; + uint16_t seg_index = 0; + ras_srv->ras_seg_offset = 0; + + do { + if (ras_srv->remaining_len > ras_srv->ras_mtu - RAS_SEG_HEADER_SIZE - 1) { + curr_seg_size = ras_srv->ras_mtu - RAS_SEG_HEADER_SIZE - 1; + LOG_INF("data send: offset:%lu, len:%d\n", ras_srv->ras_seg_offset, curr_seg_size); + subevent->seg = (ras_segment_t*)malloc(sizeof(ras_segment_t) + curr_seg_size + 1); + + if (!subevent->seg) { + LOG_ERR("Malloc fail."); + return; + } + + memcpy(&subevent->seg->data[1], &buf[ras_srv->ras_seg_offset], curr_seg_size); + // update offset + ras_srv->ras_seg_offset += curr_seg_size; + ras_srv->remaining_len -= curr_seg_size; + + subevent->seg->data[0] = (seg_index == 0) ? (0x01) : (seg_index << 2); + if (ras_srv->remaining_len == 0) { + subevent->seg->data[0] |= (0x01 << 1); + ras_srv->remaining_len = 0; + ras_srv->ras_seg_offset = 0; + } + + } else { + curr_seg_size = ras_srv->remaining_len; + subevent->seg = (ras_segment_t*)malloc(sizeof(ras_segment_t) + curr_seg_size + 1); + + if (!subevent->seg) { + LOG_ERR("Malloc fail."); + return; + } + + memcpy(&subevent->seg->data[1], &buf[ras_srv->ras_seg_offset], curr_seg_size); + + subevent->seg->data[0] = (seg_index == 0) ? (0x01) : (seg_index << 2); + subevent->seg->data[0] |= (0x01 << 1); + ras_srv->remaining_len = 0; + LOG_INF("ras_dt_rd_indicating:%d\n", ras_srv->ras_dt_rd_indicating); + } + + if ((subevent->seg->data[0] & 0x01) == 0x01) { + LOG_INF("First seg, data(%d):%s\n", curr_seg_size + 1, bt_hex(subevent->seg->data, curr_seg_size + 1)); + } else { + LOG_INF("The %d seg, data(%d):%s\n", subevent->seg->data[0] >> 2, curr_seg_size + 1, bt_hex(subevent->seg->data, curr_seg_size + 1)); + } + + subevent->seg->seg_idx = seg_index++; + subevent->seg->len = curr_seg_size + 1; + sys_slist_append(&subevent->seg_list, &subevent->seg->seg_node); + LOG_INF("subevent seg:%p, seg_node:%p.", subevent->seg, &subevent->seg->seg_node); + } while (ras_srv->remaining_len > 0); + + k_work_init_delayable(&subevent->on_demand_work, ras_on_demand_data_send_timeout); + /* Start a on demand data send timer. */ + k_work_schedule(&subevent->on_demand_work, RAS_RSP_TIMEOUT); + + return; +} + +void on_cccd_changed(const struct bt_gatt_attr* attr, uint16_t value) +{ + return; +} + +static void write_func(struct bt_conn* conn, uint8_t err, struct bt_gatt_write_params* params) +{ + if (err) { + LOG_INF("Write failed (err %d)\n", err); + + return; + } +} + +int write_cs_reflector_step_data(void) +{ + int err; + struct bt_gatt_write_params write_params; + write_params.func = write_func; + write_params.handle = ras_srv->step_data_attr_handle; + write_params.length = SAL_LE_RAS_STEP_DATA_BUF_LEN; + write_params.data = &ras_srv->latest_local_steps[0]; + write_params.offset = 0; + + err = bt_gatt_write(ras_srv->connection, &write_params); + if (err) { + LOG_INF("Write failed (err %d)\n", err); + return 0; + } + + return 0; +} + +/** + * Input: data points to the original buffer, data_len is the total length of the input + * Output: buf stores the transformed result, function returns the length of the output buffer + */ +static size_t transform_step_data_to_ras_format_filtered( + uint8_t* data, size_t data_len, + uint8_t* buf, atomic_t* ras_filter, uint8_t role, + uint8_t num_antenna_paths) +{ + size_t in_offset = 0; + size_t out_offset = 0; + + while (in_offset + 3 <= data_len) { + uint8_t step_mode = data[in_offset]; + uint8_t step_data_length = data[in_offset + 2]; + + if (in_offset + 3 + step_data_length > data_len) { + LOG_WRN("Incomplete data, exit early."); + break; + } + + uint8_t* step_data = &data[in_offset + 3]; + uint32_t filter_mask = atomic_get(&ras_filter[step_mode]); + + // Write Step_Mode to the output + buf[out_offset++] = step_mode; + + const uint8_t* p = step_data; + size_t remaining = step_data_length; + + switch (step_mode) { + /* + * MODE 0 — Basic CS Packet Mode + * -------------------------------------------------------- + * Initiator: + * 1. Packet_Quality (1) + * 2. Packet_RSSI (1) + * 3. Packet_Antenna (1) + * 4. Measured_Freq_Offset (2) + * --> Total: 5 bytes + * + * Reflector: + * 1. Packet_Quality (1) + * 2. Packet_RSSI (1) + * 3. Packet_Antenna (1) + * --> Total: 3 bytes + * + ************************************************************/ + case SAL_LE_RAS_SUBEVENT_STEP_MODE_0: + if (role == SAL_LE_RAS_ROLE_INITIATOR) { + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_FREQ_OFFSET); + } else { // Reflector + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + } + break; + /* MODE 1 — Time-of-Flight (ToF) Mode + * ------------------------------------------------------------ + * Initiator: + * 1. Packet_Quality (1) + * 2. Packet_NADM (1) + * 3. Packet_RSSI (1) + * 4. ToA_ToD_Initiator (2) + * 5. Packet_Antenna (1) + * 6. Packet_PCT1 (4) + * 7. Packet_PCT2 (4) + * --> Total: 14 bytes + * + * Reflector: + * 1. Packet_Quality (1) + * 2. Packet_NADM (1) + * 3. Packet_RSSI (1) + * 4. ToD_ToA_Reflector (2) + * 5. Packet_Antenna (1) + * 6. Packet_PCT1 (4) + * 7. Packet_PCT2 (4) + * --> Total: 14 bytes + * + **************************************************************/ + case SAL_LE_RAS_SUBEVENT_STEP_MODE_1: + if (role == SAL_LE_RAS_ROLE_REFLECTOR) { + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_NADM); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_TOA_TOD); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT1); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT2); + } else { // Reflector + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_NADM); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_TOD_TOA); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT1); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT2); + } + break; + /***************************************************************** + * MODE 2 — Tone-based Phase Measurement Mode + * ------------------------------------------------------------ + * (Same layout for Initiator and Reflector) + * 1. Antenna_Permutation_Index (1) + * 2. Tone_PCT[k] ((Num_Antenna_Paths + 1) × 3) + * 3. Tone_Quality_Indicator[k] ((Num_Antenna_Paths + 1) × 1) + * --> Example total (Num_Antenna_Paths=3): 17 bytes + * + *******************************************************************/ + case SAL_LE_RAS_SUBEVENT_STEP_MODE_2: + // Initiator and Reflector are the same. + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_ANT_PERM_IDX); + + // Tone_PCT[k] + // Actual size = (Num_Antenna_Paths + 1) × 3 octets + // COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 3, RAS_FILTER_BIT_TONE_PCT); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 3, RAS_FILTER_BIT_TONE_PCT); + // Tone_Quality_Indicator[k] + // COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 1, RAS_FILTER_BIT_TONE_QUALITY); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 1, RAS_FILTER_BIT_TONE_QUALITY); + break; + /* MODE 3 — Combined (ToF + Tone) Mode + * ------------------------------------------------------------ + * Initiator: + * 1. Packet_Quality (1) + * 2. Packet_NADM (1) + * 3. Packet_RSSI (1) + * 4. ToA_ToD_Initiator (2) + * 5. Packet_Antenna (1) + * 6. Packet_PCT1 (4) + * 7. Packet_PCT2 (4) + * 8. Antenna_Permutation_Index (1) + * 9. Tone_PCT[k] ((Num_Antenna_Paths + 1) × 3) + * 10. Tone_Quality_Indicator[k] ((Num_Antenna_Paths + 1) × 1) + * --> Example total (Num_Antenna_Paths=3): 31 bytes + * + * Reflector: + * 1. Packet_Quality (1) + * 2. Packet_NADM (1) + * 3. Packet_RSSI (1) + * 4. ToD_ToA_Reflector (2) + * 5. Packet_Antenna (1) + * 6. Packet_PCT1 (4) + * 7. Packet_PCT2 (4) + * 8. Antenna_Permutation_Index (1) + * 9. Tone_PCT[k] ((Num_Antenna_Paths + 1) × 3) + * 10. Tone_Quality_Indicator[k] ((Num_Antenna_Paths + 1) × 1) + * --> Example total (Num_Antenna_Paths=3): 31 bytes + * + ********************************************************************/ + case SAL_LE_RAS_SUBEVENT_STEP_MODE_3: + if (role == SAL_LE_RAS_ROLE_INITIATOR) { + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_NADM); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_TOA_TOD); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT1); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT2); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_ANT_PERM_IDX); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 3, RAS_FILTER_BIT_TONE_PCT); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 1, RAS_FILTER_BIT_TONE_QUALITY); + } else { // Reflector + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_QUALITY); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_NADM); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_RSSI); + COPY_FIELD_IF_ENABLED(2, RAS_FILTER_BIT_TOD_TOA); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_PKT_ANTENNA); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT1); + COPY_FIELD_IF_ENABLED(4, RAS_FILTER_BIT_PKT_PCT2); + COPY_FIELD_IF_ENABLED(1, RAS_FILTER_BIT_ANT_PERM_IDX); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 3, RAS_FILTER_BIT_TONE_PCT); + COPY_FIELD_IF_ENABLED((num_antenna_paths + 1) * 1, RAS_FILTER_BIT_TONE_QUALITY); + } + break; + default: + LOG_WRN("Unknown mode %d, copying raw data.", step_mode); + memcpy(&buf[out_offset], step_data, step_data_length); + out_offset += step_data_length; + break; + } + + in_offset += 3 + step_data_length; + } + + return out_offset; +} + +static ras_rang_on_demand_t* ras_rang_on_demand_subevent_pool_find(struct bt_conn* conn) +{ + if (!ras_srv) { + LOG_ERR("Invalid ras_srv enviranment, init it first."); + return NULL; + } + + for (int i = 0; i < SAL_LE_RAS_STORE_PROCEDURE_NUM_MAX; i++) { + if (ras_srv->subevent[i].proc_used == false) { + memset(&ras_srv->subevent[i], 0, sizeof(ras_rang_on_demand_t)); + ras_srv->subevent[i].proc_used = true; + return &ras_srv->subevent[i]; + } + } + + LOG_INF("No subvent pool can be used."); + return NULL; +} + +static ras_rang_on_demand_t* ras_rang_on_demand_find_subevent(struct bt_conn* conn, uint16_t count) +{ + if (!ras_srv) { + LOG_ERR("Invalid ras_srv enviranment, init it first."); + return NULL; + } + + for (int i = 0; i < SAL_LE_RAS_STORE_PROCEDURE_NUM_MAX; i++) { + if (ras_srv->subevent[i].count == count && ras_srv->subevent[i].proc_used == true) { + return &ras_srv->subevent[i]; + } + } + + LOG_INF("No subvent find with count(%d).", count); + return NULL; +} + +static uint8_t* ras_subevent_data_conversion(struct bt_conn* conn, struct bt_conn_le_cs_subevent_result* result) +{ + memset(ras_srv->latest_local_steps, 0, sizeof(ras_srv->latest_local_steps)); + + if (result->step_data_buf) { + if (result->step_data_buf->len <= SAL_LE_RAS_STEP_DATA_BUF_LEN) { + memcpy(ras_srv->latest_local_steps, result->step_data_buf->data, + result->step_data_buf->len); + LOG_INF("step data[%d]:%s\n", result->step_data_buf->len, bt_hex(result->step_data_buf->data, result->step_data_buf->len)); + } else { + LOG_INF("Not enough memory to store step data. (%d > %d)\n", + result->step_data_buf->len, SAL_LE_RAS_STEP_DATA_BUF_LEN); + } + } + + uint8_t* stream_buf = ras_srv->latest_local_steps; + int bit_offset = 0; + uint16_t count_id = ((result->header.procedure_counter & 0x0FFF) << 12) | (result->header.config_id & 0x0F); + /** + * CS configuration identifier. + * Range: 0 to 3 + * Rangging Counter is lower 12-bits of CS Procedure_Counter Provided by the Core Controller + */ + ras_write_bits(stream_buf, &bit_offset, count_id, 16); + /** + * Transmit power level used for the CS Procedure. + * Range: -127 to 20 + * Units: decibels referenced to 1 milliwatt(dBm) + */ + ras_write_bits(stream_buf, &bit_offset, result->header.reference_power_level, 8); + /** + * Antenna paths that are reported: + * Bit0: 1 if Antenna Path_1 included; 0 if not. + * Bit1: 1 if Antenna Path_2 included; 0 if not. + * Bit2: 1 if Antenna Path_3 included; 0 if not. + * Bit3: 1 if Antenna Path_4 included; 0 if not. + * Bits 4-7: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.num_antenna_paths, 8); + /** + * Starting ACL connection event count for the results reported in the event. + */ + ras_write_bits(stream_buf, &bit_offset, result->header.start_acl_conn_event, 16); + /** + * Frequency compensation value in units of 0.01 parts permillion(ppm)(15-bit signed integer). + */ + ras_write_bits(stream_buf, &bit_offset, result->header.frequency_compensation, 16); + /** + * 0x0: All results complete for the CS Procedure + * 0x1: Partial results with more to follow for the CS Procedure + * 0xF: All subsequent CS Procedures aborted + * All other values: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.procedure_done_status, 4); + /** + * 0x0: All results complete for the CS Subevent + * 0xF: Current CS Subevent aborted + * All other values: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.subevent_done_status, 4); + /** + * Indicates the abort reason when the Procedure_Done Status received from the Core + * Controller is set to 0xF; otherwise, the value is set to zero. + * 0x0: Report with no abort + * 0x1: Abort because of local Host or remote request + * 0x2: Abort because filtered channel map has less than 15 channels + * 0x3: Abort because the channel map update instant has passed + * 0xF: Abort because of unspecified reasons + * All other values: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.procedure_abort_reason, 4); + /** + * Indicates the abort reason when the Subevent_Done_Status received from the Core Controller + * is set to 0xF; otherwise, the default value is set to zero. + * 0x0: Report with no abort + * 0x1: Abort because of local Host or remote request + * 0x2: Abort because no CS_SYNC(mode 0) was received + * 0x3: Abort because of scheduling conflicts or limited resources + * 0xF: Abort because of unspecified reasons + * All other values: RFU + */ + ras_write_bits(stream_buf, &bit_offset, result->header.subevent_abort_reason, 4); + /** + * Reference power level. + * Range: -127 to 20 + * Units: dBm + */ + ras_write_bits(stream_buf, &bit_offset, result->header.reference_power_level, 8); + /** + * Number of steps in the CS Subevent for which results are reported. If the Subevent is + * aborted, then the Number of Steps Reported can be set to zero. + */ + ras_write_bits(stream_buf, &bit_offset, result->header.num_steps_reported, 8); + + ras_srv->ras_seg_offset = 0; + ras_srv->remaining_len = transform_step_data_to_ras_format_filtered(result->step_data_buf->data, + result->step_data_buf->len, stream_buf + SAL_LE_RAS_SUB_PROCUDURE_HEAD, + ras_srv->ras_filter, ras_srv->ras_role, result->header.num_antenna_paths & 0x0F); + ras_srv->remaining_len += SAL_LE_RAS_SUB_PROCUDURE_HEAD; + LOG_INF("stream head:%s.", bt_hex(stream_buf, SAL_LE_RAS_SUB_PROCUDURE_HEAD)); + LOG_INF("stream_buf(%ld):%s.\n", ras_srv->remaining_len, bt_hex(stream_buf + 12, ras_srv->remaining_len - SAL_LE_RAS_SUB_PROCUDURE_HEAD)); + LOG_INF("data ready indiacte count(%d)., uuid:0x%x, handle:%d\n", + result->header.procedure_counter, + BT_UUID_16(ras_attrs[SAL_LE_RAS_DT_RD_CHAR_VAL_IDX].uuid)->val, + ras_attrs[SAL_LE_RAS_DT_RD_CHAR_VAL_IDX].handle); + LOG_INF("procedure_counter:0x%x, config_id:0x%x, reference_power_level:0x%x", + result->header.procedure_counter, result->header.config_id, + result->header.reference_power_level); + LOG_INF("num_antenna_paths:0x%x, start_acl_conn_event:0x%x, frequency_compensation:0x%x", + result->header.num_antenna_paths, result->header.start_acl_conn_event, + result->header.frequency_compensation); + LOG_INF("procedure_done_status:0x%x, subevent_done_status:0x%x, :0x%x", + result->header.procedure_done_status, result->header.subevent_done_status, + result->header.procedure_abort_reason); + LOG_INF("subevent_abort_reason:0x%x, reference_power_level:0x%x, num_steps_reported:0x%x", + result->header.subevent_abort_reason, result->header.reference_power_level, + result->header.num_steps_reported); + LOG_INF("mode:0x%x, channel:0x%x, len:0x%x", result->step_data_buf->data[0], + result->step_data_buf->data[1], + result->step_data_buf->data[2]); + return stream_buf; +} + +static void ras_process_real_time_ranging_data(struct bt_conn* conn, struct bt_conn_le_cs_subevent_result* result) +{ + uint8_t* stream_buf = ras_subevent_data_conversion(conn, result); + split_real_time_segment(conn, stream_buf, ras_srv->remaining_len); +} + +static void ras_process_on_demand_ranging_data(struct bt_conn* conn, struct bt_conn_le_cs_subevent_result* result) +{ + ras_rang_on_demand_t* subevent = ras_rang_on_demand_subevent_pool_find(conn); + + if (!subevent) { + LOG_ERR("No subevent pool found."); + return; + } + + LOG_DBG("procedure counter:%d.", result->header.procedure_counter); + + subevent->count = result->header.procedure_counter; + uint8_t* stream_buf = ras_subevent_data_conversion(conn, result); + split_on_demand_segment(conn, stream_buf, ras_srv->remaining_len, subevent); + if (atomic_test_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_IDLE)) { + ras_data_ready_send(conn, subevent->count); + // Set the on-demand state to ready. + atomic_set_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_DATA_READY_INDICATE); + } + + return; +} + +static void subevent_result_cb(struct bt_conn* conn, struct bt_conn_le_cs_subevent_result* result) +{ + if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE && ras_check_ranging_mode(conn) == SAL_LE_RAS_RANGING_MODE_REAL_TIME) { + LOG_DBG("Recv the real-time ranging data."); + ras_process_real_time_ranging_data(conn, result); + return; + } + + if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE && ras_check_ranging_mode(conn) == SAL_LE_RAS_RANGING_MODE_ON_DEMAND) { + LOG_DBG("Recv the on-demand ranging data."); + ras_process_on_demand_ranging_data(conn, result); + return; + } + + LOG_ERR("No mode has been set, discard the subevent result."); + return; +} + +static int ras_data_ready_send(struct bt_conn* conn, uint16_t count) +{ + // if (!conn) { + // LOG_ERR("Invalid conn handle."); + // return -1; + // } + + uint8_t buf[2]; + sys_put_le16(count, buf); + if (atomic_test_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY)) { + return BT_GATT_NOTIFY(conn, &ras_attrs[SAL_LE_RAS_DT_RD_CHAR_IDX], &count, sizeof(count)); + } else if (atomic_test_bit(&ras_srv->char_notify_state, RAS_DATA_READY_INDICATE)) { + struct bt_gatt_indicate_params data_ready_indicate; + data_ready_indicate.attr = &ras_attrs[SAL_LE_RAS_DT_RD_CHAR_IDX]; + data_ready_indicate.func = ras_dt_rd_indicate_cb; + data_ready_indicate.destroy = ras_dt_rd_indicate_destroy; + data_ready_indicate.data = buf; + data_ready_indicate.len = sizeof(buf); + return BT_GATT_INDICATE(conn, &data_ready_indicate); + } + + LOG_ERR("Invalid data ready char indication state."); + return -1; +} + +static void ras_mtu_updated_cb(struct bt_conn* conn, uint16_t tx, uint16_t rx) +{ + if (ras_srv) { + ras_srv->ras_mtu = MIN(tx, rx); + } + + LOG_INF("Updated MTU: TX: %d RX: %d bytes, ras_mtu: %lu\n", tx, rx, ras_srv->ras_mtu); + return; +} + +static void connected_cb(struct bt_conn* conn, uint8_t err) +{ + char addr[BT_ADDR_LE_STR_LEN]; + bt_address_t bt_addr = { 0 }; + + (void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + LOG_INF("Connected to %s (err 0x%02X)\n", addr, err); + + memcpy(bt_addr.addr, addr, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(CONNECTED_EVT, &bt_addr); + bt_sal_cs_event_callback(msg); + + __ASSERT(ras_srv->connection == conn, "Unexpected connected callback"); + + if (err) { + bt_conn_unref(conn); + ras_srv->connection = NULL; + } + + LOG_INF("ras_srv:%p.", ras_srv); + ras_srv->connection = bt_conn_ref(conn); + + const struct bt_le_cs_set_default_settings_param default_settings = { + .enable_initiator_role = false, + .enable_reflector_role = true, + .cs_sync_antenna_selection = BT_LE_SRV_CS_ANTENNA_SELECTION_OPT_REPETITIVE, + .max_tx_power = BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER, + }; + + err = bt_le_cs_set_default_settings(ras_srv->connection, &default_settings); + if (err) { + LOG_INF("Failed to configure default CS settings (err %d)\n", err); + } +} + +static void disconnected_cb(struct bt_conn* conn, uint8_t reason) +{ + char addr[BT_ADDR_LE_STR_LEN]; + bt_address_t bt_addr = { 0 }; + + bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + memcpy(bt_addr.addr, addr, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(DISCONNECTED_EVT, &bt_addr); + bt_sal_cs_event_callback(msg); + + LOG_INF("Disconnected (reason 0x%02X)\n", reason); + + bt_conn_unref(conn); + ras_srv->connection = NULL; + + // int err = bt_le_adv_start(BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONN, BT_GAP_ADV_FAST_INT_MIN_1, + // BT_GAP_ADV_FAST_INT_MAX_1, NULL), + // ad, ARRAY_SIZE(ad), NULL, 0); + // if (err) { + // LOG_INF("Advertising failed to start (err %d)\n", err); + // return; + // } + + LOG_INF("Advertising start again.\n"); +} + +static void remote_capabilities_cb(struct bt_conn* conn, struct bt_conn_le_cs_capabilities* params) +{ + char addr[BT_ADDR_LE_STR_LEN]; + bt_address_t bt_addr = { 0 }; + + bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + memcpy(bt_addr.addr, addr, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(CAPBLITIES_RECEIVED_EVT, &bt_addr); + bt_srv_conn_le_cs_capabilities_t capabilities = {}; + memcpy(&capabilities, params, sizeof(bt_srv_conn_le_cs_capabilities_t)); + msg->cs_data.data = &capabilities; + bt_sal_cs_event_callback(msg); + + ARG_UNUSED(params); + LOG_INF("CS capability exchange completed.\n"); + LOG_INF("num_config_supported:%d, max_consecutive_procedures_supported:%d", + params->num_config_supported, params->max_consecutive_procedures_supported); + LOG_INF("num_antennas_supported:%d, max_antenna_paths_supported:%d.", + params->num_antennas_supported, params->max_antenna_paths_supported); + LOG_INF("initiator_supported:%d, reflector_supported:%d", + params->initiator_supported, params->reflector_supported); + LOG_INF("mode_3_supported:%d, rtt_aa_only_precision:%d", + params->mode_3_supported, params->rtt_aa_only_precision); + LOG_INF("rtt_sounding_precision:%d, rtt_random_payload_precision:%d", + params->rtt_sounding_precision, params->rtt_random_payload_precision); + LOG_INF("rtt_aa_only_n:%d, rtt_sounding_n:%d, rtt_random_payload_n:%d", + params->rtt_aa_only_n, params->rtt_sounding_n, params->rtt_random_payload_n); + LOG_INF("phase_based_nadm_sounding_supported:%d, phase_based_nadm_random_supported:%d", + params->phase_based_nadm_sounding_supported, params->phase_based_nadm_random_supported); + LOG_INF("cs_sync_2m_phy_supported:%d, cs_sync_2m_2bt_phy_supported:%d", + params->cs_sync_2m_phy_supported, params->cs_sync_2m_2bt_phy_supported); + LOG_INF("cs_without_fae_supported:%d, chsel_alg_3c_supported:%d", + params->cs_without_fae_supported, params->chsel_alg_3c_supported); + LOG_INF("pbr_from_rtt_sounding_seq_supported:%d, t_ip1_times_supported:%d", + params->pbr_from_rtt_sounding_seq_supported, params->t_ip1_times_supported); + LOG_INF("t_ip2_times_supported:%d, t_fcs_times_supported:%d", + params->t_ip2_times_supported, params->t_fcs_times_supported); + LOG_INF("t_pm_times_supported:%d, t_sw_time:%d, tx_snr_capability:%d", + params->t_pm_times_supported, params->t_sw_time, + params->tx_snr_capability); +} + +static void config_created_cb(struct bt_conn* conn, struct bt_conn_le_cs_config* config) +{ + char addr[BT_ADDR_LE_STR_LEN]; + bt_address_t bt_addr = { 0 }; + + bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + memcpy(bt_addr.addr, addr, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(CONFIG_DONE_EVT, &bt_addr); + bt_srv_conn_le_cs_config_t cs_config = {}; + memcpy(&cs_config, config, sizeof(bt_srv_conn_le_cs_config_t)); + msg->cs_data.data = (void*)&cs_config; + bt_sal_cs_event_callback(msg); + + LOG_INF("CS config creation complete. ID: %d\n", config->id); + LOG_INF("main_mode_type:%d, sub_mode_type:%d", + config->main_mode_type, config->sub_mode_type); + LOG_INF("min_main_mode_steps:%d, max_main_mode_steps:%d", + config->min_main_mode_steps, config->max_main_mode_steps); + LOG_INF("main_mode_repetition:%d, mode_0_steps:%d", + config->main_mode_repetition, config->mode_0_steps); + LOG_INF("role:%d, rtt_type:%d, cs_sync_phy:%d", + config->role, config->rtt_type, config->cs_sync_phy); + LOG_INF("channel_map_repetition:%d, channel_selection_type:%d", + config->channel_map_repetition, config->channel_selection_type); + LOG_INF("ch3c_shape:%d, ch3c_jump:%d", config->ch3c_shape, config->ch3c_jump); + LOG_INF("t_ip1_time_us:%d, t_ip2_time_us:%d", + config->t_ip1_time_us, config->t_ip2_time_us); + LOG_INF("t_fcs_time_us:%d, t_pm_time_us:%d", config->t_fcs_time_us, config->t_pm_time_us); + LOG_INF("channel_map:0x%x%x%x%x%x%x%x%x%x%x.", + config->channel_map[0], config->channel_map[1], config->channel_map[2], + config->channel_map[3], config->channel_map[4], config->channel_map[5], + config->channel_map[6], config->channel_map[7], config->channel_map[8], + config->channel_map[9]); +} + +static void security_enabled_cb(struct bt_conn* conn) +{ + char addr[BT_ADDR_LE_STR_LEN]; + bt_address_t bt_addr = { 0 }; + + bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + memcpy(bt_addr.addr, addr, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(SECURITY_DONE_EVT, &bt_addr); + bt_sal_cs_event_callback(msg); + LOG_INF("CS security enabled.\n"); +} + +static void procedure_enabled_cb(struct bt_conn* conn, + struct bt_conn_le_cs_procedure_enable_complete* params) +{ + char addr[BT_ADDR_LE_STR_LEN]; + bt_address_t bt_addr = { 0 }; + + bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + memcpy(bt_addr.addr, addr, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(PROCEDURE_DONE_EVT, &bt_addr); + bt_srv_conn_le_cs_procedure_enable_complete_t procedure = {}; + memcpy(&procedure, params, sizeof(bt_srv_conn_le_cs_procedure_enable_complete_t)); + msg->cs_data.data = (void*)&procedure; + bt_sal_cs_event_callback(msg); + + if (params->state == 1) { + LOG_INF("CS procedures enabled."); + } else { + LOG_INF("CS procedures disabled."); + } + + LOG_INF("config_id:%d, tone_antenna:%d, tx_power:%d, subevents_per_event:%d\n", + params->config_id, params->tone_antenna_config_selection, + params->selected_tx_power, params->subevents_per_event); + LOG_INF("subevent_interval:%d, event_interval:%d, procedure_interval:%d, procedure_count:%d, max_procedure_len:%d\n", + params->subevent_interval, params->event_interval, params->procedure_interval, + params->procedure_count, params->max_procedure_len); + + return; +} + +static struct bt_conn_cb conn_cbs = { + .connected = connected_cb, + .disconnected = disconnected_cb, + .le_cs_remote_capabilities_available = remote_capabilities_cb, + .le_cs_config_created = config_created_cb, + .le_cs_security_enabled = security_enabled_cb, + .le_cs_procedure_enabled = procedure_enabled_cb, + .le_cs_subevent_data_available = subevent_result_cb, +}; + +static struct bt_gatt_cb ras_gatt_callbacks = { + .att_mtu_updated = ras_mtu_updated +}; + +int le_cs_enable(void) +{ + int err; + + LOG_INF("Starting Channel Sounding Demo\n"); + + ras_srv = (sal_le_ras_srv_env_t*)malloc(sizeof(sal_le_ras_srv_env_t)); + + memset(ras_srv, 0, sizeof(sal_le_ras_srv_env_t)); + + ras_srv->ras_feature = 0x07000007; + + for (int i = 0; i < SAL_LE_RAS_FILTER_MODE_MAX; i++) { + ras_srv->ras_filter[i] = 0xFFFFFFFF; + } + + err = bt_gatt_service_register(&ras_svc); + if (err != 0) { + LOG_INF("Failed to register Ranging Service in gatt DB"); + return err; + } + + bt_conn_cb_register(&conn_cbs); + + bt_gatt_cb_register(&ras_gatt_callbacks); + + return 0; +} + +// struct bt_conn conn_back = {0}; + +struct bt_gatt_attr* ras_get_gatt_attr(void) +{ + return ras_attrs; +} + +int ras_subevent_recv_test(ras_rang_mode_t mode, ras_testcase_t test_case, + struct bt_conn* conn, struct bt_conn_le_cs_subevent_result* result) +{ + if (mode == SAL_LE_RAS_RANGING_MODE_REAL_TIME) { + atomic_set_bit(&ras_srv->char_notify_state, RAS_RTT_DATA_NOTIFY); + LOG_INF("ras server test: set ranging to real-time mode."); + } + + switch (test_case) { + case RAS_TESTCASE_REAL_TIME_NOTIFY_VALID_RANG_DATA_001: + ras_srv->ras_mtu = 253; + ras_srv->rt_dt_ccc_cfg = SAL_LE_RAS_GATT_NOTIFY; + break; + case RAS_TESTCASE_REAL_TIME_INDICATE_VALID_RANG_DATA_002: + ras_srv->ras_mtu = 253; + ras_srv->rt_dt_ccc_cfg = SAL_LE_RAS_GATT_INDICATION; + break; + case RAS_TESTCASE_ON_DEMAND_NOTIFY_VALID_RANG_DATA_003: + ras_srv->ras_mtu = 253; + atomic_set_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_NOTIFY); + atomic_set_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY); + atomic_set_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_IDLE); + atomic_set_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY); + break; + case RAS_TESTCASE_ON_DEMAND_INDICATE_VALID_RANG_DATA_004: + ras_srv->ras_mtu = 253; + atomic_set_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE); + atomic_set_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY); + atomic_set_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_IDLE); + atomic_set_bit(&ras_srv->char_notify_state, RAS_CONTROL_POINT_NOTIFY); + break; + case RAS_TESTCASE_ON_DEMAND_WRITE_RANG_DATA_TIMWOUT_010: + ras_srv->ras_mtu = 253; + atomic_set_bit(&ras_srv->char_notify_state, RAS_ON_DEMAND_DATA_INDICATE); + atomic_set_bit(&ras_srv->char_notify_state, RAS_DATA_READY_NOTIFY); + atomic_set_bit(&ras_srv->on_demand_state, SAL_LE_RAS_ON_DEMAND_STATE_IDLE); + break; + default: + LOG_ERR("Invalid test case number(%d).", test_case); + return -1; + } + + subevent_result_cb(conn, result); + return 0; +} + +int ras_ctrl_point_send_test(struct bt_conn* conn, struct bt_gatt_attr* attr, + uint8_t* data, uint16_t len, uint16_t offset, + uint8_t flags) +{ + on_ras_ctr_pt_write_cb(NULL, attr, (void*)data, len, offset, flags); + return 0; +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/service/stacks/zephyr/sal_adapter_interface.c b/service/stacks/zephyr/sal_adapter_interface.c index a617f0105..160371ee4 100644 --- a/service/stacks/zephyr/sal_adapter_interface.c +++ b/service/stacks/zephyr/sal_adapter_interface.c @@ -1486,6 +1486,7 @@ bt_status_t bt_sal_create_bond(bt_controller_id_t id, bt_address_t* addr, bt_tra #ifdef CONFIG_BLUETOOTH_BREDR_SUPPORT static void STACK_CALL(set_security_level)(void* args) { + sal_adapter_req_t* req = args; g_security_level = req->adpt.security_level; diff --git a/service/stacks/zephyr/sal_adapter_le_interface.c b/service/stacks/zephyr/sal_adapter_le_interface.c index 321d293d3..8f0ddc3ce 100644 --- a/service/stacks/zephyr/sal_adapter_le_interface.c +++ b/service/stacks/zephyr/sal_adapter_le_interface.c @@ -111,6 +111,19 @@ static enum bt_security_err zblue_on_pairing_accept(struct bt_conn* conn, const static void zblue_register_callback(void); static void zblue_unregister_callback(void); +#ifdef CONFIG_BLUETOOTH_LE_CS +static void zblue_on_cs_subevent(struct bt_conn* conn, struct bt_conn_le_cs_subevent_result* result); +void zblue_on_cs_capabilities_available(struct bt_conn *conn, + struct bt_conn_le_cs_capabilities *params); +void zblue_on_cs_remote_fae_table_available(struct bt_conn *conn, + struct bt_conn_le_cs_fae_table *params); +void zblue_on_cs_config_created(struct bt_conn *conn, struct bt_conn_le_cs_config *config); +void zblue_on_cs_config_removed(struct bt_conn *conn, uint8_t config_id); +void zblue_on_cs_security_enabled(struct bt_conn *conn); +void zblue_on_cs_procedure_enabled(struct bt_conn *conn, + struct bt_conn_le_cs_procedure_enable_complete *params); +#endif /* CONFIG_BLUETOOTH_LE_CS */ + static le_conn_info_t* le_conn_add(const bt_address_t* addr); static le_conn_info_t* le_conn_find(const bt_address_t* addr); @@ -124,6 +137,15 @@ static struct bt_conn_cb g_conn_cbs = { #if defined(CONFIG_BT_USER_PHY_UPDATE) .le_phy_updated = zblue_on_phy_updated, #endif +#if defined(CONFIG_BLUETOOTH_LE_CS) + .le_cs_remote_capabilities_available = zblue_on_cs_capabilities_available, + .le_cs_remote_fae_table_available = zblue_on_cs_remote_fae_table_available, + .le_cs_config_created = zblue_on_cs_config_created, + .le_cs_config_removed = zblue_on_cs_config_removed, + .le_cs_security_enabled = zblue_on_cs_security_enabled, + .le_cs_procedure_enabled = zblue_on_cs_procedure_enabled, + .le_cs_subevent_data_available = zblue_on_cs_subevent, +#endif /* CONFIG_BLUETOOTH_LE_CS */ }; static struct bt_conn_auth_info_cb g_conn_auth_info_cbs = { @@ -607,6 +629,183 @@ static void zblue_on_param_updated(struct bt_conn* conn, uint16_t interval, uint #endif } +void zblue_on_cs_capabilities_available(struct bt_conn *conn, + struct bt_conn_le_cs_capabilities *params) +{ + if (!conn) { + BT_LOGE("Invalid connection handle."); + return; + } + + bt_address_t bt_addr = { 0 }; + const bt_addr_le_t *addr = bt_conn_get_dst(conn); + + if (!addr) { + BT_LOGE("Cann't find address for conn:%p", conn); + return; + } + + memcpy(bt_addr.addr, addr->a.val, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(CAPBLITIES_RECEIVED_EVT, &bt_addr); + bt_srv_conn_le_cs_capabilities_t *capabilities = malloc(sizeof(bt_srv_conn_le_cs_capabilities_t)); + memcpy(capabilities, params, sizeof(bt_srv_conn_le_cs_capabilities_t)); + msg->cs_data.data = (void *)capabilities; + bt_sal_cs_event_callback(msg); + + ARG_UNUSED(params); + BT_LOGD("CS capability exchange completed."); + BT_LOGD("num_config_supported:%d, max_consecutive_procedures_supported:%d", + params->num_config_supported, params->max_consecutive_procedures_supported); + BT_LOGD("num_antennas_supported:%d, max_antenna_paths_supported:%d.", + params->num_antennas_supported, params->max_antenna_paths_supported); + BT_LOGD("initiator_supported:%d, reflector_supported:%d", + params->initiator_supported, params->reflector_supported); + BT_LOGD("mode_3_supported:%d, rtt_aa_only_precision:%d", + params->mode_3_supported, params->rtt_aa_only_precision); + BT_LOGD("rtt_sounding_precision:%d, rtt_random_payload_precision:%d", + params->rtt_sounding_precision, params->rtt_random_payload_precision); + BT_LOGD("rtt_aa_only_n:%d, rtt_sounding_n:%d, rtt_random_payload_n:%d", + params->rtt_aa_only_n, params->rtt_sounding_n, params->rtt_random_payload_n); + BT_LOGD("phase_based_nadm_sounding_supported:%d, phase_based_nadm_random_supported:%d", + params->phase_based_nadm_sounding_supported, params->phase_based_nadm_random_supported); + BT_LOGD("cs_sync_2m_phy_supported:%d, cs_sync_2m_2bt_phy_supported:%d", + params->cs_sync_2m_phy_supported, params->cs_sync_2m_2bt_phy_supported); + BT_LOGD("cs_without_fae_supported:%d, chsel_alg_3c_supported:%d", + params->cs_without_fae_supported, params->chsel_alg_3c_supported); + BT_LOGD("pbr_from_rtt_sounding_seq_supported:%d, t_ip1_times_supported:%d", + params->pbr_from_rtt_sounding_seq_supported, params->t_ip1_times_supported); + BT_LOGD("t_ip2_times_supported:%d, t_fcs_times_supported:%d", + params->t_ip2_times_supported, params->t_fcs_times_supported); + BT_LOGD("t_pm_times_supported:%d, t_sw_time:%d, tx_snr_capability:%d", + params->t_pm_times_supported, params->t_sw_time, + params->tx_snr_capability); + return; +} + +void zblue_on_cs_remote_fae_table_available(struct bt_conn *conn, + struct bt_conn_le_cs_fae_table *params) +{ + return; +} + +void zblue_on_cs_config_created(struct bt_conn *conn, struct bt_conn_le_cs_config *config) +{ + bt_address_t bt_addr = { 0 }; + const bt_addr_le_t *addr = bt_conn_get_dst(conn); + + if (!addr) { + BT_LOGE("Cann't find address for conn:%p", conn); + return; + } + + memcpy(bt_addr.addr, addr->a.val, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(CONFIG_DONE_EVT, &bt_addr); + bt_srv_conn_le_cs_config_t *cs_config = (bt_srv_conn_le_cs_config_t *)malloc(sizeof(bt_srv_conn_le_cs_config_t)); + memcpy(cs_config, config, sizeof(bt_srv_conn_le_cs_config_t)); + msg->cs_data.data = (void*)cs_config; + bt_sal_cs_event_callback(msg); + + BT_LOGD("CS config creation complete. ID: %d\n", config->id); + BT_LOGD("main_mode_type:%d, sub_mode_type:%d", + config->main_mode_type, config->sub_mode_type); + BT_LOGD("min_main_mode_steps:%d, max_main_mode_steps:%d", + config->min_main_mode_steps, config->max_main_mode_steps); + BT_LOGD("main_mode_repetition:%d, mode_0_steps:%d", + config->main_mode_repetition, config->mode_0_steps); + BT_LOGD("role:%d, rtt_type:%d, cs_sync_phy:%d", + config->role, config->rtt_type, config->cs_sync_phy); + BT_LOGD("channel_map_repetition:%d, channel_selection_type:%d", + config->channel_map_repetition, config->channel_selection_type); + BT_LOGD("ch3c_shape:%d, ch3c_jump:%d", config->ch3c_shape, config->ch3c_jump); + BT_LOGD("t_ip1_time_us:%d, t_ip2_time_us:%d", + config->t_ip1_time_us, config->t_ip2_time_us); + BT_LOGD("t_fcs_time_us:%d, t_pm_time_us:%d", config->t_fcs_time_us, config->t_pm_time_us); + BT_LOGD("channel_map:0x%x%x%x%x%x%x%x%x%x%x.", + config->channel_map[0], config->channel_map[1], config->channel_map[2], + config->channel_map[3], config->channel_map[4], config->channel_map[5], + config->channel_map[6], config->channel_map[7], config->channel_map[8], + config->channel_map[9]); + return; +} + +void zblue_on_cs_config_removed(struct bt_conn *conn, uint8_t config_id) +{ + return; +} + +void zblue_on_cs_security_enabled(struct bt_conn *conn) +{ + bt_address_t bt_addr = { 0 }; + + const bt_addr_le_t *addr = bt_conn_get_dst(conn); + + if (!addr) { + BT_LOGE("Cann't find address for conn:%p", conn); + return; + } + + memcpy(bt_addr.addr, addr->a.val, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(SECURITY_DONE_EVT, &bt_addr); + bt_sal_cs_event_callback(msg); + BT_LOGD("CS security enabled.\n"); + return; +} + +void zblue_on_cs_procedure_enabled(struct bt_conn *conn, + struct bt_conn_le_cs_procedure_enable_complete *params) +{ + bt_address_t bt_addr = { 0 }; + + const bt_addr_le_t *addr = bt_conn_get_dst(conn); + + if (!addr) { + BT_LOGE("Cann't find address for conn:%p", conn); + return; + } + + memcpy(bt_addr.addr, addr->a.val, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(PROCEDURE_DONE_EVT, &bt_addr); + bt_srv_conn_le_cs_procedure_enable_complete_t *procedure = malloc(sizeof(bt_srv_conn_le_cs_procedure_enable_complete_t)); + memcpy(procedure, params, sizeof(bt_srv_conn_le_cs_procedure_enable_complete_t)); + msg->cs_data.data = (void*)procedure; + bt_sal_cs_event_callback(msg); + + if (params->state == 1) { + BT_LOGD("CS procedures enabled."); + } else { + BT_LOGD("CS procedures disabled."); + } + + BT_LOGD("config_id:%d, tone_antenna:%d, tx_power:%d, subevents_per_event:%d\n", + params->config_id, params->tone_antenna_config_selection, + params->selected_tx_power, params->subevents_per_event); + BT_LOGD("subevent_interval:%d, event_interval:%d, procedure_interval:%d, procedure_count:%d, max_procedure_len:%d\n", + params->subevent_interval, params->event_interval, params->procedure_interval, + params->procedure_count, params->max_procedure_len); + return; +} + +void zblue_on_cs_subevent(struct bt_conn* conn, struct bt_conn_le_cs_subevent_result* result) +{ + bt_address_t bt_addr = { 0 }; + + const bt_addr_le_t *addr = bt_conn_get_dst(conn); + + if (!addr) { + BT_LOGE("Cann't find address for conn:%p", conn); + return; + } + + memcpy(bt_addr.addr, addr->a.val, sizeof(bt_address_t)); + cs_msg_t* msg = cs_msg_new(CS_SUBEVENT_RESULT_EVT, &bt_addr); + bt_srv_conn_le_cs_subevent_result_t *subevent = malloc(sizeof(bt_srv_conn_le_cs_subevent_result_t) + result->step_data_buf->len); + memcpy(subevent, result, sizeof(bt_srv_conn_le_cs_subevent_result_t)); + + memcpy(subevent->step_data_buf, result->step_data_buf->data, subevent->len); + msg->cs_data.data = (void*)subevent; + bt_sal_cs_event_callback(msg); +} + #if defined(CONFIG_BT_USER_PHY_UPDATE) ble_phy_type_t le_phy_convert_from_stack(uint8_t mode) { diff --git a/service/stacks/zephyr/sal_le_cs_interface.c b/service/stacks/zephyr/sal_le_cs_interface.c new file mode 100644 index 000000000..f543488e5 --- /dev/null +++ b/service/stacks/zephyr/sal_le_cs_interface.c @@ -0,0 +1,321 @@ +/**************************************************************************** + * Copyright (C) 2024 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ + +#include "sal_le_cs_interface.h" +#include +#include +#include +#include "sal_interface.h" +#include "utils/log.h" + +#ifdef CONFIG_BLUETOOTH_LE_CS + +bt_status_t bt_sal_cs_read_remote_supported_capabilities(bt_controller_id_t id, bt_address_t* addr) +{ + if (!addr) { + BT_LOGW("sal cs read remote capabilities, invalid addr."); + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn* conn = bt_conn_lookup_addr_le(id, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs read remote capabilities, doesn't find connection for addr:%s", + bt_addr_str(addr)); + } + + SAL_CHECK_RET_WITH_CONN(bt_le_cs_read_remote_supported_capabilities(conn), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_set_default_settings(bt_controller_id_t id, bt_address_t* addr, bt_le_srv_cs_set_default_settings_param_t *params) +{ + if (!addr || !params) { + BT_LOGW("sal cs set default settings, invalid params or addr."); + return BT_STATUS_PARM_INVALID; + } + + const struct bt_le_cs_set_default_settings_param default_settings = { + .enable_initiator_role = params->enable_initiator_role, + .enable_reflector_role = params->enable_reflector_role, + .cs_sync_antenna_selection = params->cs_sync_antenna_selection, + .max_tx_power = BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER, + }; + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn* conn = bt_conn_lookup_addr_le(id, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("sal cs set default settings, doesn't find connection for addr:%s", + bt_addr_str(addr)); + return BT_STATUS_FAIL; + } + + SAL_CHECK_RET_WITH_CONN(bt_le_cs_set_default_settings(conn, &default_settings), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_read_remote_fae_table(bt_controller_id_t id, bt_address_t* addr) +{ + if (!addr) { + BT_LOGE("cs read remote fae table, invalid addr."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn* conn = bt_conn_lookup_addr_le(id, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("sal cs read remote fae table, doesn't find connection for addr:%s", + bt_addr_str(addr)); + return BT_STATUS_FAIL; + } + + SAL_CHECK_RET_WITH_CONN(bt_le_cs_read_remote_fae_table(conn), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; + +} + +bt_status_t bt_sal_cs_create_config(bt_controller_id_t id, bt_address_t* addr, + bt_le_srv_cs_create_config_params_t *params, + bt_le_srv_cs_create_config_context_t context) +{ + if (!addr) { + BT_LOGE("cs create config, invalid addr."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn* conn = bt_conn_lookup_addr_le(id, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs create config, doesn't find connection for addr:%s", + bt_addr_str(addr)); + return BT_STATUS_FAIL; + } + + struct bt_le_cs_create_config_params config = {}; + memcpy(&config, params, sizeof(bt_le_srv_cs_create_config_params_t)); + SAL_CHECK_RET_WITH_CONN(bt_le_cs_create_config(conn, &config, (bt_le_srv_cs_create_config_context_t)context), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_security_enable(bt_controller_id_t id, bt_address_t* addr) +{ + if (!addr) { + BT_LOGE("cs create config, invalid addr."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn* conn = bt_conn_lookup_addr_le(id, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs security enable, doesn't find connection for addr:%s", + bt_addr_str(addr)); + return BT_STATUS_FAIL; + } + + SAL_CHECK_RET_WITH_CONN(bt_le_cs_security_enable(conn), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_procedure_enable(bt_address_t* addr, + const bt_le_srv_cs_procedure_enable_param_t *params) +{ + if (!addr) { + BT_LOGE("cs create config, invalid addr."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + struct bt_le_cs_procedure_enable_param enable = {}; + struct bt_conn* conn = bt_conn_lookup_addr_le(0, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs procedure enable, doesn't find the connection for addr:%s", + bt_addr_str(addr)); + return BT_STATUS_FAIL; + } + + memcpy(&enable, params, sizeof(struct bt_le_cs_procedure_enable_param)); + + SAL_CHECK_RET_WITH_CONN(bt_le_cs_procedure_enable(conn, &enable), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_remove_config(bt_controller_id_t id, bt_address_t* addr, uint8_t config_id) +{ + if (!addr) { + BT_LOGE("cs remove config, invalid addr."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn* conn = bt_conn_lookup_addr_le(id, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs remove config, doesn't find the connection for addr:%s", + bt_addr_str(addr)); + return BT_STATUS_FAIL; + } + + SAL_CHECK_RET_WITH_CONN(bt_le_cs_remove_config(conn, config_id), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_set_procedure_parameters(bt_controller_id_t id, bt_address_t* addr, + const bt_le_srv_cs_set_procedure_parameters_param_t *params) +{ + if (!addr) { + BT_LOGE("cs remove config, invalid addr."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_le_cs_set_procedure_parameters_param parameters = {}; + struct bt_conn* conn = bt_conn_lookup_addr_le(id, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs set procedure parameters, doesn't find connection for addr:%s", + bt_addr_str(addr)); + return BT_STATUS_FAIL; + } + + memcpy(¶meters, params, sizeof(struct bt_le_cs_set_procedure_parameters_param)); + + SAL_CHECK_RET_WITH_CONN(bt_le_cs_set_procedure_parameters(conn, ¶meters), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_set_channel_classification(uint8_t channel_classification[10], bt_address_t* addr) +{ + if (!addr) { + BT_LOGE("cs remove config, invalid addr."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn* conn = bt_conn_lookup_addr_le(0, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs set channel classificaition, doesn't find connection for addr:%s.", + bt_addr_str(addr)); + return BT_STATUS_FAIL; + } + + SAL_CHECK_RET_WITH_CONN(bt_le_cs_set_channel_classification(channel_classification), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_read_local_supported_capabilities(bt_srv_conn_le_cs_capabilities_t *params, bt_address_t* addr) +{ + if (!params || !addr) { + BT_LOGE("cs read local supported capabilities, invalid params or addrs."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn_le_cs_capabilities capabilities = {}; + struct bt_conn* conn = bt_conn_lookup_addr_le(0, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs read local supported capabilities, doesn't find connection for addr:%s.", + bt_addr_str(addr)); + } + + memcpy(&capabilities, params, sizeof(struct bt_conn_le_cs_capabilities)); + SAL_CHECK_RET_WITH_CONN(bt_le_cs_read_local_supported_capabilities(&capabilities), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +bt_status_t bt_sal_cs_write_cached_remote_supported_capabilities( + bt_srv_conn_le_cs_capabilities_t *params, bt_address_t* addr) +{ + if (!params || !addr) { + BT_LOGE("cs write cached remote supported capabilites, invalid params or addrs."); + return BT_STATUS_PARM_INVALID; + } + + bt_addr_le_t le_addr = {0}; + memcpy(le_addr.a.val, addr->addr, sizeof(addr->addr)); + + struct bt_conn_le_cs_capabilities capabilities = {}; + struct bt_conn* conn = bt_conn_lookup_addr_le(0, (const bt_addr_le_t *)&le_addr); + + if (!conn) { + BT_LOGE("cs write cached remote supported capabilites, doesn't find connection for addr:%s.", + bt_addr_str(addr)); + } + + memcpy(&capabilities, params, sizeof(struct bt_conn_le_cs_capabilities)); + SAL_CHECK_RET_WITH_CONN(bt_le_cs_write_cached_remote_supported_capabilities(conn, &capabilities), 0, conn); + + bt_conn_unref(conn); + + return BT_STATUS_SUCCESS; +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ diff --git a/tools/bt_tools.c b/tools/bt_tools.c index d720f74a3..e85356414 100644 --- a/tools/bt_tools.c +++ b/tools/bt_tools.c @@ -234,6 +234,9 @@ static bt_command_t g_cmd_tables[] = { #endif #ifdef CONFIG_BLUETOOTH_STORAGE_UPDATE { "storage", storage_command_exec, 0, "storage update cmd, input \'storage\' show usage" }, +#endif +#ifdef CONFIG_BLUETOOTH_LE_CS + {"cs", le_cs_command_exec, 0, "le cs cmd, input \'tbs\' show usage"}, #endif { "dump", dump_cmd, 0, "dump adapter state" }, #ifdef CONFIG_BLUETOOTH_LOG @@ -360,6 +363,9 @@ static void bt_tool_init(void* handle) #endif #ifdef CONFIG_BLUETOOTH_STORAGE_UPDATE storage_command_init(handle); +#endif +#ifdef CONFIG_BLUETOOTH_LE_CS + le_cs_commond_init(handle); #endif g_cmd_had_inited = true; } @@ -428,6 +434,9 @@ static void bt_tool_uninit(void* handle) #endif #ifdef CONFIG_BLUETOOTH_STORAGE_UPDATE storage_command_uninit(handle); +#endif +#ifdef CONFIG_BLUETOOTH_LE_CS + le_cs_commond_uninit(handle); #endif g_cmd_had_inited = false; } diff --git a/tools/bt_tools.h b/tools/bt_tools.h index e28fc350f..89f5f2068 100644 --- a/tools/bt_tools.h +++ b/tools/bt_tools.h @@ -190,8 +190,13 @@ int lea_vmicp_command_init(void* handle); void lea_vmicp_command_uninit(void* handle); int vmicp_command_exec(void* handle, int argc, char* argv[]); + int storage_command_init(void* handle); void storage_command_uninit(void* handle); int storage_command_exec(void* handle, int argc, char* argv[]); +int le_cs_commond_init(void* handle); +int le_cs_commond_uninit(void* handle); +int le_cs_command_exec(void* handle, int argc, char* argv[]); + #endif /* __BT_TOOLS_H__ */ diff --git a/tools/le_cs.c b/tools/le_cs.c new file mode 100644 index 000000000..d852d2ac0 --- /dev/null +++ b/tools/le_cs.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * Copyright (C) 2025 Xiaomi Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ***************************************************************************/ +#include +#include +#include +#include "bt_cs.h" +#include "bluetooth.h" +#include "bt_config.h" +#include "bt_device.h" +#include "bt_tools.h" + +#ifdef CONFIG_BLUETOOTH_LE_CS + +static int cs_start_distance_measurement_cmd(void* handle, int argc, char* argv[]); +static int cs_stop_distance_measurement_cmd(void* handle, int argc, char* argv[]); +static int cs_test_cmd(void* handle, int argc, char* argv[]); + +static void* cs_callbacks = NULL; + +static bt_command_t g_cs_tables[] = { + { "start", cs_start_distance_measurement_cmd, 0, "\"start distance measurement :\"" }, + { "stop", cs_stop_distance_measurement_cmd, 0, "\"stop distance measurement :\"" }, + { "test", cs_test_cmd, 0, "\"Channel Sounding test mode :\"" }, +}; + +static void usage(void) +{ + printf("Usage:\n"); + printf("\taddress: peer device address like 00:01:02:03:04:05\n"); + printf("Commands:\n"); + for (int i = 0; i < ARRAY_SIZE(g_cs_tables); i++) { + printf("\t%-8s\t%s\n", g_cs_tables[i].cmd, g_cs_tables[i].help); + } +} + +static void le_cs_distance_measure_started_cb(void* cookie, bt_address_t* addr, uint8_t method) +{ + PRINT("cs distance measure started. cookie:%p, addr:%s, method:%d\n", + cookie, bt_addr_bastr(addr), method); + return; +} + +static void le_cs_distance_measure_stopped_cb(void* cookie, bt_address_t* addr, uint8_t reason, uint8_t method) +{ + PRINT("cs distance measure started. cookie:%p, addr:%s, reason:%d, method:%d\n", + cookie, bt_addr_bastr(addr), reason, method); + return; +} + +static void le_cs_distance_measure_result_cb(void* cookie, bt_address_t* addr, uint8_t centimeter, uint8_t errorCentimeter, + uint8_t azimuthAngle, uint8_t errorAzimuthAngle, uint8_t altitudeAngle, uint8_t errorAltitudeAngle, + long elapsedRealtimeNanos, uint8_t confidenceLevel, double delaySpreadMeters, + uint8_t detectedAttackLevel, double velocityMetersPerSecond, uint8_t method) +{ + return; +} + +static const cs_callbacks_t le_cs_cbs = { + sizeof(le_cs_cbs), + le_cs_distance_measure_started_cb, + le_cs_distance_measure_stopped_cb, + le_cs_distance_measure_result_cb, +}; + +int le_cs_commond_init(void* handle) +{ + cs_callbacks = bt_cs_register_callbacks(handle, &le_cs_cbs); + PRINT("cs command init."); + return 0; +} + +int le_cs_commond_uninit(void* handle) +{ + bt_cs_unregister_callbacks(handle, cs_callbacks); + + return 0; +} + +int le_cs_command_exec(void* handle, int argc, char* argv[]) +{ + int ret = CMD_USAGE_FAULT; + + if (argc > 0) + ret = execute_command_in_table(handle, g_cs_tables, ARRAY_SIZE(g_cs_tables), argc, argv); + + if (ret < 0) + usage(); + + return ret; +} + +static int cs_start_distance_measurement_cmd(void* handle, int argc, char* argv[]) +{ + bt_distance_measurement_params_t params; + + memset(¶ms, 0, sizeof(bt_distance_measurement_params_t)); + params.method = METHOD_CS; + bt_cs_start_distance_measurement(handle, ¶ms); + return 0; +} + +static int cs_stop_distance_measurement_cmd(void* handle, int argc, char* argv[]) +{ + bt_address_t addr = { 0 }; + bt_cs_stop_distance_measurement(handle, &addr, METHOD_CS, 1000); + return 0; +} + +static int cs_test_cmd(void* handle, int argc, char* argv[]) +{ + uint8_t data[10] = { 0 }; + bt_cs_test(handle, (void*)data, sizeof(data)); + return 0; +} + +#endif /* CONFIG_BLUETOOTH_LE_CS */ \ No newline at end of file