From a82bdb87d30ff34696a50757238817242dd268bb Mon Sep 17 00:00:00 2001 From: Astha Verma Date: Thu, 12 Jun 2025 08:54:08 +0530 Subject: [PATCH] fix(nimble): Added parameter in peer_init for included service allocation --- components/bt/host/nimble/Kconfig.in | 8 +- components/bt/host/nimble/nimble | 2 +- .../nimble/ble_cts/cts_cent/main/main.c | 5 + .../enc_adv_data_cent/main/main.c | 5 + .../nimble/ble_gattc_gatts_coex/main/main.c | 6 +- .../nimble/ble_htp/htp_cent/main/main.c | 6 +- .../ble_l2cap_coc/coc_blecent/main/main.c | 6 +- .../ble_multi_conn_cent/main/main.c | 6 +- .../nimble/ble_periodic_sync/main/main.c | 6 +- .../nimble/ble_phy/phy_cent/main/main.c | 6 +- .../proximity_sensor_cent/main/main.c | 6 +- .../nimble/ble_spp/spp_client/main/main.c | 6 +- .../nimble/common/nimble_central_utils/peer.c | 15 +- .../blecent_throughput/main/gattc.h | 15 ++ .../blecent_throughput/main/main.c | 6 +- .../blecent_throughput/main/peer.c | 243 +++++++++++++++++- 16 files changed, 325 insertions(+), 22 deletions(-) diff --git a/components/bt/host/nimble/Kconfig.in b/components/bt/host/nimble/Kconfig.in index 3d6ac04836..fb3607d837 100644 --- a/components/bt/host/nimble/Kconfig.in +++ b/components/bt/host/nimble/Kconfig.in @@ -751,9 +751,11 @@ config BT_NIMBLE_GATT_CACHING_INCLUDE_SERVICES Enable this option to include *included services* (e.g., services referenced by other services) in the GATT database cache. Disabling this will skip caching of included service entries. config BT_NIMBLE_INCL_SVC_DISCOVERY - bool "Enable Included service discovery" - default y if BT_NIMBLE_GATT_CACHING_INCLUDE_SERVICES - default n + bool + default y if BT_NIMBLE_GATT_CACHING && BT_NIMBLE_GATT_CACHING_INCLUDE_SERVICES + default n if BT_NIMBLE_GATT_CACHING && !BT_NIMBLE_GATT_CACHING_INCLUDE_SERVICES + default n if !BT_NIMBLE_GATT_CACHING + prompt "Enable Included service discovery" if !BT_NIMBLE_GATT_CACHING help Enable this option to start discovery for included service. config BT_NIMBLE_GATT_CACHING_MAX_CONNS diff --git a/components/bt/host/nimble/nimble b/components/bt/host/nimble/nimble index 84e02a6b82..0e3d2d39bb 160000 --- a/components/bt/host/nimble/nimble +++ b/components/bt/host/nimble/nimble @@ -1 +1 @@ -Subproject commit 84e02a6b820734de411ae9bdc8829d8ef6128205 +Subproject commit 0e3d2d39bb7dad01a70b6e810014c43cea235d24 diff --git a/examples/bluetooth/nimble/ble_cts/cts_cent/main/main.c b/examples/bluetooth/nimble/ble_cts/cts_cent/main/main.c index cc50017611..efe14e368e 100644 --- a/examples/bluetooth/nimble/ble_cts/cts_cent/main/main.c +++ b/examples/bluetooth/nimble/ble_cts/cts_cent/main/main.c @@ -568,8 +568,13 @@ app_main(void) ble_hs_cfg.store_status_cb = ble_store_util_status_rr; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); +#endif #if CONFIG_BT_NIMBLE_GAP_SERVICE /* Set the default device name. */ diff --git a/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c b/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c index 441b2a9eed..1b3e77574d 100644 --- a/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c +++ b/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c @@ -622,8 +622,13 @@ app_main(void) ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_KEYBOARD_ONLY; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); +#endif #if CONFIG_BT_NIMBLE_GAP_SERVICE /* Set the default device name. */ diff --git a/examples/bluetooth/nimble/ble_gattc_gatts_coex/main/main.c b/examples/bluetooth/nimble/ble_gattc_gatts_coex/main/main.c index 1499c8373e..a3b60e3363 100644 --- a/examples/bluetooth/nimble/ble_gattc_gatts_coex/main/main.c +++ b/examples/bluetooth/nimble/ble_gattc_gatts_coex/main/main.c @@ -549,9 +549,11 @@ void app_main(void) nimble_port_init(); ble_hs_cfg.sync_cb = on_sync; - +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); +#else peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); - +#endif ble_svc_gap_init(); ble_svc_gatt_init(); ble_svc_ans_init(); diff --git a/examples/bluetooth/nimble/ble_htp/htp_cent/main/main.c b/examples/bluetooth/nimble/ble_htp/htp_cent/main/main.c index 0ac1b7d601..510139146e 100644 --- a/examples/bluetooth/nimble/ble_htp/htp_cent/main/main.c +++ b/examples/bluetooth/nimble/ble_htp/htp_cent/main/main.c @@ -698,9 +698,13 @@ app_main(void) ble_hs_cfg.store_status_cb = ble_store_util_status_rr; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); - +#endif #if CONFIG_BT_NIMBLE_GAP_SERVICE /* Set the default device name. */ rc = ble_svc_gap_device_name_set("nimble-htp-cent"); diff --git a/examples/bluetooth/nimble/ble_l2cap_coc/coc_blecent/main/main.c b/examples/bluetooth/nimble/ble_l2cap_coc/coc_blecent/main/main.c index 644e19476d..209d5e2e84 100644 --- a/examples/bluetooth/nimble/ble_l2cap_coc/coc_blecent/main/main.c +++ b/examples/bluetooth/nimble/ble_l2cap_coc/coc_blecent/main/main.c @@ -572,9 +572,13 @@ app_main(void) #endif /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); - +#endif #if CONFIG_BT_NIMBLE_GAP_SERVICE /* Set the default device name. */ rc = ble_svc_gap_device_name_set("blecent-l2coc"); diff --git a/examples/bluetooth/nimble/ble_multi_conn/ble_multi_conn_cent/main/main.c b/examples/bluetooth/nimble/ble_multi_conn/ble_multi_conn_cent/main/main.c index 4b5c47f1bc..6acd00750c 100644 --- a/examples/bluetooth/nimble/ble_multi_conn/ble_multi_conn_cent/main/main.c +++ b/examples/bluetooth/nimble/ble_multi_conn/ble_multi_conn_cent/main/main.c @@ -454,9 +454,13 @@ app_main(void) ble_hs_cfg.store_status_cb = ble_store_util_status_rr; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(BLE_PEER_MAX_NUM, BLE_PEER_MAX_NUM, BLE_PEER_MAX_NUM, BLE_PEER_MAX_NUM, BLE_PEER_MAX_NUM); + assert(rc == 0); +#else rc = peer_init(BLE_PEER_MAX_NUM, BLE_PEER_MAX_NUM, BLE_PEER_MAX_NUM, BLE_PEER_MAX_NUM); assert(rc == 0); - +#endif /* Set the default device name. We will act as both central and peripheral. */ rc = ble_svc_gap_device_name_set("esp-ble-role-coex"); assert(rc == 0); diff --git a/examples/bluetooth/nimble/ble_periodic_sync/main/main.c b/examples/bluetooth/nimble/ble_periodic_sync/main/main.c index 869a4d32bc..567338d5d2 100644 --- a/examples/bluetooth/nimble/ble_periodic_sync/main/main.c +++ b/examples/bluetooth/nimble/ble_periodic_sync/main/main.c @@ -203,9 +203,13 @@ app_main(void) ble_hs_cfg.store_status_cb = ble_store_util_status_rr; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); - +#endif #if CONFIG_BT_NIMBLE_GAP_SERVICE /* Set the default device name. */ rc = ble_svc_gap_device_name_set("nimble_periodic_sync"); diff --git a/examples/bluetooth/nimble/ble_phy/phy_cent/main/main.c b/examples/bluetooth/nimble/ble_phy/phy_cent/main/main.c index c6520f98fc..09eb5f6cc6 100644 --- a/examples/bluetooth/nimble/ble_phy/phy_cent/main/main.c +++ b/examples/bluetooth/nimble/ble_phy/phy_cent/main/main.c @@ -553,9 +553,13 @@ app_main(void) ble_hs_cfg.store_status_cb = ble_store_util_status_rr; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); - +#endif #if CONFIG_BT_NIMBLE_GAP_SERVICE /* Set the default device name. */ rc = ble_svc_gap_device_name_set("blecent-phy"); diff --git a/examples/bluetooth/nimble/ble_proximity_sensor/proximity_sensor_cent/main/main.c b/examples/bluetooth/nimble/ble_proximity_sensor/proximity_sensor_cent/main/main.c index 452c881554..4c73890b93 100644 --- a/examples/bluetooth/nimble/ble_proximity_sensor/proximity_sensor_cent/main/main.c +++ b/examples/bluetooth/nimble/ble_proximity_sensor/proximity_sensor_cent/main/main.c @@ -722,9 +722,13 @@ app_main(void) ble_hs_cfg.store_status_cb = ble_store_util_status_rr; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); - +#endif #if CONFIG_BT_NIMBLE_GAP_SERVICE /* Set the default device name. */ rc = ble_svc_gap_device_name_set("nimble-prox-cent"); diff --git a/examples/bluetooth/nimble/ble_spp/spp_client/main/main.c b/examples/bluetooth/nimble/ble_spp/spp_client/main/main.c index 8e9136de96..4b479214c3 100644 --- a/examples/bluetooth/nimble/ble_spp/spp_client/main/main.c +++ b/examples/bluetooth/nimble/ble_spp/spp_client/main/main.c @@ -488,9 +488,13 @@ app_main(void) ble_hs_cfg.store_status_cb = ble_store_util_status_rr; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); - +#endif #if CONFIG_BT_NIMBLE_GAP_SERVICE /* Set the default device name. */ rc = ble_svc_gap_device_name_set("nimble-ble-spp-client"); diff --git a/examples/bluetooth/nimble/common/nimble_central_utils/peer.c b/examples/bluetooth/nimble/common/nimble_central_utils/peer.c index 117b36f5c1..e4a2070cfb 100644 --- a/examples/bluetooth/nimble/common/nimble_central_utils/peer.c +++ b/examples/bluetooth/nimble/common/nimble_central_utils/peer.c @@ -602,13 +602,16 @@ peer_disc_incs(struct peer *peer) } } } - svc = peer->cur_svc; - rc = ble_gattc_find_inc_svcs(peer->conn_handle, - svc->svc.start_handle, - svc->svc.end_handle, - peer_inc_disced, peer); - if (rc != 0) { + if (svc != NULL && !peer_svc_is_empty(svc)) { + rc = ble_gattc_find_inc_svcs(peer->conn_handle, + svc->svc.start_handle, + svc->svc.end_handle, + peer_inc_disced, peer); + if (rc != 0) { + peer_disc_chrs(peer); + } + } else { peer_disc_chrs(peer); } } diff --git a/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/gattc.h b/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/gattc.h index 9983498123..aea4a3762f 100644 --- a/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/gattc.h +++ b/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/gattc.h @@ -50,10 +50,21 @@ struct peer_chr { }; SLIST_HEAD(peer_chr_list, peer_chr); +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) +struct peer_incl_svc { + SLIST_ENTRY(peer_incl_svc) next; + struct ble_gatt_incl_svc svc; +}; +SLIST_HEAD(peer_incl_svc_list, peer_incl_svc); +#endif + struct peer_svc { SLIST_ENTRY(peer_svc) next; struct ble_gatt_svc svc; +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + struct peer_incl_svc_list incl_svc; +#endif struct peer_chr_list chrs; }; SLIST_HEAD(peer_svc_list, peer_svc); @@ -90,7 +101,11 @@ const struct peer_svc * peer_svc_find_uuid(const struct peer *peer, const ble_uuid_t *uuid); int peer_delete(uint16_t conn_handle); int peer_add(uint16_t conn_handle); +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) +int peer_init(int max_peers, int max_svcs, int max_incl_svcs, int max_chrs, int max_dscs); +#else int peer_init(int max_peers, int max_svcs, int max_chrs, int max_dscs); +#endif struct peer * peer_find(uint16_t conn_handle); /* Console */ diff --git a/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/main.c b/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/main.c index 2bfbf5e040..49d1007a94 100644 --- a/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/main.c +++ b/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/main.c @@ -938,9 +938,13 @@ app_main(void) ble_hs_cfg.store_status_cb = ble_store_util_status_rr; /* Initialize data structures to track connected peers. */ +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64, 64); + assert(rc == 0); +#else rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); assert(rc == 0); - +#endif /* Set the default device name. */ rc = ble_svc_gap_device_name_set("gattc-throughput"); assert(rc == 0); diff --git a/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/peer.c b/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/peer.c index e7e655cd1f..4ef4ab09d9 100644 --- a/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/peer.c +++ b/examples/bluetooth/nimble/throughput_app/blecent_throughput/main/peer.c @@ -12,6 +12,11 @@ static void *peer_svc_mem; static struct os_mempool peer_svc_pool; +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) +static void *peer_incl_svc_mem; +static struct os_mempool peer_incl_svc_pool; +#endif + static void *peer_chr_mem; static struct os_mempool peer_chr_pool; @@ -44,7 +49,13 @@ 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); - +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) +static int +peer_inc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + const struct ble_gatt_incl_svc *service, void *arg); +static void +peer_disc_incs(struct peer *peer); +#endif struct peer * peer_find(uint16_t conn_handle) { @@ -420,6 +431,193 @@ peer_disc_chrs(struct peer *peer) peer_disc_dscs(peer); } +#if (MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)) +static struct peer_incl_svc * +peer_incl_svc_find_prev(const struct peer_svc *svc, uint16_t incl_svc_handle) +{ + struct peer_incl_svc *prev; + struct peer_incl_svc *incl_svc; + + prev = NULL; + SLIST_FOREACH(incl_svc, &svc->incl_svc, next) { + if (incl_svc->svc.handle >= incl_svc_handle) { + break; + } + + prev = incl_svc; + } + + return prev; +} + +static struct peer_incl_svc * +peer_incl_svc_find(const struct peer_svc *svc, uint16_t incl_svc_handle, + struct peer_incl_svc **out_prev) +{ + struct peer_incl_svc *prev; + struct peer_incl_svc *incl_svc; + + prev = peer_incl_svc_find_prev(svc, incl_svc_handle); + if (prev == NULL) { + incl_svc = SLIST_FIRST(&svc->incl_svc); + } else { + incl_svc = SLIST_NEXT(prev, next); + } + + if (incl_svc != NULL && incl_svc->svc.handle != incl_svc_handle) { + incl_svc = NULL; + } + + if (out_prev != NULL) { + *out_prev = prev; + } + return incl_svc; +} + +static void +peer_incl_svc_delete(struct peer_incl_svc *incl_svc) +{ + os_memblock_put(&peer_incl_svc_pool, incl_svc); +} + +static int +peer_inc_add(struct peer *peer, uint16_t svc_start_handle, + const struct ble_gatt_incl_svc *gatt_incl_svc) +{ + struct peer_incl_svc *incl_svc; + struct peer_incl_svc *incl_svc_prev; + struct peer_svc *cur_svc; + struct peer_svc *svc; + struct peer_svc *prev; + + svc = peer_svc_find(peer, gatt_incl_svc->start_handle, &prev); + + if (!svc) { + /* secondary service */ + svc = os_memblock_get(&peer_svc_pool); + if (svc == NULL) { + /* out of memory */ + return BLE_HS_ENOMEM; + } + + memset(svc, 0, sizeof *svc); + svc->svc.start_handle = gatt_incl_svc->start_handle; + svc->svc.end_handle = gatt_incl_svc->end_handle; + memcpy(&svc->svc.uuid, &gatt_incl_svc->uuid, sizeof(ble_uuid_any_t)); + + SLIST_INIT(&svc->chrs); + SLIST_INIT(&svc->incl_svc); + + if (prev == NULL) { + SLIST_INSERT_HEAD(&peer->svcs, svc, next); + } else { + SLIST_INSERT_AFTER(prev, svc, next); + } + } + + /* Including the services into inlucding list */ + + cur_svc = peer_svc_find_range(peer, gatt_incl_svc->handle); + + if (cur_svc == NULL) { + /* Can't find service for discovered included service; this shouldn't + * happen. + */ + assert(0); + return BLE_HS_EUNKNOWN; + } + + incl_svc = peer_incl_svc_find(cur_svc, gatt_incl_svc->handle, &incl_svc_prev); + if (incl_svc != NULL) { + /* Already discovered */ + return 0; + } + + incl_svc = os_memblock_get(&peer_incl_svc_pool); + if (incl_svc == NULL) { + return BLE_HS_ENOMEM; + } + + incl_svc->svc = *gatt_incl_svc; + + if (incl_svc_prev == NULL) { + SLIST_INSERT_HEAD(&cur_svc->incl_svc, incl_svc, next); + } else { + SLIST_INSERT_AFTER(incl_svc_prev, incl_svc, next); + } + + BLE_HS_LOG(DEBUG, "Inc Service added with handle = %d", gatt_incl_svc->handle); + + return 0; +} + + +static int +peer_inc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + const struct ble_gatt_incl_svc *service, void *arg) +{ + struct peer *peer; + int rc; + + peer = arg; + assert(peer->conn_handle == conn_handle); + + switch (error->status) { + case 0: + rc = peer_inc_add(peer, peer->cur_svc->svc.start_handle, service); + break; + + case BLE_HS_EDONE: + peer_disc_incs(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_incs(struct peer *peer) +{ + struct peer_svc *svc; + int rc; + + if (peer->cur_svc == NULL) { + peer->cur_svc = SLIST_FIRST(&peer->svcs); + } else { + peer->cur_svc = SLIST_NEXT(peer->cur_svc, next); + if (peer->cur_svc == NULL) { + if (peer->disc_prev_chr_val > 0) { + peer_disc_chrs(peer); + return; + } + } + } + + svc = peer->cur_svc; + if (svc != NULL && !peer_svc_is_empty(svc)) { + rc = ble_gattc_find_inc_svcs(peer->conn_handle, + svc->svc.start_handle, + svc->svc.end_handle, + peer_inc_disced, peer); + if (rc != 0) { + peer_disc_chrs(peer); + } + } else { + peer_disc_chrs(peer); + } +} +#endif + int peer_svc_is_empty(const struct peer_svc *svc) { @@ -576,6 +774,14 @@ peer_svc_delete(struct peer_svc *svc) { struct peer_chr *chr; +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + struct peer_incl_svc *incl_svc; + + while ((incl_svc = SLIST_FIRST(&svc->incl_svc)) != NULL) { + SLIST_REMOVE_HEAD(&svc->incl_svc, next); + peer_incl_svc_delete(incl_svc); + } +#endif while ((chr = SLIST_FIRST(&svc->chrs)) != NULL) { SLIST_REMOVE_HEAD(&svc->chrs, next); peer_chr_delete(chr); @@ -600,10 +806,16 @@ peer_svc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, break; case BLE_HS_EDONE: - /* All services discovered; start discovering characteristics. */ + /* All services discovered; start discovering incs.*/ +#if (MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES)) + peer->cur_svc = NULL; + peer_disc_incs(peer); +#else + /* All services discovered; start discovering characteristics. */ if (peer->disc_prev_chr_val > 0) { peer_disc_chrs(peer); } +#endif rc = 0; break; @@ -711,6 +923,11 @@ peer_free_mem(void) free(peer_svc_mem); peer_svc_mem = NULL; +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + free(peer_incl_svc_mem); + peer_incl_svc_mem = NULL; +#endif + free(peer_chr_mem); peer_chr_mem = NULL; @@ -718,8 +935,13 @@ peer_free_mem(void) peer_dsc_mem = NULL; } +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) +int +peer_init(int max_peers, int max_svcs, int max_incl_svcs, int max_chrs, int max_dscs) +#else int peer_init(int max_peers, int max_svcs, int max_chrs, int max_dscs) +#endif { int rc; @@ -756,6 +978,23 @@ peer_init(int max_peers, int max_svcs, int max_chrs, int max_dscs) goto err; } +#if MYNEWT_VAL(BLE_INCL_SVC_DISCOVERY) || MYNEWT_VAL(BLE_GATT_CACHING_INCLUDE_SERVICES) + peer_incl_svc_mem = malloc( + OS_MEMPOOL_BYTES(max_incl_svcs, sizeof(struct peer_incl_svc))); + + if (peer_incl_svc_mem == NULL) { + rc = BLE_HS_ENOMEM; + goto err; + } + + rc = os_mempool_init(&peer_incl_svc_pool, max_incl_svcs, + sizeof(struct peer_incl_svc), peer_incl_svc_mem, + "peer_incl_svc_pool"); + if (rc != 0) { + rc = BLE_HS_EOS; + goto err; + } +#endif peer_chr_mem = malloc( OS_MEMPOOL_BYTES(max_chrs, sizeof (struct peer_chr))); if (peer_chr_mem == NULL) {