NimBLE: Added custom gatt server functionality

examples/bluetooth/nimble/blecent/README.md

@@ -7,7 +7,9 @@
 
 This example creates GATT client and performs passive scan, it then connects to peripheral device if the device advertises connectability and the device advertises support for the Alert Notification service (0x1811) as primary service UUID.
 
-It performs three GATT operations against the specified peer:
+After connection it enables bonding and link encryprion if the `Enable Link Encryption` flag is set in the example config.
+
+It performs six GATT operations against the specified peer:
 
 * Reads the ANS Supported New Alert Category characteristic.
 
@@ -15,11 +17,17 @@ It performs three GATT operations against the specified peer:
 
 * After the write operation is completed, subscribes to notifications for the ANS Unread Alert Status characteristic.
 
+* After the subscribe operation is completed, it subscribes to notifications for a user defined characteristic.
+
+* After this subscribe operation is completed, it writes to the user defined characteristic.
+
+* After the write operation is completed, it reads from the user defined characteristic.
+
 If the peer does not support a required service, characteristic, or descriptor, then the peer lied when it claimed support for the alert notification service! When this happens, or if a GATT procedure fails, this function immediately terminates the connection.
 
 It uses ESP32's Bluetooth controller and NimBLE stack based BLE host.
 
-This example aims at understanding BLE service discovery, connection and characteristic operations.
+This example aims at understanding BLE service discovery, connection, encryption and characteristic operations.
 
 To test this demo, use any BLE GATT server app that advertises support for the Alert Notification service (0x1811) and includes it in the GATT database.
 
@@ -40,7 +48,7 @@ idf.py set-target <chip_name>
 
 ### Hardware Required
 
