Merge branch 'fix/gatt_cache_peer_init_incl_svc_v5.2' into 'release/v5.2'

fix(nimble): Added parameter in peer_init for included service allocation (v5.2)

See merge request espressif/esp-idf!39915

components/bt/host/nimble/Kconfig.in

@@ -737,9 +737,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"
+    bool
-    default y if BT_NIMBLE_GATT_CACHING_INCLUDE_SERVICES
+    default y if BT_NIMBLE_GATT_CACHING && BT_NIMBLE_GATT_CACHING_INCLUDE_SERVICES
-    default n
+    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

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. */

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. */

examples/bluetooth/nimble/ble_gattc_gatts_coex/main/main.c

@@ -0,0 +1,565 @@
+/*
+ * SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Unlicense OR CC0-1.0
+ */
+#include "host/ble_hs.h"
+#include "host/util/util.h"
+#include "services/gap/ble_svc_gap.h"
+#include "services/gatt/ble_svc_gatt.h"
+#include "nimble/nimble_port.h"
+#include "nimble/nimble_port_freertos.h"
+#include "nvs_flash.h"
+#include "blecoex.h"
+#include "services/ans/ble_svc_ans.h"
+
+static uint8_t own_addr_type;
+static int blecoex_gap_event(struct ble_gap_event *event, void *arg);
+void ble_coex_advertise(void);
+void ble_coex_scan(void);
+static bool client_connect = 0;
+
+static int gatt_svr_access_cb(uint16_t conn_handle, uint16_t attr_handle,
+                              struct ble_gatt_access_ctxt *ctxt, void *arg) {
+    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
+        static uint8_t data[1] = {42};  // Example value
+        os_mbuf_append(ctxt->om, data, sizeof(data));
+    }
+    return 0;
+}
+
+static const ble_uuid128_t gatt_svr_svc_uuid =
+    BLE_UUID128_INIT(0x2d, 0x71, 0xa2, 0x59, 0xb4, 0x58, 0xc8, 0x12,
+                     0x99, 0x99, 0x43, 0x95, 0x12, 0x2f, 0x46, 0x59);
+
+/* A characteristic that can be subscribed to */
+static uint16_t gatt_svr_chr_val_handle;
+static const ble_uuid128_t gatt_svr_chr_uuid =
+    BLE_UUID128_INIT(0x00, 0x00, 0x00, 0x00, 0x11, 0x11, 0x11, 0x11,
+                     0x22, 0x22, 0x22, 0x22, 0x33, 0x33, 0x33, 0x33);
+
+/* A custom descriptor */
+static const ble_uuid128_t gatt_svr_dsc_uuid =
+    BLE_UUID128_INIT(0x01, 0x01, 0x01, 0x01, 0x12, 0x12, 0x12, 0x12,
+                     0x23, 0x23, 0x23, 0x23, 0x34, 0x34, 0x34, 0x34);
+
+static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
+    {
+        /*** Service ***/
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = &gatt_svr_svc_uuid.u,
+        .characteristics = (struct ble_gatt_chr_def[])
+        { {
+                /*** This characteristic can be subscribed to by writing 0x00 and 0x01 to the CCCD ***/
+                .uuid = &gatt_svr_chr_uuid.u,
+                .access_cb = gatt_svr_access_cb,
+                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_INDICATE,
+                .val_handle = &gatt_svr_chr_val_handle,
+                .descriptors = (struct ble_gatt_dsc_def[])
+                { {
+                      .uuid = &gatt_svr_dsc_uuid.u,
+                      .att_flags = BLE_ATT_F_READ,
+                      .access_cb = gatt_svr_access_cb,
+                    }, {
+                      0, /* No more descriptors in this characteristic */
+                    }
+                },
+            }, {
+                0, /* No more characteristics in this service. */
+            }
+        },
+    },
+
+    {
+        0, /* No more services. */
+    },
+};
+
+static int
+blecoex_should_connect(const struct ble_gap_disc_desc *disc)
+{
+    struct ble_hs_adv_fields fields;
+    int rc;
+    int i;
+
+    /* The device has to be advertising connectability. */
+    if (disc->event_type != BLE_HCI_ADV_RPT_EVTYPE_ADV_IND &&
+            disc->event_type != BLE_HCI_ADV_RPT_EVTYPE_DIR_IND) {
+
+        return 0;
+    }
+
+    rc = ble_hs_adv_parse_fields(&fields, disc->data, disc->length_data);
+    if (rc != 0) {
+        return 0;
+    }
+
+    /* The device has to advertise support for the Alert Notification
+     * service (0x1811).
+     */
+    for (i = 0; i < fields.num_uuids16; i++) {
+        if (ble_uuid_u16(&fields.uuids16[i].u) == BLECOEX_SVC_ALERT_UUID) {
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+static void
+connect_if_interesting(void *disc)
+{
+    uint8_t own_addr_type;
+    int rc;
+    ble_addr_t *addr;
+
+    /* Don't do anything if we don't care about this advertiser. */
+    if (!blecoex_should_connect((struct ble_gap_disc_desc *)disc)) {
+        return;
+    }
+
+    /* Scanning must be stopped before a connection can be initiated. */
+    rc = ble_gap_disc_cancel();
+    if (rc != 0) {
+        MODLOG_DFLT(DEBUG, "Failed to cancel scan; rc=%d\n", rc);
+        return;
+    }
+
+    /* Figure out address to use for connect (no privacy for now) */
+    rc = ble_hs_id_infer_auto(0, &own_addr_type);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "error determining address type; rc=%d\n", rc);
+        return;
+    }
+
+    /* Try to connect the the advertiser.  Allow 30 seconds (30000 ms) for
+     * timeout.
+     */
+    addr = &((struct ble_gap_disc_desc *)disc)->addr;
+
+    rc = ble_gap_connect(own_addr_type, addr, 30000, NULL,
+                         blecoex_gap_event, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "Failed to connect to device \n");
+        return;
+    }
+
+    client_connect = 1;
+}
+
+static int
+blecent_on_subscribe(uint16_t conn_handle,
+                     const struct ble_gatt_error *error,
+                     struct ble_gatt_attr *attr,
+                     void *arg)
+{
+    MODLOG_DFLT(INFO, "Subscribe complete; status=%d conn_handle=%d "
+                "attr_handle=%d\n",
+                error->status, conn_handle, attr->handle);
+
+    /* Terminate connection */
+    ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+
+    return 0;
+}
+
+static int
+blecent_on_write(uint16_t conn_handle,
+                 const struct ble_gatt_error *error,
+                 struct ble_gatt_attr *attr,
+                 void *arg)
+{
+    MODLOG_DFLT(INFO,
+                "Write complete; status=%d conn_handle=%d attr_handle=%d\n",
+                error->status, conn_handle, attr->handle);
+
+    /* Subscribe to notifications for the Unread Alert Status characteristic.
+     * A central enables notifications by writing two bytes (1, 0) to the
+     * characteristic's client-characteristic-configuration-descriptor (CCCD).
+     */
+    const struct peer_dsc *dsc;
+    uint8_t value[2];
+    int rc;
+    const struct peer *peer = peer_find(conn_handle);
+
+    dsc = peer_dsc_find_uuid(peer,
+                             BLE_UUID16_DECLARE(BLECOEX_SVC_ALERT_UUID),
+                             BLE_UUID16_DECLARE(BLECOEX_CHR_UNR_ALERT_STAT_UUID),
+                             BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_CFG_UUID16));
+    if (dsc == NULL) {
+        MODLOG_DFLT(ERROR, "Error: Peer lacks a CCCD for the Unread Alert "
+                    "Status characteristic\n");
+        goto err;
+    }
+
+    value[0] = 1;
+    value[1] = 0;
+    rc = ble_gattc_write_flat(conn_handle, dsc->dsc.handle,
+                              value, sizeof value, blecent_on_subscribe, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "Error: Failed to subscribe to characteristic; "
+                    "rc=%d\n", rc);
+        goto err;
+    }
+
+    return 0;
+err:
+    /* Terminate the connection. */
+    return ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+}
+
+
+static int
+blecent_on_read(uint16_t conn_handle,
+                const struct ble_gatt_error *error,
+                struct ble_gatt_attr *attr,
+                void *arg)
+{
+    MODLOG_DFLT(INFO, "Read complete; status=%d conn_handle=%d", error->status,
+                conn_handle);
+    if (error->status == 0) {
+        MODLOG_DFLT(INFO, " attr_handle=%d value=", attr->handle);
+        print_mbuf(attr->om);
+    }
+    MODLOG_DFLT(INFO, "\n");
+
+    /* Write two bytes (99, 100) to the alert-notification-control-point
+     * characteristic.
+     */
+    const struct peer_chr *chr;
+    uint8_t value[2];
+    int rc;
+    const struct peer *peer = peer_find(conn_handle);
+
+    chr = peer_chr_find_uuid(peer,
+                             BLE_UUID16_DECLARE(BLECOEX_SVC_ALERT_UUID),
+                             BLE_UUID16_DECLARE(BLECOEX_CHR_ALERT_NOT_CTRL_PT));
+    if (chr == NULL) {
+        MODLOG_DFLT(ERROR, "Error: Peer doesn't support the Alert "
+                    "Notification Control Point characteristic\n");
+        goto err;
+    }
+    value[0] = 99;
+    value[1] = 100;
+    rc = ble_gattc_write_flat(conn_handle, chr->chr.val_handle,
+                              value, sizeof value, blecent_on_write, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "Error: Failed to write characteristic; rc=%d\n",
+                    rc);
+        goto err;
+    }
+
+    return 0;
+err:
+    /* Terminate the connection. */
+    return ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+}
+
+
+static void
+blecent_read_write_subscribe(const struct peer *peer)
+{
+    const struct peer_chr *chr;
+    int rc;
+
+    /* Read the supported-new-alert-category characteristic. */
+    chr = peer_chr_find_uuid(peer,
+                             BLE_UUID16_DECLARE(BLECOEX_SVC_ALERT_UUID),
+                             BLE_UUID16_DECLARE(BLECOEX_CHR_SUP_NEW_ALERT_CAT_UUID));
+    if (chr == NULL) {
+        MODLOG_DFLT(ERROR, "Error: Peer doesn't support the Supported New "
+                    "Alert Category characteristic\n");
+        goto err;
+    }
+
+    rc = ble_gattc_read(peer->conn_handle, chr->chr.val_handle,
+                        blecent_on_read, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "Error: Failed to read characteristic; rc=%d\n",
+                    rc);
+        goto err;
+    }
+
+    return;
+err:
+    /* Terminate the connection. */
+    ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+}
+
+
+static void
+blecent_on_disc_complete(const struct peer *peer, int status, void *arg)
+{
+
+    if (status != 0) {
+        /* Service discovery failed.  Terminate the connection. */
+        MODLOG_DFLT(ERROR, "Error: Service discovery failed; status=%d "
+                    "conn_handle=%d\n", status, peer->conn_handle);
+        ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+        return;
+    }
+
+    /* Service discovery has completed successfully.  Now we have a complete
+     * list of services, characteristics, and descriptors that the peer
+     * supports.
+     */
+    MODLOG_DFLT(INFO, "Service discovery complete; status=%d "
+                "conn_handle=%d\n", status, peer->conn_handle);
+
+    /* Now perform three GATT procedures against the peer: read,
+     * write, and subscribe to notifications for the ANS service.
+     */
+    blecent_read_write_subscribe(peer);
+}
+
+static void restart_coex(void)
+{
+        client_connect = 0;
+	ble_gap_adv_stop();
+	ble_coex_advertise();
+//TODO: Current example only restarts advertising.
+#if 0
+	ble_gap_disc_cancel();
+	ble_coex_scan();
+#endif
+}
+
+static int
+blecoex_gap_event(struct ble_gap_event *event, void *arg)
+{
+    struct ble_gap_conn_desc desc;
+    int rc;
+
+    switch (event->type) {
+    case BLE_GAP_EVENT_DISC:
+        connect_if_interesting(&event->disc);
+        return 0;
+
+    case BLE_GAP_EVENT_CONNECT:
+	MODLOG_DFLT(INFO, "%s connection %s; status=%d ",
+	            client_connect == 1 ? "Client" : "Server",
+                    event->connect.status == 0 ? "established" : "failed",
+                    event->connect.status);
+
+	if (event->connect.status == 0 ) {
+	    rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
+            assert(rc == 0);
+            MODLOG_DFLT(INFO, "\n");
+
+            const uint8_t *u8p;
+            MODLOG_DFLT(INFO, " peer_ota_addr_type=%d peer_ota_addr=",
+                       desc.peer_ota_addr.type);
+            u8p = desc.peer_ota_addr.val;
+            MODLOG_DFLT(INFO, "%02x:%02x:%02x:%02x:%02x:%02x",
+                u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]);
+            MODLOG_DFLT(INFO, " peer_id_addr_type=%d peer_id_addr=",
+                       desc.peer_id_addr.type);
+	    u8p = desc.peer_id_addr.val;
+	    MODLOG_DFLT(INFO, "%02x:%02x:%02x:%02x:%02x:%02x",
+                u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]);
+
+	}
+
+	if (client_connect == 1 ) {
+	    client_connect = 2;
+
+            /* Remember peer. */
+            rc = peer_add(event->connect.conn_handle);
+            if (rc != 0) {
+                MODLOG_DFLT(ERROR, "Failed to add peer; rc=%d\n", rc);
+                restart_coex();
+            }
+
+            /* Perform service discovery */
+            rc = peer_disc_all(event->connect.conn_handle,
+                        blecent_on_disc_complete, NULL);
+            if(rc != 0) {
+                MODLOG_DFLT(ERROR, "Failed to discover services; rc=%d\n", rc);
+                restart_coex();
+            }
+	}
+        return 0;
+
+    case BLE_GAP_EVENT_DISCONNECT:
+        MODLOG_DFLT(INFO, "Disconnect \n");
+	restart_coex();
+        return 0;
+
+    case BLE_GAP_EVENT_DISC_COMPLETE:
+	MODLOG_DFLT(INFO, "Discovery completed \n");
+	return 0;
+
+    case BLE_GAP_EVENT_SUBSCRIBE:
+        MODLOG_DFLT(INFO, "subscribe event; conn_handle=%d attr_handle=%d "
+                    "reason=%d prevn=%d curn=%d previ=%d curi=%d\n",
+                    event->subscribe.conn_handle,
+                    event->subscribe.attr_handle,
+                    event->subscribe.reason,
+                    event->subscribe.prev_notify,
+                    event->subscribe.cur_notify,
+                    event->subscribe.prev_indicate,
+                    event->subscribe.cur_indicate);
+        return 0;
+
+
+    default:
+        return 0;
+    }
+}
+
+void
+ble_coex_scan(void)
+{
+    uint8_t own_addr_type;
+    struct ble_gap_disc_params disc_params = {0};
+    int rc;
+
+    /* Figure out address to use while advertising (no privacy for now) */
+    rc = ble_hs_id_infer_auto(0, &own_addr_type);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "error determining address type; rc=%d\n", rc);
+        return;
+    }
+
+    /* Tell the controller to filter duplicates; we don't want to process
+     * repeated advertisements from the same device.
+     */
+    disc_params.filter_duplicates = 1;
+
+    /**
+     * Perform a passive scan.  I.e., don't send follow-up scan requests to
+     * each advertiser.
+     */
+    disc_params.passive = 1;
+
+    /* Use defaults for the rest of the parameters. */
+    disc_params.itvl = 0;
+    disc_params.window = 0;
+    disc_params.filter_policy = 0;
+    disc_params.limited = 0;
+
+    /* Start discovery for 120 seconds */
+    rc = ble_gap_disc(own_addr_type, (120*1000) , &disc_params,
+                      blecoex_gap_event, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "Error initiating GAP discovery procedure; rc=%d\n",
+                    rc);
+    }
+}
+
+void
+ble_coex_advertise(void)
+{
+    struct ble_gap_adv_params adv_params;
+    struct ble_hs_adv_fields fields;
+    const char *name;
+    int rc;
+
+    /**
+     *  Set the advertisement data included in our advertisements:
+     *     o Flags (indicates advertisement type and other general info).
+     *     o Advertising tx power.
+     *     o Device name.
+     *     o 16-bit service UUIDs (alert notifications).
+     */
+
+    memset(&fields, 0, sizeof fields);
+
+    /* Advertise two flags:
+     *     o Discoverability in forthcoming advertisement (general)
+     *     o BLE-only (BR/EDR unsupported).
+     */
+    fields.flags = BLE_HS_ADV_F_DISC_GEN |
+                   BLE_HS_ADV_F_BREDR_UNSUP;
+
+    /* Indicate that the TX power level field should be included; have the
+     * stack fill this value automatically.  This is done by assigning the
+     * special value BLE_HS_ADV_TX_PWR_LVL_AUTO.
+     */
+    fields.tx_pwr_lvl_is_present = 1;
+    fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
+
+    name = ble_svc_gap_device_name();
+    fields.name = (uint8_t *)name;
+    fields.name_len = strlen(name);
+    fields.name_is_complete = 1;
+
+    fields.uuids16 = (ble_uuid16_t[]) {
+        BLE_UUID16_INIT(BLECOEX_SVC_ALERT_UUID)
+    };
+    fields.num_uuids16 = 1;
+    fields.uuids16_is_complete = 1;
+
+    rc = ble_gap_adv_set_fields(&fields);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "error setting advertisement data; rc=%d\n", rc);
+        return;
+    }
+
+    /* Begin advertising. */
+    memset(&adv_params, 0, sizeof adv_params);
+    adv_params.conn_mode = BLE_GAP_CONN_MODE_UND;
+    adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN;
+    rc = ble_gap_adv_start(own_addr_type, NULL, BLE_HS_FOREVER,
+                           &adv_params, blecoex_gap_event, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR, "error enabling advertisement; rc=%d\n", rc);
+        return;
+    }
+}
+
+static void on_sync(void)
+{
+    int rc;
+
+    ble_hs_util_ensure_addr(0);
+
+    ble_hs_id_infer_auto(0, &own_addr_type);
+    ble_svc_gap_device_name_set("NimBLE Coex");
+    ble_coex_advertise();
+
+    // Start scanning as a client
+    rc = ble_hs_util_ensure_addr(0);
+    assert(rc == 0);
+
+    ble_coex_scan();
+}
+
+void ble_hs_task(void *param)
+{
+    MODLOG_DFLT(INFO, "BLE Host Task Started");
+    /* This function will return only when nimble_port_stop() is executed */
+    nimble_port_run();
+
+    nimble_port_freertos_deinit();
+}
+
+
+void app_main(void)
+{
+    /* Initialize NVS — it is used to store PHY calibration data */
+    esp_err_t ret = nvs_flash_init();
+
+    if  (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
+        ESP_ERROR_CHECK(nvs_flash_erase());
+        ret = nvs_flash_init();
+    }
+    ESP_ERROR_CHECK(ret);
+
+    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();
+
+    ble_gatts_count_cfg(gatt_svr_svcs);
+    ble_gatts_add_svcs(gatt_svr_svcs);
+
+    nimble_port_freertos_init(ble_hs_task);
+}

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");

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");

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);

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");

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");

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");

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");

examples/bluetooth/nimble/common/nimble_central_utils/peer.c

@@ -602,8 +602,8 @@ peer_disc_incs(struct peer *peer)
             }
         }
     }
-
     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,
@@ -611,6 +611,9 @@ peer_disc_incs(struct peer *peer)
         if (rc != 0) {
             peer_disc_chrs(peer);
         }
+    } else {
+        peer_disc_chrs(peer);
+    }
 }
 #endif
 

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 */

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);

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 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) {