/** Recursively unwraps 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] UUIDMatchFlags 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,
uint16_t* const UUIDMatchFlags,
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)
{
uint16_t CurrUUIDMatchMask = (1 << 0);
/* 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 (!(*UUIDMatchFlags & CurrUUIDMatchMask) && !(memcmp_P(UUIDList[i], (CurrAttribute + 1), UUID_SIZE_BYTES)))
{
/* Indicate match found for the current attribute UUID and early-abort */
*UUIDMatchFlags |= CurrUUIDMatchMask;
break;
}
CurrUUIDMatchMask <<= 1;
}
}
else if (CurrAttributeType == SDP_DATATYPE_Sequence)
{
uint8_t SequenceHeaderSize;
uint16_t SequenceSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &SequenceHeaderSize);
CurrAttribute += SequenceHeaderSize;
/* Recursively unwrap the sequence container, and re-search its contents for UUIDs */
while (SequenceSize)
{
uint8_t InnerHeaderSize;
uint16_t InnerSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &InnerHeaderSize);
/* Recursively search of the next element in the sequence, trying to match UUIDs with the UUID list */
SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, UUIDMatchFlags, CurrAttribute);
/* Skip to the next element in the sequence */
SequenceSize -= InnerHeaderSize + InnerSize;
CurrAttribute += InnerHeaderSize + InnerSize;
}
}
}
/** Adds the given attribute ID and value to the response buffer, and advances the response buffer pointer past the added data.
*
* \param[in] AttributeID Attribute ID to add to the response buffer
* \param[in] AttributeValue Pointer to the start of the Attribute's value, located in PROGMEM
* \param[in, out] ResponseBuffer Pointer to a buffer where the Attribute and Attribute Value is to be added
*
* \return Number of bytes added to the response buffer
*/
static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID,
const void* AttributeValue,
void** ResponseBuffer)
{
/* Retrieve the size of the attribute value from its container header */
uint8_t AttributeHeaderLength;
uint16_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
/* Add a Data Element header to the response for the Attribute ID */
SDP_WriteData8(ResponseBuffer, (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit));
/* Add the Attribute ID to the created Data Element */
SDP_WriteData16(ResponseBuffer, AttributeID);
/* Copy over the Attribute value Data Element container to the response */
memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
*ResponseBuffer += AttributeHeaderLength + AttributeValueLength;
return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeHeaderLength + AttributeValueLength);
}
static void SDP_ServiceAttribute(BT_StackConfig_t* const StackState,
BT_L2CAP_Channel_t* const Channel,
BT_SDP_PDUHeader_t* const SDPHeader)
{
const void* CurrParameterPos = SDPHeader->Parameters;
/* Read service record handle */
uint32_t RecordHandle = SDP_ReadData32(&CurrParameterPos);
/* Read maximum attribute size byte count for the response */
uint16_t MaximumAttributeSize = SDP_ReadData16(&CurrParameterPos);
/* Read search Attribute list sequence header and obtain its data size and a pointer to start of the data */
uint16_t AttributeList[8][2];
uint8_t TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrParameterPos);
/* Read continuation state value from the client */
uint8_t ContinuationState = SDP_ReadData8(&CurrParameterPos);
struct
{
BT_SDP_PDUHeader_t SDPHeader;
uint16_t AttributeListByteCount;
uint8_t ResponseData[200];
} ATTR_PACKED ResponsePacket;
/* Create a pointer to the buffer to indicate the current location for response data to be added */
void* CurrResponsePos = ResponsePacket.ResponseData;
/* Clamp the maximum attribute size to the size of the allocated buffer */
if (MaximumAttributeSize > sizeof(ResponsePacket.ResponseData))
MaximumAttributeSize = sizeof(ResponsePacket.ResponseData);
/* Search through all registered services, looking for UUID matches */
SDP_ServiceEntry_t* CurrentService = RegisteredServices;
while (CurrentService != NULL)
{
/* Retrieve a PROGMEM pointer to the value of the Service Record Handle */
const void* ServiceRecord = SDP_GetAttributeValue(CurrentService, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
/* Check if matching service record found in the table */
if (ServiceRecord)
{
/* 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 = be32_to_cpu(pgm_read_dword(ServiceRecord + AttrHeaderSize));
/* Check if the current service in the service table has the requested service handle */
if (RecordHandle == CurrServiceHandle)
{
/* Add the listed attributes for the found UUID to the response */
SDP_AddListedAttributesToResponse(CurrentService, AttributeList, TotalAttributes, &CurrResponsePos);
/* Requested service found, abort the search through the service table */
break;
}
}
/* Select next registered service in the service table list */
CurrentService = CurrentService->NextService;
}
/* Continuation state - always zero */
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 = cpu_to_be16((uintptr_t)CurrResponsePos - (uintptr_t)ResponsePacket.ResponseData - sizeof(uint8_t));
/* Fill in the response packet's header */
ResponsePacket.SDPHeader.PDU = SDP_PDU_SERVICEATTRIBUTERESPONSE;
ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID;
ResponsePacket.SDPHeader.ParameterLength = cpu_to_be16(sizeof(ResponsePacket.AttributeListByteCount) + be16_to_cpu(ResponsePacket.AttributeListByteCount) + sizeof(uint8_t));
/* Send the completed response packet to the sender */
SDP_SendFrame(StackState, Channel, (sizeof(ResponsePacket.SDPHeader) + be16_to_cpu(ResponsePacket.SDPHeader.ParameterLength)), &ResponsePacket);
}
static void SDP_ServiceSearch(BT_StackConfig_t* const StackState,
BT_L2CAP_Channel_t* const Channel,
BT_SDP_PDUHeader_t* const SDPHeader)
{
const void* CurrParameterPos = SDPHeader->Parameters;
/* Read search UUID list sequence header and obtain its data size and a pointer to start of the data */
uint8_t UUIDList[12][UUID_SIZE_BYTES];
uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrParameterPos);
/* Read maximum service count for the response */
uint16_t MaximumServiceCount = SDP_ReadData16(&CurrParameterPos);
/* Read continuation state value from the client */
uint8_t ContinuationState = SDP_ReadData8(&CurrParameterPos);
struct
{
BT_SDP_PDUHeader_t SDPHeader;
uint16_t TotalServiceRecordCount;
uint16_t CurrentServiceRecordCount;
uint8_t ResponseData[200];
} ATTR_PACKED 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;
/* Search through all registered services, looking for UUID matches */
SDP_ServiceEntry_t* CurrentService = RegisteredServices;
while (CurrentService != NULL)
{
/* Ignore service nodes registered to other stacks */
if (CurrentService->Stack && (CurrentService->Stack != StackState))
continue;
/* Search the current SDP service attribute table for the given UUIDs */
if (SDP_SearchServiceTable(CurrentService, UUIDList, TotalUUIDs))
{
/* Retrieve a PROGMEM pointer to the value of the service's record handle */
const void* AttributeValue = SDP_GetAttributeValue(CurrentService, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
/* Copy over the service record handle to the response list */
uint8_t AttrHeaderSize;
uint8_t AttrSize = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize);
memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, AttrSize);
CurrResponsePos += AttrSize;
AddedServiceHandles++;
if (AddedServiceHandles == MaximumServiceCount)
break;
}
/* Select next registered service in the service table list */
CurrentService = CurrentService->NextService;
}
/* Continuation state - always zero */
SDP_WriteData8(&CurrResponsePos, 0);
/* Set the total number of matching services and response services to the number of services added to the list */
ResponsePacket.TotalServiceRecordCount = cpu_to_be16(AddedServiceHandles);
ResponsePacket.CurrentServiceRecordCount = ResponsePacket.TotalServiceRecordCount;
/* Fill in the response packet's header */
ResponsePacket.SDPHeader.PDU = SDP_PDU_SERVICEATTRIBUTERESPONSE;
ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID;
ResponsePacket.SDPHeader.ParameterLength = cpu_to_be16(sizeof(ResponsePacket.TotalServiceRecordCount) +
sizeof(ResponsePacket.CurrentServiceRecordCount) +
((uintptr_t)CurrResponsePos - (uintptr_t)ResponsePacket.ResponseData));
/* Send the completed response packet to the sender */
SDP_SendFrame(StackState, Channel, (sizeof(ResponsePacket.SDPHeader) + be16_to_cpu(ResponsePacket.SDPHeader.ParameterLength)), &ResponsePacket);
}
/** Internal processing routine for SDP Service Search Requests.
*
* \param[in] SDPHeader Pointer to the start of the issued SDP request
* \param[in] Channel Pointer to the Bluetooth channel structure the request was issued to
*/
static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader,
Bluetooth_Channel_t* const Channel)
{
const void* CurrentParameter = ((const void*)SDPHeader + sizeof(SDP_PDUHeader_t));
BT_SDP_DEBUG(1, "<< Service Search");
/* Retrieve the list of search UUIDs from the request */
uint8_t UUIDList[12][UUID_SIZE_BYTES];
uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
/* Retrieve the maximum service record response count from the request */
uint16_t MaxServiceRecordCount = SDP_ReadData16(&CurrentParameter);
BT_SDP_DEBUG(2, "-- Max Return Service Count: 0x%04X", MaxServiceRecordCount);
struct
{
SDP_PDUHeader_t SDPHeader;
uint16_t TotalServiceRecordCount;
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;
/* Search through the global service list an item at a time */
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 = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]);
if (!(SDP_SearchServiceTable(UUIDList, TotalUUIDs, CurrAttributeTable)))
continue;
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(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
/* Copy over the service record handle to the response list */
uint8_t AttrHeaderSize;
uint8_t AttrSize = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize);
memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, AttrSize);
CurrResponsePos += AttrHeaderSize + AttrSize;
AddedServiceHandles++;
}
/* Continuation state - always zero */
SDP_WriteData8(&CurrResponsePos, 0);
/* Fill out the service record count values in the returned packet */
ResponsePacket.TotalServiceRecordCount = SwapEndian_16(AddedServiceHandles);
ResponsePacket.CurrentServiceRecordCount = ResponsePacket.TotalServiceRecordCount;
/* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created service
handle list and the SDP continuation state */
uint16_t ParamLength = (ResponsePacket.CurrentServiceRecordCount << 2) +
sizeof(ResponsePacket.CurrentServiceRecordCount) +
sizeof(ResponsePacket.TotalServiceRecordCount) +
sizeof(uint8_t);
/* Fill in the response packet's header */
ResponsePacket.SDPHeader.PDU = SDP_PDU_SERVICESEARCHRESPONSE;
ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID;
ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
BT_SDP_DEBUG(1, ">> Service Search Response");
/* Send the completed response packet to the sender */
Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
}
/** Internal processing routine for SDP Service Attribute Requests.
*
* \param[in] SDPHeader Pointer to the start of the issued SDP request
* \param[in] Channel Pointer to the Bluetooth channel structure the request was issued to
*/
static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader,
Bluetooth_Channel_t* const Channel)
{
const void* CurrentParameter = ((const void*)SDPHeader + sizeof(SDP_PDUHeader_t));
BT_SDP_DEBUG(1, "<< Service Attribute");
/* Retrieve the service handle whose attributes are to be examined */
uint32_t ServiceHandle = SDP_ReadData32(&CurrentParameter);
BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
/* Retrieve the maximum Attribute response size from the request */
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[8][2];
uint8_t TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
struct
{
SDP_PDUHeader_t SDPHeader;
uint16_t AttributeListByteCount;
uint8_t ResponseData[100];
} ResponsePacket;
/* Create a pointer to the buffer to indicate the current location for response data to be added */
void* CurrResponsePos = ResponsePacket.ResponseData;
/* Clamp the maximum attribute size to the size of the allocated buffer */
if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
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(void*)); CurrTableItem++)
{
/* Read in a pointer to the current UUID table entry's Attribute table */
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 (ServiceHandle == CurrServiceHandle)
{
/* 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;
}
}
/* Continuation state - always zero */
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(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) + 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);
BT_SDP_DEBUG(1, ">> Service Attribute Response");
BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength);
/* Send the completed response packet to the sender */
Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
}