-* A development board with ESP32/ESP32-C3 SoC (e.g., ESP32-DevKitC, ESP-WROVER-KIT, etc.)
+* A development board with ESP32/ESP32-C2/ESP32-C3/ESP32-S3 SoC (e.g., ESP32-DevKitC, ESP-WROVER-KIT, etc.)
 * A USB cable for Power supply and programming
 
 See [Development Boards](https://www.espressif.com/en/products/devkits) for more information about it.
@@ -79,6 +87,8 @@ GAP procedure initiated: stop advertising.
 GAP procedure initiated: discovery; own_addr_type=0 filter_policy=0 passive=1 limited=0 filter_duplicates=1 duration=forever
 GAP procedure initiated: connect; peer_addr_type=1 peer_addr=xx:xx:xx:xx:xx:xx scan_itvl=16 scan_window=16 itvl_min=24 itvl_max=40 latency=0 supervision_timeout=256 min_ce_len=16 max_ce_len=768 own_addr_type=0
 Connection established
+Connection secured
+encryption change event; status=0
 GATT procedure initiated: discover all services
 GATT procedure initiated: discover all characteristics; start_handle=1 end_handle=3
 GATT procedure initiated: discover all characteristics; start_handle=20 end_handle=26
@@ -92,6 +102,13 @@ GATT procedure initiated: write; att_handle=43 len=2
 Read complete; status=0 conn_handle=0 attr_handle=45 value=0x02
 Write complete; status=0 conn_handle=0 attr_handle=47
 Subscribe complete; status=0 conn_handle=0 attr_handle=43
+GATT procedure initiated: write; att_handle=26 len=2
+GATT procedure initiated: write; att_handle=25 len=1
+GATT procedure initiated: read; att_handle=25
+Subscribe to the custom subscribable characteristic complete; status=0 conn_handle=1 attr_handle=26 value=
+Write to the custom subscribable characteristic complete; status=0 conn_handle=1 attr_handle=25
+received notification; conn_handle=1 attr_handle=25 attr_len=4
+Read complete for the subscribable characteristic; status=0 conn_handle=1 attr_handle=25 value=0x19
 ```
 
 This is the console output on failure (or peripheral does not support New Alert Service category):

examples/bluetooth/nimble/blecent/main/Kconfig.projbuild

@@ -19,4 +19,10 @@ menu "Example Configuration"
         prompt "Perform init deinit of nimble stack in a loop"
         help
             Enable this flag, to perform only stack Init and Deinit in a loop.
+
+    config EXAMPLE_ENCRYPTION
+        bool
+        prompt "Enable Link Encryption"
+        help
+            This enables bonding and encryption after connection has been established.
 endmenu

examples/bluetooth/nimble/blecent/main/main.c

@@ -28,12 +28,183 @@
 #include "services/gap/ble_svc_gap.h"
 #include "blecent.h"
 
+/*** The UUID of the service containing the subscribable characterstic ***/
+static const ble_uuid_t * remote_svc_uuid =
+    BLE_UUID128_DECLARE(0x2d, 0x71, 0xa2, 0x59, 0xb4, 0x58, 0xc8, 0x12,
+                     	0x99, 0x99, 0x43, 0x95, 0x12, 0x2f, 0x46, 0x59);
+
+/*** The UUID of the subscribable chatacteristic ***/
+static const ble_uuid_t * remote_chr_uuid =
+    BLE_UUID128_DECLARE(0x00, 0x00, 0x00, 0x00, 0x11, 0x11, 0x11, 0x11,
+                     	0x22, 0x22, 0x22, 0x22, 0x33, 0x33, 0x33, 0x33);
+
 static const char *tag = "NimBLE_BLE_CENT";
 static int blecent_gap_event(struct ble_gap_event *event, void *arg);
 static uint8_t peer_addr[6];
 
 void ble_store_config_init(void);
 
+/**
+ * Application Callback. Called when the custom subscribable chatacteristic
+ * in the remote GATT server is read.
+ * Expect to get the recently written data.
+ **/
+static int
+blecent_on_custom_read(uint16_t conn_handle,
+                       const struct ble_gatt_error *error,
+                       struct ble_gatt_attr *attr,
+                       void *arg)
+{
+    MODLOG_DFLT(INFO,
+                "Read complete for the subscribable characteristic; "
+                "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");
+
+    return 0;
+}
+
+/**
+ * Application Callback. Called when the custom subscribable characteristic
+ * in the remote GATT server is written to.
+ * Client has previously subscribed to this characeteristic,
+ * so expect a notification from the server.
+ **/
+static int
+blecent_on_custom_write(uint16_t conn_handle,
+                        const struct ble_gatt_error *error,
+                        struct ble_gatt_attr *attr,
+                        void *arg)
+{
+    const struct peer_chr *chr;
+    const struct peer *peer;
+    int rc;
+
+    MODLOG_DFLT(INFO,
+                "Write to the custom subscribable characteristic complete; "
+                "status=%d conn_handle=%d attr_handle=%d\n",
+                error->status, conn_handle, attr->handle);
+
+    peer = peer_find(conn_handle);
+    chr = peer_chr_find_uuid(peer,
+                             remote_svc_uuid,
+                             remote_chr_uuid);
+    if (chr == NULL) {
+        MODLOG_DFLT(ERROR,
+                    "Error: Peer doesn't have the custom subscribable characteristic\n");
+        goto err;
+    }
+
+    /*** Performs a read on the characteristic, the result is handled in blecent_on_new_read callback ***/
+    rc = ble_gattc_read(conn_handle, chr->chr.val_handle,
+                        blecent_on_custom_read, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR,
+                    "Error: Failed to read the custom subscribable 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);
+}
+
+/**
+ * Application Callback. Called when the custom subscribable characteristic
+ * is subscribed to.
+ **/
+static int
+blecent_on_custom_subscribe(uint16_t conn_handle,
+                            const struct ble_gatt_error *error,
+                            struct ble_gatt_attr *attr,
+                            void *arg)
+{
+    const struct peer_chr *chr;
+    uint8_t value;
+    int rc;
+    const struct peer *peer;
+
+    MODLOG_DFLT(INFO,
+                "Subscribe to the custom subscribable characteristic 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");
+
+    peer = peer_find(conn_handle);
+    chr = peer_chr_find_uuid(peer,
+                             remote_svc_uuid,
+                             remote_chr_uuid);
+    if (chr == NULL) {
+        MODLOG_DFLT(ERROR, "Error: Peer doesn't have the subscribable characteristic\n");
+        goto err;
+    }
+
+    /* Write 1 byte to the new characteristic to test if it notifies after subscribing */
+    value = 0x19;
+    rc = ble_gattc_write_flat(conn_handle, chr->chr.val_handle,
+                              &value, sizeof(value), blecent_on_custom_write, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR,
+                    "Error: Failed to write to the subscribable 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);
+}
+
+/**
+ * Performs 3 operations on the remote GATT server.
+ * 1. Subscribes to a characteristic by writing 0x10 to it's CCCD.
+ * 2. Writes to the characteristic and expect a notification from remote.
+ * 3. Reads the characteristic and expect to get the recently written information.
+ **/
+static void
+blecent_custom_gatt_operations(const struct peer* peer)
+{
+    const struct peer_dsc *dsc;
+    int rc;
+    uint8_t value[2];
+
+    dsc = peer_dsc_find_uuid(peer,
+                             remote_svc_uuid,
+                             remote_chr_uuid,
+                             BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_CFG_UUID16));
+    if (dsc == NULL) {
+        MODLOG_DFLT(ERROR, "Error: Peer lacks a CCCD for the subscribable characterstic\n");
+        goto err;
+    }
+
+    /*** Write 0x00 and 0x01 (The subscription code) to the CCCD ***/
+    value[0] = 1;
+    value[1] = 0;
+    rc = ble_gattc_write_flat(peer->conn_handle, dsc->dsc.handle,
+                              value, sizeof(value), blecent_on_custom_subscribe, NULL);
+    if (rc != 0) {
+        MODLOG_DFLT(ERROR,
+                    "Error: Failed to subscribe to the subscribable characteristic; "
+                    "rc=%d\n", rc);
+        goto err;
+    }
+
+    return;
+err:
+    /* Terminate the connection */
+    ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+}
+
 /**
  * Application callback.  Called when the attempt to subscribe to notifications
  * for the ANS Unread Alert Status characteristic has completed.
@@ -44,10 +215,20 @@ blecent_on_subscribe(uint16_t conn_handle,
                      struct ble_gatt_attr *attr,
                      void *arg)
 {
+    struct peer *peer;
+
     MODLOG_DFLT(INFO, "Subscribe complete; status=%d conn_handle=%d "
                 "attr_handle=%d\n",
                 error->status, conn_handle, attr->handle);
 
+    peer = peer_find(conn_handle);
+    if (peer == NULL) {
+        MODLOG_DFLT(ERROR, "Error in finding peer, aborting...");
+        ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM);
+    }
+    /* Subscribe to, write to, and read the custom characteristic*/
+    blecent_custom_gatt_operations(peer);
+
     return 0;
 }
 
@@ -216,7 +397,7 @@ blecent_on_disc_complete(const struct peer *peer, int status, void *arg)
                 "conn_handle=%d\n", status, peer->conn_handle);
 
     /* Now perform three GATT procedures against the peer: read,
-     * write, and subscribe to notifications.
+     * write, and subscribe to notifications for the ANS service.
      */
     blecent_read_write_subscribe(peer);
 }
@@ -467,13 +648,30 @@ blecent_gap_event(struct ble_gap_event *event, void *arg)
                 return 0;
             }
 
-            /* Perform service discovery. */
-            rc = peer_disc_all(event->connect.conn_handle,
-                               blecent_on_disc_complete, NULL);
+#if CONFIG_EXAMPLE_ENCRYPTION
+            /** Initiate security - It will perform
+             * Pairing (Exchange keys)
+             * Bonding (Store keys)
+             * Encryption (Enable encryption)
+             * Will invoke event BLE_GAP_EVENT_ENC_CHANGE
+             **/
+            rc = ble_gap_security_initiate(event->connect.conn_handle);
             if (rc != 0) {
+                MODLOG_DFLT(INFO, "Security could not be initiated, rc = %d\n", rc);
+                return ble_gap_terminate(event->connect.conn_handle,
+                                         BLE_ERR_REM_USER_CONN_TERM);
+            } else {
+                MODLOG_DFLT(INFO, "Connection secured\n");
+            }
+#else
+            /* 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);
                 return 0;
             }
+#endif
         } else {
             /* Connection attempt failed; resume scanning. */
             MODLOG_DFLT(ERROR, "Error: Connection failed; status=%d\n",
@@ -508,6 +706,15 @@ blecent_gap_event(struct ble_gap_event *event, void *arg)
         rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
         assert(rc == 0);
         print_conn_desc(&desc);
+#if CONFIG_EXAMPLE_ENCRYPTION
+        /*** Go for service discovery after encryption has been successfully enabled ***/
+        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);
+            return 0;
+        }
+#endif
         return 0;
 
     case BLE_GAP_EVENT_NOTIFY_RX:

examples/bluetooth/nimble/bleprph/README.md

@@ -9,9 +9,9 @@ This example creates GATT server and then starts advertising, waiting to be conn
 
 It uses ESP32's Bluetooth controller and NimBLE stack based BLE host.
 
-This example aims at understanding GATT database configuration, advertisement and SMP related NimBLE APIs.
+This example aims at understanding GATT database configuration, handling GATT reads and writes, handling subscribe events, understanding advertisement and SMP related NimBLE APIs.
 
-It also demonstrates security features of NimBLE stack. SMP parameters like I/O capabilities of device, Bonding flag, MITM protection flag and Secure Connection only mode etc., can be configured through menuconfig options.
+It also demonstrates security features of NimBLE stack. SMP parameters like I/O capabilities of device, Bonding flag, MITM protection flag and Secure Connection only mode, Enabling Link Encryption etc., can be configured through menuconfig options.
 
 For RPA feature (currently Host based privacy feature is supported), use API `ble_hs_pvcy_rpa_config` to enable/disable host based privacy, `own_addr_type` needs to be set to `BLE_ADDR_RANDOM` to use this feature. Please include `ble_hs_pvcy.h` while using this API. As `ble_hs_pvcy_rpa_config` configures host privacy and sets address in controller, it is necessary to call this API after host-controller are synced (e.g. in `bleprph_on_sync` callback).
 
@@ -83,6 +83,13 @@ peer_id_addr=xx:xx:xx:xx:xx:xx conn_itvl=6 conn_latency=0 supervision_timeout=50
 
 connection updated; status=0 handle=0 our_ota_addr_type=0 our_ota_addr=xx:xx:xx:xx:xx:xx our_id_addr_type=0 our_id_addr=xx:xx:xx:xx:xx:xx
 peer_ota_addr_type=1 peer_ota_addr=xx:xx:xx:xx:xx:xx peer_id_addr_type=1 peer_id_addr=xx:xx:xx:xx:xx:xx conn_itvl=39 conn_latency=0 supervision_timeout=500 encrypted=1 authenticated=1 bonded=1
+
+subscribe event; conn_handle=1 attr_handle=19 reason=1 prevn=0 curn=1 previ=0 curi=0
+Subscribe to attribute (19) successful
+subscribe event; conn_handle=1 attr_handle=25 reason=1 prevn=0 curn=1 previ=0 curi=0
+Subscribe to attribute (25) successful
+GATT procedure initiated: notify; att_handle=25
+Notification sent succesfully
 ```
 
 ## Running Python Utility

examples/bluetooth/nimble/bleprph/main/Kconfig.projbuild

@@ -61,4 +61,10 @@ menu "Example Configuration"
         prompt "Advertise RANDOM Address"
         help
             Use this option to advertise a random address instead of public address
+
+    config EXAMPLE_ENCRYPTION
+        bool
+        prompt "Enable Link Encryption"
+        help
+            This adds Encrypted Read and Write permissions in the custom GATT server.
 endmenu

examples/bluetooth/nimble/bleprph/main/bleprph.h

@@ -41,6 +41,8 @@ struct ble_gatt_register_ctxt;
 
 void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg);
 int gatt_svr_init(void);
