Revision 259:323f588e5f57, committed 2016-09-15

Comitter:

Vincent Coubard

Date:

Thu Sep 15 10:51:21 2016 +0100

Branch:

70f819844d61400a7b6772dcc911a63fc73af09e

Parent:

258:e5ef65120b06

Child:

260:e93cbde933ce

Commit message:

Sync with 70f819844d61400a7b6772dcc911a63fc73af09e

2016-07-08 12:18:26+01:00: Vincent Coubard
Fix ServiceInstantiation_test_xx

Fix the last handle set for characteristics
Fix discovery order.
From a user perspective, the service tree is traversed in deep.

The previous behavior was:

for service in services:
service_callback(callback)

for characteristic in characteristics:
characteristic_callback(characteristic)

The new behavior is:

for service in services:
service_callback(callback)
for characteristic in service.characteristics:
characteristic_callback(characteristic)

Reserve the valid count of attribute records when declaring a service.
Report error when it is not possible to add a service.

Changed in this revision

--- a/source/BlueNRGDiscoveredCharacteristic.cpp	Thu Sep 15 10:51:20 2016 +0100
+++ b/source/BlueNRGDiscoveredCharacteristic.cpp	Thu Sep 15 10:51:21 2016 +0100
@@ -62,3 +62,7 @@
     props._indicate        = propsIn.indicate();
     props._authSignedWrite = propsIn.authSignedWrite();
 }
+
+ void BlueNRGDiscoveredCharacteristic::setLastHandle(GattAttribute::Handle_t  lastHandleIn) {
+     lastHandle = lastHandleIn;
+ }
\ No newline at end of file

--- a/source/BlueNRGGap.cpp	Thu Sep 15 10:51:20 2016 +0100
+++ b/source/BlueNRGGap.cpp	Thu Sep 15 10:51:21 2016 +0100
@@ -501,9 +501,9 @@
     );
 
     if (err) {
-        printf("impossible to set advertising parameters\n\r");
-        printf("advInterval min: %u, advInterval max: %u\n\r", advInterval, advInterval + 1);
-        printf("advType: %u, advFilterPolicy: %u\n\r", params.getAdvertisingType(), advFilterPolicy);
+        PRINTF("impossible to set advertising parameters\n\r");
+        PRINTF("advInterval min: %u, advInterval max: %u\n\r", advInterval, advInterval + 1);
+        PRINTF("advType: %u, advFilterPolicy: %u\n\r", params.getAdvertisingType(), advFilterPolicy);
         return BLE_ERROR_INVALID_PARAM;
     }
 
@@ -1352,7 +1352,7 @@
     advIntMS = (conn_min_interval*1.25)-GUARD_INT;
     advInterval = _advParams.MSEC_TO_ADVERTISEMENT_DURATION_UNITS(advIntMS);
 
-    printf("conn_min_interval is equal to %u\r\n", conn_min_interval);
+    PRINTF("conn_min_interval is equal to %u\r\n", conn_min_interval);
   } else {
     advInterval = _advParams.getIntervalInADVUnits();
   }

--- a/source/BlueNRGGattClient.cpp	Thu Sep 15 10:51:20 2016 +0100
+++ b/source/BlueNRGGattClient.cpp	Thu Sep 15 10:51:21 2016 +0100
@@ -15,7 +15,7 @@
 */
 /**
   ******************************************************************************
-  * @file    BlueNRGGattServer.cpp 
+  * @file    BlueNRGGattServer.cpp
   * @author  STMicroelectronics
   * @brief   Implementation of BlueNRG BLE_API GattServer Class
   ******************************************************************************
@@ -29,13 +29,13 @@
   * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
-  */ 
- 
+  */
+
 /** @defgroup BlueNRGGATTClient
  *  @brief BlueNRG BLE_API GattClient Adaptation
  *  @{
  */
- 
+
 #include "BlueNRGGattClient.h"
 #include "mbed-drivers/mbed.h"
 #include "BlueNRGGap.h"
@@ -59,10 +59,10 @@
     _currentState = GATT_IDLE;
     return;
   }
-    
+
   // Service Discovery complete
 /*
-  if(_currentState != GATT_IDLE && 
+  if(_currentState != GATT_IDLE &&
      _currentState != GATT_DISCOVERY_TERMINATED &&
      _currentState != GATT_WRITE_CHAR &&
      _currentState != GATT_READ_CHAR) {
@@ -86,7 +86,7 @@
     _currentState = GATT_IDLE;
   }
 }
-                                                
+
 void BlueNRGGattClient::primaryServicesCB(Gap::Handle_t connectionHandle,
                                           uint8_t event_data_length,
                                           uint8_t attribute_data_length,
@@ -107,13 +107,13 @@
 
     // UUID Type
     if (attribute_data_length == 6) {
-      
+
       PRINTF("UUID_TYPE_16\n\r");
       uuid = attribute_data_list[offset+5]<<8|attribute_data_list[offset+4];
       PRINTF("S UUID-%X attrs[%u %u]\r\n", uuid.getShortUUID(), startHandle, endHandle);
-      
+
     } else {
-      
+
       PRINTF("UUID_TYPE_128\n\r");
       uuid.setupLong(attribute_data_list+offset+4, UUID::LSB);
 
@@ -125,17 +125,12 @@
       }
 #endif
       PRINTF(" attrs[%u %u]\r\n", startHandle, endHandle);
-      
+
     }
-    
+
     PRINTF("Setup serviceIndex = %d\n\r", _numServices);
     discoveredService[_numServices].setup(uuid, startHandle, endHandle);
-    
-    if(serviceDiscoveryCallback) {
-      if(_matchingServiceUUID == BLE_UUID_UNKNOWN || _matchingServiceUUID == discoveredService[_numServices].getUUID()) {
-        serviceDiscoveryCallback(&discoveredService[_numServices]);
-      }
-    }
+
     _numServices++;
 
     offset += attribute_data_length;
@@ -181,12 +176,9 @@
     }
 
     discoveredService[i].setup(uuid, startHandle, endHandle);
-    
-    if(serviceDiscoveryCallback) {
-      serviceDiscoveryCallback(&discoveredService[_numServices]);
-    }
+
     _numServices++;
-    
+
     offset += 4;
   }
 }
@@ -225,10 +217,10 @@
       PRINTF("\r\n");
 #endif
     }
-    
+
     // Properties
     DiscoveredCharacteristic::Properties_t p;
-    
+
     p._broadcast = (props_mask[0] & handle_value_pair[offset+2]);
     p._read = (props_mask[1] & handle_value_pair[offset+2])>>1;
     p._writeWoResp = (props_mask[2] & handle_value_pair[offset+2])>>2;
@@ -263,9 +255,14 @@
                                     valueHandle,
                                     lastHandle);
 
-    if(characteristicDiscoveryCallback) {
-      characteristicDiscoveryCallback(&discoveredChar[_numChars]);
+    if (_numChars != 0) {
+        discoveredChar[_numChars - 1].setLastHandle(declHandle - 1);
+
+        if(characteristicDiscoveryCallback) {
+          characteristicDiscoveryCallback(&discoveredChar[_numChars - 1]);
+        }
     }
+
     _numChars++;
 
     offset += handle_value_pair_length;
@@ -280,9 +277,9 @@
   // Charac Properties(1), Charac Value Handle(2), Charac UUID(2/16)
   GattAttribute::Handle_t declHandle, valueHandle, lastHandle;
   UUID uuid;
-  
+
   PRINTF("serviceCharByUUIDCB\n\r");
-  
+
   // UUID Type
   if (event_data_length == 7) {
     PRINTF("Char UUID_TYPE_16\n\r");
@@ -303,7 +300,7 @@
 
   // Properties
   DiscoveredCharacteristic::Properties_t p;
-  
+
   p._broadcast = (props_mask[0] & attr_value[0]);
   p._read = (props_mask[1] & attr_value[0])>>1;
   p._writeWoResp = (props_mask[2] & attr_value[0])>>2;
@@ -336,7 +333,7 @@
                                   declHandle,
                                   valueHandle,
                                   lastHandle);
-  
+
   if(characteristicDiscoveryCallback) {
     characteristicDiscoveryCallback(&discoveredChar[_numChars]);
   }
@@ -346,24 +343,37 @@
 ble_error_t BlueNRGGattClient::findServiceChars(Gap::Handle_t connectionHandle)
 {
   PRINTF("findServiceChars\n\r");
-  
+
   tBleStatus ret;
   uint8_t uuid_type = UUID_TYPE_16;
   uint8_t short_uuid[2];
   uint8_t *uuid = NULL;
-  
+
   DiscoveredService *service;
-  
+
+  // complete the discovery of the last characteristic of the previous service.
+  // Its last handle wasn't known before this point
+  // update the handle and call the characteristic discovery callback.
+  if (_servIndex != 0 && _numChars != 0) {
+      discoveredChar[_numChars - 1].setLastHandle(discoveredService[_servIndex - 1].getEndHandle());
+
+      if(characteristicDiscoveryCallback) {
+        characteristicDiscoveryCallback(&discoveredChar[_numChars - 1]);
+      }
+  }
+
+  _numChars = 0;
+
   // We finished chars discovery for all services
   if(_servIndex >= _numServices) {
     PRINTF("!!!We finished chars discovery for all services!!!\n\r");
     //_currentState = GATT_CHARS_DISCOVERY_COMPLETE;
-    
+
     terminateServiceDiscovery();
-    
+
     return BLE_ERROR_NONE;
   }
-    
+
   service = &discoveredService[_servIndex];
   /*
   if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
@@ -377,55 +387,59 @@
     PRINTF("\r\n");
   }
   */
-  
+
+  if(serviceDiscoveryCallback) {
+    serviceDiscoveryCallback(service);
+  }
+
   PRINTF("findServiceChars (_servIndex=%d)\n\r", _servIndex);
   //ret = aci_gatt_disc_all_charac_of_serv(connectionHandle, service->getStartHandle(), service->getEndHandle());
 
-  if(_matchingCharacteristicUUIDIn == BLE_UUID_UNKNOWN) {
-    PRINTF("findServiceChars (BLE_UUID_UNKNOWN)\n\r");
-    ret = aci_gatt_disc_all_charac_of_serv(connectionHandle, service->getStartHandle(), service->getEndHandle());
-  } else {
-    
-    uint8_t type = _matchingCharacteristicUUIDIn.shortOrLong();
-    
-    if(type == UUID::UUID_TYPE_SHORT) {
-      STORE_LE_16(short_uuid, _matchingCharacteristicUUIDIn.getShortUUID());
-      
-      uuid_type = UUID_TYPE_16;
-      uuid = short_uuid;
+    if(_matchingCharacteristicUUIDIn == BLE_UUID_UNKNOWN) {
+        PRINTF("findServiceChars (BLE_UUID_UNKNOWN)\n\r");
+        ret = aci_gatt_disc_all_charac_of_serv(connectionHandle, service->getStartHandle(), service->getEndHandle());
+    } else {
+
+        uint8_t type = _matchingCharacteristicUUIDIn.shortOrLong();
+
+        if(type == UUID::UUID_TYPE_SHORT) {
+            STORE_LE_16(short_uuid, _matchingCharacteristicUUIDIn.getShortUUID());
+
+            uuid_type = UUID_TYPE_16;
+            uuid = short_uuid;
 #ifdef DEBUG
-      PRINTF("findServiceChars C UUID-");
-      for(unsigned i = 0; i < 2; i++) {
-        PRINTF("%02X", short_uuid[i]);
-      }
-      PRINTF("\n\r");
+            PRINTF("findServiceChars C UUID-");
+            for(unsigned i = 0; i < 2; i++) {
+                PRINTF("%02X", short_uuid[i]);
+            }
+            PRINTF("\n\r");
 #endif
-    } else if(type==UUID::UUID_TYPE_LONG) {
-      
-      uuid_type = UUID_TYPE_128;
-      uuid = (unsigned char*)_matchingCharacteristicUUIDIn.getBaseUUID();
+        } else if(type==UUID::UUID_TYPE_LONG) {
+
+            uuid_type = UUID_TYPE_128;
+            uuid = (unsigned char*)_matchingCharacteristicUUIDIn.getBaseUUID();
 #ifdef DEBUG
-      PRINTF("(findServiceChars) C UUID-");
-      for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
-        PRINTF("%02X", uuid[i]);
-      }
-      PRINTF("\r\n");
+            PRINTF("(findServiceChars) C UUID-");
+            for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
+                PRINTF("%02X", uuid[i]);
+            }
+            PRINTF("\r\n");
 #endif
+        }
+
+        ret = aci_gatt_disc_charac_by_uuid(connectionHandle,
+                                           service->getStartHandle(),
+                                           service->getEndHandle(),
+                                           uuid_type,
+                                           uuid);
     }
 
-    ret = aci_gatt_disc_charac_by_uuid(connectionHandle,
-                                       service->getStartHandle(),
-                                       service->getEndHandle(),
-                                       uuid_type,
-                                       uuid);
-  }
-  
   if(ret == BLE_STATUS_SUCCESS) {
     _servIndex++;
   }
-  
+
   PRINTF("findServiceChars ret=%d\n\r", ret);
-  
+
   return BLE_ERROR_NONE;
 }
 
@@ -436,7 +450,7 @@
                                                       const UUID                                 &matchingCharacteristicUUIDIn)
 {
   PRINTF("launchServiceDiscovery\n\r");
-  
+
   tBleStatus ret;
   uint8_t uuid_type = UUID_TYPE_16;
   uint8_t short_uuid[2];
@@ -466,17 +480,17 @@
   for(j = 0; j < BLE_TOTAL_DISCOVERED_SERVICES; j++) {
     discoveredService[j].setup(BLE_UUID_UNKNOWN, GattAttribute::INVALID_HANDLE, GattAttribute::INVALID_HANDLE);
   }
-  
+
   if(matchingServiceUUID == BLE_UUID_UNKNOWN) {
-    
+
     // Wildcard: search for all services
     ret = aci_gatt_disc_all_prim_services((uint16_t)connectionHandle);
-    
+
   } else {
-  
+
     uint8_t type = matchingServiceUUID.shortOrLong();
     //PRINTF("AddService(): Type:%d\n\r", type);
-    
+
     if(type == UUID::UUID_TYPE_SHORT) {
       STORE_LE_16(short_uuid, matchingServiceUUID.getShortUUID());
 #ifdef DEBUG
@@ -486,10 +500,10 @@
       }
       PRINTF("\n\r");
 #endif
-      
+
       uuid_type = UUID_TYPE_16;
       uuid = short_uuid;
-      
+
     } else if(type==UUID::UUID_TYPE_LONG) {
 
       uuid_type = UUID_TYPE_128;
@@ -503,18 +517,18 @@
       PRINTF("\n\r");
 #endif
     }
-    
+
     // search for specific service by UUID
     ret = aci_gatt_disc_prim_service_by_uuid((uint16_t)connectionHandle, uuid_type, uuid);
     //ret = aci_gatt_disc_all_prim_services((uint16_t)connectionHandle);
   }
-    
+
   if(ret == BLE_STATUS_SUCCESS) {
     _currentState = GATT_SERVICE_DISCOVERY;
   }
-  
+
   PRINTF("launchServiceDiscovery ret=%d\n\r", ret);
-  
+
   return BLE_ERROR_NONE;
 }
 
@@ -558,7 +572,7 @@
          _currentState == GATT_WRITE_CHAR ) {
     return false;
   }
-  
+
   return true;
 */
 }
@@ -566,7 +580,7 @@
 void BlueNRGGattClient::terminateServiceDiscovery(void)
 {
   _currentState = GATT_IDLE;//GATT_DISCOVERY_TERMINATED;
-  
+
   if (terminationCallback) {
     terminationCallback(_connectionHandle);
   }
@@ -590,18 +604,18 @@
   (void)offset;
 
   tBleStatus ret;
-  
+
   BlueNRGGattClient *gattc = const_cast<BlueNRGGattClient*>(this);
-  
-  // Save the attribute_handle not provided by evt_att_read_resp    
+
+  // Save the attribute_handle not provided by evt_att_read_resp
   gattc->readCBParams.handle = attributeHandle;
-  
+
   // FIXME: We need to wait for a while before starting a read
   // due to BlueNRG process queue handling
   Clock_Wait(100);
 
   ret = aci_gatt_read_charac_val(connHandle, attributeHandle);
-  
+
   if(ret == BLE_STATUS_SUCCESS) {
     gattc->_currentState = GATT_READ_CHAR;
     return BLE_ERROR_NONE;
@@ -639,7 +653,7 @@
   (void)event_data_length;
 
   writeCBParams.connHandle = connHandle;
-  
+
   BlueNRGGattClient::getInstance().processWriteResponse(&writeCBParams);
 }
 
@@ -653,9 +667,9 @@
   (void)cmd;
 
   tBleStatus ret;
-  
+
   BlueNRGGattClient *gattc = const_cast<BlueNRGGattClient*>(this);
-  
+
   // We save the write response params (used by the callback) because
   // when the aci_gatt_write_charac_value() is used the only event received is the EVT_BLUE_GATT_PROCEDURE_COMPLETE
   gattc->writeCBParams.connHandle = connHandle;
@@ -664,10 +678,10 @@
   gattc->writeCBParams.offset = 0;
   gattc->writeCBParams.len = length;
   gattc->writeCBParams.data = value;
-  
+
   ret = aci_gatt_write_charac_value(connHandle, attributeHandle, length, const_cast<uint8_t *>(value));
   //ret = aci_gatt_write_charac_reliable(connHandle, attributeHandle, 0, length, const_cast<uint8_t *>(value));
-  
+
   if (ret == BLE_STATUS_SUCCESS) {
     gattc->_currentState = GATT_WRITE_CHAR;
     return BLE_ERROR_NONE;
@@ -694,7 +708,7 @@
     handle_uuid_length = 18; //Handle + UUID_128
 
   numCharacDesc = (event_data_length - 1) / handle_uuid_length;
-  
+
   offset = 0;
 
   for (i=0; i<numCharacDesc; i++) {
@@ -702,13 +716,13 @@
 
     // UUID Type
     if (handle_uuid_length == 4) {
-      
+
       PRINTF("UUID_TYPE_16\n\r");
       uuid = handle_uuid_pair[offset+3]<<8|handle_uuid_pair[offset+2];
       PRINTF("D UUID-%X attHandle=%u\r\n", uuid.getShortUUID(), attHandle);
-      
+
     } else {
-      
+
       PRINTF("UUID_TYPE_128\n\r");
       uuid.setupLong(handle_uuid_pair+offset+2, UUID::LSB);
 #ifdef DEBUG
@@ -812,5 +826,4 @@
   memset(discoveredChar, 0, sizeof(discoveredChar));
 
   return BLE_ERROR_NONE;
-}
-
+}
\ No newline at end of file

--- a/source/BlueNRGGattServer.cpp	Thu Sep 15 10:51:20 2016 +0100
+++ b/source/BlueNRGGattServer.cpp	Thu Sep 15 10:51:21 2016 +0100
@@ -76,8 +76,17 @@
     const uint8_t *base_uuid;
     const uint8_t *base_char_uuid;
 
-    uint8_t charsCount = 0;
-    uint8_t maxAttrRecords = 0;
+    uint8_t charsCount = service.getCharacteristicCount();
+    const uint8_t available_characteristics = BLE_TOTAL_CHARACTERISTICS - characteristicCount;
+
+    // check that there is enough characteristics left in the
+    // characteristic array.
+    if (charsCount > available_characteristics) {
+        PRINTF("charCount = %u and characteristicCount = %u\r\n", charsCount, available_characteristics);
+        return BLE_ERROR_NO_MEM;
+    }
+
+    const uint16_t maxAttrRecords = computeAttributesRecord(service);
 
     type = (service.getUUID()).shortOrLong();
     PRINTF("AddService(): Type:%d\n\r", type);
@@ -92,17 +101,14 @@
         COPY_UUID_128(primary_base_uuid, base_uuid[15],base_uuid[14],primary_short_uuid[1],primary_short_uuid[0],base_uuid[11],base_uuid[10],base_uuid[9],base_uuid[8],base_uuid[7],base_uuid[6],base_uuid[5],base_uuid[4],base_uuid[3],base_uuid[2],base_uuid[1],base_uuid[0]);
     }
 
-    charsCount = service.getCharacteristicCount();
-    //1(service record)+2records*char+1record*char_desc
-    maxAttrRecords = 1+3*charsCount;
-
     if(type==UUID::UUID_TYPE_SHORT) {
         ret = aci_gatt_add_serv(UUID_TYPE_16,
                                 primary_short_uuid,
                                 PRIMARY_SERVICE,
                                 maxAttrRecords/*7*/,
                                 &servHandle);
-        PRINTF("aci_gatt_add_serv UUID_TYPE_LONG ret=%d\n\r", ret);
+        PRINTF("aci_gatt_add_serv UUID_TYPE_SHORT ret=%d\n\r", ret);
+
     }
     else if(type==UUID::UUID_TYPE_LONG) {
         ret = aci_gatt_add_serv(UUID_TYPE_128,
@@ -113,6 +119,25 @@
         PRINTF("aci_gatt_add_serv UUID_TYPE_LONG ret=%d\n\r", ret);
     }
 
+    switch (ret) {
+        case BLE_STATUS_SUCCESS:
+            break;
+
+        case BLE_STATUS_INVALID_PARAMETER:
+            return BLE_ERROR_INVALID_PARAM;
+
+        case BLE_STATUS_OUT_OF_HANDLE:
+        case BLE_STATUS_INSUFFICIENT_RESOURCES:
+        case ERR_UNSPECIFIED_ERROR:
+            return BLE_ERROR_NO_MEM;
+
+        case BLE_STATUS_ERROR:
+        default:
+            return BLE_ERROR_INTERNAL_STACK_FAILURE;
+    }
+
+
+
     service.setHandle(servHandle);
     //serviceHandleVector.push_back(servHandle);
     PRINTF("added servHandle handle =%u\n\r", servHandle);
@@ -191,6 +216,40 @@
                     p_char->getProperties(), p_char->getValueAttribute().getMaxLength(), ret);
         }
 
+        switch (ret) {
+            case BLE_STATUS_SUCCESS:
+                break;
+
+            case ERR_UNSPECIFIED_ERROR:
+            case BLE_STATUS_INSUFFICIENT_RESOURCES:
+            case BLE_STATUS_OUT_OF_HANDLE:
+                // TODO remove characteristics and the service previously added.
+                // remove service in the stack by using: Aci_Gatt_Del_Service
+                // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+                // update service counter
+                // destroy registered characteristic and updat echaracteristic counter
+                return BLE_ERROR_NO_MEM;
+
+            case BLE_STATUS_INVALID_HANDLE:
+            case BLE_STATUS_INVALID_PARAMETER:
+            case BLE_STATUS_CHARAC_ALREADY_EXISTS:
+            // TODO remove characteristics and the service previously added.
+            // remove service in the stack by using: Aci_Gatt_Del_Service
+            // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+            // update service counter
+            // destroy registered characteristic and updat echaracteristic counter
+                return BLE_ERROR_INVALID_PARAM;
+
+            case BLE_STATUS_ERROR:
+            default:
+            // TODO remove characteristics and the service previously added.
+            // remove service in the stack by using: Aci_Gatt_Del_Service
+            // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+            // update service counter
+            // destroy registered characteristic and updat echaracteristic counter
+                return BLE_ERROR_INTERNAL_STACK_FAILURE;
+        }
+
         bleCharHandleMap.insert(std::pair<uint16_t, uint16_t>(bleCharacteristic, servHandle));
 
         p_characteristics[characteristicCount++] = p_char;
@@ -199,9 +258,13 @@
         PRINTF("added bleCharacteristic (value handle =%u)\n\r", p_char->getValueAttribute().getHandle());
 
         if ((p_char->getValueAttribute().getValuePtr() != NULL) && (p_char->getValueAttribute().getLength() > 0)) {
-            write(p_char->getValueAttribute().getHandle(),
+            ble_error_t err = write(p_char->getValueAttribute().getHandle(),
                   p_char->getValueAttribute().getValuePtr(),
                   p_char->getValueAttribute().getLength(), false /* localOnly */);
+            if (err) {
+                PRINTF("ERROR HERE !!!!\r\n");
+                return err;
+            }
         }
 
         // add descriptors now
@@ -226,12 +289,45 @@
                                          CHAR_ATTRIBUTE_LEN_IS_FIXED,
                                          &descHandle);
             PRINTF("Adding Descriptor descriptor handle=%d ret=%d\n\r", descHandle, ret);
-            if(ret==(tBleStatus)0) {
-                PRINTF("Descriptor added successfully, descriptor handle=%d\n\r", descHandle);
-                descriptor->setHandle(descHandle);
+
+            switch (ret) {
+                case BLE_STATUS_SUCCESS:
+                    PRINTF("Descriptor added successfully, descriptor handle=%d\n\r", descHandle);
+                    descriptor->setHandle(descHandle);
+                    break;
+
+                case ERR_UNSPECIFIED_ERROR:
+                case BLE_STATUS_INSUFFICIENT_RESOURCES:
+                case BLE_STATUS_OUT_OF_HANDLE:
+                    // TODO remove characteristics and the service previously added.
+                    // remove service in the stack by using: Aci_Gatt_Del_Service
+                    // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+                    // update service counter
+                    // destroy registered characteristic and updat echaracteristic counter
+                    return BLE_ERROR_NO_MEM;
+
+                case BLE_STATUS_INVALID_HANDLE:
+                case BLE_STATUS_INVALID_PARAMETER:
+                // TODO remove characteristics and the service previously added.
+                // remove service in the stack by using: Aci_Gatt_Del_Service
+                // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+                // update service counter
+                // destroy registered characteristic and updat echaracteristic counter
+                    return BLE_ERROR_INVALID_PARAM;
+
+                case BLE_STATUS_INVALID_OPERATION:
+                    return BLE_ERROR_OPERATION_NOT_PERMITTED;
+
+                case BLE_STATUS_ERROR:
+                default:
+                // TODO remove characteristics and the service previously added.
+                // remove service in the stack by using: Aci_Gatt_Del_Service
+                // remove characteristics in the stack by using: Aci_Gatt_Del_Char
+                // update service counter
+                // destroy registered characteristic and updat echaracteristic counter
+                    return BLE_ERROR_INTERNAL_STACK_FAILURE;
             }
         }
-
     }
 
     serviceCount++;
@@ -528,4 +624,45 @@
     characteristicCount = 0;
 
     return BLE_ERROR_NONE;
+}
+
+
+/// compute the number of attributes needed by this service.
+uint16_t BlueNRGGattServer::computeAttributesRecord(GattService& service) {
+    uint16_t attribute_records = 1;
+
+    for (uint8_t characteristic_index = 0; characteristic_index < service.getCharacteristicCount(); ++characteristic_index) {
+        // add two attributes, one for the characteristic declaration
+        // and the other for the characteristic value.
+        attribute_records += 2;
+
+        const GattCharacteristic* characteristic = service.getCharacteristic(characteristic_index);
+        const uint8_t properties = characteristic->getProperties();
+        // if notify or indicate are present, two attributes are
+        // needed
+        if ((properties & GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY) ||
+            (properties & GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE)) {
+            attribute_records += 2;
+        }
+
+        // if broadcast is set, two attributes are needed
+        if (properties & GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_BROADCAST) {
+            attribute_records += 2;
+        }
+
+        // if extended properties flag is set, two attributes are needed
+        if (properties & GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_EXTENDED_PROPERTIES) {
+            attribute_records += 2;
+        }
+
+        attribute_records += characteristic->getDescriptorCount();
+    }
+
+    // for some reason, if there is just a service, this value should
+    // be equal to 5
+    if (attribute_records == 1) {
+        attribute_records = 5;
+    }
+
+    return attribute_records;
 }
\ No newline at end of file

--- a/x-nucleo-idb0xa1/BlueNRGDiscoveredCharacteristic.h	Thu Sep 15 10:51:20 2016 +0100
+++ b/x-nucleo-idb0xa1/BlueNRGDiscoveredCharacteristic.h	Thu Sep 15 10:51:21 2016 +0100
@@ -30,7 +30,7 @@
              GattAttribute::Handle_t  declHandleIn,
              GattAttribute::Handle_t  valueHandleIn,
              GattAttribute::Handle_t  lastHandleIn);
-  
+
   void setup(BlueNRGGattClient         *gattcIn,
              Gap::Handle_t            connectionHandleIn,
              UUID   uuidIn,
@@ -38,6 +38,9 @@
              GattAttribute::Handle_t  declHandleIn,
              GattAttribute::Handle_t  valueHandleIn,
              GattAttribute::Handle_t  lastHandleIn);
+
+
+  void setLastHandle(GattAttribute::Handle_t  lastHandleIn);
 };
 
-#endif /* __BLUENRG_DISCOVERED_CHARACTERISTIC_H__ */
+#endif /* __BLUENRG_DISCOVERED_CHARACTERISTIC_H__ */
\ No newline at end of file

--- a/x-nucleo-idb0xa1/BlueNRGGattServer.h	Thu Sep 15 10:51:20 2016 +0100
+++ b/x-nucleo-idb0xa1/BlueNRGGattServer.h	Thu Sep 15 10:51:21 2016 +0100
@@ -15,7 +15,7 @@
 */
 /**
   ******************************************************************************
-  * @file    BlueNRGGattServer.cpp 
+  * @file    BlueNRGGattServer.cpp
   * @author  STMicroelectronics
   * @brief   Header file for BLE_API GattServer Class
   ******************************************************************************
@@ -30,7 +30,7 @@
   *
   * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
   */
- 
+
 #ifndef __BLUENRG_GATT_SERVER_H__
 #define __BLUENRG_GATT_SERVER_H__
 
@@ -53,13 +53,13 @@
         static BlueNRGGattServer m_instance;
         return m_instance;
     }
-    
+
     enum HandleEnum_t {
         CHAR_HANDLE = 0,
         CHAR_VALUE_HANDLE,
         CHAR_DESC_HANDLE
     };
-    
+
     /* Functions that must be implemented from GattServer */
     virtual ble_error_t addService(GattService &);
     virtual ble_error_t read(GattAttribute::Handle_t attributeHandle, uint8_t buffer[], uint16_t *lengthP);
@@ -67,13 +67,13 @@
     virtual ble_error_t write(GattAttribute::Handle_t, const uint8_t[], uint16_t, bool localOnly = false);
     virtual ble_error_t write(Gap::Handle_t connectionHandle, GattAttribute::Handle_t, const uint8_t[], uint16_t, bool localOnly = false);
     virtual ble_error_t initializeGATTDatabase(void);
-    
+
     virtual bool isOnDataReadAvailable() const {
         return true;
     }
 
     virtual ble_error_t reset(void);
-    
+
     /* BlueNRG Functions */
     void eventCallback(void);
     //void hwCallback(void *pckt);
@@ -83,8 +83,13 @@
     void HCIDataReadEvent(const GattReadCallbackParams *params);
     void HCIEvent(GattServerEvents::gattEvent_e type, uint16_t charHandle);
     void HCIDataSentEvent(unsigned count);
-    
+
 private:
+
+    // compute the number of attribute record needed by a service
+    static uint16_t computeAttributesRecord(GattService& service);
+
+
     static const int MAX_SERVICE_COUNT = 10;
     uint8_t serviceCount;
     uint8_t characteristicCount;
@@ -101,12 +106,12 @@
 
     BlueNRGGattServer(BlueNRGGattServer const &);
     void operator=(BlueNRGGattServer const &);
-    
-    static const int CHAR_DESC_TYPE_16_BIT=0x01; 
-    static const int CHAR_DESC_TYPE_128_BIT=0x02;    
+
+    static const int CHAR_DESC_TYPE_16_BIT=0x01;
+    static const int CHAR_DESC_TYPE_128_BIT=0x02;
     static const int CHAR_DESC_SECURITY_PERMISSION=0x00;
-    static const int CHAR_DESC_ACCESS_PERMISSION=0x03;  
-    static const int CHAR_ATTRIBUTE_LEN_IS_FIXED=0x00;        
+    static const int CHAR_DESC_ACCESS_PERMISSION=0x03;
+    static const int CHAR_ATTRIBUTE_LEN_IS_FIXED=0x00;
 };
 
-#endif
+#endif
\ No newline at end of file