From 84d4edcaa4f1c4848c74757a5be8e28e61c988db Mon Sep 17 00:00:00 2001 From: Roshan Bangar Date: Wed, 8 Mar 2023 10:41:13 +0530 Subject: [PATCH] Dynamic service addition/deletion --- nimble/host/include/host/ble_gatt.h | 39 +- nimble/host/src/ble_att_priv.h | 5 + nimble/host/src/ble_att_svr.c | 36 ++ nimble/host/src/ble_gatt_priv.h | 4 + nimble/host/src/ble_gatts.c | 643 +++++++++++++++++++++++++++- 5 files changed, 724 insertions(+), 3 deletions(-) diff --git a/nimble/host/include/host/ble_gatt.h b/nimble/host/include/host/ble_gatt.h index cb6a63018..41ca814d8 100644 --- a/nimble/host/include/host/ble_gatt.h +++ b/nimble/host/include/host/ble_gatt.h @@ -31,7 +31,7 @@ #include "host/ble_att.h" #include "host/ble_uuid.h" #include "host/ble_esp_gatt.h" - +#include "syscfg/syscfg.h" #ifdef __cplusplus extern "C" { #endif @@ -919,6 +919,18 @@ struct ble_gatt_register_ctxt { typedef void ble_gatt_register_fn(struct ble_gatt_register_ctxt *ctxt, void *arg); +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +struct ble_gatts_clt_cfg { + STAILQ_ENTRY(ble_gatts_clt_cfg) next; + uint16_t chr_val_handle; + uint8_t flags; + uint8_t allowed; +}; + +/** A cached array of handles for the configurable characteristics. */ +STAILQ_HEAD(ble_gatts_clt_cfg_list, ble_gatts_clt_cfg); +#endif + /** * Queues a set of service definitions for registration. All services queued * in this manner get registered when ble_gatts_start() is called. @@ -933,6 +945,31 @@ typedef void ble_gatt_register_fn(struct ble_gatt_register_ctxt *ctxt, */ int ble_gatts_add_svcs(const struct ble_gatt_svc_def *svcs); +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +/** + * Adds a set of services for registration. All services added + * in this manner get registered immidietely. + * + * @param svcs An array of service definitions to queue for + * registration. This array must be + * terminated with an entry whose 'type' + * equals 0. + * + * @return 0 on success; + * BLE_HS_ENOMEM on heap exhaustion. + */ +int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs); +/** + * Deletes a service with corresponding uuid. All services deleted + * in this manner will be deleted immidietely. + * + * @param uuid uuid of the service to be deleted. + * + * @return 0 on success; + * BLE_HS_ENOENT on invalid uuid. + */ +int ble_gatts_delete_svc(const ble_uuid_t *uuid); +#endif /** * Set visibility of local GATT service. Invisible services are not removed * from database but are not discoverable by peer devices. Service Changed diff --git a/nimble/host/src/ble_att_priv.h b/nimble/host/src/ble_att_priv.h index 73b6aeab0..70f2b428f 100644 --- a/nimble/host/src/ble_att_priv.h +++ b/nimble/host/src/ble_att_priv.h @@ -25,6 +25,7 @@ #include "host/ble_att.h" #include "host/ble_uuid.h" #include "nimble/nimble_npl.h" +#include "syscfg/syscfg.h" #ifdef __cplusplus extern "C" { @@ -145,6 +146,10 @@ int ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags, uint8_t min_key_size, uint16_t *handle_id, ble_att_svr_access_fn *cb, void *cb_arg); +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +int ble_att_svr_deregister(uint16_t start_handle, uint16_t end_group_handle); +#endif + struct ble_att_svr_entry { STAILQ_ENTRY(ble_att_svr_entry) ha_next; diff --git a/nimble/host/src/ble_att_svr.c b/nimble/host/src/ble_att_svr.c index 31ce38a02..da48f8bbf 100644 --- a/nimble/host/src/ble_att_svr.c +++ b/nimble/host/src/ble_att_svr.c @@ -69,6 +69,12 @@ ble_att_svr_entry_alloc(void) struct ble_att_svr_entry *entry; entry = os_memblock_get(&ble_att_svr_entry_pool); +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + /* if dynamic services are enabled, try to allocate from heap */ + if (entry == NULL) { + entry = nimble_platform_mem_malloc(sizeof *entry); + } +#endif if (entry != NULL) { memset(entry, 0, sizeof *entry); } @@ -79,7 +85,16 @@ ble_att_svr_entry_alloc(void) static void ble_att_svr_entry_free(struct ble_att_svr_entry *entry) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + if (os_memblock_from(&ble_att_svr_entry_pool, entry)) { + os_memblock_put(&ble_att_svr_entry_pool, entry); + } + else { + nimble_platform_mem_free(entry); + } +#else os_memblock_put(&ble_att_svr_entry_pool, entry); +#endif } /** @@ -134,6 +149,27 @@ ble_att_svr_register(const ble_uuid_t *uuid, uint8_t flags, return 0; } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +/** + * Deregister a host attribute with the BLE stack. + * + * @param start_handle Start handle of the service entry + * @param end_group_handle End group handle of the service entry + * + * @return 0 on success, non-zero error code on failure. + */ +int ble_att_svr_deregister(uint16_t start_handle, uint16_t end_group_handle) { + uint16_t idx; + struct ble_att_svr_entry *entry; + for (idx = start_handle; idx <= end_group_handle; idx++) { + entry = ble_att_svr_find_by_handle(idx); + STAILQ_REMOVE(&ble_att_svr_list, entry, ble_att_svr_entry, ha_next); + ble_att_svr_entry_free(entry); + } + return 0; +} +#endif + uint16_t ble_att_svr_prev_handle(void) { diff --git a/nimble/host/src/ble_gatt_priv.h b/nimble/host/src/ble_gatt_priv.h index 2b1705a10..255be751b 100644 --- a/nimble/host/src/ble_gatt_priv.h +++ b/nimble/host/src/ble_gatt_priv.h @@ -92,7 +92,11 @@ extern STATS_SECT_DECL(ble_gatts_stats) ble_gatts_stats; typedef uint8_t ble_gatts_conn_flags; struct ble_gatts_conn { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_clt_cfg_list clt_cfgs; +#else struct ble_gatts_clt_cfg *clt_cfgs; +#endif int num_clt_cfgs; uint16_t indicate_val_handle; diff --git a/nimble/host/src/ble_gatts.c b/nimble/host/src/ble_gatts.c index 21565f0c0..e382719fe 100644 --- a/nimble/host/src/ble_gatts.c +++ b/nimble/host/src/ble_gatts.c @@ -26,10 +26,20 @@ #include "host/ble_store.h" #include "ble_hs_priv.h" #include "esp_nimble_mem.h" +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +#include "services/gatt/ble_svc_gatt.h" +#endif #define BLE_GATTS_INCLUDE_SZ 6 #define BLE_GATTS_CHR_MAX_SZ 19 +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +enum { + CONN_CLT_CFG_ADD = 1, + CONN_CLT_CFG_REMOVE = 2, +}; +#endif + static const ble_uuid_t *uuid_pri = BLE_UUID16_DECLARE(BLE_ATT_UUID_PRIMARY_SERVICE); static const ble_uuid_t *uuid_sec = @@ -45,17 +55,31 @@ static const struct ble_gatt_svc_def **ble_gatts_svc_defs; static int ble_gatts_num_svc_defs; struct ble_gatts_svc_entry { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + STAILQ_ENTRY(ble_gatts_svc_entry) next; +#endif const struct ble_gatt_svc_def *svc; uint16_t handle; /* 0 means unregistered. */ uint16_t end_group_handle; /* 0xffff means unset. */ }; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +STAILQ_HEAD(ble_gatts_svc_entry_list, ble_gatts_svc_entry); +static struct ble_gatts_svc_entry_list ble_gatts_svc_entries; +static void *ble_gatts_svc_entry_mem; +static struct os_mempool ble_gatts_svc_entry_pool; +#else static struct ble_gatts_svc_entry *ble_gatts_svc_entries; static uint16_t ble_gatts_num_svc_entries; +#endif static os_membuf_t *ble_gatts_clt_cfg_mem; static struct os_mempool ble_gatts_clt_cfg_pool; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +/** A cached list of handles for the configurable characteristics. */ +static struct ble_gatts_clt_cfg_list ble_gatts_clt_cfgs; +#else struct ble_gatts_clt_cfg { uint16_t chr_val_handle; uint8_t flags; @@ -64,6 +88,7 @@ struct ble_gatts_clt_cfg { /** A cached array of handles for the configurable characteristics. */ static struct ble_gatts_clt_cfg *ble_gatts_clt_cfgs; +#endif static int ble_gatts_num_cfgable_chrs; STATS_SECT_DECL(ble_gatts_stats) ble_gatts_stats; @@ -80,6 +105,63 @@ STATS_NAME_START(ble_gatts_stats) STATS_NAME(ble_gatts_stats, dsc_writes) STATS_NAME_END(ble_gatts_stats) +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +static struct ble_gatts_svc_entry * +ble_gatts_svc_entry_alloc(void) +{ + struct ble_gatts_svc_entry *entry; + + entry = os_memblock_get(&ble_gatts_svc_entry_pool); + /* if dynamic services are enabled, try to allocate from heap */ + if (entry == NULL) { + entry = nimble_platform_mem_malloc(sizeof *entry); + } + if (entry != NULL) { + memset(entry, 0, sizeof *entry); + } + + return entry; +} + +static void +ble_gatts_svc_entry_free(struct ble_gatts_svc_entry *entry) +{ + if (os_memblock_from(&ble_gatts_svc_entry_pool, entry)) { + os_memblock_put(&ble_gatts_svc_entry_pool, entry); + } + else { + nimble_platform_mem_free(entry); + } +} + +static struct ble_gatts_clt_cfg * +ble_gatts_clt_cfg_alloc(void) +{ + struct ble_gatts_clt_cfg *cfg; + + cfg = os_memblock_get(&ble_gatts_clt_cfg_pool); + /* if dynamic services are enabled, try to allocate from heap */ + if (cfg == NULL) { + cfg = nimble_platform_mem_malloc(sizeof *cfg); + } + if (cfg != NULL) { + memset(cfg, 0, sizeof *cfg); + } + return cfg; +} + +static void +ble_gatts_clt_cfg_free(struct ble_gatts_clt_cfg *cfg) +{ + if (os_memblock_from(&ble_gatts_clt_cfg_pool, cfg)) { + os_memblock_put(&ble_gatts_clt_cfg_pool, cfg); + } + else { + nimble_platform_mem_free(cfg); + } +} +#endif + static int ble_gatts_svc_access(uint16_t conn_handle, uint16_t attr_handle, uint8_t op, uint16_t offset, struct os_mbuf **om, @@ -425,6 +507,21 @@ ble_gatts_chr_val_access(uint16_t conn_handle, uint16_t attr_handle, return rc; } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +static struct ble_gatts_svc_entry* +ble_gatts_find_svc_entry(const struct ble_gatt_svc_def *svc) +{ + struct ble_gatts_svc_entry *entry; + + STAILQ_FOREACH(entry, &ble_gatts_svc_entries, next) { + if (entry->svc == svc) { + return entry; + } + } + + return NULL; +} +#else static int ble_gatts_find_svc_entry_idx(const struct ble_gatt_svc_def *svc) { @@ -438,12 +535,18 @@ ble_gatts_find_svc_entry_idx(const struct ble_gatt_svc_def *svc) return -1; } +#endif static int ble_gatts_svc_incs_satisfied(const struct ble_gatt_svc_def *svc) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + int i; + struct ble_gatts_svc_entry *entry; +#else int idx; int i; +#endif if (svc->includes == NULL) { /* No included services. */ @@ -451,8 +554,13 @@ ble_gatts_svc_incs_satisfied(const struct ble_gatt_svc_def *svc) } for (i = 0; svc->includes[i] != NULL; i++) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + entry = ble_gatts_find_svc_entry(svc->includes[i]); + if (entry == NULL || entry -> handle == 0) { +#else idx = ble_gatts_find_svc_entry_idx(svc->includes[i]); if (idx == -1 || ble_gatts_svc_entries[idx].handle == 0) { +#endif return 0; } } @@ -583,6 +691,23 @@ ble_gatts_register_dsc(const struct ble_gatt_svc_def *svc, } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +static struct ble_gatts_clt_cfg * +ble_gatts_clt_cfg_find(struct ble_gatts_clt_cfg_list *ble_gatts_clt_cfgs, + uint16_t chr_val_handle) +{ + struct ble_gatts_clt_cfg *cfg; + + STAILQ_FOREACH(cfg, ble_gatts_clt_cfgs, next) { + if (cfg->chr_val_handle == chr_val_handle) { + return cfg; + } + } + + return NULL; +} + +#else static int ble_gatts_clt_cfg_find_idx(struct ble_gatts_clt_cfg *cfgs, uint16_t chr_val_handle) @@ -613,6 +738,7 @@ ble_gatts_clt_cfg_find(struct ble_gatts_clt_cfg *cfgs, return cfgs + idx; } } +#endif static void ble_gatts_subscribe_event(uint16_t conn_handle, uint16_t attr_handle, @@ -676,8 +802,13 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle, return BLE_ATT_ERR_UNLIKELY; } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_find(&conn->bhc_gatt_svr.clt_cfgs, + chr_val_handle); +#else clt_cfg = ble_gatts_clt_cfg_find(conn->bhc_gatt_svr.clt_cfgs, chr_val_handle); +#endif if (clt_cfg == NULL) { return BLE_ATT_ERR_UNLIKELY; } @@ -822,9 +953,11 @@ ble_gatts_register_chr(const struct ble_gatt_svc_def *svc, } if (ble_gatts_chr_clt_cfg_allowed(chr) != 0) { +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) if (ble_gatts_num_cfgable_chrs > ble_hs_max_client_configs) { return BLE_HS_ENOMEM; } +#endif ble_gatts_num_cfgable_chrs++; } @@ -927,7 +1060,11 @@ ble_gatts_register_svc(const struct ble_gatt_svc_def *svc, const struct ble_gatt_chr_def *chr; struct ble_gatt_register_ctxt register_ctxt; const ble_uuid_t *uuid; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_svc_entry *entry; +#else int idx; +#endif int rc; int i; @@ -964,10 +1101,17 @@ ble_gatts_register_svc(const struct ble_gatt_svc_def *svc, /* Register each include. */ if (svc->includes != NULL) { for (i = 0; svc->includes[i] != NULL; i++) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + entry = ble_gatts_find_svc_entry(svc->includes[i]); + BLE_HS_DBG_ASSERT_EVAL(entry != NULL); + + rc = ble_gatts_register_inc(entry); +#else idx = ble_gatts_find_svc_entry_idx(svc->includes[i]); BLE_HS_DBG_ASSERT_EVAL(idx != -1); rc = ble_gatts_register_inc(ble_gatts_svc_entries + idx); +#endif if (rc != 0) { return rc; } @@ -996,12 +1140,18 @@ ble_gatts_register_round(int *out_num_registered, ble_gatt_register_fn *cb, struct ble_gatts_svc_entry *entry; uint16_t handle; int rc; +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int i; +#endif *out_num_registered = 0; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + STAILQ_FOREACH(entry, &ble_gatts_svc_entries, next) { +#else for (i = 0; i < ble_gatts_num_svc_entries; i++) { entry = ble_gatts_svc_entries + i; +#endif if (entry->handle == 0) { rc = ble_gatts_register_svc(entry->svc, &handle, cb, cb_arg); switch (rc) { @@ -1057,11 +1207,25 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs, int total_registered; int cur_registered; int num_svcs; - int idx; int rc; int i; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_svc_entry *entry; +#else + int idx; +#endif for (i = 0; svcs[i].type != BLE_GATT_SVC_TYPE_END; i++) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + entry = ble_gatts_svc_entry_alloc(); + if (entry == NULL) { + return BLE_HS_ENOMEM; + } + entry -> svc = svcs + i; + entry -> handle = 0; + entry -> end_group_handle = 0xffff; + STAILQ_INSERT_TAIL(&ble_gatts_svc_entries, entry, next); +#else idx = ble_gatts_num_svc_entries + i; if (idx >= ble_hs_max_services) { return BLE_HS_ENOMEM; @@ -1070,9 +1234,12 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs, ble_gatts_svc_entries[idx].svc = svcs + i; ble_gatts_svc_entries[idx].handle = 0; ble_gatts_svc_entries[idx].end_group_handle = 0xffff; +#endif } num_svcs = i; +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) ble_gatts_num_svc_entries += num_svcs; +#endif total_registered = 0; while (total_registered < num_svcs) { @@ -1086,12 +1253,14 @@ ble_gatts_register_svcs(const struct ble_gatt_svc_def *svcs, return 0; } +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) static int ble_gatts_clt_cfg_size(void) { return ble_gatts_num_cfgable_chrs * sizeof (struct ble_gatts_clt_cfg); } +#endif /** * Handles GATT server clean up for a terminated connection: * o Informs the application that the peer is no longer subscribed to any @@ -1101,11 +1270,17 @@ ble_gatts_clt_cfg_size(void) void ble_gatts_connection_broken(uint16_t conn_handle) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_clt_cfg_list clt_cfgs; + struct ble_hs_conn *conn; + struct ble_gatts_clt_cfg *clt_cfg; +#else struct ble_gatts_clt_cfg *clt_cfgs; struct ble_hs_conn *conn; int num_clt_cfgs; int rc; int i; +#endif /* Find the specified connection and extract its CCCD entries. Extracting * the clt_cfg pointer and setting the original to null is done for two @@ -1121,10 +1296,16 @@ ble_gatts_connection_broken(uint16_t conn_handle) ble_hs_lock(); conn = ble_hs_conn_find(conn_handle); if (conn != NULL) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + memcpy(&clt_cfgs, &(conn->bhc_gatt_svr.clt_cfgs), sizeof(struct ble_gatts_clt_cfg_list)); + + STAILQ_INIT(&(conn->bhc_gatt_svr.clt_cfgs)); +#else clt_cfgs = conn->bhc_gatt_svr.clt_cfgs; num_clt_cfgs = conn->bhc_gatt_svr.num_clt_cfgs; conn->bhc_gatt_svr.clt_cfgs = NULL; +#endif conn->bhc_gatt_svr.num_clt_cfgs = 0; } ble_hs_unlock(); @@ -1141,6 +1322,18 @@ ble_gatts_connection_broken(uint16_t conn_handle) /* Now that the mutex is unlocked, inform the application that the peer is * no longer subscribed to any characteristic updates. */ +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + if (STAILQ_FIRST(&clt_cfgs) != NULL) { + clt_cfg = NULL; + while ((clt_cfg = STAILQ_FIRST(&clt_cfgs)) != NULL) { + ble_gatts_subscribe_event(conn_handle, clt_cfg->chr_val_handle, + BLE_GAP_SUBSCRIBE_REASON_TERM, + clt_cfg->flags, 0); + STAILQ_REMOVE_HEAD(&clt_cfgs, next); + ble_gatts_clt_cfg_free(clt_cfg); + } + } +#else if (clt_cfgs != NULL) { for (i = 0; i < num_clt_cfgs; i++) { ble_gatts_subscribe_event(conn_handle, clt_cfgs[i].chr_val_handle, @@ -1151,6 +1344,7 @@ ble_gatts_connection_broken(uint16_t conn_handle) rc = os_memblock_put(&ble_gatts_clt_cfg_pool, clt_cfgs); BLE_HS_DBG_ASSERT_EVAL(rc == 0); } +#endif } static void @@ -1164,11 +1358,37 @@ ble_gatts_free_svc_defs(void) static void ble_gatts_free_mem(void) { + +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_svc_entry *entry; + struct ble_gatts_clt_cfg *clt_cfg; + /* free client configs memory */ + if (STAILQ_FIRST(&ble_gatts_clt_cfgs) != NULL) { + clt_cfg = NULL; + while ((clt_cfg = STAILQ_FIRST(&ble_gatts_clt_cfgs)) != NULL) { + STAILQ_REMOVE_HEAD(&ble_gatts_clt_cfgs, next); + ble_gatts_clt_cfg_free(clt_cfg); + } + } +#endif nimble_platform_mem_free(ble_gatts_clt_cfg_mem); ble_gatts_clt_cfg_mem = NULL; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + /* free services memory */ + if (STAILQ_FIRST(&ble_gatts_svc_entries) != NULL) { + entry = NULL; + while ((entry = STAILQ_FIRST(&ble_gatts_svc_entries)) != NULL) { + STAILQ_REMOVE_HEAD(&ble_gatts_svc_entries, next); + ble_gatts_svc_entry_free(entry); + } + } + nimble_platform_mem_free(ble_gatts_svc_entry_mem); + ble_gatts_svc_entry_mem = NULL; +#else nimble_platform_mem_free(ble_gatts_svc_entries); ble_gatts_svc_entries = NULL; +#endif } @@ -1193,7 +1413,11 @@ ble_gatts_start(void) uint16_t allowed_flags; ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC); int num_elems; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_clt_cfg *clt_cfg; +#else int idx; +#endif int rc; int i; @@ -1221,16 +1445,33 @@ ble_gatts_start(void) } if (ble_hs_max_services > 0) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + ble_gatts_svc_entry_mem = + nimble_platform_mem_malloc(ble_hs_max_services * sizeof(struct ble_gatts_svc_entry)); + if (ble_gatts_svc_entry_mem == NULL) { +#else ble_gatts_svc_entries = nimble_platform_mem_malloc(ble_hs_max_services * sizeof *ble_gatts_svc_entries); if (ble_gatts_svc_entries == NULL) { +#endif rc = BLE_HS_ENOMEM; goto done; } } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + rc = os_mempool_init(&ble_gatts_svc_entry_pool, ble_hs_max_services, + sizeof (struct ble_gatts_svc_entry), + ble_gatts_svc_entry_mem, "ble_gatts_svc_entry_pool"); + if (rc != 0) { + rc = BLE_HS_EOS; + goto done; + } +#else ble_gatts_num_svc_entries = 0; +#endif + for (i = 0; i < ble_gatts_num_svc_defs; i++) { rc = ble_gatts_register_svcs(ble_gatts_svc_defs[i], ble_hs_cfg.gatts_register_cb, @@ -1247,15 +1488,24 @@ ble_gatts_start(void) } /* Initialize client-configuration memory pool. */ +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + num_elems = ble_hs_max_client_configs; + rc = os_mempool_init(&ble_gatts_clt_cfg_pool, num_elems, + sizeof(struct ble_gatts_clt_cfg), + ble_gatts_clt_cfg_mem, + "ble_gatts_clt_cfg_pool"); +#else num_elems = ble_hs_max_client_configs / ble_gatts_num_cfgable_chrs; rc = os_mempool_init(&ble_gatts_clt_cfg_pool, num_elems, ble_gatts_clt_cfg_size(), ble_gatts_clt_cfg_mem, "ble_gatts_clt_cfg_pool"); +#endif if (rc != 0) { rc = BLE_HS_EOS; goto done; } +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) /* Allocate the cached array of handles for the configuration * characteristics. */ @@ -1267,19 +1517,34 @@ ble_gatts_start(void) memset (ble_gatts_clt_cfgs, 0, sizeof *ble_gatts_clt_cfgs); +#endif /* Fill the cache. */ +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = NULL; +#else idx = 0; +#endif ha = NULL; while ((ha = ble_att_svr_find_by_uuid(ha, &uuid.u, 0xffff)) != NULL) { chr = ha->ha_cb_arg; allowed_flags = ble_gatts_chr_clt_cfg_allowed(chr); if (allowed_flags != 0) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_alloc(); + BLE_HS_DBG_ASSERT_EVAL(clt_cfg != NULL); + + clt_cfg->chr_val_handle = ha->ha_handle_id + 1; + clt_cfg->allowed = allowed_flags; + clt_cfg->flags = 0; + STAILQ_INSERT_TAIL(&ble_gatts_clt_cfgs, clt_cfg, next); +#else BLE_HS_DBG_ASSERT_EVAL(idx < ble_gatts_num_cfgable_chrs); ble_gatts_clt_cfgs[idx].chr_val_handle = ha->ha_handle_id + 1; ble_gatts_clt_cfgs[idx].allowed = allowed_flags; ble_gatts_clt_cfgs[idx].flags = 0; idx++; +#endif } } @@ -1303,6 +1568,43 @@ ble_gatts_conn_can_alloc(void) int ble_gatts_conn_init(struct ble_gatts_conn *gatts_conn) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_clt_cfg *clt_cfg; + struct ble_gatts_clt_cfg *clt_cfg_new; + struct ble_gatts_clt_cfg_list clt_cfgs; + int rc; + STAILQ_INIT(&clt_cfgs); + rc = 0; + + if (ble_gatts_num_cfgable_chrs > 0) { + /* Initialize the client configuration with a copy of the cache. */ + STAILQ_FOREACH(clt_cfg, &ble_gatts_clt_cfgs, next) { + clt_cfg_new = ble_gatts_clt_cfg_alloc(); + if (clt_cfg_new == NULL) { + rc = BLE_HS_ENOMEM; + goto done; + } + memcpy(clt_cfg_new, clt_cfg, sizeof(struct ble_gatts_clt_cfg)); + STAILQ_INSERT_TAIL(&clt_cfgs, clt_cfg_new, next); + } + memcpy(&(gatts_conn->clt_cfgs), &clt_cfgs, sizeof(struct ble_gatts_clt_cfg_list)); + gatts_conn->num_clt_cfgs = ble_gatts_num_cfgable_chrs; + } else { + STAILQ_INIT(&(gatts_conn->clt_cfgs)); + gatts_conn->num_clt_cfgs = 0; + } + +done: + if (rc != 0) { + /* free clt_cfgs_list entries */ + clt_cfg = NULL; + while ((clt_cfg = STAILQ_FIRST(&clt_cfgs)) != NULL) { + STAILQ_REMOVE_HEAD(&clt_cfgs, next); + ble_gatts_clt_cfg_free(clt_cfg); + } + return rc; + } +#else if (ble_gatts_num_cfgable_chrs > 0) { gatts_conn->clt_cfgs = os_memblock_get(&ble_gatts_clt_cfg_pool); if (gatts_conn->clt_cfgs == NULL) { @@ -1317,11 +1619,11 @@ ble_gatts_conn_init(struct ble_gatts_conn *gatts_conn) gatts_conn->clt_cfgs = NULL; gatts_conn->num_clt_cfgs = 0; } +#endif return 0; } - /** * Schedules a notification or indication for the specified peer-CCCD pair. If * the update should be sent immediately, it is indicated in the return code. @@ -1380,7 +1682,9 @@ ble_gatts_send_next_indicate(uint16_t conn_handle) struct ble_hs_conn *conn; uint16_t chr_val_handle; int rc; +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int i; +#endif /* Assume no pending indications. */ chr_val_handle = 0; @@ -1389,8 +1693,12 @@ ble_gatts_send_next_indicate(uint16_t conn_handle) conn = ble_hs_conn_find(conn_handle); if (conn != NULL) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + STAILQ_FOREACH(clt_cfg, &conn->bhc_gatt_svr.clt_cfgs, next) { +#else for (i = 0; i < conn->bhc_gatt_svr.num_clt_cfgs; i++) { clt_cfg = conn->bhc_gatt_svr.clt_cfgs + i; +#endif if (clt_cfg->flags & BLE_GATTS_CLT_CFG_F_MODIFIED) { BLE_HS_DBG_ASSERT(clt_cfg->flags & BLE_GATTS_CLT_CFG_F_INDICATE); @@ -1428,18 +1736,28 @@ ble_gatts_rx_indicate_ack(uint16_t conn_handle, uint16_t chr_val_handle) struct ble_store_value_cccd cccd_value; struct ble_gatts_clt_cfg *clt_cfg; struct ble_hs_conn *conn; +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int clt_cfg_idx; +#endif int persist; int rc; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_find(&ble_gatts_clt_cfgs, + chr_val_handle); + if (clt_cfg == NULL) { +#else clt_cfg_idx = ble_gatts_clt_cfg_find_idx(ble_gatts_clt_cfgs, chr_val_handle); if (clt_cfg_idx == -1) { +#endif /* This characteristic does not have a CCCD. */ return BLE_HS_ENOENT; } +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) clt_cfg = ble_gatts_clt_cfgs + clt_cfg_idx; +#endif if (!(clt_cfg->allowed & BLE_GATTS_CLT_CFG_F_INDICATE)) { /* This characteristic does not allow indications. */ return BLE_HS_ENOENT; @@ -1461,8 +1779,13 @@ ble_gatts_rx_indicate_ack(uint16_t conn_handle, uint16_t chr_val_handle) * been modified since we sent the indication, there is no indication * pending. */ +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_find(&conn->bhc_gatt_svr.clt_cfgs, chr_val_handle); + BLE_HS_DBG_ASSERT(clt_cfg != NULL); +#else BLE_HS_DBG_ASSERT(conn->bhc_gatt_svr.num_clt_cfgs > clt_cfg_idx); clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx; +#endif BLE_HS_DBG_ASSERT(clt_cfg->chr_val_handle == chr_val_handle); persist = conn->bhc_sec_state.bonded && @@ -1506,7 +1829,9 @@ ble_gatts_chr_updated(uint16_t chr_val_handle) struct ble_gatts_clt_cfg *clt_cfg; struct ble_hs_conn *conn; int new_notifications = 0; +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int clt_cfg_idx; +#endif int persist; int rc; int i; @@ -1514,9 +1839,15 @@ ble_gatts_chr_updated(uint16_t chr_val_handle) /* Determine if notifications or indications are allowed for this * characteristic. If not, return immediately. */ +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_find(&ble_gatts_clt_cfgs, + chr_val_handle); + if (clt_cfg == NULL) { +#else clt_cfg_idx = ble_gatts_clt_cfg_find_idx(ble_gatts_clt_cfgs, chr_val_handle); if (clt_cfg_idx == -1) { +#endif return; } @@ -1532,9 +1863,14 @@ ble_gatts_chr_updated(uint16_t chr_val_handle) break; } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_find(&conn->bhc_gatt_svr.clt_cfgs, + chr_val_handle); +#else BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs > clt_cfg_idx); clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx; +#endif BLE_HS_DBG_ASSERT_EVAL(clt_cfg->chr_val_handle == chr_val_handle); /* Mark the CCCD entry as modified. */ @@ -1610,15 +1946,23 @@ ble_gatts_tx_notifications_one_chr(uint16_t chr_val_handle) struct ble_hs_conn *conn; uint16_t conn_handle; uint8_t att_op; +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int clt_cfg_idx; +#endif int i; /* Determine if notifications / indications are enabled for this * characteristic. */ +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_find(&ble_gatts_clt_cfgs, + chr_val_handle); + if (clt_cfg == NULL) { +#else clt_cfg_idx = ble_gatts_clt_cfg_find_idx(ble_gatts_clt_cfgs, chr_val_handle); if (clt_cfg_idx == -1) { +#endif return; } @@ -1627,9 +1971,14 @@ ble_gatts_tx_notifications_one_chr(uint16_t chr_val_handle) conn = ble_hs_conn_find_by_idx(i); if (conn != NULL) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_find(&conn->bhc_gatt_svr.clt_cfgs, + chr_val_handle); +#else BLE_HS_DBG_ASSERT_EVAL(conn->bhc_gatt_svr.num_clt_cfgs > clt_cfg_idx); clt_cfg = conn->bhc_gatt_svr.clt_cfgs + clt_cfg_idx; +#endif BLE_HS_DBG_ASSERT_EVAL(clt_cfg->chr_val_handle == chr_val_handle); /* Determine what type of command should get sent, if any. */ @@ -1675,10 +2024,16 @@ void ble_gatts_tx_notifications(void) { uint16_t chr_val_handle; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_clt_cfg *clt_cfg; + STAILQ_FOREACH(clt_cfg, &ble_gatts_clt_cfgs, next) { + chr_val_handle = clt_cfg->chr_val_handle; +#else int i; for (i = 0; i < ble_gatts_num_cfgable_chrs; i++) { chr_val_handle = ble_gatts_clt_cfgs[i].chr_val_handle; +#endif ble_gatts_tx_notifications_one_chr(chr_val_handle); } } @@ -1690,7 +2045,9 @@ ble_gatts_bonding_established(uint16_t conn_handle) struct ble_gatts_clt_cfg *clt_cfg; struct ble_gatts_conn *gatt_srv; struct ble_hs_conn *conn; +#if !MYNEWT_VAL(BLE_DYNAMIC_SERVICE) int i; +#endif ble_hs_lock(); @@ -1703,8 +2060,12 @@ ble_gatts_bonding_established(uint16_t conn_handle) ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type); gatt_srv = &conn->bhc_gatt_svr; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + STAILQ_FOREACH(clt_cfg, &gatt_srv->clt_cfgs, next) { +#else for (i = 0; i < gatt_srv->num_clt_cfgs; ++i) { clt_cfg = &gatt_srv->clt_cfgs[i]; +#endif if (clt_cfg->flags != 0) { cccd_value.chr_val_handle = clt_cfg->chr_val_handle; @@ -1770,8 +2131,13 @@ ble_gatts_bonding_restored(uint16_t conn_handle) conn = ble_hs_conn_find(conn_handle); BLE_HS_DBG_ASSERT(conn != NULL); +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + clt_cfg = ble_gatts_clt_cfg_find(&conn->bhc_gatt_svr.clt_cfgs, + cccd_value.chr_val_handle); +#else clt_cfg = ble_gatts_clt_cfg_find(conn->bhc_gatt_svr.clt_cfgs, cccd_value.chr_val_handle); +#endif if (clt_cfg != NULL) { clt_cfg->flags = cccd_value.flags; @@ -1819,6 +2185,21 @@ ble_gatts_bonding_restored(uint16_t conn_handle) } } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +static struct ble_gatts_svc_entry * +ble_gatts_find_svc_entry_by_uuid(const ble_uuid_t *uuid) +{ + struct ble_gatts_svc_entry *entry; + + STAILQ_FOREACH(entry, &ble_gatts_svc_entries, next) { + if (ble_uuid_cmp(uuid, entry->svc->uuid) == 0) { + return entry; + } + } + + return NULL; +} +#else static struct ble_gatts_svc_entry * ble_gatts_find_svc_entry(const ble_uuid_t *uuid) { @@ -1834,6 +2215,7 @@ ble_gatts_find_svc_entry(const ble_uuid_t *uuid) return NULL; } +#endif static int ble_gatts_find_svc_chr_attr(const ble_uuid_t *svc_uuid, @@ -1846,7 +2228,11 @@ ble_gatts_find_svc_chr_attr(const ble_uuid_t *svc_uuid, struct ble_att_svr_entry *next; struct ble_att_svr_entry *cur; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + svc_entry = ble_gatts_find_svc_entry_by_uuid(svc_uuid); +#else svc_entry = ble_gatts_find_svc_entry(svc_uuid); +#endif if (svc_entry == NULL) { return BLE_HS_ENOENT; } @@ -1891,7 +2277,11 @@ ble_gatts_find_svc(const ble_uuid_t *uuid, uint16_t *out_handle) { struct ble_gatts_svc_entry *entry; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + entry = ble_gatts_find_svc_entry_by_uuid(uuid); +#else entry = ble_gatts_find_svc_entry(uuid); +#endif if (entry == NULL) { return BLE_HS_ENOENT; } @@ -1967,6 +2357,220 @@ ble_gatts_find_dsc(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid, } } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) +static void ble_gatts_add_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle, uint16_t allowed_flags, uint8_t flags) { + struct ble_gatts_clt_cfg *cfg; + cfg = ble_gatts_clt_cfg_alloc(); + BLE_HS_DBG_ASSERT_EVAL(cfg != NULL); + + cfg->chr_val_handle = chr_val_handle; + cfg->allowed = allowed_flags; + cfg->flags = flags; + STAILQ_INSERT_TAIL(clt_cfgs, cfg, next); +} + +static void ble_gatts_remove_clt_cfg(struct ble_gatts_clt_cfg_list *clt_cfgs, uint16_t chr_val_handle) { + struct ble_gatts_clt_cfg *cfg; + + STAILQ_FOREACH(cfg, clt_cfgs, next) { + if (cfg->chr_val_handle == chr_val_handle) { + break; + } + } + if (cfg != NULL) { + STAILQ_REMOVE(clt_cfgs, cfg, ble_gatts_clt_cfg, next); + ble_gatts_clt_cfg_free(cfg); + } +} + +/* takes two arguments +arg[0] : added/removed +arg[1] : affected chr_val_handle +arg[2] : allowed_flags +*/ +static int update_conn_clt_cfg(struct ble_hs_conn *conn, void *arg) { + uint16_t action = ((uint16_t *) arg)[0]; + uint16_t chr_val_handle = ((uint16_t *) arg)[1]; + uint16_t allowed_flags; + switch(action) { + case 1: + /* added */ + allowed_flags = ((uint16_t *) arg)[2]; + ble_gatts_add_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, chr_val_handle, + allowed_flags, 0); + (conn->bhc_gatt_svr.num_clt_cfgs)++; + return 0; + case 2: + /* removed */ + ble_gatts_remove_clt_cfg(&conn->bhc_gatt_svr.clt_cfgs, + chr_val_handle); + (conn->bhc_gatt_svr.num_clt_cfgs)--; + return 0; + default: + BLE_HS_DBG_ASSERT(0); + } + return 0; +} +static struct ble_gatts_clt_cfg * ble_gatts_get_last_cfg(struct ble_gatts_clt_cfg_list *ble_gatts_clt_cfgs) +{ + struct ble_gatts_clt_cfg *cfg, *prev; + prev = NULL; + STAILQ_FOREACH(cfg, ble_gatts_clt_cfgs, next) { + prev = cfg; + } + return prev; +} +int ble_gatts_add_dynamic_svcs(const struct ble_gatt_svc_def *svcs) { + void *p; + int i; + int rc = 0; + struct ble_att_svr_entry *ha; + struct ble_gatt_chr_def *chr; + struct ble_gatts_svc_entry *entry; + ble_uuid16_t uuid = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC); + uint16_t allowed_flags; + struct ble_gatts_clt_cfg *cfg; + uint16_t arg[3]; + uint16_t start_handle, end_handle; + + p = nimble_platform_mem_malloc(sizeof *ble_gatts_svc_defs); + if (p == NULL) { + rc = BLE_HS_ENOMEM; + goto done; + } + ble_hs_lock(); + + ble_gatts_svc_defs = p; + ble_gatts_svc_defs[0] = svcs; + rc = ble_gatts_register_svcs(ble_gatts_svc_defs[0], + ble_hs_cfg.gatts_register_cb, + ble_hs_cfg.gatts_register_arg); + if (rc != 0) { + goto done; +#if BLE_HS_DEBUG + BLE_HS_DBG_ASSERT(0); /* memory leak expected */ +#endif + } + ble_gatts_free_svc_defs(); + /* Fill the cache. */ + cfg = ble_gatts_get_last_cfg(&ble_gatts_clt_cfgs); + ha = ble_att_svr_find_by_handle(cfg->chr_val_handle - 1); + while ((ha = ble_att_svr_find_by_uuid(ha, &uuid.u, 0xffff)) != NULL) { + chr = ha->ha_cb_arg; + allowed_flags = ble_gatts_chr_clt_cfg_allowed(chr); + if (allowed_flags != 0) { + ble_gatts_add_clt_cfg(&ble_gatts_clt_cfgs, ha->ha_handle_id + 1, allowed_flags, 0); + /* update connections */ + arg[0] = CONN_CLT_CFG_ADD; + arg[1] = ha->ha_handle_id + 1; + arg[2] = allowed_flags; + ble_hs_conn_foreach(update_conn_clt_cfg, arg); + } + } + /* send service change indication */ + i = 0; + entry = ble_gatts_find_svc_entry(&svcs[i]); + start_handle = entry->handle; + while(svcs[i].type != BLE_GATT_SVC_TYPE_END) { + i++; + } + entry = ble_gatts_find_svc_entry(&svcs[i - 1]); + end_handle = entry->end_group_handle; + ble_svc_gatt_changed(start_handle, end_handle); +done: + ble_hs_unlock(); + return rc; +} + +static int +ble_gatts_deregister_svc(const ble_uuid_t *uuid) { + int rc; + + struct ble_gatts_svc_entry *entry; + entry = ble_gatts_find_svc_entry_by_uuid(uuid); + if (entry == NULL) { + /* no such service */ + return BLE_HS_ENOENT; + } + rc = 0; + /* if the service is not yet registered, no need to deregister */ + if (entry->handle != 0) { + rc = ble_att_svr_deregister(entry->handle, entry->end_group_handle); + } + return rc; +} + +static int +ble_gatts_remove_svc_entry(const ble_uuid_t *uuid) +{ + struct ble_gatts_svc_entry *entry; + + STAILQ_FOREACH(entry, &ble_gatts_svc_entries, next) { + if (ble_uuid_cmp(uuid, entry->svc->uuid) == 0) { + break; + } + } + if (entry == NULL) { + return BLE_HS_ENOENT; + } + STAILQ_REMOVE(&ble_gatts_svc_entries, entry, ble_gatts_svc_entry, next); + ble_gatts_svc_entry_free(entry); + return 0; +} + +int ble_gatts_delete_svc(const ble_uuid_t *uuid) { + int rc; + struct ble_gatts_svc_entry * entry; + int chr_val_handle; + struct ble_gatt_chr_def *chr; + uint16_t allowed_flags; + uint16_t arg[2]; + ble_uuid16_t uuid_chr = BLE_UUID16_INIT(BLE_ATT_UUID_CHARACTERISTIC); + struct ble_att_svr_entry *ha; + uint16_t start_handle, end_handle; + /* Update the cache. and connections*/ + ble_hs_lock(); + entry = ble_gatts_find_svc_entry_by_uuid(uuid); + if (entry == NULL) { + rc = BLE_HS_ENOENT; + goto done; + } + ha = ble_att_svr_find_by_handle(entry->handle); + if (ha == NULL) { + rc = BLE_HS_ENOENT; + goto done; + } + while ((ha = ble_att_svr_find_by_uuid(ha, &uuid_chr.u, entry->end_group_handle)) != NULL) { + chr = ha->ha_cb_arg; + allowed_flags = ble_gatts_chr_clt_cfg_allowed(chr); + if (allowed_flags != 0) { + chr_val_handle = ha->ha_handle_id + 1; + ble_gatts_remove_clt_cfg(&ble_gatts_clt_cfgs, chr_val_handle); + + /* update connections */ + arg[0] = CONN_CLT_CFG_REMOVE; + arg[1] = chr_val_handle; + ble_hs_conn_foreach(update_conn_clt_cfg, arg); + } + } + /* keep the start handle and end handle before deleting the service */ + entry = ble_gatts_find_svc_entry_by_uuid(uuid); + start_handle = entry->handle; + end_handle = entry->end_group_handle; + /* deregister service now */ + rc = ble_gatts_deregister_svc(uuid); + +done: + if (rc == 0) { + rc = ble_gatts_remove_svc_entry(uuid); + /* send service change indication */ + ble_svc_gatt_changed(start_handle, end_handle); + } + ble_hs_unlock(); + return rc; +} +#endif + int ble_gatts_add_svcs(const struct ble_gatt_svc_def *svcs) { @@ -2000,10 +2604,16 @@ done: int ble_gatts_svc_set_visibility(uint16_t handle, int visible) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_svc_entry *entry; + + STAILQ_FOREACH(entry, &ble_gatts_svc_entries, next) { +#else int i; for (i = 0; i < ble_gatts_num_svc_entries; i++) { struct ble_gatts_svc_entry *entry = &ble_gatts_svc_entries[i]; +#endif if (entry->handle == handle) { if (visible) { @@ -2150,6 +2760,15 @@ ble_gatts_count_cfg(const struct ble_gatt_svc_def *defs) void ble_gatts_lcl_svc_foreach(ble_gatt_svc_foreach_fn cb, void *arg) { +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_svc_entry *entry; + + STAILQ_FOREACH(entry, &ble_gatts_svc_entries, next) { + cb(entry -> svc, + entry -> handle, + entry -> end_group_handle, arg); + } +#else int i; for (i = 0; i < ble_gatts_num_svc_entries; i++) { @@ -2157,12 +2776,16 @@ ble_gatts_lcl_svc_foreach(ble_gatt_svc_foreach_fn cb, void *arg) ble_gatts_svc_entries[i].handle, ble_gatts_svc_entries[i].end_group_handle, arg); } +#endif } int ble_gatts_reset(void) { int rc; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + struct ble_gatts_svc_entry *entry; +#endif ble_hs_lock(); @@ -2173,6 +2796,14 @@ ble_gatts_reset(void) ble_att_svr_reset(); ble_gatts_num_cfgable_chrs = 0; rc = 0; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + /* free svc entries */ + while ((entry = STAILQ_FIRST(&ble_gatts_svc_entries)) != NULL) { + STAILQ_REMOVE_HEAD(&ble_gatts_svc_entries, next); + ble_gatts_svc_entry_free(entry); + } + +#endif /* Note: gatts memory gets freed on next call to ble_gatts_start(). */ } @@ -2188,7 +2819,11 @@ ble_gatts_init(void) int rc; ble_gatts_num_cfgable_chrs = 0; +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + STAILQ_INIT(&ble_gatts_clt_cfgs); +#else ble_gatts_clt_cfgs = NULL; +#endif rc = stats_init_and_reg( STATS_HDR(ble_gatts_stats), STATS_SIZE_INIT_PARMS(ble_gatts_stats, @@ -2197,6 +2832,10 @@ ble_gatts_init(void) return BLE_HS_EOS; } +#if MYNEWT_VAL(BLE_DYNAMIC_SERVICE) + STAILQ_INIT(&ble_gatts_svc_entries); +#endif + return 0; }