+int gatt_svr_subscribe(uint16_t);
+void gatt_svr_subscription_delete(void);
 
 #ifdef __cplusplus
 }

examples/bluetooth/nimble/bleprph/main/gatt_svr.c

@@ -27,54 +27,75 @@
 #include "bleprph.h"
 #include "services/ans/ble_svc_ans.h"
 
-/**
- * The vendor specific security test service consists of two characteristics:
- *     o random-number-generator: generates a random 32-bit number each time
- *       it is read.  This characteristic can only be read over an encrypted
- *       connection.
- *     o static-value: a single-byte characteristic that can always be read,
- *       but can only be written over an encrypted connection.
- */
+/*** Maximum number of characteristics with the notify flag ***/
+#define MAX_NOTIFY 5
 
-/* 59462f12-9543-9999-12c8-58b459a2712d */
-static const ble_uuid128_t gatt_svr_svc_sec_test_uuid =
+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);
 
-/* 5c3a659e-897e-45e1-b016-007107c96df6 */
-static const ble_uuid128_t gatt_svr_chr_sec_test_rand_uuid =
-    BLE_UUID128_INIT(0xf6, 0x6d, 0xc9, 0x07, 0x71, 0x00, 0x16, 0xb0,
-                     0xe1, 0x45, 0x7e, 0x89, 0x9e, 0x65, 0x3a, 0x5c);
-
-/* 5c3a659e-897e-45e1-b016-007107c96df7 */
-static const ble_uuid128_t gatt_svr_chr_sec_test_static_uuid =
-    BLE_UUID128_INIT(0xf7, 0x6d, 0xc9, 0x07, 0x71, 0x00, 0x16, 0xb0,
-                     0xe1, 0x45, 0x7e, 0x89, 0x9e, 0x65, 0x3a, 0x5c);
-
-static uint8_t gatt_svr_sec_test_static_val;
+/* A characteristic that can be subscribed to */
+static uint8_t gatt_svr_chr_val;
+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 uint8_t gatt_svr_dsc_val;
+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 void* subscription_list_buffer;
+static struct os_mempool subscription_list_mempool;
+struct subscription {
+    SLIST_ENTRY(subscription) next;
+    uint16_t val_handle;
+};
+SLIST_HEAD(subscription_list, subscription);
+/*** SLIST that contains the handles of all the characteristics that are subscribed ***/
+static struct subscription_list slist;
 
 static int
