Service Discovery ServiceSearchAttribute requests require the response to be grouped...

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
index c140c17..8be4cc8 100644 (file)
--- 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 7986d5e..34d9bab 100644 (file)
--- 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