From 96f5fbed3938f2f52fd308147960c12d8253a00e Mon Sep 17 00:00:00 2001 From: Magdalena Kasenberg Date: Thu, 22 Aug 2024 12:23:43 +0200 Subject: [PATCH] [wip] Add GATT Ranging Service --- nimble/host/include/host/ble_cs.h | 81 +- nimble/host/include/host/ble_peer.h | 85 ++ nimble/host/pkg.yml | 6 + .../ras/include/services/ras/ble_svc_ras.h | 155 ++ nimble/host/services/ras/pkg.yml | 43 + nimble/host/services/ras/src/ble_svc_ras.c | 1247 +++++++++++++++++ .../host/services/ras/src/ble_svc_ras_clt.c | 899 ++++++++++++ nimble/host/services/ras/syscfg.yml | 39 + nimble/host/src/ble_cs.c | 636 ++++++++- nimble/host/src/ble_cs_priv.h | 16 + nimble/host/src/ble_peer.c | 731 ++++++++++ nimble/host/syscfg.yml | 21 + 12 files changed, 3885 insertions(+), 74 deletions(-) create mode 100644 nimble/host/include/host/ble_peer.h create mode 100644 nimble/host/services/ras/include/services/ras/ble_svc_ras.h create mode 100644 nimble/host/services/ras/pkg.yml create mode 100644 nimble/host/services/ras/src/ble_svc_ras.c create mode 100644 nimble/host/services/ras/src/ble_svc_ras_clt.c create mode 100644 nimble/host/services/ras/syscfg.yml create mode 100644 nimble/host/src/ble_peer.c diff --git a/nimble/host/include/host/ble_cs.h b/nimble/host/include/host/ble_cs.h index 3abbd11e65..bc71171211 100644 --- a/nimble/host/include/host/ble_cs.h +++ b/nimble/host/include/host/ble_cs.h @@ -23,35 +23,86 @@ #define H_BLE_CS_ #include "syscfg/syscfg.h" +#define BLE_HS_CS_MODE0 (0) +#define BLE_HS_CS_MODE1 (1) +#define BLE_HS_CS_MODE2 (2) +#define BLE_HS_CS_MODE3 (3) +#define BLE_HS_CS_MODE_UNUSED (0xff) +#define BLE_HS_CS_SUBMODE_TYPE BLE_HS_CS_MODE_UNUSED + +#define BLE_HS_CS_ROLE_INITIATOR (0) +#define BLE_HS_CS_ROLE_REFLECTOR (1) + #define BLE_CS_EVENT_CS_PROCEDURE_COMPLETE (0) +#define BLE_CS_EVENT_CS_STEP_DATA (1) + +#define BLE_HS_CS_TOA_TOD_NOT_AVAILABLE (0x00008000) +#define BLE_HS_CS_N_AP_MAX (4) + +struct ble_cs_mode0_result { + uint16_t measured_freq_offset; + uint8_t packet_quality; + uint8_t packet_rssi; + uint8_t packet_antenna; +}; + +struct ble_cs_mode1_result { + uint32_t packet_pct1; + uint32_t packet_pct2; + int16_t toa_tod; + uint8_t packet_quality; + uint8_t packet_nadm; + uint8_t packet_rssi; + uint8_t packet_antenna; +}; + +struct ble_cs_mode2_result { + uint32_t tone_pct[BLE_HS_CS_N_AP_MAX + 1]; + uint8_t tone_quality_ind[BLE_HS_CS_N_AP_MAX + 1]; + uint8_t antenna_paths[4]; + uint8_t antenna_path_permutation_id; +}; + +struct ble_cs_mode3_result { + uint32_t packet_pct1; + uint32_t packet_pct2; + uint32_t tone_pct[BLE_HS_CS_N_AP_MAX + 1]; + uint8_t tone_quality_ind[BLE_HS_CS_N_AP_MAX + 1]; + uint8_t antenna_paths[4]; + int16_t toa_tod; + uint8_t antenna_path_permutation_id; + uint8_t packet_quality; + uint8_t packet_nadm; + uint8_t packet_rssi; + uint8_t packet_antenna; +}; struct ble_cs_event { + uint16_t conn_handle; + uint8_t status; uint8_t type; - union - { - struct - { - uint16_t conn_handle; - uint8_t status; - } procedure_complete; + union { + struct { + uint8_t role; + uint8_t mode; + uint8_t *data; + } step_data; }; - }; typedef int ble_cs_event_fn(struct ble_cs_event *event, void *arg); -struct ble_cs_initiator_procedure_start_params { +struct ble_cs_procedure_start_params { uint16_t conn_handle; - ble_cs_event_fn *cb; - void *cb_arg; }; -struct ble_cs_reflector_setup_params { +struct ble_cs_setup_params { + uint16_t conn_handle; ble_cs_event_fn *cb; void *cb_arg; }; -int ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_params *params); -int ble_cs_initiator_procedure_terminate(uint16_t conn_handle); -int ble_cs_reflector_setup(struct ble_cs_reflector_setup_params *params); +int ble_cs_procedure_start(const struct ble_cs_procedure_start_params *params); +int ble_cs_procedure_terminate(uint16_t conn_handle); +int ble_cs_setup(struct ble_cs_setup_params *params); #endif diff --git a/nimble/host/include/host/ble_peer.h b/nimble/host/include/host/ble_peer.h new file mode 100644 index 0000000000..1e30268112 --- /dev/null +++ b/nimble/host/include/host/ble_peer.h @@ -0,0 +1,85 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you 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 H_BLE_PEER_ +#define H_BLE_PEER_ + +#include "os/mynewt.h" +#ifdef __cplusplus +extern "C" { +#endif + +/** Peer. */ +struct ble_peer_dsc { + SLIST_ENTRY(ble_peer_dsc) next; + struct ble_gatt_dsc dsc; +}; +SLIST_HEAD(ble_peer_dsc_list, ble_peer_dsc); + +struct ble_peer_chr { + SLIST_ENTRY(ble_peer_chr) next; + struct ble_gatt_chr chr; + + struct ble_peer_dsc_list dscs; +}; +SLIST_HEAD(ble_peer_chr_list, ble_peer_chr); + +struct ble_peer_svc { + SLIST_ENTRY(ble_peer_svc) next; + struct ble_gatt_svc svc; + + struct ble_peer_chr_list chrs; +}; +SLIST_HEAD(ble_peer_svc_list, ble_peer_svc); + +struct ble_peer; +typedef void ble_peer_disc_fn(const struct ble_peer *peer, int status, void *arg); + +struct ble_peer { + SLIST_ENTRY(ble_peer) next; + + uint16_t conn_handle; + + /** List of discovered GATT services. */ + struct ble_peer_svc_list svcs; + + /** Keeps track of where we are in the service discovery process. */ + uint16_t disc_prev_chr_val; + struct ble_peer_svc *cur_svc; + + /** Callback that gets executed when service discovery completes. */ + ble_peer_disc_fn *disc_cb; + void *disc_cb_arg; +}; + +struct ble_peer *ble_peer_find(uint16_t conn_handle); +int ble_peer_disc_all(uint16_t conn_handle, ble_peer_disc_fn *disc_cb, void *disc_cb_arg); +const struct ble_peer_dsc *ble_peer_dsc_find_uuid(const struct ble_peer *peer, + const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid, const ble_uuid_t *dsc_uuid); +const struct ble_peer_chr *ble_peer_chr_find_uuid(const struct ble_peer *peer, + const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid); +const struct ble_peer_svc *ble_peer_svc_find_uuid(const struct ble_peer *peer, const ble_uuid_t *uuid); +int ble_peer_delete(uint16_t conn_handle); +int ble_peer_add(uint16_t conn_handle); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/nimble/host/pkg.yml b/nimble/host/pkg.yml index b4df8f8712..9fe05cecd7 100644 --- a/nimble/host/pkg.yml +++ b/nimble/host/pkg.yml @@ -54,3 +54,9 @@ pkg.req_apis: pkg.down.BLE_HS_STOP_ON_SHUTDOWN: ble_hs_shutdown: 200 + +pkg.init.'BLE_PEER': + ble_peer_init: $after:ble_transport_hs_init + +pkg.init.'BLE_CHANNEL_SOUNDING': + ble_cs_init: $after:ble_transport_hs_init diff --git a/nimble/host/services/ras/include/services/ras/ble_svc_ras.h b/nimble/host/services/ras/include/services/ras/ble_svc_ras.h new file mode 100644 index 0000000000..bcf926edef --- /dev/null +++ b/nimble/host/services/ras/include/services/ras/ble_svc_ras.h @@ -0,0 +1,155 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you 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 H_BLE_SVC_RAS_ +#define H_BLE_SVC_RAS_ + +#include +#include "host/ble_cs.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_CONN_HANDLE_INVALID 0xffff + +#define BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID 0x185B +#define BLE_SVC_RAS_CHR_RAS_FEATURES_UUID 0x2C14 +#define BLE_SVC_RAS_CHR_REAL_TIME_RANGING_DATA_UUID 0x2C15 +#define BLE_SVC_RAS_CHR_ON_DEMAND_RANGING_DATA_UUID 0x2C16 +#define BLE_SVC_RAS_CHR_RAS_CONTROL_POINT_UUID 0x2C17 +#define BLE_SVC_RAS_CHR_RANGING_DATA_READY_UUID 0x2C18 +#define BLE_SVC_RAS_CHR_RANGING_DATA_OVERWRITTEN_UUID 0x2C19 + +#define BLE_SVC_RAS_FILTER_MODE0_POSITION (0) +#define BLE_SVC_RAS_FILTER_MODE1_POSITION (BLE_SVC_RAS_FILTER_MODE0_POSITION << 14) +#define BLE_SVC_RAS_FILTER_MODE2_POSITION (BLE_SVC_RAS_FILTER_MODE1_POSITION << 14) +#define BLE_SVC_RAS_FILTER_MODE3_POSITION (BLE_SVC_RAS_FILTER_MODE3_POSITION << 14) + +#define BLE_SVC_RAS_FILTER_MODE0_MASK (0xF) +#define BLE_SVC_RAS_FILTER_MODE1_MASK (0x7F) +#define BLE_SVC_RAS_FILTER_MODE2_MASK (0x7F) +#define BLE_SVC_RAS_FILTER_MODE3_MASK (0x3FFF) + +#define BLE_SVC_RAS_FILTER_MODE0_PACKET_QUALITY (0) +#define BLE_SVC_RAS_FILTER_MODE0_PACKET_RSSI (1) +#define BLE_SVC_RAS_FILTER_MODE0_PACKET_ANTENNA (2) +#define BLE_SVC_RAS_FILTER_MODE0_MEASURED_FREQ_OFFSET (3) + +#define BLE_SVC_RAS_FILTER_MODE1_PACKET_QUALITY (0) +#define BLE_SVC_RAS_FILTER_MODE1_PACKET_NADM (1) +#define BLE_SVC_RAS_FILTER_MODE1_PACKET_RSSI (2) +#define BLE_SVC_RAS_FILTER_MODE1_TOD_TOA (3) +#define BLE_SVC_RAS_FILTER_MODE1_PACKET_ANTENNA (4) +#define BLE_SVC_RAS_FILTER_MODE1_PACKET_PCT1 (5) +#define BLE_SVC_RAS_FILTER_MODE1_PACKET_PCT2 (6) + +#define BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PERMUTATION_ID (0) +#define BLE_SVC_RAS_FILTER_MODE2_TONE_PCT (1) +#define BLE_SVC_RAS_FILTER_MODE2_TONE_QUALITY (2) +#define BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_1 (3) +#define BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_2 (4) +#define BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_3 (5) +#define BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_4 (6) + +#define BLE_SVC_RAS_FILTER_MODE3_PACKET_QUALITY (0) +#define BLE_SVC_RAS_FILTER_MODE3_PACKET_NADM (1) +#define BLE_SVC_RAS_FILTER_MODE3_PACKET_RSSI (2) +#define BLE_SVC_RAS_FILTER_MODE3_TOD_TOA (3) +#define BLE_SVC_RAS_FILTER_MODE3_PACKET_ANTENNA (4) +#define BLE_SVC_RAS_FILTER_MODE3_PACKET_PCT1 (5) +#define BLE_SVC_RAS_FILTER_MODE3_PACKET_PCT2 (6) +#define BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PERMUTATION_ID (7) +#define BLE_SVC_RAS_FILTER_MODE3_TONE_PCT (8) +#define BLE_SVC_RAS_FILTER_MODE3_TONE_QUALITY (9) +#define BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_1 (10) +#define BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_2 (11) +#define BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_3 (12) +#define BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_4 (13) + +#define BLE_SVC_RAS_CP_CMD_GET_RANGING_DATA (0x00) +#define BLE_SVC_RAS_CP_CMD_ACK_RANGING_DATA (0x01) +#define BLE_SVC_RAS_CP_CMD_RETRIEVE_LOST_SEGMENT (0x02) +#define BLE_SVC_RAS_CP_CMD_ABORT_OPERATION (0x03) +#define BLE_SVC_RAS_CP_CMD_SET_FILTER (0x04) + +#define BLE_SVC_RAS_CP_RSP_COMPLETE_RANGING_DATA (0x00) +#define BLE_SVC_RAS_CP_RSP_COMPLETE_LOST_SEGMENT (0x01) +#define BLE_SVC_RAS_CP_RSP_RESPONSE_CODE (0x02) + +#define BLE_SVC_RAS_CP_RSPCODE_SUCCESS (0x01) +#define BLE_SVC_RAS_CP_RSPCODE_OP_CODE_NOT_SUPPORTED (0x02) +#define BLE_SVC_RAS_CP_RSPCODE_INVALID_PARAMETER (0x03) +#define BLE_SVC_RAS_CP_RSPCODE_SUCCESS_PERSISTED (0x04) +#define BLE_SVC_RAS_CP_RSPCODE_ABORT_UNSUCCESSFUL (0x05) +#define BLE_SVC_RAS_CP_RSPCODE_PROCEDURE_NOT_COMPLETED (0x06) +#define BLE_SVC_RAS_CP_RSPCODE_SERVER_BUSY (0x07) +#define BLE_SVC_RAS_CP_RSPCODE_NO_RECORDS_FOUND (0x08) + +#define BLE_SVC_RAS_MODE_REAL_TIME (0) +#define BLE_SVC_RAS_MODE_ON_DEMAND (1) + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) +void ble_svc_ras_init(void); +int ble_svc_ras_ranging_data_body_init(uint16_t conn_handle, uint16_t procedure_counter, uint8_t config_id, + uint8_t tx_power, uint8_t antenna_paths_mask); +int ble_svc_ras_ranging_subevent_init(uint16_t conn_handle, uint16_t start_acl_conn_event, uint16_t frequency_compensation, + uint8_t ranging_done_status, uint8_t subevent_done_status, + uint8_t ranging_abort_reason, uint8_t subevent_abort_reason, + uint8_t reference_power_level, uint8_t number_of_steps_reported); +int ble_svc_ras_ranging_subevent_update_status(uint16_t conn_handle, uint8_t number_of_steps_reported, + uint8_t ranging_done_status, uint8_t subevent_done_status, + uint8_t ranging_abort_reason, uint8_t subevent_abort_reason); +int ble_svc_ras_add_step_mode(uint16_t conn_handle, uint8_t mode, uint8_t status); +int ble_svc_ras_add_mode0_result(struct ble_cs_mode0_result *result, uint16_t conn_handle, uint8_t local_role); +int ble_svc_ras_add_mode1_result(struct ble_cs_mode1_result *result, uint16_t conn_handle, uint8_t rtt_pct_included); +int ble_svc_ras_add_mode2_result(struct ble_cs_mode2_result *result, uint16_t conn_handle, uint8_t n_ap); +int ble_svc_ras_add_mode3_result(struct ble_cs_mode3_result *result, uint16_t conn_handle, uint8_t n_ap, uint8_t rtt_pct_included); +int ble_svc_ras_ranging_data_ready(uint16_t conn_handle); +#endif + +#if MYNEWT_VAL(BLE_SVC_RAS_CLIENT) +struct ble_svc_ras_clt_ranging_header { + uint16_t ranging_counter; + uint8_t config_id; + uint8_t tx_power; + uint8_t antenna_paths_mask; +}; + +struct ble_svc_ras_clt_subevent_header { + uint16_t start_acl_conn_event; + uint16_t frequency_compensation; + uint8_t done_status; + uint8_t abort_reason; + uint8_t reference_power_level; + uint8_t number_of_steps_reported; +}; + +typedef void ble_svc_ras_clt_subscribe_cb(uint16_t conn_handle); +typedef void ble_svc_ras_clt_step_data_received_cb(void *data, uint16_t conn_handle, uint8_t step_mode); +int ble_svc_ras_clt_config_set(uint16_t conn_handle, uint8_t rtt_pct_included, uint8_t n_ap, uint8_t local_role); +int ble_svc_ras_clt_subscribe(ble_svc_ras_clt_subscribe_cb *subscribe_cb, + ble_svc_ras_clt_step_data_received_cb *step_data_cb, + uint16_t conn_handle, uint8_t mode); +#endif +#ifdef __cplusplus +} +#endif + +#endif diff --git a/nimble/host/services/ras/pkg.yml b/nimble/host/services/ras/pkg.yml new file mode 100644 index 0000000000..a907bbc7bc --- /dev/null +++ b/nimble/host/services/ras/pkg.yml @@ -0,0 +1,43 @@ +# +# Licensed to the Apache Software Foundation (ASF) under one +# or more contributor license agreements. See the NOTICE file +# distributed with this work for additional information +# regarding copyright ownership. The ASF licenses this file +# to you 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. +# + +pkg.name: nimble/host/services/ras +pkg.description: Implements the RAS Service. +pkg.author: "Apache Mynewt " +pkg.homepage: "http://mynewt.apache.org/" +pkg.keywords: + - ble + - bluetooth + - nimble + - ras + +pkg.deps: + - nimble/host + +pkg.init.'BLE_SVC_RAS_SERVER': + ble_svc_ras_init: 'MYNEWT_VAL(BLE_SVC_RAS_SYSINIT_STAGE)' + +pkg.init.'BLE_SVC_RAS_CLIENT': + ble_svc_ras_clt_init: 'MYNEWT_VAL(BLE_SVC_RAS_SYSINIT_STAGE)' + +pkg.source_files.'BLE_SVC_RAS_SERVER': + - "src/ble_svc_ras.c" + +pkg.source_files.'BLE_SVC_RAS_CLIENT': + - "src/ble_svc_ras_clt.c" diff --git a/nimble/host/services/ras/src/ble_svc_ras.c b/nimble/host/services/ras/src/ble_svc_ras.c new file mode 100644 index 0000000000..27cf82b6d3 --- /dev/null +++ b/nimble/host/services/ras/src/ble_svc_ras.c @@ -0,0 +1,1247 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you 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 "syscfg/syscfg.h" +#include "os/os_mbuf.h" +//#include "host/ble_hs_log.h" +#include "host/ble_hs.h" +#include "host/ble_cs.h" +#include "host/ble_uuid.h" +#include "nimble/hci_common.h" +#include "sys/queue.h" +//#include "ble_hs_priv.h" +//#include "ble_hs_hci_priv.h" +#include "services/ras/ble_svc_ras.h" + +//#include +//#include +//#include +//#include "bsp/bsp.h" +//#include "host/ble_hs.h" +//#include "host/ble_uuid.h" +//#include "cs_reflector.h" + +//struct ble_hs_cfg; +//struct ble_gatt_register_ctxt; + +#define MBUF_POOL_BUF_SIZE MYNEWT_VAL(BLE_SVC_RAS_TXRX_BUF_SIZE) +#define MBUF_POOL_BUF_COUNT MYNEWT_VAL(BLE_SVC_RAS_TXRX_BUF_COUNT) + +static const ble_uuid16_t ble_svc_ras_ranging_service_uuid = + BLE_UUID16_INIT(BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID); + +static const ble_uuid16_t ble_svc_ras_features_uuid = + BLE_UUID16_INIT(BLE_SVC_RAS_CHR_RAS_FEATURES_UUID); + +static const ble_uuid16_t ble_svc_ras_real_time_ranging_data_uuid = + BLE_UUID16_INIT(BLE_SVC_RAS_CHR_REAL_TIME_RANGING_DATA_UUID); + +static const ble_uuid16_t ble_svc_ras_on_demand_ranging_data_uuid = + BLE_UUID16_INIT(BLE_SVC_RAS_CHR_ON_DEMAND_RANGING_DATA_UUID); + +static const ble_uuid16_t ble_svc_ras_control_point_uuid = + BLE_UUID16_INIT(BLE_SVC_RAS_CHR_RAS_CONTROL_POINT_UUID); + +static const ble_uuid16_t ble_svc_ras_ranging_data_ready_uuid = + BLE_UUID16_INIT(BLE_SVC_RAS_CHR_RANGING_DATA_READY_UUID); + +static const ble_uuid16_t ble_svc_ras_ranging_data_overwritten_uuid = + BLE_UUID16_INIT(BLE_SVC_RAS_CHR_RANGING_DATA_OVERWRITTEN_UUID); + +static uint16_t ble_svc_ras_features_val_handle; +static uint16_t ble_svc_ras_real_time_ranging_data_val_handle; +static uint16_t ble_svc_ras_on_demand_ranging_data_val_handle; +static uint16_t ble_svc_ras_control_point_val_handle; +static uint16_t ble_svc_ras_ranging_data_ready_val_handle; +static uint16_t ble_svc_ras_ranging_data_overwritten_val_handle; + +static struct os_mempool ras_mempool; +static struct os_mbuf_pool ras_mbuf_pool; +static uint8_t ras_mbuf_area[MBUF_POOL_BUF_COUNT * MBUF_POOL_BUF_SIZE]; +static struct ble_gap_event_listener ble_svc_ras_listener; + +#ifndef BIT +#define BIT(n) (1UL << (n)) +#endif + +#define NOTIFY_REAL_TIME_RANGING_DATA (0) +#define NOTIFY_ON_DEMAND_RANGING_DATA (1) +#define NOTIFY_RANGING_DATA_READY (2) +#define NOTIFY_RANGING_DATA_OVERWRITTEN (3) +#define NOTIFY_INVALID (255) + +#define INDICATE_REAL_TIME_RANGING_DATA (0) +#define INDICATE_ON_DEMAND_RANGING_DATA (1) +#define INDICATE_CONTROL_POINT (2) +#define INDICATE_RANGING_DATA_READY (3) +#define INDICATE_RANGING_DATA_OVERWRITTEN (4) +#define INDICATE_INVALID (255) + +struct ble_svc_ras_sm { + int status; + struct os_mbuf *ranging_data_body_mbuf; + uint8_t *cur_subevent_header_ptr; + uint8_t notify; + uint8_t indicate; + uint32_t supported_features; + uint16_t data_overwritten_counter; + uint16_t ranging_counter; + uint16_t filter_bitmask[4]; + uint16_t sent_data_len; + uint16_t remaining_data_len; + uint16_t conn_handle; + uint16_t mtu; + uint8_t rolling_segment_counter; + uint8_t segment_counter; + uint8_t busy; +}; + +static struct ble_svc_ras_sm g_ble_ras_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)]; + +int ble_att_conn_chan_find(uint16_t conn_handle, uint16_t cid, + struct ble_hs_conn **out_conn, + struct ble_l2cap_chan **out_chan); +uint16_t ble_att_chan_mtu(const struct ble_l2cap_chan *chan); + + +static struct ble_svc_ras_sm * +ble_svc_ras_sm_get(uint16_t conn_handle) +{ + struct ble_svc_ras_sm *rassm = NULL; + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ras_sm); i++) { + if (g_ble_ras_sm[i].conn_handle == conn_handle) { + rassm = &g_ble_ras_sm[i]; + break; + } + } + + return rassm; +} + +int +ble_svc_ras_ranging_data_body_init(uint16_t conn_handle, uint16_t procedure_counter, + uint8_t config_id, uint8_t tx_power, uint8_t antenna_paths_mask) +{ + struct ble_svc_ras_sm *rassm; + struct os_mbuf *om; + uint8_t *buf; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + assert(rassm->ranging_data_body_mbuf == NULL); + + om = os_mbuf_get_pkthdr(&ras_mbuf_pool, 0); + assert(om != NULL); + rassm->ranging_counter = procedure_counter & 0x0FFF; + rassm->ranging_data_body_mbuf = om; + + buf = os_mbuf_extend(om, 4); + assert(buf != NULL); + + put_le16(buf, ((uint16_t)config_id << 12) | (rassm->ranging_counter & 0x0FFF)); + buf[2] = tx_power; + buf[3] = antenna_paths_mask; + + return 0; +} + +int +ble_svc_ras_ranging_subevent_init(uint16_t conn_handle, + uint16_t start_acl_conn_event, + uint16_t frequency_compensation, + uint8_t ranging_done_status, + uint8_t subevent_done_status, + uint8_t ranging_abort_reason, + uint8_t subevent_abort_reason, + uint8_t reference_power_level, + uint8_t number_of_steps_reported) +{ + struct ble_svc_ras_sm *rassm; + struct os_mbuf *om; + uint8_t *buf; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + om = rassm->ranging_data_body_mbuf; + buf = os_mbuf_extend(om, 8); + assert(buf != NULL); + + put_le16(buf, start_acl_conn_event); + put_le16(buf + 2, frequency_compensation); + buf[4] = ranging_done_status | subevent_done_status << 4; + buf[5] = ranging_abort_reason | subevent_abort_reason << 4; + buf[6] = reference_power_level; + buf[7] = number_of_steps_reported; + + rassm->cur_subevent_header_ptr = buf; + + return 0; +} + +int +ble_svc_ras_ranging_subevent_update_status(uint16_t conn_handle, + uint8_t number_of_steps_reported, + uint8_t ranging_done_status, + uint8_t subevent_done_status, + uint8_t ranging_abort_reason, + uint8_t subevent_abort_reason) +{ + struct ble_svc_ras_sm *rassm; + uint8_t *buf; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + buf = rassm->cur_subevent_header_ptr; + assert(buf != NULL); + + buf[4] = ranging_done_status | subevent_done_status << 4; + buf[5] = ranging_abort_reason | subevent_abort_reason << 4; + buf[7] += number_of_steps_reported; + + return 0; +} + +int +ble_svc_ras_add_step_mode(uint16_t conn_handle, uint8_t mode, uint8_t status) +{ + int rc; + struct ble_svc_ras_sm *rassm; + struct os_mbuf *om; + uint8_t step_mode = mode | (status << 7); + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + om = rassm->ranging_data_body_mbuf; + + rc = os_mbuf_append(om, &step_mode, sizeof(step_mode)); + assert(rc == 0); + + return rc; +} + +int +ble_svc_ras_add_mode0_result(struct ble_cs_mode0_result *result, uint16_t conn_handle, uint8_t local_role) +{ + int rc; + struct ble_svc_ras_sm *rassm; + struct os_mbuf *om; + uint8_t buf[5]; + uint8_t i = 0; + uint16_t filter; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + om = rassm->ranging_data_body_mbuf; + filter = rassm->filter_bitmask[0]; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_QUALITY)) { + buf[i++] = result->packet_quality; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_RSSI)) { + buf[i++] = result->packet_rssi; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_ANTENNA)) { + buf[i++] = result->packet_antenna; + } + + if ((filter & BIT(BLE_SVC_RAS_FILTER_MODE0_MEASURED_FREQ_OFFSET)) && + local_role == BLE_HS_CS_ROLE_INITIATOR) { + put_le16(buf + i, result->measured_freq_offset); + i += 2; + } + + rc = os_mbuf_append(om, buf, i); + assert(rc == 0); + + return rc; +} + +int +ble_svc_ras_add_mode1_result(struct ble_cs_mode1_result *result, uint16_t conn_handle, uint8_t rtt_pct_included) +{ + struct ble_svc_ras_sm *rassm; + struct os_mbuf *om; + struct os_mbuf *om2; + uint8_t *buf; + uint16_t filter; + uint8_t i = 0; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + om = rassm->ranging_data_body_mbuf; + filter = rassm->filter_bitmask[1]; + + om2 = os_mbuf_get(&ras_mbuf_pool, 0); + assert(om2 != NULL); + + buf = os_mbuf_extend(om2, 14); + assert(buf != NULL); + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_QUALITY)) { + buf[i++] = result->packet_quality; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_NADM)) { + buf[i++] = result->packet_nadm; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_RSSI)) { + buf[i++] = result->packet_rssi; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_TOD_TOA)) { + put_le16(buf + i, result->toa_tod); + i += 2; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_ANTENNA)) { + buf[i++] = result->packet_antenna; + } + + if (rtt_pct_included) { + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_PCT1)) { + put_le32(buf + i, result->packet_pct1); + i += 4; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_PCT2)) { + put_le32(buf + i, result->packet_pct2); + i += 4; + } + } + + if (i < 14) { + os_mbuf_adj(om2, 14 - i); + } + + os_mbuf_pack_chains(om, om2); + + return 0; +} + +int +ble_svc_ras_add_mode2_result(struct ble_cs_mode2_result *result, uint16_t conn_handle, uint8_t n_ap) +{ + struct ble_svc_ras_sm *rassm; + struct os_mbuf *om; + struct os_mbuf *om2; + uint8_t *buf; + uint16_t filter; + uint8_t i = 0; + uint8_t k = 0; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + om = rassm->ranging_data_body_mbuf; + filter = rassm->filter_bitmask[2]; + + om2 = os_mbuf_get(&ras_mbuf_pool, 0); + assert(om2 != NULL); + + buf = os_mbuf_extend(om2, 25); + assert(buf != NULL); + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PERMUTATION_ID)) { + buf[i++] = result->antenna_path_permutation_id; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_TONE_PCT)) { + for (k = 0; k < n_ap + 1; ++k) { + put_le24(buf + i, result->tone_pct[k]); + i += 3; + } + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_TONE_QUALITY)) { + for (k = 0; k < n_ap + 1; ++k) { + buf[i++] = result->tone_quality_ind[k]; + } + } + + k = 0; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_1)) { + buf[i++] = result->antenna_paths[k++]; + } + + if (n_ap > 1 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_2))) { + buf[i++] = result->antenna_paths[k++]; + } + + if (n_ap > 2 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_3))) { + buf[i++] = result->antenna_paths[k++]; + } + + if (n_ap > 3 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_4))) { + buf[i++] = result->antenna_paths[k++]; + } + + if (i < 25) { + os_mbuf_adj(om2, 25 - i); + } + + os_mbuf_pack_chains(om, om2); + + return 0; +} + +int +ble_svc_ras_add_mode3_result(struct ble_cs_mode3_result *result, uint16_t conn_handle, + uint8_t n_ap, uint8_t rtt_pct_included) +{ + struct ble_svc_ras_sm *rassm; + struct os_mbuf *om; + struct os_mbuf *om2; + uint8_t *buf; + uint16_t filter; + uint8_t i = 0; + uint8_t k = 0; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + om = rassm->ranging_data_body_mbuf; + filter = rassm->filter_bitmask[3]; + + om2 = os_mbuf_get(&ras_mbuf_pool, 0); + assert(om2 != NULL); + + buf = os_mbuf_extend(om2, 39); + assert(buf != NULL); + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_QUALITY)) { + buf[i++] = result->packet_quality; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_NADM)) { + buf[i++] = result->packet_nadm; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_RSSI)) { + buf[i++] = result->packet_rssi; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TOD_TOA)) { + put_le16(buf + i, result->toa_tod); + i += 2; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_ANTENNA)) { + buf[i++] = result->packet_antenna; + } + + if (rtt_pct_included) { + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_PCT1)) { + put_le32(buf + i, result->packet_pct1); + i += 4; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_PCT2)) { + put_le32(buf + i, result->packet_pct2); + i += 4; + } + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PERMUTATION_ID)) { + buf[i++] = result->antenna_path_permutation_id; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TONE_PCT)) { + for (k = 0; k < n_ap + 1; ++k) { + put_le24(buf + i, result->tone_pct[k]); + i += 3; + } + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TONE_QUALITY)) { + for (k = 0; k < n_ap + 1; ++k) { + buf[i++] = result->tone_quality_ind[k]; + } + } + + k = 0; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_1)) { + buf[i++] = result->antenna_paths[k++]; + } + + if (n_ap > 1 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_2))) { + buf[i++] = result->antenna_paths[k++]; + } + + if (n_ap > 2 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_3))) { + buf[i++] = result->antenna_paths[k++]; + } + + if (n_ap > 3 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_4))) { + buf[i++] = result->antenna_paths[k++]; + } + + if (i < 39) { + os_mbuf_adj(om, 39 - i); + } + + os_mbuf_pack_chains(om, om2); + + return 0; +} + +static int +ble_svc_ras_ranging_data_notify(struct ble_svc_ras_sm *rassm, uint8_t notify, uint8_t indicate) +{ + int rc; + int len; + struct os_mbuf *om; + struct os_mbuf *om2; + uint8_t *buf; + uint16_t val_handle; + uint8_t segment_header; + + if (((rassm->notify & BIT(notify)) == 0) && + ((rassm->indicate & BIT(indicate)) == 0)) { + return 1; + } + + om = rassm->ranging_data_body_mbuf; + assert(om != NULL); + + segment_header = rassm->rolling_segment_counter << 2; + + if (rassm->remaining_data_len == 0) { + rassm->remaining_data_len = OS_MBUF_PKTLEN(om); + rassm->sent_data_len = 0; + rassm->segment_counter = 0; + /* Set the First Segment bit */ + segment_header |= 0b01; + } + + len = min(rassm->mtu - 4, rassm->remaining_data_len + 1) - 1; + rassm->remaining_data_len -= len; + + om2 = os_mbuf_get(&ras_mbuf_pool, 0); + assert(om2 != NULL); + + buf = os_mbuf_extend(om2, len + 1); + assert(buf != NULL); + + if (rassm->remaining_data_len == 0) { + /* Set the Last Segment bit */ + segment_header |= 0b10; + } + buf[0] = segment_header; + + rc = os_mbuf_copydata(om, rassm->sent_data_len, len, &buf[1]); + assert(rc == 0); + + ++rassm->segment_counter; + rassm->sent_data_len += len; + rassm->rolling_segment_counter = (rassm->rolling_segment_counter + 1) % 64; + + if (notify == NOTIFY_REAL_TIME_RANGING_DATA) { + val_handle = ble_svc_ras_real_time_ranging_data_val_handle; + } else { + val_handle = ble_svc_ras_on_demand_ranging_data_val_handle; + } + + if (rassm->notify & BIT(notify)) { + rc = ble_gatts_notify_custom(rassm->conn_handle, val_handle, om2); + } else { + rc = ble_gatts_indicate_custom(rassm->conn_handle, val_handle, om2); + } + + return rc; +} + +static int +ble_svc_ras_features_access(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + struct ble_svc_ras_sm *rassm; + uint8_t *buf; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + switch (ctxt->op) { + case BLE_GATT_ACCESS_OP_READ_CHR: + buf = os_mbuf_extend(ctxt->om, 4); + if (buf == NULL) { + return BLE_ATT_ERR_INSUFFICIENT_RES; + } + + put_le32(buf, rassm->supported_features); + break; + default: + return BLE_ATT_ERR_UNLIKELY; + } + + return 0; +} + +static int +ble_svc_ras_cp_rsp_complete_ranging_data_notify(struct ble_svc_ras_sm *rassm) +{ + int rc; + struct os_mbuf *om; + uint8_t *buf; + + if ((rassm->indicate & BIT(INDICATE_CONTROL_POINT)) == 0) { + return 1; + } + + om = os_msys_get_pkthdr(3, 0); + assert(om != NULL); + + buf = os_mbuf_extend(om, 3); + assert(buf != NULL); + + buf[0] = BLE_SVC_RAS_CP_RSP_COMPLETE_RANGING_DATA; + put_le16(buf + 1, rassm->ranging_counter); + + rc = ble_gatts_indicate_custom(rassm->conn_handle, ble_svc_ras_control_point_val_handle, om); + + return rc; +} + +static int +ble_svc_ras_cp_rsp_complete_lost_segment_notify(struct ble_svc_ras_sm *rassm, uint16_t ranging_counter, + uint8_t first_segment_id, uint8_t last_segment_id) +{ + int rc; + struct os_mbuf *om; + uint8_t *buf; + + if ((rassm->indicate & BIT(INDICATE_CONTROL_POINT)) == 0) { + return 1; + } + + om = os_msys_get_pkthdr(5, 0); + assert(om != NULL); + + buf = os_mbuf_extend(om, 5); + assert(buf != NULL); + + buf[0] = BLE_SVC_RAS_CP_RSP_COMPLETE_LOST_SEGMENT; + put_le16(buf + 1, ranging_counter); + buf[3] = first_segment_id; + buf[4] = last_segment_id; + + rc = ble_gatts_indicate_custom(rassm->conn_handle, ble_svc_ras_control_point_val_handle, om); + + return rc; +} + +static int +ble_svc_ras_cp_rsp_response_code_notify(struct ble_svc_ras_sm *rassm, uint8_t rsp_code) +{ + int rc; + struct os_mbuf *om; + uint8_t *buf; + + if ((rassm->indicate & BIT(INDICATE_CONTROL_POINT)) == 0) { + return 1; + } + + om = os_msys_get_pkthdr(2, 0); + assert(om != NULL); + + buf = os_mbuf_extend(om, 2); + assert(buf != NULL); + + buf[0] = BLE_SVC_RAS_CP_RSP_RESPONSE_CODE; + buf[1] = rsp_code; + + rc = ble_gatts_indicate_custom(rassm->conn_handle, ble_svc_ras_control_point_val_handle, om); + + return rc; +} + +static int +ras_cp_cmd_get_ranging_data(struct ble_svc_ras_sm *rassm, const struct os_mbuf *om, int off) +{ + int rc; + uint16_t ranging_counter; + + rc = os_mbuf_copydata(om, off, sizeof(ranging_counter), &ranging_counter); + if (rc != 0){ + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + ranging_counter = get_le16(&ranging_counter); + + if (rassm->ranging_counter != ranging_counter) { + return ble_svc_ras_cp_rsp_response_code_notify( + rassm, BLE_SVC_RAS_CP_RSPCODE_NO_RECORDS_FOUND); + } + + rassm->busy = 1; + + return ble_svc_ras_ranging_data_notify(rassm, NOTIFY_ON_DEMAND_RANGING_DATA, + INDICATE_ON_DEMAND_RANGING_DATA); +} + +static int +ras_cp_cmd_ack_ranging_data(struct ble_svc_ras_sm *rassm, const struct os_mbuf *om, int off) +{ + int rc; + uint16_t ranging_counter; + + rc = os_mbuf_copydata(om, off, sizeof(ranging_counter), &ranging_counter); + if (rc != 0){ + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + ranging_counter = get_le16(&ranging_counter); + + return ble_svc_ras_cp_rsp_response_code_notify(rassm, BLE_SVC_RAS_CP_RSPCODE_SUCCESS); +} + +static int +ras_cp_cmd_retrieve_lost_segment(struct ble_svc_ras_sm *rassm, const struct os_mbuf *om, int off) +{ + int rc; + uint8_t buf[4]; + uint16_t ranging_counter; + uint8_t first_segment_id; + uint8_t last_segment_id; + + rc = os_mbuf_copydata(om, off, 4, buf); + if (rc != 0){ + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + + ranging_counter = get_le16(buf); + first_segment_id = buf[2]; + last_segment_id = buf[3]; + /* TODO */ + (void) ranging_counter; + (void) first_segment_id; + (void) last_segment_id; + + return ble_svc_ras_cp_rsp_response_code_notify( + rassm, BLE_SVC_RAS_CP_RSPCODE_OP_CODE_NOT_SUPPORTED); +} + +static void +ble_svc_ras_flush(struct ble_svc_ras_sm *rassm) +{ + if (rassm->ranging_data_body_mbuf != NULL) { + os_mbuf_free_chain(rassm->ranging_data_body_mbuf); + rassm->ranging_data_body_mbuf = NULL; + rassm->cur_subevent_header_ptr = NULL; + } +} + +static int +ras_cp_cmd_abort_operation(struct ble_svc_ras_sm *rassm) +{ + ble_svc_ras_flush(rassm); + + return ble_svc_ras_cp_rsp_response_code_notify(rassm, BLE_SVC_RAS_CP_RSPCODE_SUCCESS); +} + +static int +ras_cp_cmd_set_filter(struct ble_svc_ras_sm *rassm, const struct os_mbuf *om, int off) +{ + int rc; + uint16_t filter_config; + uint16_t filter; + uint8_t mode; + + rc = os_mbuf_copydata(om, off, sizeof(filter_config), &filter_config); + if (rc != 0){ + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + + filter_config = get_le16(&filter_config); + mode = filter_config & 0b11; + filter = filter_config >> 2; + rassm->filter_bitmask[mode] = filter; + + return ble_svc_ras_cp_rsp_response_code_notify(rassm, BLE_SVC_RAS_CP_RSPCODE_SUCCESS); +} + +static int +ble_svc_ras_control_point_access(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + int rc; + struct ble_svc_ras_sm *rassm; + uint8_t opcode; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + if (ctxt->op != BLE_GATT_ACCESS_OP_WRITE_CHR) { + return BLE_ATT_ERR_UNLIKELY; + } + + rc = os_mbuf_copydata(ctxt->om, 0, sizeof(opcode), &opcode); + if (rc != 0){ + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + + if (rassm->busy && opcode != BLE_SVC_RAS_CP_CMD_ABORT_OPERATION) { + return ble_svc_ras_cp_rsp_response_code_notify(rassm, BLE_SVC_RAS_CP_RSPCODE_SERVER_BUSY); + } + + switch (opcode) { + case BLE_SVC_RAS_CP_CMD_GET_RANGING_DATA: + rc = ras_cp_cmd_get_ranging_data(rassm, ctxt->om, sizeof(opcode)); + break; + case BLE_SVC_RAS_CP_CMD_ACK_RANGING_DATA: + rc = ras_cp_cmd_ack_ranging_data(rassm, ctxt->om, sizeof(opcode)); + break; + case BLE_SVC_RAS_CP_CMD_RETRIEVE_LOST_SEGMENT: + rc = ras_cp_cmd_retrieve_lost_segment(rassm, ctxt->om, sizeof(opcode)); + break; + case BLE_SVC_RAS_CP_CMD_ABORT_OPERATION: + rc = ras_cp_cmd_abort_operation(rassm); + break; + case BLE_SVC_RAS_CP_CMD_SET_FILTER: + rc = ras_cp_cmd_set_filter(rassm, ctxt->om, sizeof(opcode)); + break; + default: + return BLE_ATT_ERR_UNLIKELY; + } + + if (rc == BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN) { + return ble_svc_ras_cp_rsp_response_code_notify( + rassm, BLE_SVC_RAS_CP_RSPCODE_INVALID_PARAMETER); + } + + return 0; +} + +static int +ble_svc_ras_ranging_data_ready_notify(struct ble_svc_ras_sm *rassm) +{ + int rc; + struct os_mbuf *om; + uint8_t *buf; + + if (((rassm->notify & BIT(NOTIFY_RANGING_DATA_READY)) == 0) && + ((rassm->indicate & BIT(INDICATE_RANGING_DATA_READY)) == 0)) { + return 1; + } + + om = os_msys_get_pkthdr(2, 0); + if (!om) { + return 1; + } + + buf = os_mbuf_extend(om, 2); + assert(buf != NULL); + + put_le16(buf, rassm->ranging_counter); + + if (rassm->notify & BIT(NOTIFY_RANGING_DATA_READY)) { + rc = ble_gatts_notify_custom(rassm->conn_handle, + ble_svc_ras_ranging_data_ready_val_handle, om); + } else { + rc = ble_gatts_indicate_custom(rassm->conn_handle, + ble_svc_ras_ranging_data_ready_val_handle, om); + } + + return rc; +} + +int +ble_svc_ras_ranging_data_ready(uint16_t conn_handle) +{ + struct ble_svc_ras_sm *rassm; + + rassm = ble_svc_ras_sm_get(conn_handle); + assert(rassm != NULL); + + if ((rassm->notify & BIT(NOTIFY_RANGING_DATA_READY)) || + (rassm->indicate & BIT(INDICATE_RANGING_DATA_READY))) { + return ble_svc_ras_ranging_data_ready_notify(rassm); + } + + if ((rassm->notify & BIT(NOTIFY_REAL_TIME_RANGING_DATA)) || + (rassm->indicate & BIT(INDICATE_REAL_TIME_RANGING_DATA))) { + return ble_svc_ras_ranging_data_notify(rassm, NOTIFY_REAL_TIME_RANGING_DATA, + INDICATE_REAL_TIME_RANGING_DATA); + } + + return 0; +} + +static int +ble_svc_ras_ranging_data_ready_access(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + struct ble_svc_ras_sm *rassm; + uint8_t *buf; + + rassm = ble_svc_ras_sm_get(conn_handle); + + switch (ctxt->op) { + case BLE_GATT_ACCESS_OP_READ_CHR: + buf = os_mbuf_extend(ctxt->om, 2); + if (buf == NULL) { + return BLE_ATT_ERR_INSUFFICIENT_RES; + } + + put_le16(buf, rassm->ranging_counter); + break; + default: + return BLE_ATT_ERR_UNLIKELY; + } + + return 0; +} + +static int +ble_svc_ras_ranging_data_overwritten_notify(struct ble_svc_ras_sm *rassm, uint32_t toa_tod_val) +{ + int rc; + struct os_mbuf *om; + uint8_t *buf; + + if (((rassm->notify & BIT(NOTIFY_RANGING_DATA_OVERWRITTEN)) == 0) && + ((rassm->indicate & BIT(INDICATE_RANGING_DATA_OVERWRITTEN)) == 0)) { + return 1; + } + + om = os_msys_get_pkthdr(2, 0); + if (!om) { + return 1; + } + + buf = os_mbuf_extend(om, 2); + assert(buf != NULL); + + put_le16(buf, rassm->data_overwritten_counter); + + if (rassm->notify & BIT(NOTIFY_RANGING_DATA_OVERWRITTEN)) { + rc = ble_gatts_notify_custom(rassm->conn_handle, + ble_svc_ras_ranging_data_overwritten_val_handle, om); + } else { + rc = ble_gatts_indicate_custom(rassm->conn_handle, + ble_svc_ras_ranging_data_overwritten_val_handle, om); + } + + return rc; +} + +static int +ble_svc_ras_ranging_data_overwritten_access(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + struct ble_svc_ras_sm *rassm; + uint8_t *buf; + + rassm = ble_svc_ras_sm_get(conn_handle); + + switch (ctxt->op) { + case BLE_GATT_ACCESS_OP_READ_CHR: + buf = os_mbuf_extend(ctxt->om, 2); + if (buf == NULL) { + return BLE_ATT_ERR_INSUFFICIENT_RES; + } + + put_le16(buf, rassm->data_overwritten_counter); + break; + default: + return BLE_ATT_ERR_UNLIKELY; + } + + return 0; +} + +static int +gatt_forbidden_access_cb(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + return BLE_ATT_ERR_UNLIKELY; +} + +static const struct ble_gatt_svc_def gatt_svr_svcs[] = { + { + /*** Service: ToA_ToD samples */ + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = &ble_svc_ras_ranging_service_uuid.u, + .characteristics = (struct ble_gatt_chr_def[]) { + { + .uuid = &ble_svc_ras_features_uuid.u, + .access_cb = ble_svc_ras_features_access, + .val_handle = &ble_svc_ras_features_val_handle, + .flags = BLE_GATT_CHR_F_READ, + }, + { + .uuid = &ble_svc_ras_real_time_ranging_data_uuid.u, + .access_cb = gatt_forbidden_access_cb, + .val_handle = &ble_svc_ras_real_time_ranging_data_val_handle, + .flags = BLE_GATT_CHR_F_INDICATE | BLE_GATT_CHR_F_NOTIFY, + }, + { + .uuid = &ble_svc_ras_on_demand_ranging_data_uuid.u, + .access_cb = gatt_forbidden_access_cb, + .val_handle = &ble_svc_ras_on_demand_ranging_data_val_handle, + .flags = BLE_GATT_CHR_F_INDICATE | BLE_GATT_CHR_F_NOTIFY, + }, + { + .uuid = &ble_svc_ras_control_point_uuid.u, + .access_cb = ble_svc_ras_control_point_access, + .val_handle = &ble_svc_ras_control_point_val_handle, + .flags = BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_INDICATE, + }, + { + .uuid = &ble_svc_ras_ranging_data_ready_uuid.u, + .access_cb = ble_svc_ras_ranging_data_ready_access, + .val_handle = &ble_svc_ras_ranging_data_ready_val_handle, + .flags = BLE_GATT_CHR_F_INDICATE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_READ, + }, + { + .uuid = &ble_svc_ras_ranging_data_overwritten_uuid.u, + .access_cb = ble_svc_ras_ranging_data_overwritten_access, + .val_handle = &ble_svc_ras_ranging_data_overwritten_val_handle, + .flags = BLE_GATT_CHR_F_INDICATE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_READ, + }, + { + 0, /* No more characteristics in this service. */ + } + } + }, + { + 0, /* No more services. */ + }, +}; + +static void +ble_svc_ras_on_disconnect(uint16_t conn_handle) +{ + struct ble_svc_ras_sm *rassm; + + rassm = ble_svc_ras_sm_get(conn_handle); + if (rassm == NULL) { + return; + } + + rassm->conn_handle = conn_handle; + + ble_svc_ras_flush(rassm); +} + +static void +ble_svc_ras_on_connect(struct ble_gap_event *event) +{ + int rc; + struct ble_l2cap_chan *chan; + struct ble_svc_ras_sm *rassm = NULL; + uint16_t conn_handle; + uint8_t i; + + if (event->connect.status) { + return; + } + + conn_handle = event->connect.conn_handle; + + rassm = ble_svc_ras_sm_get(BLE_CONN_HANDLE_INVALID); + assert(rassm != NULL); + assert(rassm->ranging_data_body_mbuf == NULL); + memset(rassm, 0, sizeof(*rassm)); + rassm->conn_handle = conn_handle; + rassm->supported_features = 0b1111; + + rc = ble_att_conn_chan_find(conn_handle, BLE_L2CAP_CID_ATT, NULL, &chan); + if (rc == 0) { + rassm->mtu = ble_att_chan_mtu(chan); + } + + for (i = 0; i < 4; i++) { + rassm->filter_bitmask[i] = 0xffff; + } +} + +static void +ble_svc_ras_on_mtu(struct ble_gap_event *event) +{ + struct ble_svc_ras_sm *rassm; + + rassm = ble_svc_ras_sm_get(event->mtu.conn_handle); + if (rassm == NULL || event->mtu.channel_id != BLE_L2CAP_CID_ATT) { + return; + } + + rassm->mtu = event->mtu.value; +} + +static void +ble_svc_ras_on_notify_tx(struct ble_gap_event *event) +{ + struct ble_svc_ras_sm *rassm; + uint16_t attr_handle = event->notify_tx.attr_handle; + uint8_t notify; + uint8_t indicate; + + if (event->notify_tx.status != BLE_HS_EDONE) { + return; + } + + rassm = ble_svc_ras_sm_get(event->notify_tx.conn_handle); + if (rassm == NULL) { + return; + } + + if (attr_handle == ble_svc_ras_real_time_ranging_data_val_handle) { + notify = NOTIFY_REAL_TIME_RANGING_DATA; + indicate = INDICATE_REAL_TIME_RANGING_DATA; + } else if (attr_handle == ble_svc_ras_on_demand_ranging_data_val_handle) { + notify = NOTIFY_ON_DEMAND_RANGING_DATA; + indicate = INDICATE_ON_DEMAND_RANGING_DATA; + } else { + return; + } + + if (rassm->remaining_data_len > 0) { + ble_svc_ras_ranging_data_notify(rassm, notify, indicate); + } else { + ble_svc_ras_cp_rsp_complete_ranging_data_notify(rassm); + rassm->busy = 0; + } +} + +static int +ble_svc_ras_on_subscribe(struct ble_gap_event *event) +{ + struct ble_svc_ras_sm *rassm; + uint8_t notify = NOTIFY_INVALID; + uint8_t indicate = INDICATE_INVALID; + + rassm = ble_svc_ras_sm_get(event->subscribe.conn_handle); + if (rassm == NULL) { + return 0; + } + + if (event->subscribe.attr_handle == ble_svc_ras_real_time_ranging_data_val_handle) { + if (rassm->notify & BIT(NOTIFY_ON_DEMAND_RANGING_DATA) || + rassm->indicate & BIT(INDICATE_ON_DEMAND_RANGING_DATA)) { + /* The RAS Server shall operate in either Real-time or On-demand mode, + * but not both simultaneously. + */ + return BLE_ATT_ERR_CCCD_IMPORER_CONF; + } + notify = NOTIFY_REAL_TIME_RANGING_DATA; + indicate = INDICATE_REAL_TIME_RANGING_DATA; + } else if (event->subscribe.attr_handle == ble_svc_ras_on_demand_ranging_data_val_handle) { + if (rassm->notify & BIT(NOTIFY_REAL_TIME_RANGING_DATA) || + rassm->indicate & BIT(INDICATE_REAL_TIME_RANGING_DATA)) { + return BLE_ATT_ERR_CCCD_IMPORER_CONF; + } + notify = NOTIFY_ON_DEMAND_RANGING_DATA; + indicate = INDICATE_ON_DEMAND_RANGING_DATA; + } else if (event->subscribe.attr_handle == ble_svc_ras_control_point_val_handle) { + indicate = INDICATE_CONTROL_POINT; + } else if (event->subscribe.attr_handle == ble_svc_ras_ranging_data_ready_val_handle) { + notify = NOTIFY_RANGING_DATA_READY; + indicate = INDICATE_RANGING_DATA_READY; + } else if (event->subscribe.attr_handle == ble_svc_ras_ranging_data_overwritten_val_handle) { + notify = NOTIFY_RANGING_DATA_OVERWRITTEN; + indicate = INDICATE_RANGING_DATA_OVERWRITTEN; + } + + if (notify != NOTIFY_INVALID) { + if (event->subscribe.cur_notify) { + rassm->notify |= BIT(notify); + } else if (event->subscribe.prev_notify) { + rassm->notify &= ~BIT(notify); + } + } + + if (indicate != INDICATE_INVALID) { + if (event->subscribe.cur_indicate) { + rassm->indicate |= BIT(indicate); + } else if (event->subscribe.prev_indicate) { + rassm->indicate &= ~BIT(indicate); + } + } + + return 0; +} + +static int +ble_svc_ras_gap_event(struct ble_gap_event *event, void *arg) +{ + int rc = 0; + + switch (event->type) { + case BLE_GAP_EVENT_CONNECT: + ble_svc_ras_on_connect(event); + break; + case BLE_GAP_EVENT_MTU: + ble_svc_ras_on_mtu(event); + break; + case BLE_GAP_EVENT_NOTIFY_TX: + ble_svc_ras_on_notify_tx(event); + break; + case BLE_GAP_EVENT_SUBSCRIBE: + rc = ble_svc_ras_on_subscribe(event); + /* TODO: We have to be able to reject a write to CCCD with + * Client Characteristic Configuration Descriptor Improperly Configured, + * but for now API does not allow that. + */ + break; + case BLE_GAP_EVENT_DISCONNECT: + ble_svc_ras_on_disconnect(event->disconnect.conn.conn_handle); + break; + default: + break; + } + + return rc; +} + +void +ble_svc_ras_init(void) +{ + int rc; + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ras_sm); i++) { + g_ble_ras_sm[i].conn_handle = BLE_CONN_HANDLE_INVALID; + } + + rc = os_mempool_init(&ras_mempool, MBUF_POOL_BUF_COUNT, + MBUF_POOL_BUF_SIZE, &ras_mbuf_area[0], "ras_mbuf_pool"); + assert(rc == 0); + + rc = os_mbuf_pool_init(&ras_mbuf_pool, &ras_mempool, + MBUF_POOL_BUF_SIZE, MBUF_POOL_BUF_COUNT); + assert(rc == 0); + + rc = ble_gatts_count_cfg(gatt_svr_svcs); + assert(rc == 0); + + rc = ble_gatts_add_svcs(gatt_svr_svcs); + assert(rc == 0); + + rc = ble_gap_event_listener_register(&ble_svc_ras_listener, ble_svc_ras_gap_event, NULL); + assert(rc == 0); +} diff --git a/nimble/host/services/ras/src/ble_svc_ras_clt.c b/nimble/host/services/ras/src/ble_svc_ras_clt.c new file mode 100644 index 0000000000..d992655648 --- /dev/null +++ b/nimble/host/services/ras/src/ble_svc_ras_clt.c @@ -0,0 +1,899 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you 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 "syscfg/syscfg.h" +#include "os/os_mbuf.h" +//#include "host/ble_hs_log.h" +#include "host/ble_hs.h" +#include "host/ble_cs.h" +#include "host/ble_uuid.h" +#include "host/ble_peer.h" +#include "nimble/hci_common.h" +#include "sys/queue.h" +//#include "ble_hs_priv.h" +//#include "ble_hs_hci_priv.h" +//#include "ble_cs_priv.h" +#include "services/ras/ble_svc_ras.h" +#include "bs_tracing.h" +//#include +//#include +//#include +//#include "bsp/bsp.h" +//#include "host/ble_hs.h" +//#include "host/ble_uuid.h" +//#include "cs_reflector.h" + +//struct ble_hs_cfg; +//struct ble_gatt_register_ctxt; + +#ifndef BIT +#define BIT(n) (1UL << (n)) +#endif + +#define MBUF_POOL_BUF_SIZE MYNEWT_VAL(BLE_SVC_RAS_TXRX_BUF_SIZE) +#define MBUF_POOL_BUF_COUNT MYNEWT_VAL(BLE_SVC_RAS_TXRX_BUF_COUNT) + +static struct os_mempool ras_clt_mempool; +static struct os_mbuf_pool ras_clt_mbuf_pool; +static uint8_t ras_clt_mbuf_area[MBUF_POOL_BUF_COUNT * MBUF_POOL_BUF_SIZE]; +#define SUBEVENT_BUF_LEN 40 +static uint8_t subevent_buf[SUBEVENT_BUF_LEN]; +static struct ble_gap_event_listener ble_svc_ras_clt_listener; + +struct ble_svc_ras_clt_sm { + int status; + struct os_mbuf *ranging_data_body_mbuf; + ble_svc_ras_clt_subscribe_cb *subscribe_cb; + ble_svc_ras_clt_step_data_received_cb *step_data_cb; + uint16_t real_time_ranging_data_val_handle; + uint16_t on_demand_ranging_data_val_handle; + uint16_t control_point_val_handle; + uint16_t ranging_data_ready_val_handle; + uint16_t ranging_data_overwritten_val_handle; + uint16_t filter_bitmask[4]; + uint16_t conn_handle; + uint16_t mtu; + uint16_t ranging_counter; + uint8_t rtt_pct_included; + uint8_t local_role; + uint8_t n_ap; +}; + +static struct ble_svc_ras_clt_sm g_ble_ras_clt_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)]; + +int ble_att_conn_chan_find(uint16_t conn_handle, uint16_t cid, + struct ble_hs_conn **out_conn, + struct ble_l2cap_chan **out_chan); +uint16_t ble_att_chan_mtu(const struct ble_l2cap_chan *chan); + +static struct ble_svc_ras_clt_sm * +ble_svc_ras_clt_sm_get(uint16_t conn_handle) +{ + struct ble_svc_ras_clt_sm *rassm = NULL; + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ras_clt_sm); i++) { + if (g_ble_ras_clt_sm[i].conn_handle == conn_handle) { + rassm = &g_ble_ras_clt_sm[i]; + break; + } + } + + return rassm; +} + +static int +ble_svc_ras_clt_mode0_parse(struct ble_svc_ras_clt_sm *rassm, struct os_mbuf *om, int *off) +{ + int rc; + uint8_t *buf = subevent_buf; + struct ble_cs_mode0_result result; + uint8_t len = 0; + uint8_t i = 0; + uint16_t filter = rassm->filter_bitmask[BLE_HS_CS_MODE0]; + uint8_t remote_initiator = rassm->local_role == BLE_HS_CS_ROLE_REFLECTOR; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_QUALITY)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_RSSI)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_ANTENNA)) len += 1; + if ((filter & BIT(BLE_SVC_RAS_FILTER_MODE0_MEASURED_FREQ_OFFSET)) && remote_initiator) len += 2; + + rc = os_mbuf_copydata(om, *off, len, buf); + if (rc) { + return rc; + } + + *off += len; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_QUALITY)) { + result.packet_quality = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_RSSI)) { + result.packet_rssi = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE0_PACKET_ANTENNA)) { + result.packet_antenna = buf[i++]; + } + + if ((filter & BIT(BLE_SVC_RAS_FILTER_MODE0_MEASURED_FREQ_OFFSET)) && remote_initiator) { + result.measured_freq_offset = get_le16(buf + i); + } + + rassm->step_data_cb(&result, rassm->conn_handle, BLE_HS_CS_MODE0); + + return rc; +} + +static int +ble_svc_ras_clt_mode1_parse(struct ble_svc_ras_clt_sm *rassm, struct os_mbuf *om, int *off) +{ + int rc; + uint8_t *buf = subevent_buf; + struct ble_cs_mode1_result result; + uint16_t filter = rassm->filter_bitmask[BLE_HS_CS_MODE1]; + uint8_t len = 0; + uint8_t i = 0; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_QUALITY)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_NADM)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_RSSI)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_TOD_TOA)) len += 2; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_ANTENNA)) len += 1; + if (rassm->rtt_pct_included) { + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_PCT1)) len += 4; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_PCT2)) len += 4; + } + + rc = os_mbuf_copydata(om, *off, len, buf); + if (rc) { + return rc; + } + + *off += len; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_QUALITY)) { + result.packet_quality = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_NADM)) { + result.packet_nadm = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_RSSI)) { + result.packet_rssi = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_TOD_TOA)) { + result.toa_tod = get_le16(buf + i); + i += 2; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_ANTENNA)) { + result.packet_antenna = buf[i++]; + } + + if (rassm->rtt_pct_included) { + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_PCT1)) { + result.packet_pct1 = get_le32(buf + i); + i += 4; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE1_PACKET_PCT2)) { + result.packet_pct2 = get_le32(buf + i); + i += 4; + } + } + + rassm->step_data_cb(&result, rassm->conn_handle, BLE_HS_CS_MODE1); + + return 0; +} + +static int +ble_svc_ras_clt_mode2_parse(struct ble_svc_ras_clt_sm *rassm, struct os_mbuf *om, int *off) +{ + int rc; + uint8_t *buf = subevent_buf; + struct ble_cs_mode2_result result; + uint16_t filter = rassm->filter_bitmask[BLE_HS_CS_MODE2]; + uint8_t n_ap = rassm->n_ap; + uint8_t i = 0; + uint8_t k = 0; + uint8_t len = 0; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PERMUTATION_ID)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_TONE_PCT)) len += 3 * (n_ap + 1); + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_TONE_QUALITY)) len += n_ap + 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_1)) len += 1; + if (n_ap > 1 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_2))) len += 1; + if (n_ap > 2 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_3))) len += 1; + if (n_ap > 3 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_4))) len += 1; + + rc = os_mbuf_copydata(om, *off, len, buf); + if (rc) { + return rc; + } + + *off += len; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PERMUTATION_ID)) { + result.antenna_path_permutation_id = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_TONE_PCT)) { + for (k = 0; k < n_ap + 1; ++k) { + result.tone_pct[k] = get_le24(buf + i); + i += 3; + } + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_TONE_QUALITY)) { + for (k = 0; k < n_ap + 1; ++k) { + result.tone_quality_ind[k] = buf[i++]; + } + } + + k = 0; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_1)) { + result.antenna_paths[k++] = buf[i++]; + } + + if (n_ap > 1 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_2))) { + result.antenna_paths[k++] = buf[i++]; + } + + if (n_ap > 2 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_3))) { + result.antenna_paths[k++] = buf[i++]; + } + + if (n_ap > 3 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE2_ANTENNA_PATH_4))) { + result.antenna_paths[k++] = buf[i++]; + } + + rassm->step_data_cb(&result, rassm->conn_handle, BLE_HS_CS_MODE2); + + return 0; +} + +static int +ble_svc_ras_clt_mode3_parse(struct ble_svc_ras_clt_sm *rassm, struct os_mbuf *om, int *off) +{ + int rc; + uint8_t *buf = subevent_buf; + struct ble_cs_mode3_result result; + uint16_t filter = rassm->filter_bitmask[BLE_HS_CS_MODE3]; + uint8_t n_ap = rassm->n_ap; + uint8_t len = 0; + uint8_t i = 0; + uint8_t k = 0; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_QUALITY)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_NADM)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_RSSI)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TOD_TOA)) len += 2; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_ANTENNA)) len += 1; + if (rassm->rtt_pct_included) { + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_PCT1)) len += 4; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_PCT2)) len += 4; + } + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PERMUTATION_ID)) len += 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TONE_PCT)) len += 3 * (n_ap + 1); + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TONE_QUALITY)) len += n_ap + 1; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_1)) len += 1; + if (n_ap > 1 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_2))) len += 1; + if (n_ap > 2 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_3))) len += 1; + if (n_ap > 3 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_4))) len += 1; + + rc = os_mbuf_copydata(om, *off, len, buf); + if (rc) { + return rc; + } + + *off += len; + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_QUALITY)) { + result.packet_quality = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_NADM)) { + result.packet_nadm = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_RSSI)) { + result.packet_rssi = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TOD_TOA)) { + result.toa_tod = get_le16(buf + i); + i += 2; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_ANTENNA)) { + result.packet_antenna = buf[i++]; + } + + if (rassm->rtt_pct_included) { + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_PCT1)) { + result.packet_pct1 = get_le32(buf + i); + i += 4; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_PACKET_PCT2)) { + result.packet_pct2 = get_le32(buf + i); + i += 4; + } + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PERMUTATION_ID)) { + result.antenna_path_permutation_id = buf[i++]; + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TONE_PCT)) { + for (k = 0; k < n_ap + 1; ++k) { + result.tone_pct[k] = get_le24(buf + i); + i += 3; + } + } + + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_TONE_QUALITY)) { + for (k = 0; k < n_ap + 1; ++k) { + result.tone_quality_ind[k] = buf[i++]; + } + } + + k = 0; + if (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_1)) { + result.antenna_paths[k++] = buf[i++]; + } + + if (n_ap > 1 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_2))) { + result.antenna_paths[k++] = buf[i++]; + } + + if (n_ap > 2 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_3))) { + result.antenna_paths[k++] = buf[i++]; + } + + if (n_ap > 3 && (filter & BIT(BLE_SVC_RAS_FILTER_MODE3_ANTENNA_PATH_4))) { + result.antenna_paths[k++] = buf[i++]; + } + + rassm->step_data_cb(&result, rassm->conn_handle, BLE_HS_CS_MODE3); + + return 0; +} + +static int +ble_svc_ras_clt_subevent_data_parse(struct ble_svc_ras_clt_sm *rassm, struct os_mbuf *om, int *off, + uint8_t number_of_steps) +{ + int rc = 1; + uint8_t step_mode; + uint8_t i; + + for (i = 0; i < number_of_steps; ++i) { + rc = os_mbuf_copydata(om, *off, 1, &step_mode); + if (rc || step_mode > BLE_HS_CS_MODE3) { + return rc; + } + + *off += 1; + + switch (step_mode) { + case BLE_HS_CS_MODE0: + rc = ble_svc_ras_clt_mode0_parse(rassm, om, off); + break; + case BLE_HS_CS_MODE1: + rc = ble_svc_ras_clt_mode1_parse(rassm, om, off); + break; + case BLE_HS_CS_MODE2: + rc = ble_svc_ras_clt_mode2_parse(rassm, om, off); + break; + case BLE_HS_CS_MODE3: + rc = ble_svc_ras_clt_mode3_parse(rassm, om, off); + break; + default: + rc = 1; + } + + if (rc) { + return 0; + } + } + + return rc; +} + +static int +ble_svc_ras_clt_subevent_parse(struct ble_svc_ras_clt_subevent_header *header, + struct os_mbuf *om, int *off) +{ + int rc; + uint8_t *buf = subevent_buf; + + rc = os_mbuf_copydata(om, *off, 8, buf); + if (rc) { + return rc; + } + + *off += 8; + header->start_acl_conn_event = get_le16(buf); + header->frequency_compensation = get_le16(&buf[2]); + header->done_status = buf[4]; + header->abort_reason = buf[5]; + header->reference_power_level = buf[6]; + header->number_of_steps_reported = buf[7]; + + return rc; +} + +static int +ble_svc_ras_clt_ranging_header_parse(struct ble_svc_ras_clt_ranging_header *header, + struct os_mbuf *om, int *off) +{ + int rc; + uint8_t *buf = subevent_buf; + + rc = os_mbuf_copydata(om, *off, 4, buf); + if (rc) { + return rc; + } + + *off += 4; + header->ranging_counter = get_le16(buf) & 0x0FFF; + header->config_id = buf[1] >> 4; + header->tx_power = buf[2]; + header->antenna_paths_mask = buf[3]; + + return rc; +} + +static int +ble_svc_ras_clt_ranging_data_parse(struct ble_svc_ras_clt_sm *rassm, struct os_mbuf *om, + struct ble_svc_ras_clt_ranging_header *header) +{ + int rc; + int off = 0; + int buf_len = OS_MBUF_PKTLEN(om); + uint8_t *buf = subevent_buf; + struct ble_svc_ras_clt_subevent_header sub_header; + + rc = ble_svc_ras_clt_ranging_header_parse(header, om, &off); + if (rc) { + return rc; + } + + while (off < buf_len) { + ble_svc_ras_clt_subevent_parse(&sub_header, om, &off); + + rc = ble_svc_ras_clt_subevent_data_parse(rassm, om, &off, sub_header.number_of_steps_reported); + if (rc) { + return rc; + } + } + + return rc; +} + +static int +ble_svc_ras_clt_notification_parse(struct os_mbuf *src_om, uint16_t conn_handle) +{ + int rc; + struct os_mbuf *cache_om; + struct ble_svc_ras_clt_sm *rassm; + struct ble_svc_ras_clt_ranging_header ranging_header; + uint8_t segment_header; + uint8_t is_first_segment; + uint8_t is_last_segment; + + rassm = ble_svc_ras_clt_sm_get(conn_handle); + assert(rassm != NULL); + + rc = os_mbuf_copydata(src_om, 0, sizeof(segment_header), &segment_header); + if (rc != 0){ + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + + is_first_segment = segment_header & 0b01; + is_last_segment = segment_header & 0b10; + + if (is_first_segment) { + cache_om = os_mbuf_get_pkthdr(&ras_clt_mbuf_pool, 0); + assert(cache_om != NULL); + rassm->ranging_data_body_mbuf = cache_om; + } else { + cache_om = rassm->ranging_data_body_mbuf; + } + + rc = os_mbuf_appendfrom(cache_om, src_om, 1, OS_MBUF_PKTLEN(src_om) - 1); + + if (is_last_segment) { + ble_svc_ras_clt_ranging_data_parse(rassm, cache_om, &ranging_header); + } + + return 0; +} + +static void +ble_svc_ras_clt_on_disconnect(uint16_t conn_handle) +{ + struct ble_svc_ras_clt_sm *rassm; + + rassm = ble_svc_ras_clt_sm_get(conn_handle); + if (rassm == NULL) { + return; + } + + rassm->conn_handle = BLE_CONN_HANDLE_INVALID; + + if (rassm->ranging_data_body_mbuf != NULL) { + os_mbuf_free_chain(rassm->ranging_data_body_mbuf); + rassm->ranging_data_body_mbuf = NULL; + } +} + +static int +ble_svc_ras_clt_cp_write_flat(uint16_t conn_handle, const void *data, uint16_t data_len, + ble_gatt_attr_fn *cb, void *cb_arg) +{ + int rc; + const struct ble_peer *peer; + const struct ble_peer_chr *chr; + + peer = ble_peer_find(conn_handle); + if (peer == NULL) { + return 1; + } + + chr = ble_peer_chr_find_uuid(peer, BLE_UUID16_DECLARE(BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID), + BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_RAS_CONTROL_POINT_UUID)); + + rc = ble_gattc_write_flat(conn_handle, chr->chr.val_handle, data, data_len, cb, cb_arg); + + return rc; +} + +static int +ble_svc_ras_clt_cp_get_ranging_data(struct ble_svc_ras_clt_sm *rassm, uint16_t ranging_counter) +{ + uint8_t value[3]; + + value[0] = BLE_SVC_RAS_CP_CMD_GET_RANGING_DATA; + put_le16(value + 1, ranging_counter); + + return ble_svc_ras_clt_cp_write_flat(rassm->conn_handle, value, sizeof(value), NULL, NULL); +} + +static int +ble_svc_ras_clt_cp_ack_ranging_data(struct ble_svc_ras_clt_sm *rassm, uint16_t ranging_counter) +{ + uint8_t value[3]; + + value[0] = BLE_SVC_RAS_CP_CMD_ACK_RANGING_DATA; + put_le16(value + 1, ranging_counter); + + return ble_svc_ras_clt_cp_write_flat(rassm->conn_handle, value, sizeof(value), NULL, NULL); +} + +static int +ble_svc_ras_clt_cp_ack_retrieve_lost_segment(struct ble_svc_ras_clt_sm *rassm, + uint16_t ranging_counter, uint8_t first_segment_id, + uint8_t last_segment_id) +{ + uint8_t value[5]; + + value[0] = BLE_SVC_RAS_CP_CMD_RETRIEVE_LOST_SEGMENT; + put_le16(value + 1, ranging_counter); + value[3] = first_segment_id; + value[4] = last_segment_id; + + return ble_svc_ras_clt_cp_write_flat(rassm->conn_handle, value, sizeof(value), NULL, NULL); +} + +static int +ble_svc_ras_clt_cp_abort_operation(struct ble_svc_ras_clt_sm *rassm, uint16_t ranging_counter) +{ + uint8_t value = BLE_SVC_RAS_CP_CMD_ABORT_OPERATION; + + return ble_svc_ras_clt_cp_write_flat(rassm->conn_handle, &value, sizeof(value), NULL, NULL); +} + +static int +ble_svc_ras_clt_cp_set_filter(struct ble_svc_ras_clt_sm *rassm, uint8_t mode, uint8_t filter) +{ + uint8_t value[2]; + + assert(mode <= BLE_HS_CS_MODE3); + rassm->filter_bitmask[mode] = filter; + value[0] = BLE_SVC_RAS_CP_CMD_SET_FILTER; + value[1] = filter << 2 | (mode & 0b11); + + return ble_svc_ras_clt_cp_write_flat(rassm->conn_handle, value, sizeof(value), NULL, NULL); +} + +static void +ble_svc_ras_clt_on_connect(struct ble_gap_event *event) +{ + int rc; + struct ble_l2cap_chan *chan; + struct ble_svc_ras_clt_sm *rassm = NULL; + uint8_t i; + + if (event->connect.status) { + return; + } + + rassm = ble_svc_ras_clt_sm_get(BLE_CONN_HANDLE_INVALID); + assert(rassm != NULL); + assert(rassm->ranging_data_body_mbuf == NULL); + memset(rassm, 0, sizeof(*rassm)); + rassm->conn_handle = event->connect.conn_handle; +// rassm->data_overwritten_counter = 0; +// rassm->completed_procedures_counter = 0; +// rassm->rolling_segment_counter = 0; + +// ble_hs_lock(); + rc = ble_att_conn_chan_find(rassm->conn_handle, BLE_L2CAP_CID_ATT, NULL, &chan); + if (rc == 0) { + rassm->mtu = ble_att_chan_mtu(chan); + } +// ble_hs_unlock(); + + for (i = 0; i < 4; i++) { + rassm->filter_bitmask[i] = 0xffff; + } +} + +static void +ble_svc_ras_clt_on_mtu(struct ble_gap_event *event) +{ + struct ble_svc_ras_clt_sm *rassm; + + rassm = ble_svc_ras_clt_sm_get(event->mtu.conn_handle); + if (rassm == NULL || event->mtu.channel_id != BLE_L2CAP_CID_ATT) { + return; + } + + rassm->mtu = event->mtu.value; +} + +static int +ble_svc_ras_clt_cp_on_ranging_data_ready(struct ble_svc_ras_clt_sm *rassm, struct os_mbuf *om) +{ + int rc; + uint16_t ranging_counter; + + rc = os_mbuf_copydata(om, 0, sizeof(ranging_counter), &ranging_counter); + if (rc != 0){ + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + rassm->ranging_counter = get_le16(&ranging_counter); + + return ble_svc_ras_clt_cp_get_ranging_data(rassm, rassm->ranging_counter); +} + +static void +ble_svc_ras_clt_on_notify_rx(struct ble_gap_event *event) +{ + struct ble_svc_ras_clt_sm *rassm; + uint16_t attr_handle = event->notify_rx.attr_handle; + + rassm = ble_svc_ras_clt_sm_get(event->notify_rx.conn_handle); + if (rassm == NULL) { + return; + } + + if (attr_handle == rassm->ranging_data_ready_val_handle) { + ble_svc_ras_clt_cp_on_ranging_data_ready(rassm, event->notify_rx.om); + } else if (attr_handle == rassm->real_time_ranging_data_val_handle || + attr_handle == rassm->on_demand_ranging_data_val_handle) { + ble_svc_ras_clt_notification_parse(event->notify_rx.om, event->notify_rx.conn_handle); + } else if (attr_handle == rassm->ranging_data_overwritten_val_handle) { + } else if (attr_handle == rassm->control_point_val_handle) { + } +} + +static int +ble_svc_ras_clt_gap_event(struct ble_gap_event *event, void *arg) +{ + switch (event->type) { + case BLE_GAP_EVENT_CONNECT: + ble_svc_ras_clt_on_connect(event); + break; + case BLE_GAP_EVENT_MTU: + ble_svc_ras_clt_on_mtu(event); + break; + case BLE_GAP_EVENT_NOTIFY_RX: + ble_svc_ras_clt_on_notify_rx(event); + break; + case BLE_GAP_EVENT_DISCONNECT: + ble_svc_ras_clt_on_disconnect(event->disconnect.conn.conn_handle); + break; + default: + break; + } + + return 0; +} + +static int +ble_cs_subscribe_indication(struct ble_svc_ras_clt_sm *rassm, ble_gatt_attr_fn *cb, + uint16_t svc_uuid, uint16_t chr_uuid) +{ + int rc; + const struct ble_peer *peer; + const struct ble_peer_dsc *dsc; + uint8_t value[2]; + + peer = ble_peer_find(rassm->conn_handle); + if (peer == NULL) { + return 1; + } + + dsc = ble_peer_dsc_find_uuid(peer, BLE_UUID16_DECLARE(svc_uuid), BLE_UUID16_DECLARE(chr_uuid), + BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_CFG_UUID16)); + + value[0] = 0b10; + value[1] = 0; + rc = ble_gattc_write_flat(rassm->conn_handle, dsc->dsc.handle, value, sizeof(value), cb, rassm); + + return rc; +} + +static int +ble_cs_subscribe_control_point_cb(uint16_t conn_handle, const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, void *arg) +{ + struct ble_svc_ras_clt_sm *rassm = arg; + + if (rassm->subscribe_cb) { + rassm->subscribe_cb(conn_handle); + } + + return 0; +} + +static int +ble_cs_subscribe_ranging_data_overwritten_cb(uint16_t conn_handle, const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, void *arg) +{ + return ble_cs_subscribe_indication(arg, ble_cs_subscribe_control_point_cb, + BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID, + BLE_SVC_RAS_CHR_RAS_CONTROL_POINT_UUID); +} + +static int +ble_cs_subscribe_ranging_data_ready_cb(uint16_t conn_handle, const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, void *arg) +{ + return ble_cs_subscribe_indication(arg, ble_cs_subscribe_ranging_data_overwritten_cb, + BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID, + BLE_SVC_RAS_CHR_RANGING_DATA_OVERWRITTEN_UUID); +} + +static int +ble_cs_subscribe_on_demand_ranging_data_cb(uint16_t conn_handle, const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, void *arg) +{ + return ble_cs_subscribe_indication(arg, ble_cs_subscribe_ranging_data_ready_cb, + BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID, + BLE_SVC_RAS_CHR_RANGING_DATA_READY_UUID); +} + +static int +ble_cs_subscribe_real_time_ranging_data_cb(uint16_t conn_handle, const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, void *arg) +{ + return ble_cs_subscribe_indication(arg, ble_cs_subscribe_control_point_cb, + BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID, + BLE_SVC_RAS_CHR_RAS_CONTROL_POINT_UUID); +} + +int +ble_svc_ras_clt_config_set(uint16_t conn_handle, uint8_t rtt_pct_included, + uint8_t n_ap, uint8_t local_role) +{ + struct ble_svc_ras_clt_sm *rassm = NULL; + + rassm = ble_svc_ras_clt_sm_get(conn_handle); + assert(rassm != NULL); + + rassm->rtt_pct_included = rtt_pct_included; + rassm->n_ap = n_ap; + rassm->local_role = local_role; + + return 0; +} + +int +ble_svc_ras_clt_subscribe(ble_svc_ras_clt_subscribe_cb *subscribe_cb, + ble_svc_ras_clt_step_data_received_cb *step_data_cb, + uint16_t conn_handle, uint8_t mode) +{ + struct ble_svc_ras_clt_sm *rassm = NULL; + const struct ble_peer *peer; + const struct ble_peer_chr *chr; + ble_gatt_attr_fn *cb; + uint16_t chr_uuid; + + rassm = ble_svc_ras_clt_sm_get(conn_handle); + assert(rassm != NULL); + rassm->subscribe_cb = subscribe_cb; + rassm->step_data_cb = step_data_cb; + + peer = ble_peer_find(conn_handle); + if (peer == NULL) { + return 1; + } + + chr = ble_peer_chr_find_uuid(peer, BLE_UUID16_DECLARE(BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID), + BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_REAL_TIME_RANGING_DATA_UUID)); + rassm->real_time_ranging_data_val_handle = chr->chr.val_handle; + + chr = ble_peer_chr_find_uuid(peer, BLE_UUID16_DECLARE(BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID), + BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_ON_DEMAND_RANGING_DATA_UUID)); + rassm->on_demand_ranging_data_val_handle = chr->chr.val_handle; + + chr = ble_peer_chr_find_uuid(peer, BLE_UUID16_DECLARE(BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID), + BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_RAS_CONTROL_POINT_UUID)); + rassm->control_point_val_handle = chr->chr.val_handle; + + chr = ble_peer_chr_find_uuid(peer, BLE_UUID16_DECLARE(BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID), + BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_RANGING_DATA_READY_UUID)); + rassm->ranging_data_ready_val_handle = chr->chr.val_handle; + + chr = ble_peer_chr_find_uuid(peer, BLE_UUID16_DECLARE(BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID), + BLE_UUID16_DECLARE(BLE_SVC_RAS_CHR_RANGING_DATA_OVERWRITTEN_UUID)); + rassm->ranging_data_overwritten_val_handle = chr->chr.val_handle; + + if (mode == BLE_SVC_RAS_MODE_ON_DEMAND) { + cb = ble_cs_subscribe_on_demand_ranging_data_cb; + chr_uuid = BLE_SVC_RAS_CHR_ON_DEMAND_RANGING_DATA_UUID; + } else { + cb = ble_cs_subscribe_real_time_ranging_data_cb; + chr_uuid = BLE_SVC_RAS_CHR_REAL_TIME_RANGING_DATA_UUID; + } + + return ble_cs_subscribe_indication(rassm, cb, BLE_SVC_RAS_SVC_RANGING_SERVICE_UUID, chr_uuid); +} + +void +ble_svc_ras_clt_init(void) +{ + int rc; + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ras_clt_sm); i++) { + g_ble_ras_clt_sm[i].conn_handle = BLE_CONN_HANDLE_INVALID; + } + + rc = os_mempool_init(&ras_clt_mempool, MBUF_POOL_BUF_COUNT, + MBUF_POOL_BUF_SIZE, &ras_clt_mbuf_area[0], "ras_clt_mbuf_pool"); + assert(rc == 0); + + rc = os_mbuf_pool_init(&ras_clt_mbuf_pool, &ras_clt_mempool, + MBUF_POOL_BUF_SIZE, MBUF_POOL_BUF_COUNT); + assert(rc == 0); + + rc = ble_gap_event_listener_register(&ble_svc_ras_clt_listener, ble_svc_ras_clt_gap_event, NULL); + assert(rc == 0); +} diff --git a/nimble/host/services/ras/syscfg.yml b/nimble/host/services/ras/syscfg.yml new file mode 100644 index 0000000000..8a001c5650 --- /dev/null +++ b/nimble/host/services/ras/syscfg.yml @@ -0,0 +1,39 @@ +# Licensed to the Apache Software Foundation (ASF) under one +# or more contributor license agreements. See the NOTICE file +# distributed with this work for additional information +# regarding copyright ownership. The ASF licenses this file +# to you 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. +# + +syscfg.defs: + BLE_SVC_RAS_SERVER: + description: > + Enable Ranging Service server API. + value: 0 + BLE_SVC_RAS_CLIENT: + description: > + Enable Ranging Service client API. + value: 0 + BLE_SVC_RAS_SYSINIT_STAGE: + description: > + Sysinit stage for the Ranging Service. + value: 303 + BLE_SVC_RAS_TXRX_BUF_SIZE: + description: > + Notification/indication buffer size. + value: 320 + BLE_SVC_RAS_TXRX_BUF_COUNT: + description: > + Notification/indication buffer count. + value: 10 diff --git a/nimble/host/src/ble_cs.c b/nimble/host/src/ble_cs.c index cef622206b..cc6d372431 100644 --- a/nimble/host/src/ble_cs.c +++ b/nimble/host/src/ble_cs.c @@ -27,9 +27,30 @@ #include "host/ble_hs_log.h" #include "host/ble_hs.h" #include "host/ble_cs.h" +#include "host/ble_peer.h" #include "nimble/hci_common.h" #include "sys/queue.h" #include "ble_hs_hci_priv.h" +#include "ble_hs_priv.h" +#include "ble_cs_priv.h" +#if MYNEWT_VAL(BLE_SVC_RAS_CLIENT) || MYNEWT_VAL(BLE_SVC_RAS_SERVER) +#include "services/ras/ble_svc_ras.h" +#endif +#define IN_RANGE(_n, _min, _max) (((_n) >= (_min)) && ((_n) <= (_max))) +#define N_AP_MAX (4) + +#define BLE_CS_CONFIG_MAX_NUM 4 +static const uint8_t antenna_path_permutations[24][4] = { + {1, 2, 3, 4}, {2, 1, 3, 4}, {1, 3, 2, 4}, {3, 1, 2, 4}, {3, 2, 1, 4}, + {2, 3, 1, 4}, {1, 2, 4, 3}, {2, 1, 4, 3}, {1, 4, 2, 3}, {4, 1, 2, 3}, + {4, 2, 1, 3}, {2, 4, 1, 3}, {1, 4, 3, 2}, {4, 1, 3, 2}, {1, 3, 4, 2}, + {3, 1, 4, 2}, {3, 4, 1, 2}, {4, 3, 1, 2}, {4, 2, 3, 1}, {2, 4, 3, 1}, + {4, 3, 2, 1}, {3, 4, 2, 1}, {3, 2, 4, 1}, {2, 3, 4, 1} +}; +static uint8_t aci_to_num_of_paths[] = {1, 2, 3, 4, 2, 3, 4, 4}; +static uint8_t aci_to_antenna_paths_mask[] = {0b1, 0b11, 0b111, 0b1111, 0b11, 0b111, 0b1111, 0b1111}; + +static struct ble_gap_event_listener ble_cs_listener; struct ble_cs_rd_rem_supp_cap_cp { uint16_t conn_handle; @@ -115,7 +136,6 @@ struct ble_cs_create_config_cp { uint8_t channel_selection_type; uint8_t ch3c_shape; uint8_t ch3c_jump; - uint8_t companion_signal_enable; } __attribute__((packed)); struct ble_cs_remove_config_cp { @@ -166,21 +186,52 @@ struct ble_cs_proc_enable_cp { uint8_t enable; } __attribute__((packed)); -struct ble_cs_state { - uint8_t op; - ble_cs_event_fn *cb; - void *cb_arg; +struct ble_cs_config { + uint8_t local_role; + uint8_t rtt_type; + uint8_t rtt_pct_included; + uint8_t sounding_pct_estimate; + uint8_t antenna_config_id; +}; + +struct ble_cs_sm { + ble_cs_event_fn *event_cb; + void *event_cb_arg; + uint16_t conn_handle; + struct ble_cs_config config[BLE_CS_CONFIG_MAX_NUM]; + uint8_t active_config_id; + uint8_t pending_procedure; +}; +static struct ble_cs_sm g_ble_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)]; + +struct ble_cs_loc_supp_cap { + uint8_t rtt_pct; }; +static struct ble_cs_loc_supp_cap g_loc_supp_cap; -static struct ble_cs_state cs_state; +static struct ble_cs_sm * +ble_cs_sm_get(uint16_t conn_handle) +{ + struct ble_cs_sm *cssm = NULL; + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(g_ble_cs_sm); i++) { + if (g_ble_cs_sm[i].conn_handle == conn_handle) { + cssm = &g_ble_cs_sm[i]; + break; + } + } + + return cssm; +} static int -ble_cs_call_event_cb(struct ble_cs_event *event) +ble_cs_call_event_cb(struct ble_cs_sm *cssm, struct ble_cs_event *event) { int rc; - if (cs_state.cb != NULL) { - rc = cs_state.cb(event, cs_state.cb_arg); + if (cssm->event_cb != NULL) { + rc = cssm->event_cb(event, cssm->event_cb_arg); } else { rc = 0; } @@ -189,19 +240,57 @@ ble_cs_call_event_cb(struct ble_cs_event *event) } static void -ble_cs_call_procedure_complete_cb(uint16_t conn_handle, uint8_t status) +ble_cs_call_procedure_complete_cb(struct ble_cs_sm *cssm, uint8_t status) { struct ble_cs_event event; memset(&event, 0, sizeof event); + event.conn_handle = cssm->conn_handle; + event.status = status; event.type = BLE_CS_EVENT_CS_PROCEDURE_COMPLETE; - event.procedure_complete.conn_handle = conn_handle; - event.procedure_complete.status = status; - ble_cs_call_event_cb(&event); + ble_cs_call_event_cb(cssm, &event); +} + +static void +ble_cs_call_step_data_cb(struct ble_cs_sm *cssm, void *step_data, uint8_t status, + uint8_t step_mode, uint8_t role) +{ + struct ble_cs_event event; + + memset(&event, 0, sizeof event); + event.conn_handle = cssm->conn_handle; + event.status = status; + event.type = BLE_CS_EVENT_CS_STEP_DATA; + event.step_data.role = role; + event.step_data.mode = step_mode; + event.step_data.data = step_data; + ble_cs_call_event_cb(cssm, &event); +} + +#if MYNEWT_VAL(BLE_SVC_RAS_CLIENT) +static void +ble_cs_step_data_received_cb(void *data, uint16_t conn_handle, uint8_t step_mode) +{ + struct ble_cs_sm *cssm = NULL; + struct ble_cs_config *config; + uint8_t remote_role; + + if (data == NULL) { + return; + } + + cssm = ble_cs_sm_get(conn_handle); + config = &cssm->config[cssm->active_config_id]; + + remote_role = config->local_role == BLE_HS_CS_ROLE_INITIATOR ? + BLE_HS_CS_ROLE_REFLECTOR : BLE_HS_CS_ROLE_INITIATOR; + + ble_cs_call_step_data_cb(cssm, data, 0, step_mode, remote_role); } +#endif static int -ble_cs_rd_loc_supp_cap(void) +ble_cs_rd_loc_supp_cap(struct ble_cs_sm *cssm) { int rc; struct ble_hci_le_cs_rd_loc_supp_cap_rp rp; @@ -218,7 +307,8 @@ ble_cs_rd_loc_supp_cap(void) rp.optional_t_ip2_times_supported = le16toh(rp.optional_t_ip2_times_supported); rp.optional_t_fcs_times_supported = le16toh(rp.optional_t_fcs_times_supported); rp.optional_t_pm_times_supported = le16toh(rp.optional_t_pm_times_supported); - (void) rp; + + g_loc_supp_cap.rtt_pct = (rp.optional_subfeatures_supported >> 3) & 1; return rc; } @@ -361,7 +451,7 @@ ble_cs_create_config(const struct ble_cs_create_config_cp *cmd) cp.channel_selection_type = cmd->channel_selection_type; cp.ch3c_shape = cmd->ch3c_shape; cp.ch3c_jump = cmd->ch3c_jump; - cp.companion_signal_enable = cmd->companion_signal_enable; + cp.reserved = 0x00; return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CS_CREATE_CONFIG), @@ -447,6 +537,7 @@ ble_hs_hci_evt_le_cs_rd_rem_supp_cap_complete(uint8_t subevent, const void *data unsigned int len) { int rc; + struct ble_cs_sm *cssm = NULL; const struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete *ev = data; struct ble_cs_set_def_settings_cp set_cmd; struct ble_cs_set_def_settings_rp set_rsp; @@ -456,11 +547,18 @@ ble_hs_hci_evt_le_cs_rd_rem_supp_cap_complete(uint8_t subevent, const void *data return BLE_HS_ECONTROLLER; } + cssm = ble_cs_sm_get(le16toh(ev->conn_handle)); + assert(cssm != NULL); + + if (!cssm->pending_procedure) { + return 0; + } + BLE_HS_LOG(DEBUG, "CS capabilities exchanged"); /* TODO: Save the remote capabilities somewhere */ - set_cmd.conn_handle = le16toh(ev->conn_handle); + set_cmd.conn_handle = cssm->conn_handle; /* Only initiator role is enabled */ set_cmd.role_enable = 0x01; /* Use antenna with ID 0x01 */ @@ -476,7 +574,7 @@ ble_hs_hci_evt_le_cs_rd_rem_supp_cap_complete(uint8_t subevent, const void *data } /* Read the mode 0 Frequency Actuation Error table */ - fae_cmd.conn_handle = le16toh(ev->conn_handle); + fae_cmd.conn_handle = cssm->conn_handle; rc = ble_cs_rd_rem_fae(&fae_cmd); if (rc) { BLE_HS_LOG(DEBUG, "Failed to read FAE table"); @@ -490,14 +588,26 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, unsigned int len) { const struct ble_hci_ev_le_subev_cs_rd_rem_fae_complete *ev = data; + struct ble_cs_sm *cssm; + struct ble_cs_config *config; struct ble_cs_create_config_cp cmd; int rc; - if (len != sizeof(*ev) || ev->status) { + if (len != sizeof(*ev) || (ev->status != BLE_ERR_SUCCESS && + ev->status != BLE_ERR_UNSUPPORTED)) { return BLE_HS_ECONTROLLER; } - cmd.conn_handle = le16toh(ev->conn_handle); + cssm = ble_cs_sm_get(le16toh(ev->conn_handle)); + assert(cssm != NULL); + + if (!cssm->pending_procedure) { + return 0; + } + + config = &cssm->config[cssm->active_config_id]; + + cmd.conn_handle = cssm->conn_handle; /* The config will use ID 0x00 */ cmd.config_id = 0x00; /* Create the config on the remote controller too */ @@ -505,12 +615,17 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, /* Measure phase rotations in main mode */ cmd.main_mode_type = 0x01; /* Do not use sub mode for now. */ - cmd.sub_mode_type = 0xFF; - /* Range from which the number of CS main mode steps to execute - * will be randomly selected. - */ - cmd.min_main_mode_steps = 0x02; - cmd.max_main_mode_steps = 0x06; + cmd.sub_mode_type = BLE_HS_CS_SUBMODE_TYPE; + if (cmd.sub_mode_type == BLE_HS_CS_MODE_UNUSED) { + cmd.min_main_mode_steps = 0; + cmd.max_main_mode_steps = 0; + } else { + /* Range from which the number of CS main mode steps to execute + * will be randomly selected. + */ + cmd.min_main_mode_steps = 0x02; + cmd.max_main_mode_steps = 0x06; + } /* The number of main mode steps to be repeated at the beginning of * the current CS, irrespectively if there are some overlapping main * mode steps from previous CS subevent or not. @@ -520,19 +635,17 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, * each CS subevent */ cmd.mode_0_steps = 0x03; - /* Take the Initiator role */ - cmd.role = 0x00; - cmd.rtt_type = 0x01; + cmd.role = config->local_role; + cmd.rtt_type = config->rtt_type; + cmd.cs_sync_phy = 0x01; - memcpy(cmd.channel_map, (uint8_t[10]) {0x0a, 0xfa, 0xcf, 0xac, 0xfa, 0xc0}, 10); + memcpy(cmd.channel_map, (uint8_t[10]) {0xFC, 0xFF, 0x0F}, 10); cmd.channel_map_repetition = 0x01; /* Use Channel Selection Algorithm #3b */ cmd.channel_selection_type = 0x00; /* Ignore these as used only with #3c algorithm */ cmd.ch3c_shape = 0x00; cmd.ch3c_jump = 0x00; - /* EDLC/ECLD attack protection not supported */ - cmd.companion_signal_enable = 0x00; /* Create CS config */ rc = ble_cs_create_config(&cmd); @@ -548,6 +661,8 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, unsigned int len) { int rc; + struct ble_cs_sm *cssm; + struct ble_cs_config *config; struct ble_cs_set_proc_params_cp cmd; struct ble_cs_set_proc_params_rp rsp; struct ble_cs_proc_enable_cp enable_cmd; @@ -561,10 +676,19 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, BLE_HS_LOG(DEBUG, "CS setup phase completed"); - cmd.conn_handle = le16toh(ev->conn_handle); + cssm = ble_cs_sm_get(le16toh(ev->conn_handle)); + assert(cssm != NULL); + + if (!cssm->pending_procedure) { + return 0; + } + + config = &cssm->config[cssm->active_config_id]; + + cmd.conn_handle = cssm->conn_handle; cmd.config_id = 0x00; /* The maximum duration of each CS procedure (time = N × 0.625 ms) */ - cmd.max_procedure_len = 8; + cmd.max_procedure_len = 800; /* The maximum number of consecutive CS procedures to be scheduled * as part of this measurement */ @@ -575,12 +699,10 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, cmd.min_procedure_interval = 0x0000; cmd.max_procedure_interval = 0x0000; /* Minimum/maximum suggested durations for each CS subevent in microseconds. - * 1250us and 5000us selected. */ - cmd.min_subevent_len = 1250; + cmd.min_subevent_len = 5000; cmd.max_subevent_len = 5000; - /* Use ACI 0 as we have only one antenna on each side */ - cmd.tone_antenna_config_selection = 0x00; + cmd.tone_antenna_config_selection = config->antenna_config_id; /* Use LE 1M PHY for CS procedures */ cmd.phy = 0x01; /* Transmit power delta set to 0x80 means Host does not have a recommendation. */ @@ -598,7 +720,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, BLE_HS_LOG(DEBUG, "Failed to set CS procedure parameters"); } - enable_cmd.conn_handle = le16toh(ev->conn_handle); + enable_cmd.conn_handle = cssm->conn_handle; enable_cmd.config_id = 0x00; enable_cmd.enable = 0x01; @@ -616,47 +738,356 @@ ble_hs_hci_evt_le_cs_config_complete(uint8_t subevent, const void *data, { int rc; const struct ble_hci_ev_le_subev_cs_config_complete *ev = data; + struct ble_cs_config *config; + struct ble_cs_sm *cssm; struct ble_cs_sec_enable_cp cmd; if (len != sizeof(*ev) || ev->status) { return BLE_HS_ECONTROLLER; } - cmd.conn_handle = le16toh(ev->conn_handle); + BLE_HS_LOG(DEBUG, "CS config completed"); + + cssm = ble_cs_sm_get(le16toh(ev->conn_handle)); + config = &cssm->config[ev->config_id]; + config->rtt_type = ev->rtt_type; + + if (g_loc_supp_cap.rtt_pct && config->rtt_type != BLE_HS_CS_RTT_AA_ONLY) { + config->rtt_pct_included = 1; + } + +#if MYNEWT_VAL(BLE_SVC_RAS_CLIENT) + ble_svc_ras_clt_config_set(cssm->conn_handle, config->rtt_pct_included, + aci_to_num_of_paths[config->antenna_config_id], + config->local_role); +#endif + + if (!cssm->pending_procedure) { + config->local_role = ev->role; + + return 0; + } + + cmd.conn_handle = cssm->conn_handle; /* Exchange CS security keys */ rc = ble_cs_sec_enable(&cmd); if (rc) { BLE_HS_LOG(DEBUG, "Failed to enable CS security"); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status); + ble_cs_call_procedure_complete_cb(cssm, ev->status); } return 0; } +volatile static int retry_counter = 10; + int ble_hs_hci_evt_le_cs_proc_enable_complete(uint8_t subevent, const void *data, unsigned int len) { + int rc; const struct ble_hci_ev_le_subev_cs_proc_enable_complete *ev = data; + struct ble_cs_proc_enable_cp enable_cmd; + struct ble_cs_config *config; + struct ble_cs_sm *cssm; + uint8_t antenna_paths_mask; - if (len != sizeof(*ev) || ev->status) { + if (len != sizeof(*ev)) { return BLE_HS_ECONTROLLER; } + BLE_HS_LOG(DEBUG, "Received CS procedure enable completed, status %d\n", ev->status); + + cssm = ble_cs_sm_get(le16toh(ev->conn_handle)); + assert(cssm != NULL); + + if (ev->status == BLE_ERR_DIFF_TRANS_COLL) { + if (retry_counter == 0) { + BLE_HS_LOG(DEBUG, "Failed to enable CS procedure\n"); + return 0; + } + + BLE_HS_LOG(DEBUG, "Retrying CS procedure enable...\n"); + --retry_counter; + enable_cmd.conn_handle = cssm->conn_handle; + enable_cmd.config_id = 0x00; + enable_cmd.enable = 0x01; + + rc = ble_cs_proc_enable(&enable_cmd); + if (rc) { + BLE_HS_LOG(DEBUG, "Failed to enable CS procedure"); + } + return 0; + } else if (ev->status != 0) { + BLE_HS_LOG(DEBUG, "Failed to enable CS procedure\n"); + return 0; + } + retry_counter = 10; + + + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + config = &cssm->config[cssm->active_config_id]; + antenna_paths_mask = aci_to_antenna_paths_mask[config->antenna_config_id]; + ble_svc_ras_ranging_data_body_init(cssm->conn_handle, ev->procedure_count, ev->config_id, + ev->selected_tx_power, antenna_paths_mask); +#endif + + return 0; +} + +static int +ble_cs_add_mode0_result(struct ble_cs_sm *cssm, const uint8_t *data, uint8_t data_len) +{ + struct ble_cs_mode0_result result; + struct ble_cs_config *config = &cssm->config[cssm->active_config_id]; + + if (!IN_RANGE(data_len, 3, 5)) { + /* Ignore invalid formatted results */ + return 1; + } + + result.packet_quality = data[0]; + result.packet_rssi = data[1]; + result.packet_antenna = data[2]; + + if (config->local_role == BLE_HS_CS_ROLE_INITIATOR) { + if (data_len < 5) { + /* Ignore invalid formatted results */ + return 1; + } + + result.measured_freq_offset = get_le16(data + 3); + } + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + ble_svc_ras_add_mode0_result(&result, cssm->conn_handle, config->local_role); +#else + ble_cs_call_step_data_cb(cssm, (void *)&result, 0, BLE_HS_CS_MODE0, config->local_role); +#endif + + return 0; +} + +static int +ble_cs_add_mode1_result(struct ble_cs_sm *cssm, const uint8_t *data, uint8_t data_len) +{ + struct ble_cs_mode1_result result; + struct ble_cs_config *config = &cssm->config[cssm->active_config_id]; + + if (!IN_RANGE(data_len, 6, 14)) { + /* Ignore invalid formatted results */ + return 1; + } + + result.packet_quality = data[0]; + result.packet_nadm = data[1]; + result.packet_rssi = data[2]; + result.toa_tod = get_le16(data + 3); + result.packet_antenna = data[5]; + + if (config->rtt_pct_included) { + result.packet_pct1 = get_le32(data + 6); + result.packet_pct2 = get_le32(data + 10); + } + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + ble_svc_ras_add_mode1_result(&result, cssm->conn_handle, config->rtt_pct_included); +#else + ble_cs_call_step_data_cb(cssm, (void *)&result, 0, BLE_HS_CS_MODE1, config->local_role); +#endif + + return 0; +} + +static int +ble_cs_add_mode2_result(struct ble_cs_sm *cssm, const uint8_t *data, uint8_t data_len) +{ + struct ble_cs_mode2_result result; + struct ble_cs_config *config = &cssm->config[cssm->active_config_id]; + uint8_t n_ap; + uint8_t i; + + result.antenna_path_permutation_id = *(data++); + memcpy(result.antenna_paths, antenna_path_permutations[result.antenna_path_permutation_id], + sizeof(result.antenna_paths)); + + n_ap = aci_to_num_of_paths[config->antenna_config_id]; + + if (data_len < 1 + n_ap * 4) { + /* Ignore invalid formatted results */ + return 1; + } + + for (i = 0; i < n_ap + 1; ++i) { + result.tone_pct[i] = get_le24(data); + data += 3; + } + + for (i = 0; i < n_ap + 1; ++i) { + result.tone_quality_ind[i] = *(data++); + } + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + ble_svc_ras_add_mode2_result(&result, cssm->conn_handle, n_ap); +#else + ble_cs_call_step_data_cb(cssm, (void *)&result, 0, BLE_HS_CS_MODE2, config->local_role); +#endif + + return 0; +} + +static int +ble_cs_add_mode3_result(struct ble_cs_sm *cssm, const uint8_t *data_buf, uint8_t data_len) +{ + const uint8_t *data = data_buf; + struct ble_cs_mode3_result result; + const uint8_t *antenna_paths; + struct ble_cs_config *config = &cssm->config[cssm->active_config_id]; + uint32_t tone_pct[N_AP_MAX + 1]; + uint8_t tone_quality_ind[N_AP_MAX + 1]; + uint32_t packet_pct1; + uint32_t packet_pct2; + uint8_t antenna_path_permutation_id; + uint8_t n_ap; + uint8_t i; + + result.packet_quality = data[0]; + result.packet_nadm = data[1]; + result.packet_rssi = data[2]; + result.toa_tod = get_le16(data + 3); + result.packet_antenna = data[5]; + data += 6; + + if (config->rtt_pct_included) { + result.packet_pct1 = get_le32(data); + data += 4; + result.packet_pct2 = get_le32(data + 10); + data += 4; + } + + result.antenna_path_permutation_id = *(data++); + memcpy(result.antenna_paths, antenna_path_permutations[result.antenna_path_permutation_id], + sizeof(result.antenna_paths)); + n_ap = aci_to_num_of_paths[config->antenna_config_id]; + + for (i = 0; i < n_ap + 1; ++i) { + result.tone_pct[i] = get_le24(data); + data += 3; + } + + for (i = 0; i < n_ap + 1; ++i) { + result.tone_quality_ind[i] = *(data++); + } + + if (data_len < data - data_buf) { + /* Ignore invalid formatted results */ + return 1; + } + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + ble_svc_ras_add_mode3_result(&result, cssm->conn_handle, n_ap, config->rtt_pct_included); +#else + ble_cs_call_step_data_cb(cssm, (void *)&result, 0, BLE_HS_CS_MODE3, config->local_role); +#endif + return 0; } +static int +ble_cs_add_steps(struct ble_cs_sm *cssm, const struct cs_steps_data *step_data, + uint8_t step_count) +{ + int rc = 1; + const void *data; + uint8_t data_len; + uint8_t i; + + for (i = 0; i < step_count; ++i) { + data = step_data->data; + data_len = step_data->data_len; + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + ble_svc_ras_add_step_mode(cssm->conn_handle, step_data->mode, data_len == 0); +#endif + + if (data_len == 0) { + /* Ignore step with missing results */ + continue; + } + + switch (step_data->mode) { + case BLE_HS_CS_MODE0: + rc = ble_cs_add_mode0_result(cssm, data, data_len); + break; + case BLE_HS_CS_MODE1: + rc = ble_cs_add_mode1_result(cssm, data, data_len); + break; + case BLE_HS_CS_MODE2: + rc = ble_cs_add_mode2_result(cssm, data, data_len); + break; + case BLE_HS_CS_MODE3: + rc = ble_cs_add_mode3_result(cssm, data, data_len); + break; + default: + rc = 1; + } + + if (rc) { + /* Ignore invalid formatted results */ + return 0; + } + + step_data = data + step_data->data_len; + } + return rc; +} + int ble_hs_hci_evt_le_cs_subevent_result(uint8_t subevent, const void *data, unsigned int len) { + int rc; const struct ble_hci_ev_le_subev_cs_subevent_result *ev = data; + struct ble_cs_sm *cssm = NULL; +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + uint8_t ranging_abort_reason; + uint8_t subevent_abort_reason; +#endif - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { return BLE_HS_ECONTROLLER; } + cssm = ble_cs_sm_get(le16toh(ev->conn_handle)); + assert(cssm != NULL); + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + ranging_abort_reason = ev->abort_reason & 0xF; + subevent_abort_reason = ev->abort_reason >> 4; + + ble_svc_ras_ranging_subevent_init(cssm->conn_handle, + le16toh(ev->start_acl_conn_event_counter), + le16toh(ev->frequency_compensation), + ev->procedure_done_status, + ev->subevent_done_status, + ranging_abort_reason, + subevent_abort_reason, + ev->reference_power_level, + ev->num_steps_reported); +#endif + + rc = ble_cs_add_steps(cssm, ev->steps, ev->num_steps_reported); + + if (ev->procedure_done_status == BLE_HS_CS_PROC_DONE_STATUS_COMPLETED || + ev->procedure_done_status == BLE_HS_CS_PROC_DONE_STATUS_ABORTED) { +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + rc = ble_svc_ras_ranging_data_ready(cssm->conn_handle); +#endif + ble_cs_call_procedure_complete_cb(cssm, rc); + } + return 0; } @@ -664,12 +1095,43 @@ int ble_hs_hci_evt_le_cs_subevent_result_continue(uint8_t subevent, const void *data, unsigned int len) { + int rc; + struct ble_cs_sm *cssm = NULL; const struct ble_hci_ev_le_subev_cs_subevent_result_continue *ev = data; +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + uint8_t ranging_abort_reason; + uint8_t subevent_abort_reason; +#endif - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { return BLE_HS_ECONTROLLER; } + cssm = ble_cs_sm_get(le16toh(ev->conn_handle)); + assert(cssm != NULL); + +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + ranging_abort_reason = ev->abort_reason & 0xF; + subevent_abort_reason = ev->abort_reason >> 4; + + ble_svc_ras_ranging_subevent_update_status(cssm->conn_handle, + ev->num_steps_reported, + ev->procedure_done_status, + ev->subevent_done_status, + ranging_abort_reason, + subevent_abort_reason); +#endif + + rc = ble_cs_add_steps(cssm, ev->steps, ev->num_steps_reported); + + if (ev->procedure_done_status == BLE_HS_CS_PROC_DONE_STATUS_COMPLETED || + ev->procedure_done_status == BLE_HS_CS_PROC_DONE_STATUS_ABORTED) { +#if MYNEWT_VAL(BLE_SVC_RAS_SERVER) + rc = ble_svc_ras_ranging_data_ready(cssm->conn_handle); +#endif + ble_cs_call_procedure_complete_cb(cssm, rc); + } + return 0; } @@ -687,11 +1149,44 @@ ble_hs_hci_evt_le_cs_test_end_complete(uint8_t subevent, const void *data, } int -ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_params *params) +ble_cs_init(void) +{ + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(g_ble_cs_sm); i++) { + g_ble_cs_sm[i].conn_handle = BLE_CONN_HANDLE_INVALID; + } + + return 0; +} + +static int +ble_cs_setup_phase_start(uint16_t conn_handle) { - struct ble_hci_le_cs_rd_loc_supp_cap_rp rsp; struct ble_cs_rd_rem_supp_cap_cp cmd; + struct ble_cs_sm *cssm = NULL; + + cssm = ble_cs_sm_get(conn_handle); + assert(cssm != NULL); + ble_cs_rd_loc_supp_cap(cssm); + + cmd.conn_handle = cssm->conn_handle; + return ble_cs_rd_rem_supp_cap(&cmd); +} + +#if MYNEWT_VAL(BLE_SVC_RAS_CLIENT) +static void +ble_cs_initiator_config_cb(uint16_t conn_handle) +{ + ble_cs_setup_phase_start(conn_handle); +} +#endif + +int +ble_cs_procedure_start(const struct ble_cs_procedure_start_params *params) +{ int rc; + struct ble_cs_sm *cssm = NULL; /* Channel Sounding setup phase: * 1. Set local default CS settings @@ -701,30 +1196,53 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p * 5. Start the CS Security Start procedure */ - cs_state.cb = params->cb; - cs_state.cb_arg = params->cb_arg; + (void) ble_cs_set_chan_class; + (void) ble_cs_remove_config; + (void) ble_cs_wr_cached_rem_fae; + (void) ble_cs_wr_cached_rem_supp_cap; - cmd.conn_handle = params->conn_handle; - rc = ble_cs_rd_rem_supp_cap(&cmd); - if (rc) { - BLE_HS_LOG(DEBUG, "Failed to read local supported CS capabilities," - "err %dt", rc); - } + cssm = ble_cs_sm_get(params->conn_handle); + assert(cssm != NULL); + cssm->pending_procedure = 1; - return rc; +#if MYNEWT_VAL(BLE_SVC_RAS_CLIENT) + return ble_svc_ras_clt_subscribe(ble_cs_initiator_config_cb, ble_cs_step_data_received_cb, + cssm->conn_handle, BLE_SVC_RAS_MODE_ON_DEMAND); +#else + return ble_cs_setup_phase_start(cssm->conn_handle); +#endif } int -ble_cs_initiator_procedure_terminate(uint16_t conn_handle) +ble_cs_procedure_terminate(uint16_t conn_handle) { return 0; } int -ble_cs_reflector_setup(struct ble_cs_reflector_setup_params *params) +ble_cs_setup(struct ble_cs_setup_params *params) { - cs_state.cb = params->cb; - cs_state.cb_arg = params->cb_arg; + struct ble_cs_sm *cssm = NULL; + struct ble_cs_config *config; + + cssm = ble_cs_sm_get(params->conn_handle); + if (cssm == NULL) { + cssm = ble_cs_sm_get(BLE_CONN_HANDLE_INVALID); + } + assert(cssm != NULL); + + cssm->conn_handle = params->conn_handle; + cssm->event_cb = params->cb; + cssm->event_cb_arg = params->cb_arg; + cssm->active_config_id = 0; + cssm->pending_procedure = 0; + config = &cssm->config[cssm->active_config_id]; +#if MYNEWT_VAL(BLE_SVC_RAS_CLIENT) + config->local_role = BLE_HS_CS_ROLE_REFLECTOR; +// config->local_role = BLE_HS_CS_ROLE_INITIATOR; +#else + config->local_role = BLE_HS_CS_ROLE_REFLECTOR; +#endif return 0; } diff --git a/nimble/host/src/ble_cs_priv.h b/nimble/host/src/ble_cs_priv.h index f02565852d..0435e8ca3e 100644 --- a/nimble/host/src/ble_cs_priv.h +++ b/nimble/host/src/ble_cs_priv.h @@ -20,6 +20,22 @@ #ifndef H_BLE_CS_PRIV_ #define H_BLE_CS_PRIV_ +#define BLE_HS_CS_RTT_AA_ONLY (0x00) +#define BLE_HS_CS_RTT_32_BIT_SOUNDING_SEQUENCE (0x01) +#define BLE_HS_CS_RTT_96_BIT_SOUNDING_SEQUENCE (0x02) +#define BLE_HS_CS_RTT_32_BIT_RANDOM_SEQUENCE (0x03) +#define BLE_HS_CS_RTT_64_BIT_RANDOM_SEQUENCE (0x04) +#define BLE_HS_CS_RTT_96_BIT_RANDOM_SEQUENCE (0x05) +#define BLE_HS_CS_RTT_128_BIT_RANDOM_SEQUENCE (0x06) + +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_COMPLETED (0x0) +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_PARTIAL (0x1) +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_ABORTED (0xF) + +#define BLE_HS_CS_PROC_DONE_STATUS_COMPLETED (0x0) +#define BLE_HS_CS_PROC_DONE_STATUS_PARTIAL (0x1) +#define BLE_HS_CS_PROC_DONE_STATUS_ABORTED (0xF) + int ble_hs_hci_evt_le_cs_rd_rem_supp_cap_complete(uint8_t subevent, const void *data, unsigned int len); int ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, unsigned int len); int ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, unsigned int len); diff --git a/nimble/host/src/ble_peer.c b/nimble/host/src/ble_peer.c new file mode 100644 index 0000000000..775dd4c031 --- /dev/null +++ b/nimble/host/src/ble_peer.c @@ -0,0 +1,731 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you 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. + */ + +//#if MYNEWT_VAL(BLE_PEER) +#include +#include +#include "host/ble_hs.h" +#include "host/ble_peer.h" + +#define MAX_PEERS MYNEWT_VAL(BLE_PEER_MAX_PEERS) +#define MAX_SVCS MYNEWT_VAL(BLE_PEER_MAX_SVCS) +#define MAX_CHRS MYNEWT_VAL(BLE_PEER_MAX_CHRS) +#define MAX_DSCS MYNEWT_VAL(BLE_PEER_MAX_DSCS) +static uint8_t peer_mem[MAX_PEERS * sizeof(struct ble_peer)]; +static uint8_t peer_svc_mem[MAX_SVCS * sizeof(struct ble_peer_svc)]; +static uint8_t peer_chr_mem[MAX_CHRS * sizeof(struct ble_peer_chr)]; +static uint8_t peer_dsc_mem[MAX_DSCS * sizeof(struct ble_peer_dsc)]; + +static struct os_mempool peer_svc_pool; +static struct os_mempool peer_chr_pool; +static struct os_mempool peer_dsc_pool; +static struct os_mempool peer_pool; +static SLIST_HEAD(, ble_peer) peers; + +static struct ble_peer_svc *peer_svc_find_range(struct ble_peer *peer, uint16_t attr_handle); +static struct ble_peer_svc *peer_svc_find(struct ble_peer *peer, uint16_t svc_start_handle, struct ble_peer_svc **out_prev); +int ble_peer_svc_is_empty(const struct ble_peer_svc *svc); +uint16_t ble_peer_chr_end_handle(const struct ble_peer_svc *svc, const struct ble_peer_chr *chr); +int ble_peer_chr_is_empty(const struct ble_peer_svc *svc, const struct ble_peer_chr *chr); +static struct ble_peer_chr *peer_chr_find(const struct ble_peer_svc *svc, uint16_t chr_def_handle, struct ble_peer_chr **out_prev); +static void peer_disc_chrs(struct ble_peer *peer); +static int peer_dsc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, uint16_t chr_val_handle, + const struct ble_gatt_dsc *dsc, void *arg); + +struct ble_peer * +ble_peer_find(uint16_t conn_handle) +{ + struct ble_peer *peer; + + SLIST_FOREACH(peer, &peers, next) { + if (peer->conn_handle == conn_handle) { + return peer; + } + } + + return NULL; +} + +static void +peer_disc_complete(struct ble_peer *peer, int rc) +{ + peer->disc_prev_chr_val = 0; + + /* Notify caller that discovery has completed. */ + if (peer->disc_cb != NULL) { + peer->disc_cb(peer, rc, peer->disc_cb_arg); + } +} + +static struct ble_peer_dsc * +peer_dsc_find_prev(const struct ble_peer_chr *chr, uint16_t dsc_handle) +{ + struct ble_peer_dsc *prev; + struct ble_peer_dsc *dsc; + + prev = NULL; + SLIST_FOREACH(dsc, &chr->dscs, next) { + if (dsc->dsc.handle >= dsc_handle) { + break; + } + + prev = dsc; + } + + return prev; +} + +static struct ble_peer_dsc * +peer_dsc_find(const struct ble_peer_chr *chr, uint16_t dsc_handle, + struct ble_peer_dsc **out_prev) +{ + struct ble_peer_dsc *prev; + struct ble_peer_dsc *dsc; + + prev = peer_dsc_find_prev(chr, dsc_handle); + if (prev == NULL) { + dsc = SLIST_FIRST(&chr->dscs); + } else { + dsc = SLIST_NEXT(prev, next); + } + + if (dsc != NULL && dsc->dsc.handle != dsc_handle) { + dsc = NULL; + } + + if (out_prev != NULL) { + *out_prev = prev; + } + return dsc; +} + +static int +peer_dsc_add(struct ble_peer *peer, uint16_t chr_val_handle, + const struct ble_gatt_dsc *gatt_dsc) +{ + struct ble_peer_dsc *prev; + struct ble_peer_dsc *dsc; + struct ble_peer_svc *svc; + struct ble_peer_chr *chr; + + svc = peer_svc_find_range(peer, chr_val_handle); + if (svc == NULL) { + /* Can't find service for discovered descriptor; this shouldn't + * happen. + */ + assert(0); + return BLE_HS_EUNKNOWN; + } + + chr = peer_chr_find(svc, chr_val_handle, NULL); + if (chr == NULL) { + /* Can't find characteristic for discovered descriptor; this shouldn't + * happen. + */ + assert(0); + return BLE_HS_EUNKNOWN; + } + + dsc = peer_dsc_find(chr, gatt_dsc->handle, &prev); + if (dsc != NULL) { + /* Descriptor already discovered. */ + return 0; + } + + dsc = os_memblock_get(&peer_dsc_pool); + if (dsc == NULL) { + /* Out of memory. */ + return BLE_HS_ENOMEM; + } + memset(dsc, 0, sizeof *dsc); + + dsc->dsc = *gatt_dsc; + + if (prev == NULL) { + SLIST_INSERT_HEAD(&chr->dscs, dsc, next); + } else { + SLIST_NEXT(prev, next) = dsc; + } + + return 0; +} + +static void +peer_disc_dscs(struct ble_peer *peer) +{ + struct ble_peer_chr *chr; + struct ble_peer_svc *svc; + int rc; + + /* Search through the list of discovered characteristics for the first + * characteristic that contains undiscovered descriptors. Then, discover + * all descriptors belonging to that characteristic. + */ + SLIST_FOREACH(svc, &peer->svcs, next) { + SLIST_FOREACH(chr, &svc->chrs, next) { + if (!ble_peer_chr_is_empty(svc, chr) && + SLIST_EMPTY(&chr->dscs) && + peer->disc_prev_chr_val <= chr->chr.def_handle) { + + rc = ble_gattc_disc_all_dscs(peer->conn_handle, + chr->chr.val_handle, + ble_peer_chr_end_handle(svc, chr), + peer_dsc_disced, peer); + if (rc != 0) { + peer_disc_complete(peer, rc); + } + + peer->disc_prev_chr_val = chr->chr.val_handle; + return; + } + } + } + + /* All descriptors discovered. */ + peer_disc_complete(peer, 0); +} + +static int +peer_dsc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + uint16_t chr_val_handle, const struct ble_gatt_dsc *dsc, + void *arg) +{ + struct ble_peer *peer; + int rc; + + peer = arg; + assert(peer->conn_handle == conn_handle); + + switch (error->status) { + case 0: + rc = peer_dsc_add(peer, chr_val_handle, dsc); + break; + + case BLE_HS_EDONE: + /* All descriptors in this characteristic discovered; start discovering + * descriptors in the next characteristic. + */ + if (peer->disc_prev_chr_val > 0) { + peer_disc_dscs(peer); + } + rc = 0; + break; + + default: + /* Error; abort discovery. */ + rc = error->status; + break; + } + + if (rc != 0) { + /* Error; abort discovery. */ + peer_disc_complete(peer, rc); + } + + return rc; +} + +uint16_t +ble_peer_chr_end_handle(const struct ble_peer_svc *svc, const struct ble_peer_chr *chr) +{ + const struct ble_peer_chr *next_chr; + + next_chr = SLIST_NEXT(chr, next); + if (next_chr != NULL) { + return next_chr->chr.def_handle - 1; + } else { + return svc->svc.end_handle; + } +} + +int +ble_peer_chr_is_empty(const struct ble_peer_svc *svc, const struct ble_peer_chr *chr) +{ + return ble_peer_chr_end_handle(svc, chr) <= chr->chr.val_handle; +} + +static struct ble_peer_chr * +peer_chr_find_prev(const struct ble_peer_svc *svc, uint16_t chr_val_handle) +{ + struct ble_peer_chr *prev; + struct ble_peer_chr *chr; + + prev = NULL; + SLIST_FOREACH(chr, &svc->chrs, next) { + if (chr->chr.val_handle >= chr_val_handle) { + break; + } + + prev = chr; + } + + return prev; +} + +static struct ble_peer_chr * +peer_chr_find(const struct ble_peer_svc *svc, uint16_t chr_val_handle, + struct ble_peer_chr **out_prev) +{ + struct ble_peer_chr *prev; + struct ble_peer_chr *chr; + + prev = peer_chr_find_prev(svc, chr_val_handle); + if (prev == NULL) { + chr = SLIST_FIRST(&svc->chrs); + } else { + chr = SLIST_NEXT(prev, next); + } + + if (chr != NULL && chr->chr.val_handle != chr_val_handle) { + chr = NULL; + } + + if (out_prev != NULL) { + *out_prev = prev; + } + return chr; +} + +static void +peer_chr_delete(struct ble_peer_chr *chr) +{ + struct ble_peer_dsc *dsc; + + while ((dsc = SLIST_FIRST(&chr->dscs)) != NULL) { + SLIST_REMOVE_HEAD(&chr->dscs, next); + os_memblock_put(&peer_dsc_pool, dsc); + } + + os_memblock_put(&peer_chr_pool, chr); +} + +static int +peer_chr_add(struct ble_peer *peer, uint16_t svc_start_handle, + const struct ble_gatt_chr *gatt_chr) +{ + struct ble_peer_chr *prev; + struct ble_peer_chr *chr; + struct ble_peer_svc *svc; + + svc = peer_svc_find(peer, svc_start_handle, NULL); + if (svc == NULL) { + /* Can't find service for discovered characteristic; this shouldn't + * happen. + */ + assert(0); + return BLE_HS_EUNKNOWN; + } + + chr = peer_chr_find(svc, gatt_chr->def_handle, &prev); + if (chr != NULL) { + /* Characteristic already discovered. */ + return 0; + } + + chr = os_memblock_get(&peer_chr_pool); + if (chr == NULL) { + /* Out of memory. */ + return BLE_HS_ENOMEM; + } + memset(chr, 0, sizeof *chr); + + chr->chr = *gatt_chr; + + if (prev == NULL) { + SLIST_INSERT_HEAD(&svc->chrs, chr, next); + } else { + SLIST_NEXT(prev, next) = chr; + } + + return 0; +} + +static int +peer_chr_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + const struct ble_gatt_chr *chr, void *arg) +{ + struct ble_peer *peer; + int rc; + + peer = arg; + assert(peer->conn_handle == conn_handle); + + switch (error->status) { + case 0: + rc = peer_chr_add(peer, peer->cur_svc->svc.start_handle, chr); + break; + + case BLE_HS_EDONE: + /* All characteristics in this service discovered; start discovering + * characteristics in the next service. + */ + if (peer->disc_prev_chr_val > 0) { + peer_disc_chrs(peer); + } + rc = 0; + break; + + default: + rc = error->status; + break; + } + + if (rc != 0) { + /* Error; abort discovery. */ + peer_disc_complete(peer, rc); + } + + return rc; +} + +static void +peer_disc_chrs(struct ble_peer *peer) +{ + struct ble_peer_svc *svc; + int rc; + + /* Search through the list of discovered service for the first service that + * contains undiscovered characteristics. Then, discover all + * characteristics belonging to that service. + */ + SLIST_FOREACH(svc, &peer->svcs, next) { + if (!ble_peer_svc_is_empty(svc) && SLIST_EMPTY(&svc->chrs)) { + peer->cur_svc = svc; + rc = ble_gattc_disc_all_chrs(peer->conn_handle, + svc->svc.start_handle, + svc->svc.end_handle, + peer_chr_disced, peer); + if (rc != 0) { + peer_disc_complete(peer, rc); + } + return; + } + } + + /* All characteristics discovered. */ + peer_disc_dscs(peer); +} + +int +ble_peer_svc_is_empty(const struct ble_peer_svc *svc) +{ + return svc->svc.end_handle <= svc->svc.start_handle; +} + +static struct ble_peer_svc * +peer_svc_find_prev(struct ble_peer *peer, uint16_t svc_start_handle) +{ + struct ble_peer_svc *prev; + struct ble_peer_svc *svc; + + prev = NULL; + SLIST_FOREACH(svc, &peer->svcs, next) { + if (svc->svc.start_handle >= svc_start_handle) { + break; + } + + prev = svc; + } + + return prev; +} + +static struct ble_peer_svc * +peer_svc_find(struct ble_peer *peer, uint16_t svc_start_handle, + struct ble_peer_svc **out_prev) +{ + struct ble_peer_svc *prev; + struct ble_peer_svc *svc; + + prev = peer_svc_find_prev(peer, svc_start_handle); + if (prev == NULL) { + svc = SLIST_FIRST(&peer->svcs); + } else { + svc = SLIST_NEXT(prev, next); + } + + if (svc != NULL && svc->svc.start_handle != svc_start_handle) { + svc = NULL; + } + + if (out_prev != NULL) { + *out_prev = prev; + } + return svc; +} + +static struct ble_peer_svc * +peer_svc_find_range(struct ble_peer *peer, uint16_t attr_handle) +{ + struct ble_peer_svc *svc; + + SLIST_FOREACH(svc, &peer->svcs, next) { + if (svc->svc.start_handle <= attr_handle && + svc->svc.end_handle >= attr_handle) { + + return svc; + } + } + + return NULL; +} + +const struct ble_peer_svc * +ble_peer_svc_find_uuid(const struct ble_peer *peer, const ble_uuid_t *uuid) +{ + const struct ble_peer_svc *svc; + + SLIST_FOREACH(svc, &peer->svcs, next) { + if (ble_uuid_cmp(&svc->svc.uuid.u, uuid) == 0) { + return svc; + } + } + + return NULL; +} + +const struct ble_peer_chr * +ble_peer_chr_find_uuid(const struct ble_peer *peer, const ble_uuid_t *svc_uuid, + const ble_uuid_t *chr_uuid) +{ + const struct ble_peer_svc *svc; + const struct ble_peer_chr *chr; + + svc = ble_peer_svc_find_uuid(peer, svc_uuid); + if (svc == NULL) { + return NULL; + } + + SLIST_FOREACH(chr, &svc->chrs, next) { + if (ble_uuid_cmp(&chr->chr.uuid.u, chr_uuid) == 0) { + return chr; + } + } + + return NULL; +} + +const struct ble_peer_dsc * +ble_peer_dsc_find_uuid(const struct ble_peer *peer, const ble_uuid_t *svc_uuid, + const ble_uuid_t *chr_uuid, const ble_uuid_t *dsc_uuid) +{ + const struct ble_peer_chr *chr; + const struct ble_peer_dsc *dsc; + + chr = ble_peer_chr_find_uuid(peer, svc_uuid, chr_uuid); + if (chr == NULL) { + return NULL; + } + + SLIST_FOREACH(dsc, &chr->dscs, next) { + if (ble_uuid_cmp(&dsc->dsc.uuid.u, dsc_uuid) == 0) { + return dsc; + } + } + + return NULL; +} + +static int +peer_svc_add(struct ble_peer *peer, const struct ble_gatt_svc *gatt_svc) +{ + struct ble_peer_svc *prev; + struct ble_peer_svc *svc; + + svc = peer_svc_find(peer, gatt_svc->start_handle, &prev); + if (svc != NULL) { + /* Service already discovered. */ + return 0; + } + + svc = os_memblock_get(&peer_svc_pool); + if (svc == NULL) { + /* Out of memory. */ + return BLE_HS_ENOMEM; + } + memset(svc, 0, sizeof *svc); + + svc->svc = *gatt_svc; + SLIST_INIT(&svc->chrs); + + if (prev == NULL) { + SLIST_INSERT_HEAD(&peer->svcs, svc, next); + } else { + SLIST_INSERT_AFTER(prev, svc, next); + } + + return 0; +} + +static void +peer_svc_delete(struct ble_peer_svc *svc) +{ + struct ble_peer_chr *chr; + + while ((chr = SLIST_FIRST(&svc->chrs)) != NULL) { + SLIST_REMOVE_HEAD(&svc->chrs, next); + peer_chr_delete(chr); + } + + os_memblock_put(&peer_svc_pool, svc); +} + +static int +peer_svc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + const struct ble_gatt_svc *service, void *arg) +{ + struct ble_peer *peer; + int rc; + + peer = arg; + assert(peer->conn_handle == conn_handle); + + switch (error->status) { + case 0: + rc = peer_svc_add(peer, service); + break; + + case BLE_HS_EDONE: + /* All services discovered; start discovering characteristics. */ + if (peer->disc_prev_chr_val > 0) { + peer_disc_chrs(peer); + } + rc = 0; + break; + + default: + rc = error->status; + break; + } + + if (rc != 0) { + /* Error; abort discovery. */ + peer_disc_complete(peer, rc); + } + + return rc; +} + + +int +ble_peer_disc_all(uint16_t conn_handle, ble_peer_disc_fn *disc_cb, void *disc_cb_arg) +{ + struct ble_peer_svc *svc; + struct ble_peer *peer; + int rc; + + peer = ble_peer_find(conn_handle); + if (peer == NULL) { + return BLE_HS_ENOTCONN; + } + + /* Undiscover everything first. */ + while ((svc = SLIST_FIRST(&peer->svcs)) != NULL) { + SLIST_REMOVE_HEAD(&peer->svcs, next); + peer_svc_delete(svc); + } + + peer->disc_prev_chr_val = 1; + peer->disc_cb = disc_cb; + peer->disc_cb_arg = disc_cb_arg; + + rc = ble_gattc_disc_all_svcs(conn_handle, peer_svc_disced, peer); + if (rc != 0) { + return rc; + } + + return 0; +} + +int +ble_peer_delete(uint16_t conn_handle) +{ + struct ble_peer_svc *svc; + struct ble_peer *peer; + int rc; + + peer = ble_peer_find(conn_handle); + if (peer == NULL) { + return BLE_HS_ENOTCONN; + } + + SLIST_REMOVE(&peers, peer, ble_peer, next); + + while ((svc = SLIST_FIRST(&peer->svcs)) != NULL) { + SLIST_REMOVE_HEAD(&peer->svcs, next); + peer_svc_delete(svc); + } + + rc = os_memblock_put(&peer_pool, peer); + if (rc != 0) { + return BLE_HS_EOS; + } + + return 0; +} + +int +ble_peer_add(uint16_t conn_handle) +{ + struct ble_peer *peer; + + /* Make sure the connection handle is unique. */ + peer = ble_peer_find(conn_handle); + if (peer != NULL) { + return BLE_HS_EALREADY; + } + + peer = os_memblock_get(&peer_pool); + if (peer == NULL) { + /* Out of memory. */ + return BLE_HS_ENOMEM; + } + + memset(peer, 0, sizeof *peer); + peer->conn_handle = conn_handle; + + SLIST_INSERT_HEAD(&peers, peer, next); + + return 0; +} + +int +ble_peer_init(void) +{ + int rc; + + rc = os_mempool_init(&peer_pool, MAX_PEERS, sizeof(struct ble_peer), + peer_mem, "peer_pool"); + assert(rc == 0); + + rc = os_mempool_init(&peer_svc_pool, MAX_SVCS, sizeof(struct ble_peer_svc), + peer_svc_mem, "peer_svc_pool"); + assert(rc == 0); + + rc = os_mempool_init(&peer_chr_pool, MAX_CHRS, sizeof(struct ble_peer_chr), + peer_chr_mem, "peer_chr_pool"); + assert(rc == 0); + + rc = os_mempool_init(&peer_dsc_pool, MAX_DSCS, sizeof(struct ble_peer_dsc), + peer_dsc_mem, "peer_dsc_pool"); + assert(rc == 0); + + return 0; +} +//#endif diff --git a/nimble/host/syscfg.yml b/nimble/host/syscfg.yml index 873dcae4bf..a6e08d6f56 100644 --- a/nimble/host/syscfg.yml +++ b/nimble/host/syscfg.yml @@ -539,6 +539,27 @@ syscfg.defs: restrictions: - 'BLE_ISO_BROADCAST_SOURCE if 0' + BLE_PEER: + description: > + Number of supported + value: 0 + BLE_PEER_MAX_PEERS: + description: > + Number of supported + value: 'MYNEWT_VAL_BLE_MAX_CONNECTIONS' + BLE_PEER_MAX_SVCS: + description: > + Number of supported + value: 64 + BLE_PEER_MAX_CHRS: + description: > + Number of supported + value: 96 + BLE_PEER_MAX_DSCS: + description: > + Number of supported + value: 64 + syscfg.logs: BLE_HS_LOG: module: MYNEWT_VAL(BLE_HS_LOG_MOD)