-gatt_svr_chr_access_sec_test(uint16_t conn_handle, uint16_t attr_handle,
-                             struct ble_gatt_access_ctxt *ctxt,
-                             void *arg);
+gatt_svc_access(uint16_t conn_handle, uint16_t attr_handle,
+                struct ble_gatt_access_ctxt *ctxt,
+                void *arg);
+
+static bool
+gatt_svr_is_subscribed(uint16_t val_handle);
 
 static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
     {
-        /*** Service: Security test. */
+        /*** Service ***/
         .type = BLE_GATT_SVC_TYPE_PRIMARY,
-        .uuid = &gatt_svr_svc_sec_test_uuid.u,
+        .uuid = &gatt_svr_svc_uuid.u,
         .characteristics = (struct ble_gatt_chr_def[])
         { {
-                /*** Characteristic: Random number generator. */
-                .uuid = &gatt_svr_chr_sec_test_rand_uuid.u,
-                .access_cb = gatt_svr_chr_access_sec_test,
-                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_READ_ENC,
-            }, {
-                /*** Characteristic: Static value. */
-                .uuid = &gatt_svr_chr_sec_test_static_uuid.u,
-                .access_cb = gatt_svr_chr_access_sec_test,
-                .flags = BLE_GATT_CHR_F_READ |
-                BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_ENC,
+                /*** This characteristic can be subscribed to by writing 0x00 and 0x01 to the CCCD ***/
+                .uuid = &gatt_svr_chr_uuid.u,
+                .access_cb = gatt_svc_access,
+#if CONFIG_EXAMPLE_ENCRYPTION
+                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE |
+                BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_WRITE_ENC |
+                BLE_GATT_CHR_F_NOTIFY,
+#else
+                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY,
+#endif
+                .val_handle = &gatt_svr_chr_val_handle,
+                .descriptors = (struct ble_gatt_dsc_def[])
+                { {
+                      .uuid = &gatt_svr_dsc_uuid.u,
+#if CONFIG_EXAMPLE_ENCRYPTION
+                      .att_flags = BLE_ATT_F_READ | BLE_ATT_F_READ_ENC,
+#else
+                      .att_flags = BLE_ATT_F_READ,
+#endif
+                      .access_cb = gatt_svc_access,
+                    }, {
+                      0, /* No more descriptors in this characteristic */
+                    }
+                },
             }, {
                 0, /* No more characteristics in this service. */
             }
@@ -87,8 +108,8 @@ static const struct ble_gatt_svc_def gatt_svr_svcs[] = {
 };
 
 static int
-gatt_svr_chr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len,
-                   void *dst, uint16_t *len)
+gatt_svr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len,
+               void *dst, uint16_t *len)
 {
     uint16_t om_len;
     int rc;
@@ -106,52 +127,78 @@ gatt_svr_chr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len,
     return 0;
 }
 
+/**
+ * Access callback whenever a characteristic/descriptor is read or written to.
+ * Here reads and writes need to be handled.
+ * ctxt->op tells weather the operation is read or write and
+ * weather it is on a characteristic or descriptor,
+ * ctxt->dsc->uuid tells which characteristic/descriptor is accessed.
+ * Accordingly do:
+ *     Append the value to ctxt->om if the operation is READ
+ *     Write ctxt->om to the value if the operation is WRITE
+ **/
 static int
-gatt_svr_chr_access_sec_test(uint16_t conn_handle, uint16_t attr_handle,
-                             struct ble_gatt_access_ctxt *ctxt,
-                             void *arg)
+gatt_svc_access(uint16_t conn_handle, uint16_t attr_handle,
+                struct ble_gatt_access_ctxt *ctxt, void *arg)
 {
     const ble_uuid_t *uuid;
-    int rand_num;
     int rc;
 
-    uuid = ctxt->chr->uuid;
-
-    /* Determine which characteristic is being accessed by examining its
-     * 128-bit UUID.
-     */
-
-    if (ble_uuid_cmp(uuid, &gatt_svr_chr_sec_test_rand_uuid.u) == 0) {
-        assert(ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR);
-
-        /* Respond with a 32-bit random number. */
-        rand_num = rand();
-        rc = os_mbuf_append(ctxt->om, &rand_num, sizeof rand_num);
-        return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
-    }
-
-    if (ble_uuid_cmp(uuid, &gatt_svr_chr_sec_test_static_uuid.u) == 0) {
-        switch (ctxt->op) {
-        case BLE_GATT_ACCESS_OP_READ_CHR:
-            rc = os_mbuf_append(ctxt->om, &gatt_svr_sec_test_static_val,
-                                sizeof gatt_svr_sec_test_static_val);
+    switch (ctxt->op) {
+    case BLE_GATT_ACCESS_OP_READ_CHR:
+        uuid = ctxt->chr->uuid;
+        if (ble_uuid_cmp(uuid, &gatt_svr_chr_uuid.u) == 0) {
+            rc = os_mbuf_append(ctxt->om,
+                                &gatt_svr_chr_val,
+                                sizeof(gatt_svr_chr_val));
             return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
-
-        case BLE_GATT_ACCESS_OP_WRITE_CHR:
-            rc = gatt_svr_chr_write(ctxt->om,
-                                    sizeof gatt_svr_sec_test_static_val,
-                                    sizeof gatt_svr_sec_test_static_val,
-                                    &gatt_svr_sec_test_static_val, NULL);
+        }
+        goto unknown;
+
+    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+        uuid = ctxt->chr->uuid;
+        if (ble_uuid_cmp(uuid, &gatt_svr_chr_uuid.u) == 0) {
+            rc = gatt_svr_write(ctxt->om,
+                                sizeof(gatt_svr_chr_val),
+                                sizeof(gatt_svr_chr_val),
+                                &gatt_svr_chr_val, NULL);
+            if (gatt_svr_is_subscribed(attr_handle)) {
+                struct os_mbuf *txom;
+                uint8_t notification[] = {0x00, 0x01, 0x02, 0x03};
+                txom = ble_hs_mbuf_from_flat(notification, sizeof(notification));
+                /* No need to free txom as it is consumed by ble_gattc_notify_custom */
+                rc = ble_gattc_notify_custom(conn_handle,
+                                             gatt_svr_chr_val_handle, txom);
+                if (rc == 0) {
+                    MODLOG_DFLT(INFO, "Notification sent succesfully\n");
+                } else {
+                    MODLOG_DFLT(INFO, "Error in sending notification rc = %d\n", rc);
+                }
+            }
             return rc;
-
-        default:
-            assert(0);
-            return BLE_ATT_ERR_UNLIKELY;
         }
+        goto unknown;
+
+    case BLE_GATT_ACCESS_OP_READ_DSC:
+        uuid = ctxt->dsc->uuid;
+        if (ble_uuid_cmp(uuid, &gatt_svr_dsc_uuid.u) == 0) {
+            rc = os_mbuf_append(ctxt->om,
+                                &gatt_svr_dsc_val,
+                                sizeof(gatt_svr_chr_val));
+            return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+        }
+        goto unknown;
+
+    case BLE_GATT_ACCESS_OP_WRITE_DSC:
+        goto unknown;
+
+    default:
+        goto unknown;
     }
 
-    /* Unknown characteristic; the nimble stack should not have called this
-     * function.
+unknown:
+    /* Unknown characteristic/descriptor;
+     * The NimBLE host should not have called this function;
      */
     assert(0);
     return BLE_ATT_ERR_UNLIKELY;
@@ -189,6 +236,77 @@ gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
     }
 }
 
+/**
+ * Tells if a characteristic has been subscribed to
+ * based on it's value handle.
+ * All subscribed characteristic value handles are stored
+ * in an SLIST.
+ **/
+static bool
+gatt_svr_is_subscribed(uint16_t val_handle)
+{
+    struct subscription *sub;
+    SLIST_FOREACH(sub, &slist, next) {
+        if (sub->val_handle == val_handle) {
+            return true;
+        }
+    }
+    return false;
+}
+
+/*
+ * Call this with the value handle of the characteristic
+ * that needs to be subscribed to, to add the handle in the
+ * list of subscribed characteristics
+ */
+int
+gatt_svr_subscribe(uint16_t val_handle) {
+    struct subscription *sub;
+
+    SLIST_FOREACH(sub, &slist, next) {
+        if (sub->val_handle == val_handle) {
+            return BLE_ATT_ERR_INVALID_HANDLE;
+        }
+    }
+    sub = os_memblock_get(&subscription_list_mempool);
+    if (sub == NULL) {
+        /* Out of memory */
+        return BLE_ATT_ERR_INSUFFICIENT_RES;
+    }
+    memset(sub, 0, sizeof(struct subscription));
+    SLIST_NEXT(sub, next) = NULL;
+    sub->val_handle = val_handle;
+
+    SLIST_INSERT_HEAD(&slist, sub, next);
+
+    return 0;
+}
+
+/**
+ * Removes all subscriptions from the list
+ * of subscribed characteristics.
+ * Called upon disconnect
+ **/
+void
+gatt_svr_subscription_delete()
+{
+    struct subscription *sub;
+
+    while(true) {
+        sub = SLIST_FIRST(&slist);
+        if (sub == NULL) {
+            break;
+        }
+        SLIST_REMOVE_HEAD(&slist, next);
+        os_memblock_put(&subscription_list_mempool, sub);
+        sub = NULL;
+    }
+
+    os_mempool_clear(&subscription_list_mempool);
+    free(subscription_list_buffer);
+    subscription_list_buffer = NULL;
+}
+
 int
 gatt_svr_init(void)
 {
@@ -208,5 +326,28 @@ gatt_svr_init(void)
         return rc;
     }
 
+    /* Setting a value for the read-only descriptor */
+    gatt_svr_dsc_val = 0x99;
+
+    /* Allocating mempool for the SLIST */
+    subscription_list_buffer = malloc(OS_MEMPOOL_BYTES(MAX_NOTIFY, sizeof(struct subscription)));
+    if (subscription_list_buffer == NULL) {
+        rc = BLE_HS_ENOMEM;
+        return rc;
+    }
+    rc = os_mempool_init(&subscription_list_mempool, MAX_NOTIFY,
+                         sizeof(struct subscription), subscription_list_buffer,
+                         "subscription_list_mempool");
+    if (rc != 0) {
+        rc = BLE_HS_EOS;
+        free(subscription_list_buffer);
+        subscription_list_buffer = NULL;
+        MODLOG_DFLT(ERROR, "Error while allocating memory\n");
+        return rc;
+    }
+
+    SLIST_INIT(&slist);
+    assert(SLIST_EMPTY(&slist));
+
     return 0;
 }

