Fix the encoding of UUIDs in the SDP server so that they match the Bluetooth UUID format properly.

4 files changed, 161 insertions, 112 deletions

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/SDPServices.c b/Demos/Host/Incomplete/BluetoothHost/Lib/SDPServices.c
index 9df89d51b..537c81f0a 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/SDPServices.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/SDPServices.c
@@ -40,14 +40,14 @@ const struct
 {

 	uint8_t     Header;

 	uint16_t    Size;

-	ClassUUID_t UUIDList[];

+	ItemUUID_t UUIDList[];

 } PROGMEM SDP_Attribute_ServiceClassIDs =

 	{

 		.Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit),

-		.Size   = SWAPENDIAN_16(sizeof(ClassUUID_t) * 1),

+		.Size   = SWAPENDIAN_16(sizeof(ItemUUID_t) * 1),

 		.UUIDList =

 			{

-				{.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x00, 0x10, 0x00, 0x00}}

+				{.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_80BIT_UUID, {0x00, 0x00, 0x00, 0x00, 0x10, 0x00}},}

 			}

 	};

 

@@ -128,14 +128,14 @@ const struct
 {

 	uint8_t     Header;

 	uint16_t    Size;

-	ClassUUID_t UUIDList[];

+	ItemUUID_t UUIDList[];

 } PROGMEM RFCOMM_Attribute_ServiceClassIDs =

 	{

 		.Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit),

-		.Size   = SWAPENDIAN_16(sizeof(ClassUUID_t) * 1),

+		.Size   = SWAPENDIAN_16(sizeof(ItemUUID_t) * 1),

 		.UUIDList =

 			{

-				{.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x01, 0x11, 0x00, 0x00}}

+				{.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_80BIT_UUID, {0x00, 0x00, 0x00, 0x00, 0x11, 0x01}},}

 			}

 	};

 

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/SDPServices.h b/Demos/Host/Incomplete/BluetoothHost/Lib/SDPServices.h
index ce920579b..92fe226bc 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/SDPServices.h
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/SDPServices.h
@@ -43,8 +43,8 @@
 		/** Size of a full 128 bit UUID, in bytes. */

 		#define UUID_SIZE_BYTES                         16

 		

-		/** First 96 bits common to all standardized Bluetooth services. */

-		#define BASE_96BIT_UUID                         0xFB, 0x34, 0x9B, 0x5F, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00

+		/** First 80 bits common to all standardized Bluetooth services. */

+		#define BASE_80BIT_UUID                         SWAPENDIAN_32(0xFB349B5F), SWAPENDIAN_16(0x8000), SWAPENDIAN_16(0x0080), SWAPENDIAN_16(0x0010)

 		

 		#define SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE    0x0000

 		#define SDP_ATTRIBUTE_ID_SERVICECLASSIDS        0x0001

@@ -61,6 +61,15 @@
 		#define SERVICE_ATTRIBUTE_TABLE_TERMINATOR      {.Data = NULL}

 		

 	/* Type Defines: */

+		typedef struct

+		{

+			uint32_t A;

+			uint16_t B;

+			uint16_t C;

+			uint16_t D;

+			uint8_t  E[6];

+		} UUID_t;

+	

 		/** Structure for the association of attribute ID values to an attribute value in FLASH. A table of these

 		 *  structures can then be built up for each supported UUID service within the device.

 		 */

@@ -75,7 +84,7 @@
 		 */

 		typedef struct

 		{

-			uint8_t     UUID[UUID_SIZE_BYTES]; /**< UUID of a service supported by the device */

+			UUID_t      UUID; /**< UUID of a service supported by the device */

 			const void* AttributeTable; /**< Pointer to the UUID's attribute table, located in PROGMEM memory space */

 		} ServiceTable_t;

 

@@ -83,13 +92,13 @@
 		typedef struct

 		{

 			uint8_t Header; /**< Data Element header, should be (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit) */

-			uint8_t UUID[UUID_SIZE_BYTES]; /**< UUID to store in the list Data Element */

-		} ClassUUID_t;

+			UUID_t  UUID; /**< UUID to store in the list Data Element */

+		} ItemUUID_t;

 

 		/** Structure for a list of Data Elements containing 8-bit integers, for service attributes requiring such lists. */

 		typedef struct

 		{

-			uint8_t  Header; /**< Data Element header, should be (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_8Bit) */

+			uint8_t Header; /**< Data Element header, should be (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_8Bit) */

 			uint8_t Value; /**< Value to store in the list Data Element */

 		} Item8Bit_t;

 

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
index 2b61cb463..82cb60e29 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
@@ -32,7 +32,7 @@
 #include "ServiceDiscoveryProtocol.h"
 
 /** Base UUID value common to all standardized Bluetooth services */
