Bluetooth/NimBLE : Fixed clearing connection_handle[] after disconnect event

examples/bluetooth/nimble/ble_spp/spp_client/main/ble_spp_client.h

@@ -19,12 +19,11 @@ struct ble_hs_cfg;
 union ble_store_value;
 union ble_store_key;
 
-#define GATT_SVR_SVC_ALERT_UUID              0x1811
-#define GATT_SVR_CHR_SUP_NEW_ALERT_CAT_UUID  0x2A47
-#define GATT_SVR_CHR_NEW_ALERT               0x2A46
-#define GATT_SVR_CHR_SUP_UNR_ALERT_CAT_UUID  0x2A48
-#define GATT_SVR_CHR_UNR_ALERT_STAT_UUID     0x2A45
-#define GATT_SVR_CHR_ALERT_NOT_CTRL_PT       0x2A44
+/* 16 Bit SPP Service UUID */
+#define GATT_SPP_SVC_UUID                                  0xABF0
+
+/* 16 Bit SPP Service Characteristic UUID */
+#define GATT_SPP_CHR_UUID                                  0xABF1
 
 #ifdef __cplusplus
 }

examples/bluetooth/nimble/ble_spp/spp_client/main/main.c

@@ -14,24 +14,17 @@
 #include "console/console.h"
 #include "services/gap/ble_svc_gap.h"
 #include "ble_spp_client.h"
-
 #include "driver/uart.h"
+
+#define PEER_ADDR_VAL_SIZE      6
+
 static const char *tag = "NimBLE_SPP_BLE_CENT";
 static int ble_spp_client_gap_event(struct ble_gap_event *event, void *arg);
 QueueHandle_t spp_common_uart_queue = NULL;
 void ble_store_config_init(void);
-uint16_t attribute_handle[CONFIG_BT_NIMBLE_MAX_CONNECTIONS];
+uint16_t attribute_handle[CONFIG_BT_NIMBLE_MAX_CONNECTIONS + 1];
 static void ble_spp_client_scan(void);
-static ble_addr_t connected_addr[CONFIG_BT_NIMBLE_MAX_CONNECTIONS];
-
-/* 16 Bit Alert Notification Service UUID */
-#define GATT_SVR_SVC_ALERT_UUID                            0x1811
-
-/* 16 Bit SPP Service UUID */
-#define GATT_SPP_SVC_UUID                                  0xABF0
-
-/* 16 Bit SPP Service Characteristic UUID */
-#define GATT_SPP_CHR_UUID                                  0xABF1
+static ble_addr_t connected_addr[CONFIG_BT_NIMBLE_MAX_CONNECTIONS + 1];
 
 static void
 ble_spp_client_set_handle(const struct peer *peer)
@@ -40,7 +33,7 @@ ble_spp_client_set_handle(const struct peer *peer)
     chr = peer_chr_find_uuid(peer,
                              BLE_UUID16_DECLARE(GATT_SPP_SVC_UUID),
                              BLE_UUID16_DECLARE(GATT_SPP_CHR_UUID));
-    attribute_handle[peer->conn_handle - 1] = chr->chr.val_handle;
+    attribute_handle[peer->conn_handle] = chr->chr.val_handle;
 }
 
 /**
@@ -125,11 +118,11 @@ ble_spp_client_should_connect(const struct ble_gap_disc_desc *disc)
     int i;
 
     /* Check if device is already connected or not */
-    for ( i = 0; i < CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) {
-	if (memcmp(&connected_addr[i].val,disc->addr.val, sizeof(disc->addr.val)) == 0) {
-	    MODLOG_DFLT(DEBUG, "Device already connected");
-	    return 0;
-	}
+    for ( i = 0; i <= CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) {
+        if (memcmp(&connected_addr[i].val, disc->addr.val, PEER_ADDR_VAL_SIZE) == 0) {
+            MODLOG_DFLT(DEBUG, "Device already connected");
+            return 0;
+        }
     }
 
     /* The device has to be advertising connectability. */
@@ -144,15 +137,14 @@ ble_spp_client_should_connect(const struct ble_gap_disc_desc *disc)
         return rc;
     }
 
-    /* The device has to advertise support for the Alert Notification
-     * service (0x1811).
+    /* The device has to advertise support for the SPP
+     * service (0xABF0).
      */
     for (i = 0; i < fields.num_uuids16; i++) {
-        if (ble_uuid_u16(&fields.uuids16[i].u) == GATT_SVR_SVC_ALERT_UUID) {
+        if (ble_uuid_u16(&fields.uuids16[i].u) == GATT_SPP_SVC_UUID) {
             return 1;
         }
     }
-
     return 0;
 }
 
@@ -223,7 +215,7 @@ ble_spp_client_gap_event(struct ble_gap_event *event, void *arg)
 
     switch (event->type) {
     case BLE_GAP_EVENT_DISC:
-	rc = ble_hs_adv_parse_fields(&fields, event->disc.data,
+        rc = ble_hs_adv_parse_fields(&fields, event->disc.data,
                                      event->disc.length_data);
         if (rc != 0) {
             return 0;
@@ -237,14 +229,14 @@ ble_spp_client_gap_event(struct ble_gap_event *event, void *arg)
         return 0;
 
     case BLE_GAP_EVENT_CONNECT:
-	/* A new connection was established or a connection attempt failed. */
+        /* A new connection was established or a connection attempt failed. */
         if (event->connect.status == 0) {
             /* Connection successfully established. */
             MODLOG_DFLT(INFO, "Connection established ");
             rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
             assert(rc == 0);
-	    memcpy(&connected_addr[event->connect.conn_handle - 1].val, desc.peer_id_addr.val,
-		   sizeof(desc.peer_id_addr.val));
+            memcpy(&connected_addr[event->connect.conn_handle].val, desc.peer_id_addr.val,
+                   PEER_ADDR_VAL_SIZE);
             print_conn_desc(&desc);
             MODLOG_DFLT(INFO, "\n");
 
@@ -278,7 +270,8 @@ ble_spp_client_gap_event(struct ble_gap_event *event, void *arg)
         MODLOG_DFLT(INFO, "\n");
 
         /* Forget about peer. */
-	attribute_handle[event->disconnect.conn.conn_handle] = 0;
+        memset(&connected_addr[event->disconnect.conn.conn_handle].val, 0, PEER_ADDR_VAL_SIZE);
+        attribute_handle[event->disconnect.conn.conn_handle] = 0;
         peer_delete(event->disconnect.conn.conn_handle);
 
         /* Resume scanning. */
@@ -346,69 +339,68 @@ void ble_spp_client_host_task(void *param)
 }
 void ble_client_uart_task(void *pvParameters)
 {
-	ESP_LOGI(tag,"BLE client UART task started\n");
-	int rc;
-	int i;
-	uart_event_t event;
-        for (;;) {
-            //Waiting for UART event.
-            if (xQueueReceive(spp_common_uart_queue, (void * )&event, (TickType_t)portMAX_DELAY)) {
-             switch (event.type) {
-             //Event of UART receving data
-             case UART_DATA:
+    ESP_LOGI(tag, "BLE client UART task started\n");
+    int rc;
+    int i;
+    uart_event_t event;
+    for (;;) {
+        //Waiting for UART event.
+        if (xQueueReceive(spp_common_uart_queue, (void * )&event, (TickType_t)portMAX_DELAY)) {
+            switch (event.type) {
+            //Event of UART receving data
+            case UART_DATA:
                 if (event.size) {
 
-                     /* Writing characteristics */
-		     uint8_t * temp = NULL;
-                     temp = (uint8_t *)malloc(sizeof(uint8_t)*event.size);
-                     if(temp == NULL){
-                         ESP_LOGE(tag, "malloc failed,%s L#%d\n", __func__, __LINE__);
-                         break;
-                     }
-                     memset(temp, 0x0, event.size);
-                     uart_read_bytes(UART_NUM_0,temp,event.size,portMAX_DELAY);
-		     for ( i = 0; i < CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) {
-	                 if (attribute_handle[i] != 0) {
-			     rc = ble_gattc_write_flat(i+1, attribute_handle[i],temp, event.size,NULL,NULL);
-			     if (rc == 0) {
-			         ESP_LOGI(tag,"Write in uart task success!");
-			     }
-			     else {
-			         ESP_LOGI(tag,"Error in writing characteristic rc=%d",rc);
-			     }
-			     vTaskDelay(10);
-			 }
-		     }
-		     free(temp);
-		 }
-                 break;
-             default:
-                 break;
-             }
-         }
-     }
-     vTaskDelete(NULL);
+                    /* Writing characteristics */
+                    uint8_t *temp = NULL;
+                    temp = (uint8_t *)malloc(sizeof(uint8_t) * event.size);
+                    if (temp == NULL) {
+                        ESP_LOGE(tag, "malloc failed,%s L#%d\n", __func__, __LINE__);
+                        break;
+                    }
+                    memset(temp, 0x0, event.size);
+                    uart_read_bytes(UART_NUM_0, temp, event.size, portMAX_DELAY);
+                    for ( i = 0; i <= CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) {
+                        if (attribute_handle[i] != 0) {
+                            rc = ble_gattc_write_flat(i, attribute_handle[i], temp, event.size, NULL, NULL);
+                            if (rc == 0) {
+                                ESP_LOGI(tag, "Write in uart task success!");
+                            } else {
+                                ESP_LOGI(tag, "Error in writing characteristic rc=%d", rc);
+                            }
+                            vTaskDelay(10);
+                        }
+                    }
+                    free(temp);
+                }
+                break;
+            default:
+                break;
+            }
+        }
+    }
+    vTaskDelete(NULL);
 
 }
 static void ble_spp_uart_init(void)
 {
-     uart_config_t uart_config = {
-         .baud_rate = 115200,
-         .data_bits = UART_DATA_8_BITS,
-         .parity = UART_PARITY_DISABLE,
-         .stop_bits = UART_STOP_BITS_1,
-         .flow_ctrl = UART_HW_FLOWCTRL_RTS,
-         .rx_flow_ctrl_thresh = 122,
-         .source_clk = UART_SCLK_DEFAULT,
-     };
-
-     //Install UART driver, and get the queue.
-     uart_driver_install(UART_NUM_0, 4096, 8192, 10, &spp_common_uart_queue, 0);
-     //Set UART parameters
-     uart_param_config(UART_NUM_0, &uart_config);
-     //Set UART pins
-     uart_set_pin(UART_NUM_0, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
-     xTaskCreate(ble_client_uart_task, "uTask", 4096, (void*)UART_NUM_0, 8, NULL);
+    uart_config_t uart_config = {
+        .baud_rate = 115200,
+        .data_bits = UART_DATA_8_BITS,
+        .parity = UART_PARITY_DISABLE,
+        .stop_bits = UART_STOP_BITS_1,
+        .flow_ctrl = UART_HW_FLOWCTRL_RTS,
+        .rx_flow_ctrl_thresh = 122,
+        .source_clk = UART_SCLK_DEFAULT,
+    };
+
+    //Install UART driver, and get the queue.
+    uart_driver_install(UART_NUM_0, 4096, 8192, 10, &spp_common_uart_queue, 0);
+    //Set UART parameters
+    uart_param_config(UART_NUM_0, &uart_config);
+    //Set UART pins
+    uart_set_pin(UART_NUM_0, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+    xTaskCreate(ble_client_uart_task, "uTask", 4096, (void *)UART_NUM_0, 8, NULL);
 }
 void
 app_main(void)

examples/bluetooth/nimble/ble_spp/spp_server/main/CMakeLists.txt

@@ -1,5 +1,4 @@
-set(srcs "main.c"
-        "gatt_svr.c")
+set(srcs "main.c")
 
 idf_component_register(SRCS "${srcs}"
                        INCLUDE_DIRS ".")

examples/bluetooth/nimble/ble_spp/spp_server/main/ble_spp_server.h

@@ -15,19 +15,14 @@
 extern "C" {
 #endif
 
-struct ble_hs_cfg;
-struct ble_gatt_register_ctxt;
+/* 16 Bit SPP Service UUID */
+#define BLE_SVC_SPP_UUID16                                  0xABF0
 
-/** GATT server. */
-#define GATT_SVR_SVC_ALERT_UUID               0x1811
-#define GATT_SVR_CHR_SUP_NEW_ALERT_CAT_UUID   0x2A47
-#define GATT_SVR_CHR_NEW_ALERT                0x2A46
-#define GATT_SVR_CHR_SUP_UNR_ALERT_CAT_UUID   0x2A48
-#define GATT_SVR_CHR_UNR_ALERT_STAT_UUID      0x2A45
-#define GATT_SVR_CHR_ALERT_NOT_CTRL_PT        0x2A44
+/* 16 Bit SPP Service Characteristic UUID */
+#define BLE_SVC_SPP_CHR_UUID16                              0xABF1
 
-void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg);
-int new_gatt_svr_init(void);
+struct ble_hs_cfg;
+struct ble_gatt_register_ctxt;
 
 #ifdef __cplusplus
 }

examples/bluetooth/nimble/ble_spp/spp_server/main/gatt_svr.c

@@ -1,207 +0,0 @@
-/*
- * SPDX-FileCopyrightText: 2021 Espressif Systems (Shanghai) CO LTD
- *
- * SPDX-License-Identifier: Unlicense OR CC0-1.0
- */
-
-#include <assert.h>
-#include <stdio.h>
-#include <string.h>
-#include "host/ble_hs.h"
-#include "host/ble_uuid.h"
-#include "services/gap/ble_svc_gap.h"
-#include "services/gatt/ble_svc_gatt.h"
-#include "ble_spp_server.h"
-
-/* 16 Bit Alert Notification Service UUID */
-#define BLE_SVC_ANS_UUID16                                  0x1811
-
-/* 16 Bit Alert Notification Service Characteristic UUIDs */
-#define BLE_SVC_ANS_CHR_UUID16_SUP_NEW_ALERT_CAT            0x2a47
-#define BLE_SVC_ANS_CHR_UUID16_NEW_ALERT                    0x2a46
-#define BLE_SVC_ANS_CHR_UUID16_SUP_UNR_ALERT_CAT            0x2a48
-#define BLE_SVC_ANS_CHR_UUID16_UNR_ALERT_STAT               0x2a45
-#define BLE_SVC_ANS_CHR_UUID16_ALERT_NOT_CTRL_PT            0x2a44
-
-/**
- * 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.
- */
-
-/* 59462f12-9543-9999-12c8-58b459a2712d */
-static const ble_uuid128_t gatt_svr_svc_sec_test_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;
-
-static int
-gatt_svr_chr_access_sec_test(uint16_t conn_handle, uint16_t attr_handle,
-                             struct ble_gatt_access_ctxt *ctxt,
-                             void *arg);
-
-struct ble_gatt_svc_def gatt_svr_svcs[] = {
-    {
-        /*** Service: Security test. */
-        .type = BLE_GATT_SVC_TYPE_PRIMARY,
-        .uuid = &gatt_svr_svc_sec_test_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,
-            }, {
-                0, /* No more characteristics in this service. */
-            }
-        },
-    },
-
-    {
-        0, /* No more services. */
-    },
-};
-
-static int
-gatt_svr_chr_write(struct os_mbuf *om, uint16_t min_len, uint16_t max_len,
-                   void *dst, uint16_t *len)
-{
-    uint16_t om_len;
-    int rc;
-
-    om_len = OS_MBUF_PKTLEN(om);
-    if (om_len < min_len || om_len > max_len) {
-        return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
-    }
-
-    rc = ble_hs_mbuf_to_flat(om, dst, max_len, len);
-    if (rc != 0) {
-        return BLE_ATT_ERR_UNLIKELY;
-    }
-
-    return 0;
-}
-
-static int
-gatt_svr_chr_access_sec_test(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);
-            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);
-            return rc;
-
-        default:
-            assert(0);
-            return BLE_ATT_ERR_UNLIKELY;
-        }
-    }
-
-    /* Unknown characteristic; the nimble stack should not have called this
-     * function.
-     */
-    assert(0);
-    return BLE_ATT_ERR_UNLIKELY;
-}
-
-void
-gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
-{
-    char buf[BLE_UUID_STR_LEN];
-
-    switch (ctxt->op) {
-    case BLE_GATT_REGISTER_OP_SVC:
-        MODLOG_DFLT(DEBUG, "registered service %s with handle=%d\n",
-                    ble_uuid_to_str(ctxt->svc.svc_def->uuid, buf),
-                    ctxt->svc.handle);
-        break;
-
-    case BLE_GATT_REGISTER_OP_CHR:
-        MODLOG_DFLT(DEBUG, "registering characteristic %s with "
-                    "def_handle=%d val_handle=%d\n",
-                    ble_uuid_to_str(ctxt->chr.chr_def->uuid, buf),
-                    ctxt->chr.def_handle,
-                    ctxt->chr.val_handle);
-        break;
-
-    case BLE_GATT_REGISTER_OP_DSC:
-        MODLOG_DFLT(DEBUG, "registering descriptor %s with handle=%d\n",
-                    ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buf),
-                    ctxt->dsc.handle);
-        break;
-
-    default:
-        assert(0);
-        break;
-    }
-}
-
-int
-new_gatt_svr_init(void)
-{
-    int rc;
-
-    ble_svc_gap_init();
-    ble_svc_gatt_init();
-
-    rc = ble_gatts_count_cfg(gatt_svr_svcs);
-    if (rc != 0) {
-        return rc;
-    }
-
-    rc = ble_gatts_add_svcs(gatt_svr_svcs);
-    if (rc != 0) {
-        return rc;
-    }
-
-    return 0;
-}

examples/bluetooth/nimble/ble_spp/spp_server/main/main.c

@@ -13,6 +13,7 @@
 #include "host/util/util.h"
 #include "console/console.h"
 #include "services/gap/ble_svc_gap.h"
+#include "services/gatt/ble_svc_gatt.h"
 #include "ble_spp_server.h"
 #include "driver/uart.h"
 
@@ -21,20 +22,8 @@ static int ble_spp_server_gap_event(struct ble_gap_event *event, void *arg);
 static uint8_t own_addr_type;
 int gatt_svr_register(void);
 QueueHandle_t spp_common_uart_queue = NULL;
-uint16_t connection_handle[CONFIG_BT_NIMBLE_MAX_CONNECTIONS];
-static uint16_t ble_svc_gatt_read_val_handle,ble_spp_svc_gatt_read_val_handle;
-
-/* 16 Bit Alert Notification Service UUID */
-#define BLE_SVC_ANS_UUID16                                  0x1811
-
-/* 16 Bit Alert Notification Service Characteristic UUIDs */
-#define BLE_SVC_ANS_CHR_UUID16_SUP_NEW_ALERT_CAT            0x2a47
-
-/* 16 Bit SPP Service UUID */
-#define BLE_SVC_SPP_UUID16				    0xABF0
-
-/* 16 Bit SPP Service Characteristic UUID */
-#define BLE_SVC_SPP_CHR_UUID16                              0xABF1
+int connection_handle[CONFIG_BT_NIMBLE_MAX_CONNECTIONS + 1];
+static uint16_t ble_spp_svc_gatt_read_val_handle;
 
 void ble_store_config_init(void);
 