examples/bluetooth/nimble/bleprph/main/main.c

@@ -274,6 +274,7 @@ bleprph_gap_event(struct ble_gap_event *event, void *arg)
 #else
         bleprph_advertise();
 #endif
+        gatt_svr_subscription_delete();
         return 0;
 
     case BLE_GAP_EVENT_CONN_UPDATE:
@@ -314,6 +315,20 @@ bleprph_gap_event(struct ble_gap_event *event, void *arg)
                     event->subscribe.cur_notify,
                     event->subscribe.prev_indicate,
                     event->subscribe.cur_indicate);
+        if (event->subscribe.reason != BLE_GAP_SUBSCRIBE_REASON_TERM) {
+            int rc = gatt_svr_subscribe(event->subscribe.attr_handle);
+            if (rc == 0) {
+                MODLOG_DFLT(INFO,
+                            "Subscribe to attribute (%d) successful\n",
+                            event->subscribe.attr_handle);
+            } else {
+                MODLOG_DFLT(INFO,
+                            "Subscribe to attribute (%d) failed. RC = %d\n",
+                            event->subscribe.attr_handle, rc);
+            }
+        } else {
+            MODLOG_DFLT(INFO, "CCCD cleared on connection termination\n");
+        }
         return 0;
 
     case BLE_GAP_EVENT_MTU:
@@ -520,8 +535,11 @@ app_main(void)
     ble_hs_cfg.sm_sc = 0;
 #endif
 #ifdef CONFIG_EXAMPLE_BONDING
-    ble_hs_cfg.sm_our_key_dist = 1;
-    ble_hs_cfg.sm_their_key_dist = 1;
+    /* Enable the appropriate bit masks to make sure the keys
+     * that are needed are exchanged
+     */
+    ble_hs_cfg.sm_our_key_dist = BLE_SM_PAIR_KEY_DIST_ENC;
+    ble_hs_cfg.sm_their_key_dist = BLE_SM_PAIR_KEY_DIST_ENC;
 #endif