X-Git-Url: http://git.linex4red.de/pub/USBasp.git/blobdiff_plain/587f3203333adb68fe3d4c89bc1a07698f180030..96063b9f98c8201623f8c91ec5cbe3bb88aca6c3:/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c?ds=inline diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c index ba6670716..5223d29b1 100644 --- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c +++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c @@ -28,170 +28,35 @@ this software. */ -#define INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C -#include "ServiceDiscoveryProtocol.h" - -const struct -{ - uint8_t Header; - uint32_t Data; -} PROGMEM SDP_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_32Bit), SWAPENDIAN_32(0x00010000)}; - -const struct -{ - uint8_t Header; - uint16_t Size; - ClassUUID_t UUIDList[]; -} PROGMEM SDP_Attribute_ServiceClassIDs = - { - .Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit), - .Size = SWAPENDIAN_16(sizeof(ClassUUID_t) * 1), - .UUIDList = - { - {.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x00, 0x10, 0x00, 0x00}} - } - }; - -const struct -{ - uint8_t Header; - uint8_t Size; - Item16Bit_t VersionList[]; -} PROGMEM SDP_Attribute_Version = - { - .Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable8Bit), - .Size = (sizeof(Item16Bit_t) * 1), - .VersionList = - { - {.Header = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit), .Value = SWAPENDIAN_16(0x0100)} - } - }; - -const struct -{ - uint8_t Header; - uint8_t Size; - Item16Bit_t OffsetList[]; -} PROGMEM SDP_Attribute_LangOffset = - { - .Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable8Bit), - .Size = (sizeof(Item16Bit_t) * 1), - .OffsetList = - { - {.Header = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit), .Value = SWAPENDIAN_16(0x0100)} - } - }; - -const struct -{ - uint8_t Header; - uint8_t Size; - char Text[]; -} PROGMEM SDP_Attribute_ServiceName = - { - .Header = (SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit), - .Size = sizeof("SDP") - 1, - .Text = "SDP", - }; - -const struct -{ - uint8_t Header; - uint8_t Size; - char Text[]; -} PROGMEM SDP_Attribute_ServiceDescription = - { - .Header = (SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit), - .Size = sizeof("Service Discovery Protocol Server") - 1, - .Text = "Service Discovery Protocol Server", - }; - -/** Service Discovery Protocol attribute table, listing all supported attributes of the service. */ -const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM = - { - {.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE, .Data = &SDP_Attribute_ServiceHandle }, - {.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS, .Data = &SDP_Attribute_ServiceClassIDs }, - {.AttributeID = SDP_ATTRIBUTE_ID_VERSION, .Data = &SDP_Attribute_Version }, - {.AttributeID = SDP_ATTRIBUTE_ID_LANGIDOFFSET, .Data = &SDP_Attribute_LangOffset }, - {.AttributeID = SDP_ATTRIBUTE_ID_SERVICENAME, .Data = &SDP_Attribute_ServiceName }, - {.AttributeID = SDP_ATTRIBUTE_ID_SERVICEDESCRIPTION, .Data = &SDP_Attribute_ServiceDescription }, - - SERVICE_ATTRIBUTE_TABLE_TERMINATOR - }; - -const struct -{ - uint8_t Header; - uint32_t Data; -} PROGMEM RFCOMM_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_32Bit), SWAPENDIAN_32(0x00010001)}; - -const struct -{ - uint8_t Header; - uint16_t Size; - ClassUUID_t UUIDList[]; -} PROGMEM RFCOMM_Attribute_ServiceClassIDs = - { - .Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit), - .Size = SWAPENDIAN_16(sizeof(ClassUUID_t) * 1), - .UUIDList = - { - {.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x01, 0x11, 0x00, 0x00}} - } - }; - -const struct -{ - uint8_t Header; - uint8_t Size; - char Text[]; -} PROGMEM RFCOMM_Attribute_ServiceName = - { - .Header = (SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit), - .Size = sizeof("Serial Port") - 1, - .Text = "Serial Port", - }; +/** \file + * + * SDP layer module. This module implements a simple Service Discovery + * Protocol server, which can broadcast the device's supported services + * to other Bluetooth devices upon request, so that they can determine + * what services are available. + */ -const struct -{ - uint8_t Header; - uint8_t Size; - char Text[]; -} PROGMEM RFCOMM_Attribute_ServiceDescription = - { - .Header = (SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit), - .Size = sizeof("Wireless Serial Port Service") - 1, - .Text = "Wireless Serial Port Service", - }; +/* + TODO: Honor remote device's buffer size constraints via continuation state + */ -const ServiceAttributeTable_t RFCOMM_Attribute_Table[] PROGMEM = - { - {.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE, .Data = &RFCOMM_Attribute_ServiceHandle }, - {.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS, .Data = &RFCOMM_Attribute_ServiceClassIDs }, - {.AttributeID = SDP_ATTRIBUTE_ID_SERVICENAME, .Data = &RFCOMM_Attribute_ServiceName }, - {.AttributeID = SDP_ATTRIBUTE_ID_SERVICEDESCRIPTION, .Data = &RFCOMM_Attribute_ServiceDescription }, +#define INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C +#include "ServiceDiscoveryProtocol.h" - SERVICE_ATTRIBUTE_TABLE_TERMINATOR - }; - -/** 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[] PROGMEM = +/** Service attribute table list, containing a pointer to each service attribute table the device contains */ +const ServiceAttributeTable_t* SDP_Services_Table[] PROGMEM = { - { // 128-bit UUID for the SDP service - .UUID = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00}, - .AttributeTable = SDP_Attribute_Table, - }, - { // 128-bit UUID for the RFCOMM service - .UUID = {BASE_96BIT_UUID, 0x03, 0x00, 0x00, 0x00}, - .AttributeTable = RFCOMM_Attribute_Table, - }, + SerialPort_Attribute_Table, }; /** 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 = {0x00000000, BASE_80BIT_UUID}; +/** Initializes the SDP service, ready for new connections from a SDP client. */ +void SDP_Initialize(void) +{ + /* Not currently used */ +} /** Main Service Discovery Protocol packet processing routine. This function processes incomming SDP packets from * a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the @@ -200,7 +65,7 @@ const uint8_t BaseUUID[] PROGMEM = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00}; * \param[in] Data Incomming packet data containing the SDP request * \param[in] Channel Channel the request was issued to by the remote device */ -void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel) +void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel) { SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data; SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength); @@ -209,6 +74,7 @@ void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel) BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU); BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength); + /* Dispatch to the correct processing routine for the given SDP packet type */ switch (SDPHeader->PDU) { case SDP_PDU_SERVICESEARCHREQUEST: @@ -240,8 +106,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 @@ -251,39 +116,37 @@ static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Blu uint16_t CurrentServiceRecordCount; uint8_t ResponseData[100]; } ResponsePacket; + + uint8_t AddedServiceHandles = 0; /* Create a pointer to the buffer to indicate the current location for response data to be added */ void* CurrResponsePos = ResponsePacket.ResponseData; - - uint8_t AddedServiceHandles = 0; - /* Search through the list of UUIDs one at a time looking for matching search Attributes */ - for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++) + /* Search through the global service list an item at a time */ + for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(void*)); CurrTableItem++) { - /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */ - ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]); - - /* If the UUID does not exist in the global UUID table, continue on to the next search UUID */ - if (AttributeTable == NULL) + /* Read in a pointer to the current UUID table entry's Attribute table */ + ServiceAttributeTable_t* CurrAttributeTable = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]); + + if (!(SDP_SearchServiceTable(UUIDList, TotalUUIDs, CurrAttributeTable))) continue; - - BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem); + + BT_SDP_DEBUG(2, " -- Found search match in table"); /* Retrieve a PROGMEM pointer to the value of the service's record handle */ - const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE); + const void* AttributeValue = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE); /* Copy over the service record handle to the response list */ uint8_t AttrHeaderSize; - SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize); - memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, sizeof(uint32_t)); - CurrResponsePos += AttrHeaderSize + sizeof(uint32_t); + uint8_t AttrSize = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize); + memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, AttrSize); + CurrResponsePos += AttrHeaderSize + AttrSize; - /* Increment the total number of service records added to the list */ AddedServiceHandles++; } /* 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); @@ -319,17 +182,15 @@ 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 */ - uint16_t AttributeList[15][2]; + uint16_t AttributeList[8][2]; uint8_t TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter); BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes); @@ -347,42 +208,30 @@ static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader, if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData)) MaxAttributeSize = sizeof(ResponsePacket.ResponseData); - /* Add the outer Data Element Sequence header for all of the retrieved Attributes */ - uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence); + uint16_t TotalResponseSize = 0; /* Search through the global UUID list an item at a time */ - for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++) + for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(void*)); CurrTableItem++) { /* Read in a pointer to the current UUID table entry's Attribute table */ - ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable); + ServiceAttributeTable_t* CurrAttributeTable = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]); /* 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); + + /* Retrieve the endian-swapped service handle of the current service being examined */ + uint32_t CurrServiceHandle = SwapEndian_32(pgm_read_dword(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 (ServiceHandle == CurrServiceHandle) { - /* 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); - } - } + /* Add the listed attributes for the found UUID to the response */ + TotalResponseSize = SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes, + &CurrResponsePos); /* Requested service found, abort the search through the service table */ break; @@ -390,25 +239,20 @@ 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); + ResponsePacket.AttributeListByteCount = SwapEndian_16(TotalResponseSize); /* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created attribute value list and the SDP continuation state */ - uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) + - (3 + *TotalResponseSize) + - sizeof(uint8_t)); - + uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) + TotalResponseSize + sizeof(uint8_t)); + /* Fill in the response packet's header */ ResponsePacket.SDPHeader.PDU = SDP_PDU_SERVICEATTRIBUTERESPONSE; ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID; ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength); - /* Flip the endianness of the container's size */ - *TotalResponseSize = SwapEndian_16(*TotalResponseSize); - BT_SDP_DEBUG(1, ">> Service Attribute Response"); BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength); @@ -433,12 +277,11 @@ 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 */ - uint16_t AttributeList[15][2]; + uint16_t AttributeList[8][2]; uint8_t TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter); BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes); @@ -457,54 +300,26 @@ static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHe MaxAttributeSize = sizeof(ResponsePacket.ResponseData); /* Add the outer Data Element Sequence header for all of the retrieved Attributes */ - uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence); + uint16_t* TotalResponseSize = SDP_AddSequence16(&CurrResponsePos); - /* Search through the list of UUIDs one at a time looking for matching search Attributes */ - for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++) + /* Search through the global service list an item at a time */ + for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(void*)); CurrTableItem++) { - /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */ - ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]); - - /* If the UUID does not exist in the global UUID table, continue on to the next search UUID */ - if (AttributeTable == NULL) + /* Read in a pointer to the current UUID table entry's Attribute table */ + ServiceAttributeTable_t* CurrAttributeTable = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]); + + if (!(SDP_SearchServiceTable(UUIDList, TotalUUIDs, CurrAttributeTable))) continue; - 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; - } + BT_SDP_DEBUG(2, " -- Found search match in table"); - /* Flip the endianness of the container's size */ - *CurrentUUIDResponseSize = SwapEndian_16(*CurrentUUIDResponseSize); + /* Add the listed attributes for the found UUID to the response */ + *TotalResponseSize += SDP_AddListedAttributesToResponse(CurrAttributeTable, 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); @@ -515,14 +330,14 @@ static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHe (3 + *TotalResponseSize) + sizeof(uint8_t)); + /* Flip the endianness of the container's size */ + *TotalResponseSize = SwapEndian_16(*TotalResponseSize); + /* Fill in the response packet's header */ ResponsePacket.SDPHeader.PDU = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE; ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID; ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength); - /* Flip the endianness of the container's size */ - *TotalResponseSize = SwapEndian_16(*TotalResponseSize); - BT_SDP_DEBUG(1, ">> Service Search Attribute Response"); BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength); @@ -530,6 +345,55 @@ 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], + const uint8_t TotalAttributes, void** const BufferPos) +{ + uint16_t TotalResponseSize; + + /* Add an inner Data Element Sequence header for the current services's found Attributes */ + uint16_t* AttributeListSize = SDP_AddSequence16(BufferPos); + + /* 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]; + void* AttributeValue; + + /* Look through the current service's attribute list, examining all the attributes */ + while ((AttributeValue = pgm_read_ptr(&AttributeTable->Data)) != NULL) + { + /* Get the current Attribute's ID from the current attribute table entry */ + uint16_t CurrAttributeID = pgm_read_word(&AttributeTable->AttributeID); + + /* Check if the current Attribute's ID is within the current Attribute range */ + if ((CurrAttributeID >= AttributeIDRange[0]) && (CurrAttributeID <= AttributeIDRange[1])) + { + /* Increment the current UUID's returned Attribute container size by the number of added bytes */ + *AttributeListSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, BufferPos); + } + + AttributeTable++; + } + } + + /* Record the total number of added bytes to the buffer */ + 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 @@ -542,15 +406,15 @@ static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const voi { /* Retrieve the size of the attribute value from its container header */ uint8_t AttributeHeaderLength; - uint32_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength); + uint16_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength); + BT_SDP_DEBUG(2, " -- Add Attribute (0x%04X) 0x%04X", (AttributeHeaderLength + AttributeValueLength), AttributeID); + /* 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); @@ -571,7 +435,7 @@ static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable void* CurrTableItemData; /* Search through the current Attribute table, abort when the terminator item has been reached */ - while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL) + while ((CurrTableItemData = pgm_read_ptr(&AttributeTable->Data)) != NULL) { /* Check if the current Attribute ID matches the search ID - if so return a pointer to it */ if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID) @@ -583,49 +447,92 @@ static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable return NULL; } -/** Retrieves the Attribute table for the given UUID if it exists. +/** Retrieves the Attribute table for the given UUID list if it exists. * - * \param[in] UUID UUID to search for + * \param[in] UUIDList List of UUIDs which must be matched within the service attribute table + * \param[in] TotalUUIDs Total number of UUIDs stored in the UUID list + * \param[in] CurrAttributeTable Pointer to the service attribute table to search through * - * \return Pointer to the UUID's associated Attribute table if found in the global UUID table, NULL otherwise + * \return True if all the UUIDs given in the UUID list appear in the given attribute table, false otherwise */ -static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID) +static bool SDP_SearchServiceTable(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs, + const ServiceAttributeTable_t* CurrAttributeTable) { - /* Search through the global UUID list an item at a time */ - for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++) + bool UUIDMatch[TotalUUIDs]; + + /* Set all the match flags to false (not matched) before starting the search */ + memset(UUIDMatch, false, sizeof(UUIDMatch)); + + const void* CurrAttribute; + + /* Search through the current attribute table, checking each attribute value for UUID matches */ + while ((CurrAttribute = pgm_read_ptr(&CurrAttributeTable->Data)) != NULL) { - /* Read in a pointer to the current UUID table entry's Attribute table */ - ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable); + SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, UUIDMatch, CurrAttribute); + CurrAttributeTable++; + } + + /* Determine how many UUID matches in the list we have found */ + uint8_t UUIDMatches = 0; + for (uint8_t i = 0; i < TotalUUIDs; i++) + { + if (UUIDMatch[i]) + UUIDMatches++; + } - /* 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))) - return CurrAttributeTable; - - /* Retrieve the list of the service's Class UUIDs from its Attribute table */ - void* ClassUUIDs = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICECLASSIDS); + /* If all UUIDs have been matched to the current service, return true */ + return (UUIDMatches == TotalUUIDs); +} + +/** Recursively upwraps the given locally stored attribute (in PROGMEM space), searching for UUIDs to match against + * the given UUID list. As matches are found, they are indicated in the UUIDMatch flag list. + * + * \param[in] UUIDList List of UUIDs which must be matched within the service attribute table + * \param[in] TotalUUIDs Total number of UUIDs stored in the UUID list + * \param[in, out] UUIDMatch Array of flags indicating which UUIDs in the list have already been matched + * \param[in] CurrAttribute Pointer to the current attribute to search through + * + * \return True if all the UUIDs given in the UUID list appear in the given attribute table, false otherwise + */ +static void SDP_CheckUUIDMatch(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs, bool UUIDMatch[], + const void* CurrAttribute) +{ + uint8_t CurrAttributeType = (pgm_read_byte(CurrAttribute) & ~0x07); + + /* Check the data type of the current attribute value - if UUID, compare, if Sequence, unwrap and recurse */ + if (CurrAttributeType == SDP_DATATYPE_UUID) + { + /* Look for matches in the UUID list against the current attribute UUID value */ + for (uint8_t i = 0; i < TotalUUIDs; i++) + { + /* Check if the current unmatched UUID is identical to the search UUID */ + if (!(UUIDMatch[i]) && !(memcmp_P(UUIDList[i], (CurrAttribute + 1), UUID_SIZE_BYTES))) + { + /* Indicate match found for the current attribute UUID and early-abort */ + UUIDMatch[i] = true; + break; + } + } + } + else if (CurrAttributeType == SDP_DATATYPE_Sequence) + { + uint8_t SequenceHeaderSize; + uint16_t SequenceSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &SequenceHeaderSize); - /* Go to the next UUID in the table if the current item does not have a list of Class UUIDs */ - if (ClassUUIDs == NULL) - continue; - - /* Retrieve the size of the Class UUID list and skip past the header to the first Class UUID in the list */ - uint8_t ClassUUIDListHeaderSize; - uint32_t ClassUUIDListSize = SDP_GetLocalAttributeContainerSize(ClassUUIDs, &ClassUUIDListHeaderSize); - ClassUUIDs += ClassUUIDListHeaderSize; + CurrAttribute += SequenceHeaderSize; - /* Check each class UUID in turn for a match */ - while (ClassUUIDListSize) + /* Recursively unwrap the sequence container, and re-search its contents for UUIDs */ + while (SequenceSize) { - /* 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))) - return CurrAttributeTable; - - ClassUUIDListSize -= sizeof(ClassUUID_t); - ClassUUIDs += sizeof(ClassUUID_t); - } - } - - return NULL; + uint8_t InnerHeaderSize; + uint16_t InnerSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &InnerHeaderSize); + + SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, UUIDMatch, CurrAttribute); + + SequenceSize -= InnerHeaderSize + InnerSize; + CurrAttribute += InnerHeaderSize + InnerSize; + } + } } /** Reads in the collection of Attribute ranges from the input buffer's Data Element Sequence container, into the given @@ -692,27 +599,28 @@ static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** uint8_t* CurrentUUID = UUIDList[TotalUUIDs++]; 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)); - - /* 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 */ + /* Copy over UUID from the container to the free slot */ if (UUIDLength <= 4) { - memcpy(&CurrentUUID[UUID_SIZE_BYTES - 4], *CurrentParameter, UUIDLength); - SwapEndian_n(&CurrentUUID[UUID_SIZE_BYTES - 4], UUIDLength); + /* Copy over the base UUID value to the free UUID slot in the list */ + memcpy_P(CurrentUUID, &BaseUUID, sizeof(BaseUUID)); + + /* Copy over short UUID */ + memcpy(CurrentUUID + (4 - UUIDLength), *CurrentParameter, UUIDLength); } else { - memcpy(&CurrentUUID[0], *CurrentParameter, UUIDLength); + /* Copy over full UUID */ + memcpy(CurrentUUID, *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; @@ -723,7 +631,8 @@ static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** /** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container. * - * \param[in] AttributeData Pointer to the start of the Attribute container, located in PROGMEM + * \param[in] AttributeData Pointer to the start of the Attribute container, located in PROGMEM + * \param[out] HeaderSize Pointer to a location where the header size of the data element is to be stored * * \return Size in bytes of the entire attribute container, including the header */ @@ -732,24 +641,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 @@ -763,32 +678,28 @@ 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); *ElementHeaderSize = (1 + sizeof(uint8_t)); + ElementValueSize = SDP_ReadData8(DataElementHeader); break; case SDP_DATASIZE_Variable16Bit: - ElementValueSize = SwapEndian_16(*((uint16_t*)*DataElementHeader)); - *DataElementHeader += sizeof(uint16_t); *ElementHeaderSize = (1 + sizeof(uint16_t)); + ElementValueSize = SDP_ReadData16(DataElementHeader); break; case SDP_DATASIZE_Variable32Bit: - ElementValueSize = SwapEndian_32(*((uint32_t*)*DataElementHeader)); - *DataElementHeader += sizeof(uint32_t); *ElementHeaderSize = (1 + sizeof(uint32_t)); + ElementValueSize = SDP_ReadData32(DataElementHeader); break; default: - ElementValueSize = (1 << SizeIndex); *ElementHeaderSize = 1; + ElementValueSize = (1 << SizeIndex); break; }