-const uint8_t BaseUUID[] PROGMEM = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};
+const UUID_t BaseUUID PROGMEM = {SWAPENDIAN_32(0xFB349B5F), SWAPENDIAN_16(0x8000), SWAPENDIAN_16(0x0080), SWAPENDIAN_16(0x0010)};
 
 /** Master service table, listing all supported services (and their attribute tables) of the device, including
  *  each service's UUID.
@@ -40,11 +40,11 @@ const uint8_t BaseUUID[] PROGMEM = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};
 const ServiceTable_t SDP_Services_Table[] PROGMEM =
 	{
 		{   // 128-bit UUID for the SDP service
-			.UUID  = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00},
+			.UUID  = {BASE_80BIT_UUID, {0x00, 0x00, 0x00, 0x00, 0x00, 0x01}},
 			.AttributeTable = SDP_Attribute_Table,
 		},
 		{   // 128-bit UUID for the RFCOMM service
-			.UUID  = {BASE_96BIT_UUID, 0x03, 0x00, 0x00, 0x00},
+			.UUID  = {BASE_80BIT_UUID, {0x00, 0x00, 0x00, 0x00, 0x00, 0x03}},
 			.AttributeTable = RFCOMM_Attribute_Table,
 		},
 	};
@@ -97,8 +97,7 @@ static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Blu
 	BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
 	
 	/* Retrieve the maximum service record reponse count from the request */
-	uint16_t MaxServiceRecordCount = SwapEndian_16(*((uint16_t*)CurrentParameter));
-	CurrentParameter += sizeof(uint16_t);
+	uint16_t MaxServiceRecordCount = SDP_ReadData16(&CurrentParameter);
 	BT_SDP_DEBUG(2, "-- Max Return Service Count: 0x%04X", MaxServiceRecordCount);
 	
 	struct
@@ -140,7 +139,7 @@ static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Blu
 	}
 
 	/* Continuation state - always zero */
-	*((uint8_t*)CurrResponsePos) = 0;
+	SDP_WriteData8(&CurrResponsePos, 0);
 
 	/* Fill out the service record count values in the returned packet */
 	ResponsePacket.TotalServiceRecordCount   = SwapEndian_16(AddedServiceHandles);
@@ -176,13 +175,11 @@ static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader,
 	BT_SDP_DEBUG(1, "<< Service Attribute");
 
 	/* Retrieve the service handle whose attributes are to be examined */
-	uint32_t ServiceHandle = SwapEndian_32(*((uint16_t*)CurrentParameter));
-	CurrentParameter += sizeof(uint32_t);
+	uint32_t ServiceHandle = SDP_ReadData32(&CurrentParameter);
 	BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
 	
 	/* Retrieve the maximum Attribute reponse size from the request */
-	uint16_t MaxAttributeSize = SwapEndian_16(*((uint16_t*)CurrentParameter));
-	CurrentParameter += sizeof(uint16_t);
+	uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
 	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 	
 	/* Retrieve the list of Attributes from the request */
@@ -216,30 +213,15 @@ static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader,
 		/* Retrieve a PROGMEM pointer to the value of the Service Record Handle */
 		const void* ServiceRecord = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
 		
+		/* Get the size of the header for the Service Record Handle */
+		uint8_t AttrHeaderSize;
+		SDP_GetLocalAttributeContainerSize(ServiceRecord, &AttrHeaderSize);
+
 		/* Check if the current service in the service table has the requested service handle */
-		if (memcmp_P(ServiceRecord, &ServiceHandle, sizeof(uint32_t)) == 0)
+		if (memcmp_P(ServiceRecord + AttrHeaderSize, &ServiceHandle, sizeof(uint32_t)) == 0)
 		{
-			/* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
-			for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
-			{
-				uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
-			
-				/* Look in the current Attribute Range for a matching Attribute ID in the service's Attribute table */
-				for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
-				{
-					/* Retrieve a PROGMEM pointer to the value of the current Attribute ID, if it exists in the service's Attribute table */
-					const void* AttributeValue = SDP_GetAttributeValue(CurrAttributeTable, CurrAttributeID);
-					
-					/* If the Attribute does not exist in the current service's Attribute table, continue to the next Attribute ID */
-					if (AttributeValue == NULL)
-					  continue;
-					
-					BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
-
-					/* Increment the service's returned Attribute container size by the number of added bytes */
-					*TotalResponseSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, &CurrResponsePos);
-				}
-			}
+			*TotalResponseSize += SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes,
+		                                                            &CurrResponsePos);
 			
 			/* Requested service found, abort the search through the service table */
 			break;
@@ -247,7 +229,7 @@ static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader,
 	}
 
 	/* Continuation state - always zero */
-	*((uint8_t*)CurrResponsePos) = 0;
+	SDP_WriteData8(&CurrResponsePos, 0);
 
 	/* Set the total response list size to the size of the outer container plus its header size and continuation state */
 	ResponsePacket.AttributeListByteCount    = SwapEndian_16(3 + *TotalResponseSize);
@@ -290,8 +272,7 @@ static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHe
 	BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
 	
 	/* Retrieve the maximum Attribute reponse size from the request */
-	uint16_t MaxAttributeSize = SwapEndian_16(*((uint16_t*)CurrentParameter));
-	CurrentParameter += sizeof(uint16_t);
+	uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
 	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 	
 	/* Retrieve the list of Attributes from the request */
@@ -328,40 +309,12 @@ static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHe
 		  
 		BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);
 
-		/* Add an inner Data Element Sequence header for the current UUID's found Attributes */
-		uint16_t* CurrentUUIDResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
-		
-		/* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
-		for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
-		{
-			uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
-		
-			/* Look in the current Attribute Range for a matching Attribute ID in the UUID's Attribute table */
-			for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
-			{
-				/* Retrieve a PROGMEM pointer to the value of the current Attribute ID, if it exists in the UUID's Attribute table */
-				const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, CurrAttributeID);
-				
-				/* If the Attribute does not exist in the current UUID's Attribute table, continue to the next Attribute ID */
-				if (AttributeValue == NULL)
-				  continue;
-				
-				BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
-
-				/* Increment the current UUID's returned Attribute container size by the number of added bytes */
-				*CurrentUUIDResponseSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, &CurrResponsePos);
-			}
-
-			/* Increment the outer container size by the number of added bytes */
-			*TotalResponseSize += 3 + *CurrentUUIDResponseSize;
-		}
-
-		/* Flip the endianness of the container's size */
-		*CurrentUUIDResponseSize = SwapEndian_16(*CurrentUUIDResponseSize);
+		*TotalResponseSize += SDP_AddListedAttributesToResponse(AttributeTable, AttributeList, TotalAttributes,
+		                                                        &CurrResponsePos);
 	}
 	
 	/* Continuation state - always zero */
-	*((uint8_t*)CurrResponsePos) = 0;
+	SDP_WriteData8(&CurrResponsePos, 0);
 
 	/* Set the total response list size to the size of the outer container plus its header size and continuation state */
 	ResponsePacket.AttributeListByteCount    = SwapEndian_16(3 + *TotalResponseSize);
@@ -387,6 +340,54 @@ static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHe
 	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
 }
 
+/** Adds all the Attributes in the given service table to the response that appear in the Attribute table.
+ *
+ *  \param[in]  AttributeTable   Pointer to an Attribute table for the service to examine
+ *  \param[in]  AttributeList    Pointer to a list of Attribute ranges
+ *  \param[in]  TotalAttributes  Number of Attributes stored in the Attribute list
+ *  \param[out] BufferPos       Pointer to the output buffer position where the retrieved attributes are to be stored
+ *
+ *  \return Number of bytes added to the output buffer
+ */
+static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t* AttributeTable,
+                                                  uint16_t AttributeList[][2], uint8_t TotalAttributes, void** BufferPos)
+{
+	uint16_t TotalResponseSize = 0;
+
+	/* Add an inner Data Element Sequence header for the current services's found Attributes */
+	uint16_t* AttributeListSize = SDP_AddDataElementHeader16(BufferPos, SDP_DATATYPE_Sequence);
+
+	/* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
+	for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
+	{
+		uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
+	
+		/* Look in the current Attribute Range for a matching Attribute ID in the UUID's Attribute table */
+		for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
+		{
+			/* Retrieve a PROGMEM pointer to the value of the current Attribute ID, if it exists in the UUID's Attribute table */
+			const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, CurrAttributeID);
+			
+			/* If the Attribute does not exist in the current UUID's Attribute table, continue to the next Attribute ID */
+			if (AttributeValue == NULL)
+			  continue;
+			
+			BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
+
+			/* Increment the current UUID's returned Attribute container size by the number of added bytes */
+			*AttributeListSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, BufferPos);
+		}
+
+		/* Increment the outer container size by the number of added bytes */
+		TotalResponseSize += 3 + *AttributeListSize;
+	}
+
+	/* Fix endianness of the added attribute data element sequence */
+	*AttributeListSize = SwapEndian_16(*AttributeListSize);
+
+	return TotalResponseSize;
+}
+
 /** Adds the given attribute ID and value to the reponse buffer, and advances the response buffer pointer past the added data.
  *
  *  \param[in] AttributeID          Attribute ID to add to the response buffer
@@ -402,12 +403,10 @@ static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const voi
 	uint32_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
 	
 	/* Add a Data Element header to the response for the Attribute ID */
-	*((uint8_t*)*ResponseBuffer) = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit);
-	*ResponseBuffer += sizeof(uint8_t);
+	SDP_WriteData8(ResponseBuffer, (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit));
 	
 	/* Add the Attribute ID to the created Data Element */
-	*((uint16_t*)*ResponseBuffer) = SwapEndian_16(AttributeID);
-	*ResponseBuffer += sizeof(uint16_t);
+	SDP_WriteData16(ResponseBuffer, AttributeID);
 	
 	/* Copy over the Attribute value Data Element container to the response */
 	memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
@@ -455,7 +454,7 @@ static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID)
 		ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 	
 		/* If the current table item's UUID matches the search UUID, return a pointer the table item's Attribute table */
-		if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
+		if (!(memcmp_P(UUID, &SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
 		  return CurrAttributeTable;
 		
 		/* Retrieve the list of the service's Class UUIDs from its Attribute table */
@@ -474,11 +473,11 @@ static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID)
 		while (ClassUUIDListSize)
 		{
 			/* Current Service UUID's Class UUID list has a matching entry, return the Attribute table */
-			if (!(memcmp_P(UUID, &((ClassUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
+			if (!(memcmp_P(UUID, &((ItemUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
 			  return CurrAttributeTable;
 		
-			ClassUUIDListSize -= sizeof(ClassUUID_t);
-			ClassUUIDs        += sizeof(ClassUUID_t);
+			ClassUUIDListSize -= sizeof(ItemUUID_t);
+			ClassUUIDs        += sizeof(ItemUUID_t);
 		}	
 	}
 	
@@ -550,26 +549,22 @@ static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void**
 		uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 		
 		/* Copy over the base UUID value to the free UUID slot in the list */
-		memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
+		memcpy_P(CurrentUUID, &BaseUUID, sizeof(BaseUUID));
 
 		/* Copy over UUID from the container to the free slot - if a short UUID (<= 4 bytes) it replaces the lower
 		   4 bytes of the base UUID, otherwise it replaces the UUID completely */
 		if (UUIDLength <= 4)
-		{
-			memcpy(&CurrentUUID[UUID_SIZE_BYTES - 4], *CurrentParameter, UUIDLength);
-			SwapEndian_n(&CurrentUUID[UUID_SIZE_BYTES - 4], UUIDLength);				
-		}
+		  memcpy(&CurrentUUID[UUID_SIZE_BYTES - UUIDLength], *CurrentParameter, UUIDLength);
 		else
-		{
-			memcpy(&CurrentUUID[0], *CurrentParameter, UUIDLength);		
-		}
+		  memcpy(&CurrentUUID[0], *CurrentParameter, UUIDLength);		
 		
 		BT_SDP_DEBUG(2, "-- UUID (%d): %02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 		                UUIDLength,
-		                CurrentUUID[15], CurrentUUID[14], CurrentUUID[13], CurrentUUID[12],
-		                CurrentUUID[11], CurrentUUID[10], CurrentUUID[9],  CurrentUUID[8],
-		                CurrentUUID[7],  CurrentUUID[6],  CurrentUUID[5],  CurrentUUID[4],
-		                CurrentUUID[3],  CurrentUUID[2],  CurrentUUID[1],  CurrentUUID[0]);
+		                CurrentUUID[0], CurrentUUID[1], CurrentUUID[2], CurrentUUID[3],
+		                CurrentUUID[4], CurrentUUID[5],
+						CurrentUUID[6],  CurrentUUID[7],
+		                CurrentUUID[8],  CurrentUUID[9],
+						CurrentUUID[10],  CurrentUUID[11], CurrentUUID[12],  CurrentUUID[13],  CurrentUUID[14],  CurrentUUID[15]);
 
 		ServicePatternLength -= (UUIDLength + ElementHeaderSize);
 		*CurrentParameter    += UUIDLength;
@@ -589,24 +584,30 @@ static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeDa
 	/* Fetch the size of the Data Element structure from the header */
 	uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 	
+	uint32_t ElementValueSize;
+
 	/* Convert the Data Element size index into a size in bytes */
 	switch (SizeIndex)
 	{
 		case SDP_DATASIZE_Variable8Bit:
 			*HeaderSize = (1 + sizeof(uint8_t));
-			return pgm_read_byte(AttributeData + 1);
+			ElementValueSize = pgm_read_byte(AttributeData + 1);
+			break;
 		case SDP_DATASIZE_Variable16Bit:
 			*HeaderSize = (1 + sizeof(uint16_t));
-			return SwapEndian_16(pgm_read_word(AttributeData + 1));
+			ElementValueSize = SwapEndian_16(pgm_read_word(AttributeData + 1));
+			break;
 		case SDP_DATASIZE_Variable32Bit:
 			*HeaderSize = (1 + sizeof(uint32_t));
-			return SwapEndian_32(pgm_read_dword(AttributeData + 1));
+			ElementValueSize = SwapEndian_32(pgm_read_dword(AttributeData + 1));
+			break;
 		default:
 			*HeaderSize = 1;
-			return (1 << SizeIndex);
+			ElementValueSize = (1 << SizeIndex);
+			break;
 	}
 
-	return 0;
+	return ElementValueSize;
 }
 
 /** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer
@@ -620,27 +621,23 @@ static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeDa
 static uint32_t SDP_GetDataElementSize(const void** const DataElementHeader, uint8_t* const ElementHeaderSize)
 {
 	/* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
-	uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
-	*DataElementHeader += sizeof(uint8_t);
-	
+	uint8_t  SizeIndex = (SDP_ReadData8(DataElementHeader) & 0x07);	
+
 	uint32_t ElementValueSize;
 
 	/* Convert the Data Element size index into a size in bytes */
 	switch (SizeIndex)
 	{
 		case SDP_DATASIZE_Variable8Bit:
-			ElementValueSize    = *((uint8_t*)*DataElementHeader);
-			*DataElementHeader += sizeof(uint8_t);
+			ElementValueSize    = SDP_ReadData8(DataElementHeader);
 			*ElementHeaderSize  = (1 + sizeof(uint8_t));
 			break;
 		case SDP_DATASIZE_Variable16Bit:
-			ElementValueSize    = SwapEndian_16(*((uint16_t*)*DataElementHeader));
-			*DataElementHeader += sizeof(uint16_t);
+			ElementValueSize    = SDP_ReadData16(DataElementHeader);
 			*ElementHeaderSize  = (1 + sizeof(uint16_t));
 			break;
 		case SDP_DATASIZE_Variable32Bit:
-			ElementValueSize    = SwapEndian_32(*((uint32_t*)*DataElementHeader));
-			*DataElementHeader += sizeof(uint32_t);
+			ElementValueSize    = SDP_ReadData32(DataElementHeader);
 			*ElementHeaderSize  = (1 + sizeof(uint32_t));
 			break;
 		default:
diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
index ce7d5f26d..a3913f6de 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
@@ -129,6 +129,48 @@
 			return SizePos;
 		}
 		
+		static inline void SDP_WriteData8(void** BufferPos, uint8_t Data)
+		{
+			*((uint8_t*)*BufferPos) = Data;
+			*BufferPos += sizeof(uint8_t);
+		}
+		
+		static inline void SDP_WriteData16(void** BufferPos, uint16_t Data)
+		{
+			*((uint16_t*)*BufferPos) = SwapEndian_16(Data);
+			*BufferPos += sizeof(uint16_t);
+		}		
+
+		static inline void SDP_WriteData32(void** BufferPos, uint32_t Data)
+		{
+			*((uint32_t*)*BufferPos) = SwapEndian_32(Data);
+			*BufferPos += sizeof(uint32_t);
+		}
+
+		static inline uint8_t SDP_ReadData8(const void** BufferPos)
+		{
+			uint8_t Data = *((uint8_t*)*BufferPos);
+			*BufferPos += sizeof(uint8_t);
+			
+			return Data;
+		}
+
+		static inline uint16_t SDP_ReadData16(const void** BufferPos)
+		{
+			uint16_t Data = SwapEndian_16(*((uint16_t*)*BufferPos));
+			*BufferPos += sizeof(uint16_t);
+			
+			return Data;
+		}
+
+		static inline uint32_t SDP_ReadData32(const void** BufferPos)
+		{
+			uint32_t Data = SwapEndian_32(*((uint32_t*)*BufferPos));
+			*BufferPos += sizeof(uint32_t);
+			
+			return Data;
+		}
+		
 	/* Function Prototypes: */
 		void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel);
 
@@ -137,6 +179,7 @@
 			static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel);
 			static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel);
 
+			static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t* AttributeTable, uint16_t AttributeList[][2], uint8_t TotalAttributes, void** BufferPos);
 			static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const void* AttributeValue, void** ResponseBuffer);
 			static void*    SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable, const uint16_t AttributeID);
 			static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID);