Begin processing code for Service Discovery Protocol attributes.

[pub/USBasp.git] / Demos / Host / Incomplete / BluetoothHost / Lib / ServiceDiscoveryProtocol.c

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
index bd64941..f91ec98 100644 (file)
--- 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"
 
 #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 =
        {
 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_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 =
        {
 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
        };
 
                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");
        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);
 
        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 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)
        {
        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);
                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;
        }
        
        
                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)
 }
 
 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++];
        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));
                
                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,
                
                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)
 {
 
 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;
        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:
        switch (SizeIndex)
        {
                case 5: