diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
index c140c17d2f971cb8fbeeb0e5e5778405029d0c0b..8be4cc894f561ae737572f1835ed03b92dccd5e9 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 7986d5e72ad82e03b85f2419bcdb8882cc8bfe08..34d9baba74781c81a57b220cfa7d0ac9a3873f81 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