@@ -108,7 +97,7 @@ ble_spp_server_advertise(void)
     fields.name_is_complete = 1;
 
     fields.uuids16 = (ble_uuid16_t[]) {
-        BLE_UUID16_INIT(GATT_SVR_SVC_ALERT_UUID)
+        BLE_UUID16_INIT(BLE_SVC_SPP_UUID16)
     };
     fields.num_uuids16 = 1;
     fields.uuids16_is_complete = 1;
@@ -154,7 +143,7 @@ ble_spp_server_gap_event(struct ble_gap_event *event, void *arg)
 
     switch (event->type) {
     case BLE_GAP_EVENT_CONNECT:
-	/* A new connection was established or a connection attempt failed. */
+        /* A new connection was established or a connection attempt failed. */
         MODLOG_DFLT(INFO, "connection %s; status=%d ",
                     event->connect.status == 0 ? "established" : "failed",
                     event->connect.status);
@@ -162,10 +151,10 @@ ble_spp_server_gap_event(struct ble_gap_event *event, void *arg)
             rc = ble_gap_conn_find(event->connect.conn_handle, &desc);
             assert(rc == 0);
             ble_spp_server_print_conn_desc(&desc);
-	    connection_handle[event->connect.conn_handle - 1] = event->connect.conn_handle;
+            connection_handle[event->connect.conn_handle] = event->connect.conn_handle;
         }
         MODLOG_DFLT(INFO, "\n");
-	if (event->connect.status != 0 || CONFIG_BT_NIMBLE_MAX_CONNECTIONS > 1) {
+        if (event->connect.status != 0 || CONFIG_BT_NIMBLE_MAX_CONNECTIONS > 1) {
             /* Connection failed or if multiple connection allowed; resume advertising. */
             ble_spp_server_advertise();
         }
@@ -176,6 +165,8 @@ ble_spp_server_gap_event(struct ble_gap_event *event, void *arg)
         ble_spp_server_print_conn_desc(&event->disconnect.conn);
         MODLOG_DFLT(INFO, "\n");
 
+        connection_handle[event->disconnect.conn.conn_handle] = -1;
+
         /* Connection terminated; resume advertising. */
         ble_spp_server_advertise();
         return 0;
@@ -204,7 +195,7 @@ ble_spp_server_gap_event(struct ble_gap_event *event, void *arg)
         return 0;
 
     default:
-    	return 0;
+        return 0;
     }
 }
 
@@ -250,137 +241,155 @@ void ble_spp_server_host_task(void *param)
 }
 
 /* Callback function for custom service */
-static int  ble_svc_gatt_handler(uint16_t conn_handle, uint16_t attr_handle,struct ble_gatt_access_ctxt *ctxt, void *arg)
+static int  ble_svc_gatt_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg)
 {
-      switch(ctxt->op){
-      case BLE_GATT_ACCESS_OP_READ_CHR:
-         ESP_LOGI(tag, "Callback for read");
-      break;
-
-      case BLE_GATT_ACCESS_OP_WRITE_CHR:
-	 ESP_LOGI(tag,"Data received in write event,conn_handle = %x,attr_handle = %x",conn_handle,attr_handle);
-      break;
-
-      default:
-         ESP_LOGI(tag, "\nDefault Callback");
-      break;
-      }
-      return 0;
+    switch (ctxt->op) {
+    case BLE_GATT_ACCESS_OP_READ_CHR:
+        ESP_LOGI(tag, "Callback for read");
+        break;
+
+    case BLE_GATT_ACCESS_OP_WRITE_CHR:
+        ESP_LOGI(tag, "Data received in write event,conn_handle = %x,attr_handle = %x", conn_handle, attr_handle);
+        break;
+
+    default:
+        ESP_LOGI(tag, "\nDefault Callback");
+        break;
+    }
+    return 0;
 }
 
 /* Define new custom service */
 static const struct ble_gatt_svc_def new_ble_svc_gatt_defs[] = {
-      {
-          /*** Service: GATT */
-          .type = BLE_GATT_SVC_TYPE_PRIMARY,
-          .uuid = BLE_UUID16_DECLARE(BLE_SVC_ANS_UUID16),
-          .characteristics = (struct ble_gatt_chr_def[]) { {
-	  		/* Support new alert category */
-              		.uuid = BLE_UUID16_DECLARE(BLE_SVC_ANS_CHR_UUID16_SUP_NEW_ALERT_CAT),
-              		.access_cb = ble_svc_gatt_handler,
-              		.val_handle = &ble_svc_gatt_read_val_handle,
-              		.flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_INDICATE,
-	  	},
-	  	{
-	  		0, /* No more characteristics */
-		}
-	  },
-      },
-      {
-      	/*** Service: SPP */
-          .type = BLE_GATT_SVC_TYPE_PRIMARY,
-          .uuid = BLE_UUID16_DECLARE(BLE_SVC_SPP_UUID16),
-          .characteristics = (struct ble_gatt_chr_def[]) { {
-                        /* Support SPP service */
-                        .uuid = BLE_UUID16_DECLARE(BLE_SVC_SPP_CHR_UUID16),
-                        .access_cb = ble_svc_gatt_handler,
-                        .val_handle = &ble_spp_svc_gatt_read_val_handle,
-                        .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_INDICATE,
-		},
-                {
-                         0, /* No more characteristics */
-                }
-           },
-      },
-      {
-          0, /* No more services. */
-      },
+    {
+        /*** Service: SPP */
+        .type = BLE_GATT_SVC_TYPE_PRIMARY,
+        .uuid = BLE_UUID16_DECLARE(BLE_SVC_SPP_UUID16),
+        .characteristics = (struct ble_gatt_chr_def[])
+        { {
+                /* Support SPP service */
+                .uuid = BLE_UUID16_DECLARE(BLE_SVC_SPP_CHR_UUID16),
+                .access_cb = ble_svc_gatt_handler,
+                .val_handle = &ble_spp_svc_gatt_read_val_handle,
+                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_INDICATE,
+            }, {
+                0, /* No more characteristics */
+            }
+        },
+    },
+    {
+        0, /* No more services. */
+    },
 };
 
-int gatt_svr_register(void)
+static void
+gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg)
+{
+    char buf[BLE_UUID_STR_LEN];
+
+    switch (ctxt->op) {
+    case BLE_GATT_REGISTER_OP_SVC:
+        MODLOG_DFLT(DEBUG, "registered service %s with handle=%d\n",
+                    ble_uuid_to_str(ctxt->svc.svc_def->uuid, buf),
+                    ctxt->svc.handle);
+        break;
+
+    case BLE_GATT_REGISTER_OP_CHR:
+        MODLOG_DFLT(DEBUG, "registering characteristic %s with "
+                    "def_handle=%d val_handle=%d\n",
+                    ble_uuid_to_str(ctxt->chr.chr_def->uuid, buf),
+                    ctxt->chr.def_handle,
+                    ctxt->chr.val_handle);
+        break;
+
+    case BLE_GATT_REGISTER_OP_DSC:
+        MODLOG_DFLT(DEBUG, "registering descriptor %s with handle=%d\n",
+                    ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buf),
+                    ctxt->dsc.handle);
+        break;
+
+    default:
+        assert(0);
+        break;
+    }
+}
+
+int gatt_svr_init(void)
 {
-     int rc=0;
+    int rc = 0;
+    ble_svc_gap_init();
+    ble_svc_gatt_init();
 
-     rc = ble_gatts_count_cfg(new_ble_svc_gatt_defs);
+    rc = ble_gatts_count_cfg(new_ble_svc_gatt_defs);
 
-     if (rc != 0) {
-         return rc;
-     }
+    if (rc != 0) {
+        return rc;
+    }
 
-     rc = ble_gatts_add_svcs(new_ble_svc_gatt_defs);
-     if (rc != 0) {
-         return rc;
-     }
+    rc = ble_gatts_add_svcs(new_ble_svc_gatt_defs);
+    if (rc != 0) {
+        return rc;
+    }
 
-     return 0;
+    return 0;
 }
 
 
-void ble_server_uart_task(void *pvParameters){
-     ESP_LOGI(tag,"BLE server UART_task started\n");
-     uart_event_t event;
-     int rc=0;
-     for (;;) {
-         //Waiting for UART event.
-         if (xQueueReceive(spp_common_uart_queue, (void * )&event, (TickType_t)portMAX_DELAY))            {
-	     switch (event.type) {
-             //Event of UART receving data
-             case UART_DATA:
-		if (event.size) {
-			static uint8_t ntf[1];
-                        ntf[0] = 90;
-			for ( int i = 0; i < CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) {
-			    if (connection_handle[i] != 0) {
-				struct os_mbuf *txom;
-				txom = ble_hs_mbuf_from_flat(ntf, sizeof(ntf));
-				rc = ble_gatts_notify_custom(connection_handle[i],
-							     ble_spp_svc_gatt_read_val_handle,
-							     txom);
-				if ( rc == 0 ) {
-				    ESP_LOGI(tag,"Notification sent successfully");
-				}
-				else {
-				    ESP_LOGI(tag,"Error in sending notification rc = %d", rc);
-				}
-			    }
-			}
-		}
-             	break;
-	     default:
-	     	break;
-	     }
-	  }
-	}
-	  vTaskDelete(NULL);
+void ble_server_uart_task(void *pvParameters)
+{
+    ESP_LOGI(tag, "BLE server UART_task started\n");
+    uart_event_t event;
+    int rc = 0;
+    for (;;) {
+        //Waiting for UART event.
+        if (xQueueReceive(spp_common_uart_queue, (void * )&event, (TickType_t)portMAX_DELAY))            {
+            switch (event.type) {
+            //Event of UART receving data
+            case UART_DATA:
+                if (event.size) {
+                    static uint8_t ntf[1];
+                    ntf[0] = 90;
+                    for ( int i = 0; i <= CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) {
+                        if (connection_handle[i] != -1) {
+                            struct os_mbuf *txom;
+                            txom = ble_hs_mbuf_from_flat(ntf, sizeof(ntf));
+                            rc = ble_gatts_notify_custom(connection_handle[i],
+                                                         ble_spp_svc_gatt_read_val_handle,
+                                                         txom);
+                            if ( rc == 0 ) {
+                                ESP_LOGI(tag, "Notification sent successfully");
+                            } else {
+                                ESP_LOGI(tag, "Error in sending notification rc = %d", rc);
+                            }
+                        }
+                    }
+                }
+                break;
+            default:
+                break;
+            }
+        }
+    }
+    vTaskDelete(NULL);
 }
 static void ble_spp_uart_init(void)
 {
-     uart_config_t uart_config = {
-         .baud_rate = 115200,
-         .data_bits = UART_DATA_8_BITS,
-         .parity = UART_PARITY_DISABLE,
-         .stop_bits = UART_STOP_BITS_1,
-         .flow_ctrl = UART_HW_FLOWCTRL_RTS,
-         .rx_flow_ctrl_thresh = 122,
-         .source_clk = UART_SCLK_DEFAULT,
-     };
-     //Install UART driver, and get the queue.
-     uart_driver_install(UART_NUM_0, 4096, 8192, 10,&spp_common_uart_queue,0);
-     //Set UART parameters
-     uart_param_config(UART_NUM_0, &uart_config);
-     //Set UART pins
-     uart_set_pin(UART_NUM_0, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
-     xTaskCreate(ble_server_uart_task, "uTask", 4096, (void*)UART_NUM_0, 8, NULL);
+    uart_config_t uart_config = {
+        .baud_rate = 115200,
+        .data_bits = UART_DATA_8_BITS,
+        .parity = UART_PARITY_DISABLE,
+        .stop_bits = UART_STOP_BITS_1,
+        .flow_ctrl = UART_HW_FLOWCTRL_RTS,
+        .rx_flow_ctrl_thresh = 122,
+        .source_clk = UART_SCLK_DEFAULT,
+    };
+    //Install UART driver, and get the queue.
+    uart_driver_install(UART_NUM_0, 4096, 8192, 10, &spp_common_uart_queue, 0);
+    //Set UART parameters
+    uart_param_config(UART_NUM_0, &uart_config);
+    //Set UART pins
+    uart_set_pin(UART_NUM_0, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+    xTaskCreate(ble_server_uart_task, "uTask", 4096, (void *)UART_NUM_0, 8, NULL);
 }
 
 
@@ -399,6 +408,11 @@ app_main(void)
 
     nimble_port_init();
 
+    /* Initialize connection_handle array */
+    for (int i = 0; i <= CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) {
+        connection_handle[i] = -1;
+    }
+
     /* Initialize uart driver and start uart task */
     ble_spp_uart_init();
 
@@ -425,12 +439,8 @@ app_main(void)
     ble_hs_cfg.sm_their_key_dist = 1;
 #endif
 
-
-    rc = new_gatt_svr_init();
-    assert(rc == 0);
-
     /* Register custom service */
-    rc = gatt_svr_register();
+    rc = gatt_svr_init();
     assert(rc == 0);
 
     /* Set the default device name. */