Service Discovery ServiceSearchAttribute requests require the response to be grouped by UUID, so the response can't be generated as the attributes are read in. Buffer both UUIDs and Attributes instead and generate the response afterwards.

2 files changed, 76 insertions, 46 deletions

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
index c140c17d2..8be4cc894 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
@@ -64,7 +64,7 @@ const ServiceAttributeTable_t RFCOMM_Attribute_Table[] PROGMEM =
 /** Master service table, listing all supported services (and their attribute tables) of the device, including
  *  each service's UUID.
  */
-const ServiceTable_t SDP_Services_Table[] =
+const ServiceTable_t SDP_Services_Table[] PROGMEM =
 	{
 		{   // 128-bit UUID for the SDP service
 			.UUID  = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00},
@@ -77,7 +77,7 @@ const ServiceTable_t SDP_Services_Table[] =
 	};
 
 /** Base UUID value common to all standardized Bluetooth services */
-const uint8_t BaseUUID[] = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};
+const uint8_t BaseUUID[] PROGMEM = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};
 
 
 /** Main Service Discovery Protocol packet processing routine. This function processes incomming SDP packets from
@@ -132,7 +132,11 @@ static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPH
 	
 	uint16_t MaxAttributeSize = ServiceDiscovery_Read16BitParameter(&CurrentParameter);
 	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
-
+	
+	uint16_t AttributeList[20][2];
+	uint8_t  TotalAttributes = ServiceDiscovery_GetAttributeList(AttributeList, &CurrentParameter);
+	BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
+	
 	struct
 	{
 		SDP_PDUHeader_t SDPHeader;
@@ -140,12 +144,38 @@ static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPH
 		uint8_t         ResponseData[100];
 	} ResponsePacket;
 	
+	uint8_t* CurrResponsePos = ResponsePacket.ResponseData;
+
 	if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
 	  MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
 	  
-	ResponsePacket.AttributeListByteCount    = ServiceDiscovery_ProcessAttributes(UUIDList, TotalUUIDs,
-	                                                                              ResponsePacket.ResponseData,
-	                                                                              MaxAttributeSize, &CurrentParameter);
+	uint16_t* TotalResponseSize = ServiceDiscovery_AddDataElementHeader(&CurrResponsePos, SDP_DATATYPE_Sequence);
+	for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
+	{
+		ServiceAttributeTable_t* AttributeTable = ServiceDiscovery_GetAttributeTable(UUIDList[CurrUUIDItem]);
+		
+		if (AttributeTable == NULL)
+		  continue;
+		  
+		uint16_t* CurrentUUIDResponseSize = ServiceDiscovery_AddDataElementHeader(&CurrResponsePos, SDP_DATATYPE_Sequence);
+		*TotalResponseSize += sizeof(ServiceAttributeData16Bit_t);
+		for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
+		{
+			uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
+		
+			for (uint16_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID < AttributeIDRange[1]; CurrAttributeID++)
+			{
+				void* AttributeValue = ServiceDiscovery_GetAttributeValue(AttributeTable, CurrAttributeID);
+				
+				if (AttributeValue == NULL)
+				  continue;
+
+				BT_SDP_DEBUG(2, "GUID + ATTRIBUTE FOUND");
+			}
+		}
+	}
+
+	ResponsePacket.AttributeListByteCount    = (*TotalResponseSize + sizeof(ServiceAttributeData16Bit_t));
 	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
 	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 	ResponsePacket.SDPHeader.ParameterLength = (ResponsePacket.AttributeListByteCount + sizeof(ResponsePacket.AttributeListByteCount));
@@ -154,53 +184,55 @@ static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPH
 	                     Channel);
 }
 
-static uint8_t ServiceDiscovery_ProcessAttributes(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs, uint8_t* ResponseBuffer,
-                                                  uint8_t MaxResponseSize, const void** CurrentParameter)
+static void* ServiceDiscovery_GetAttributeValue(ServiceAttributeTable_t* AttributeTable, uint16_t AttributeID)
+{
+	while ((void*)pgm_read_word(&AttributeTable->Data) != NULL)
+	{
+		if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
+		  return &AttributeTable->Data;
+		
+		AttributeTable++;
+	}
+			
+	return NULL;
+}
+
+static ServiceAttributeTable_t* ServiceDiscovery_GetAttributeTable(uint8_t* UUID)
+{
+	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
+	{
+		if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
+		  return (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
+	}
+	
+	return NULL;
+}
+
+static uint8_t ServiceDiscovery_GetAttributeList(uint16_t AttributeList[][2], const void** CurrentParameter)
 {
-	uint16_t* AttributeResponseSize = ServiceDiscovery_AddDataElementHeader(&ResponseBuffer, SDP_DATATYPE_UnsignedInt);
-	uint8_t   ElementHeaderSize;
+	uint8_t ElementHeaderSize;
+	uint8_t TotalAttributes = 0;
 
 	uint16_t AttributeIDListLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 	BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
 	while (AttributeIDListLength)
 	{
-		uint8_t  AttributeLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
-		uint32_t Attribute       = 0;
+		uint8_t AttributeLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 		
-		memcpy(&((char*)&Attribute)[sizeof(uint32_t) - AttributeLength], *CurrentParameter, AttributeLength);
+		memcpy(&AttributeList[TotalAttributes][0], *CurrentParameter, AttributeLength);
+		
+		if (AttributeLength == 2)
+		  memcpy(&AttributeList[TotalAttributes][1], *CurrentParameter, 2);
 
-		BT_SDP_DEBUG(2, "-- Attribute(%d): 0x%08lX", AttributeLength, Attribute);
+		BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", AttributeList[TotalAttributes][0], AttributeList[TotalAttributes][1]);
+		
+		TotalAttributes++;
 		
-		*AttributeResponseSize += ServiceDiscovery_GetAttribute(UUIDList, TotalUUIDs, Attribute, &ResponseBuffer,
-		                                                        (MaxResponseSize - *AttributeResponseSize));
-	
 		AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 		*CurrentParameter     += AttributeLength;
 	}
 	
-	return *AttributeResponseSize;
-}
-
-static uint8_t ServiceDiscovery_GetAttribute(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs, const uint32_t Attribute,
-                                             uint8_t** DataBuffer, uint8_t BufferLen)
-{
-	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
-	{
-		for (uint8_t CurrUUIDIndex = 0; CurrUUIDIndex < TotalUUIDs; CurrUUIDIndex++)
-		{
-			if (!(memcmp(SDP_Services_Table[CurrTableItem].UUID, UUIDList[CurrUUIDIndex], UUID_SIZE_BYTES)))
-			{
-				const ServiceAttributeTable_t* AttributeTable = SDP_Services_Table[CurrTableItem].AttributeTable;
-
-				// TODO: Process attribute table
-				BT_SDP_DEBUG(2, "FOUND UUID IN TABLE");
-
-				break;
-			}
-		}
-	}
-		
-	return 0;
+	return TotalAttributes;
 }
 
 static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** CurrentParameter)
@@ -215,7 +247,7 @@ static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES],
 		uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
 		uint8_t  UUIDLength  = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 		
-		memcpy(CurrentUUID, BaseUUID, sizeof(BaseUUID));
+		memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
 		memcpy(&CurrentUUID[(UUIDLength <= 4) ? (UUID_SIZE_BYTES - 4) : 0], *CurrentParameter, UUIDLength);
 		
 		BT_SDP_DEBUG(2, "-- UUID (%d): 0x%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
index 7986d5e72..34d9baba7 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
@@ -195,11 +195,9 @@
 			static void ServiceDiscovery_ProcessServiceAttribute(SDP_PDUHeader_t* SDPHeader, Bluetooth_Channel_t* Channel);
 			static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPHeader, Bluetooth_Channel_t* Channel);
 
-			static uint8_t ServiceDiscovery_ProcessAttributes(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs, 
-			                                                  uint8_t* ResponseBuffer, uint8_t MaxResponseSize,
-			                                                  const void** CurrentParameter);
-			static uint8_t ServiceDiscovery_GetAttribute(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs,
-			                                             const uint32_t Attribute, uint8_t** DataBuffer, uint8_t BufferLen);
+			static void* ServiceDiscovery_GetAttributeValue(ServiceAttributeTable_t* AttributeTable, uint16_t AttributeID);
+			static ServiceAttributeTable_t* ServiceDiscovery_GetAttributeTable(uint8_t* UUID);
+			static uint8_t ServiceDiscovery_GetAttributeList(uint16_t AttributeList[][2], const void** CurrentParameter);
 			static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** CurrentParameter);
 			static uint32_t ServiceDiscovery_GetDataElementSize(const void** AttributeHeader, uint8_t* ElementHeaderSize);
 		#endif