Begin processing code for Service Discovery Protocol attributes.

4 files changed, 89 insertions, 43 deletions

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothStack.c b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothStack.c
index 542a783c9..d78a9efa1 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothStack.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothStack.c
@@ -59,6 +59,9 @@ void Bluetooth_Stack_Init(void)
  */
 void Bluetooth_Stack_USBTask(void)
 {
+	if (USB_HostState != HOST_STATE_Configured)
+	  return;
+
 	Bluetooth_HCITask();
 	Bluetooth_ACLTask();
 }
diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
index bd649419a..f91ec9840 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
@@ -31,25 +31,25 @@
 #define  INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C
 #include "ServiceDiscoveryProtocol.h"
 
-SERVICE_ATTRIBUTE_TEXT(SDP_Attribute_Name,             "SDP");
-SERVICE_ATTRIBUTE_TEXT(SDP_Attribute_Description,      "BT Service Discovery");
-SERVICE_ATTRIBUTE_8BIT_LEN(SDP_Attribute_Availability, SDP_DATATYPE_UNSIGNED_INT, 1, {0xFF});
+SERVICE_ATTRIBUTE_TEXT(SDP_Attribute_Name,         "SDP");
+SERVICE_ATTRIBUTE_TEXT(SDP_Attribute_Description,  "BT Service Discovery");
+SERVICE_ATTRIBUTE_LEN8(SDP_Attribute_Availability, SDP_DATATYPE_UNSIGNED_INT, 1, {0xFF});
 const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM =
 	{
-		{.AttributeID = SDP_ATTRIBUTE_NAME        , .AttributeData = &SDP_Attribute_Name},
-		{.AttributeID = SDP_ATTRIBUTE_DESCRIPTION , .AttributeData = &SDP_Attribute_Description},
-		{.AttributeID = SDP_ATTRIBUTE_AVAILABILITY, .AttributeData = &SDP_Attribute_Availability},
+		{.ID = SDP_ATTRIBUTE_NAME        , .Data = &SDP_Attribute_Name},
+		{.ID = SDP_ATTRIBUTE_DESCRIPTION , .Data = &SDP_Attribute_Description},
+		{.ID = SDP_ATTRIBUTE_AVAILABILITY, .Data = &SDP_Attribute_Availability},
 		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
 	};
 
-SERVICE_ATTRIBUTE_TEXT(RFCOMM_Attribute_Name,             "RFCOMM");
-SERVICE_ATTRIBUTE_TEXT(RFCOMM_Attribute_Description,      "Virtual Serial");
-SERVICE_ATTRIBUTE_8BIT_LEN(RFCOMM_Attribute_Availability, SDP_DATATYPE_UNSIGNED_INT, 1, {0xFF});
+SERVICE_ATTRIBUTE_TEXT(RFCOMM_Attribute_Name,         "RFCOMM");
+SERVICE_ATTRIBUTE_TEXT(RFCOMM_Attribute_Description,  "Virtual Serial");
+SERVICE_ATTRIBUTE_LEN8(RFCOMM_Attribute_Availability, SDP_DATATYPE_UNSIGNED_INT, 1, {0xFF});
 const ServiceAttributeTable_t RFCOMM_Attribute_Table[] PROGMEM =
 	{
-		{.AttributeID = SDP_ATTRIBUTE_NAME        , .AttributeData = &RFCOMM_Attribute_Name},
-		{.AttributeID = SDP_ATTRIBUTE_DESCRIPTION , .AttributeData = &RFCOMM_Attribute_Description},
-		{.AttributeID = SDP_ATTRIBUTE_AVAILABILITY, .AttributeData = &RFCOMM_Attribute_Availability},
+		{.ID = SDP_ATTRIBUTE_NAME        , .Data = &RFCOMM_Attribute_Name},
+		{.ID = SDP_ATTRIBUTE_DESCRIPTION , .Data = &RFCOMM_Attribute_Description},
+		{.ID = SDP_ATTRIBUTE_AVAILABILITY, .Data = &RFCOMM_Attribute_Availability},
 		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
 	};
 
@@ -106,8 +106,6 @@ static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPH
 	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 	
 	BT_SDP_DEBUG(1, "<< Service Search Attribute");
-	
-	uint8_t ElementHeaderSize;
 
 	uint8_t UUIDList[12][16];
 	uint8_t TotalUUIDs = ServiceDiscovery_GetUUIDList(UUIDList, &CurrentParameter);
@@ -116,32 +114,75 @@ 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 AttributeIDListLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);
+	struct
+	{
+		SDP_PDUHeader_t SDPHeader;
+		uint8_t         ResponseData[100];
+	} ResponsePacket;
+	
+	ResponsePacket.SDPHeader.PDU           = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
+	ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID;
+	
+	if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
+	  MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
+	  
+	ResponsePacket.SDPHeader.ParameterLength = ServiceDiscovery_ProcessAttributes(UUIDList, TotalUUIDs,
+	                                                                              ResponsePacket.ResponseData,
+	                                                                              MaxAttributeSize, &CurrentParameter);
+
+	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
+	                     Channel);
+}
+
+static uint8_t ServiceDiscovery_ProcessAttributes(uint8_t UUIDList[12][16], const uint8_t TotalUUIDs, uint8_t* ResponseBuffer,
+                                                  uint8_t MaxResponseSize, const void** CurrentParameter)
+{
+	uint8_t ElementHeaderSize;
+	uint8_t TotalResponseSize = 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);
+		uint8_t  AttributeLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 		uint32_t Attribute       = 0;
 		
 		memcpy(&Attribute, CurrentParameter, AttributeLength);
 		
 		BT_SDP_DEBUG(2, "-- Attribute(%d): 0x%08lX", AttributeLength, Attribute);
+		
+		uint8_t TotalBytesAdded = ServiceDiscovery_GetAttribute(UUIDList, TotalUUIDs, Attribute, &ResponseBuffer,
+		                                                        MaxResponseSize);
+		TotalResponseSize += TotalBytesAdded;
+		MaxResponseSize   -= TotalBytesAdded;
 	
 		AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 		CurrentParameter      += AttributeLength;
 	}
 	
-	struct
+	return TotalResponseSize;
+}
+
+static uint8_t ServiceDiscovery_GetAttribute(uint8_t UUIDList[12][16], 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++)
 	{
-		SDP_PDUHeader_t SDPHeader;
-		uint8_t         ResponseData[100];
-	} ResponsePacket;
-	
-	ResponsePacket.SDPHeader.PDU           = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
-	ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID;
-	
-	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
-	                     Channel);
+		for (uint8_t CurrUUIDIndex = 0; CurrUUIDIndex < TotalUUIDs; CurrUUIDIndex++)
+		{
+			if (!(memcmp(SDP_Services_Table[CurrTableItem].UUID, UUIDList[CurrUUIDIndex], sizeof(BaseUUID))))
+			{
+				const ServiceAttributeTable_t* AttributeTable = SDP_Services_Table[CurrTableItem].AttributeTable;
+
+				// Process attribute table
+				BT_SDP_DEBUG(2, "FOUND UUID IN TABLE");
+
+				break;
+			}
+		}
+	}
+		
+	return 0;
 }
 
 static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[12][16], const void** CurrentParameter)
@@ -154,15 +195,10 @@ static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[12][16], const void
 	while (ServicePatternLength)
 	{
 		uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
-	
-		uint8_t UUIDLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
+		uint8_t  UUIDLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 		
 		memcpy(CurrentUUID, BaseUUID, sizeof(BaseUUID));
-			
-		if (UUIDLength <= 32)
-		  memcpy(&CurrentUUID[sizeof(BaseUUID) - sizeof(uint32_t)], *CurrentParameter, UUIDLength);
-		else
-		  memcpy(CurrentUUID, *CurrentParameter, UUIDLength);
+		memcpy(&CurrentUUID[(UUIDLength <= 32) ? (sizeof(BaseUUID) - 32) : 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",
 		                UUIDLength,
@@ -180,11 +216,13 @@ static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[12][16], const void
 
 static uint32_t ServiceDiscovery_GetDataElementSize(const void** DataElementHeader, uint8_t* 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);
 	
 	uint32_t ElementValue;
-	
+
+	/* Convert the Data Element size index into a size in bytes */
 	switch (SizeIndex)
 	{
 		case 5:
diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
index 1a5106f54..d90652b23 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.h
@@ -71,12 +71,12 @@
 		
 		#define BASE_96BIT_UUID                         0xFB, 0x34, 0x9B, 0x5F, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00
 		
-		#define SERVICE_ATTRIBUTE_TEXT(name, string)    SERVICE_ATTRIBUTE_8BIT_LEN(name, SDP_DATATYPE_TEXT, sizeof(string), string)
-		#define SERVICE_ATTRIBUTE_8BIT_LEN(name, type, size, ...)  const ServiceAttributeData8Bit_t  name PROGMEM = \
+		#define SERVICE_ATTRIBUTE_TEXT(name, string)    SERVICE_ATTRIBUTE_LEN8(name, SDP_DATATYPE_TEXT, sizeof(string), string)
+		#define SERVICE_ATTRIBUTE_LEN8(name, type, size, ...)  const ServiceAttributeData8Bit_t  name PROGMEM = \
 		                                                {.Header = (type | 5), .Size = size, .Data = __VA_ARGS__}
-		#define SERVICE_ATTRIBUTE_16BIT_LEN(name, type, size, ...) const ServiceAttributeData16Bit_t name PROGMEM = \
+		#define SERVICE_ATTRIBUTE_LEN16(name, type, size, ...) const ServiceAttributeData16Bit_t name PROGMEM = \
 		                                                {.Header = (type | 5), .Size = size, .Data = __VA_ARGS__}
-		#define SERVICE_ATTRIBUTE_32BIT_LEN(name, type, size, ...) const ServiceAttributeData32Bit_t name PROGMEM = \
+		#define SERVICE_ATTRIBUTE_LEN32(name, type, size, ...) const ServiceAttributeData32Bit_t name PROGMEM = \
 		                                                {.Header = (type | 5), .Size = size, .Data = __VA_ARGS__}
 		#define SERVICE_ATTRIBUTE_TABLE_TERMINATOR      {.AttributeData = NULL}
 
@@ -90,8 +90,8 @@
 		
 		typedef struct
 		{
-			uint16_t    AttributeID;
-			const void* AttributeData;
+			uint16_t    ID;
+			const void* Data;
 		} ServiceAttributeTable_t;
 
 		typedef struct
@@ -142,7 +142,12 @@
 				return ParamValue;
 			}
 
-			static uint8_t  ServiceDiscovery_GetUUIDList(uint8_t UUIDList[12][16], const void** CurrentParameter);
+			static uint8_t ServiceDiscovery_ProcessAttributes(uint8_t UUIDList[12][16], const uint8_t TotalUUIDs, 
+			                                                  uint8_t* ResponseBuffer, uint8_t MaxResponseSize,
+			                                                  const void** CurrentParameter);
+			static uint8_t ServiceDiscovery_GetAttribute(uint8_t UUIDList[12][16], const uint8_t TotalUUIDs,
+			                                             const uint32_t Attribute, uint8_t** DataBuffer, uint8_t BufferLen);
+			static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[12][16], const void** CurrentParameter);
 			static uint32_t ServiceDiscovery_GetDataElementSize(const void** AttributeHeader, uint8_t* ElementHeaderSize);
 		#endif
 
diff --git a/Demos/Host/Incomplete/BluetoothHost/makefile b/Demos/Host/Incomplete/BluetoothHost/makefile
index cd3bc305d..ab535f906 100644
--- a/Demos/Host/Incomplete/BluetoothHost/makefile
+++ b/Demos/Host/Incomplete/BluetoothHost/makefile
@@ -60,7 +60,7 @@
 
 
 # MCU name
-MCU = at90usb647
+MCU = at90usb1287
 
 
 # Target board (see library "Board Types" documentation, NONE for projects not requiring
@@ -87,7 +87,7 @@ BOARD  = USBKEY
 #         F_CPU = 16000000
 #         F_CPU = 18432000
 #         F_CPU = 20000000
-F_CPU = 8000000
+F_CPU = 16000000
 
 
 # Input clock frequency.