AFR NimBLE: Add included services in `pxAddServiceBlob()`
@@ -265,6 +265,21 @@ static uint8_t prvAFRToESPDescPerm( BTCharPermissions_t xPermissions )
return flags;
}
+static struct ble_gatt_svc_def * prvAFRtoESPIncludedServices( BTIncludedService_t xIncludedService )
+{
+ uint16_t index;
+
+ for ( index = 0; index < serviceCnt; index++ )
+ {
+ if( memcmp( xIncludedService.pxPtrToService, afrServices[ index ], sizeof( BTService_t ) ) == 0 )
+ {
+ return &espServices[ index ];
+ }
+ }
+
+ return NULL;
+}
+
ble_uuid_t * prvCopytoESPUUID( BTUuid_t * pxUuid,
ble_uuid_t * pUuid )
{
@@ -747,6 +762,22 @@ BTStatus_t prvBTSendResponse( uint16_t usConnId,
}
/*-----------------------------------------------------------*/
+static uint16_t prvCountIncludedServices( BTService_t * pxService )
+{
+ uint16_t nbIncludedServices = 0;
+ uint16_t index;
+
+ for( index = 0; index < pxService->xNumberOfAttributes; index++ )
+ {
+ if( pxService->pxBLEAttributes[ index ].xAttributeType == eBTDbIncludedService )
+ {
+ nbIncludedServices++;
+ }
+ }
+
+ return nbIncludedServices;
+}
+
static uint16_t prvCountCharacteristics( BTService_t * pxService )
{
uint16_t nbCharacteristics = 0;
@@ -821,6 +852,11 @@ static void prvCleanupService( struct ble_gatt_svc_def * pSvc )
vPortFree( ( void * ) pSvc->characteristics );
}
+ if ( pSvc->includes != NULL )
+ {
+ vPortFree( ( void * ) pSvc->includes );
+ }
+
memset( pSvc, 0x00, sizeof( struct ble_gatt_svc_def ) );
}
@@ -837,7 +873,9 @@ BTStatus_t prvAddServiceBlob( uint8_t ucServerIf,
uint16_t index;
uint16_t charCount;
uint16_t dscrCount;
+ uint16_t IncludedSvcCount;
struct ble_gatt_chr_def * pCharacteristics = NULL;
+ struct ble_gatt_svc_def ** pIncludedServices = NULL;
struct ble_gatt_dsc_def * pDescriptors = NULL;
BTStatus_t xStatus = eBTStatusSuccess;
ble_uuid_t * uuid;
@@ -920,9 +958,18 @@ BTStatus_t prvAddServiceBlob( uint8_t ucServerIf,
}
pSvc->characteristics = pCharacteristics;
- pSvc->includes = NULL;
+ pIncludedServices = pvPortCalloc( prvCountIncludedServices( pxService ) + 1, sizeof( struct ble_gatt_svc_def * ) );
+
+ if( pIncludedServices == NULL )
+ {
+ configPRINTF( ( "Could not allocate memory for included services \n" ) );
+ prvCleanupService( pSvc );
+ xStatus = eBTStatusNoMem;
+ }
+ pSvc->includes = ( const struct ble_gatt_svc_def ** ) pIncludedServices;
charCount = 0;
dscrCount = 0;
+ IncludedSvcCount = 0;
}
}
@@ -960,6 +1007,8 @@ BTStatus_t prvAddServiceBlob( uint8_t ucServerIf,
case eBTDbIncludedService:
handle += 1;
pxService->pusHandlesBuffer[ attributeCount++ ] = handle;
+ pIncludedServices[ IncludedSvcCount ] = prvAFRtoESPIncludedServices( pxService->pxBLEAttributes[ index ].xIncludedService );
+ IncludedSvcCount++;
break;
case eBTDbDescriptor:
