/** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer * pointer to the start of the Data Element's contents. * * \param[in, out] DataElementHeader Pointer to the start of a Data Element header * \param[out] ElementHeaderSize Size in bytes of the header that was skipped * * \return Size in bytes of the Data Element container's contents, minus the header */ 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 = (SDP_ReadData8(DataElementHeader) & 0x07); uint32_t ElementValueSize; /* Convert the Data Element size index into a size in bytes */ switch (SizeIndex) { case SDP_DATASIZE_Variable8Bit: *ElementHeaderSize = (1 + sizeof(uint8_t)); ElementValueSize = SDP_ReadData8(DataElementHeader); break; case SDP_DATASIZE_Variable16Bit: *ElementHeaderSize = (1 + sizeof(uint16_t)); ElementValueSize = SDP_ReadData16(DataElementHeader); break; case SDP_DATASIZE_Variable32Bit: *ElementHeaderSize = (1 + sizeof(uint32_t)); ElementValueSize = SDP_ReadData32(DataElementHeader); break; default: *ElementHeaderSize = 1; ElementValueSize = (1 << SizeIndex); break; } return ElementValueSize; }
static void SDP_ServiceSearchAttribute(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 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; 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); /* Write out response sequence header */ uint16_t* RespAttributeListSize = SDP_WriteSequenceHeader16(&CurrResponsePos); /* Search through all registered services, looking for UUID matches */ SDP_ServiceEntry_t* CurrentService = RegisteredServices; while (CurrentService != NULL) { /* Search the current SDP service attribute table for the given UUIDs */ if (SDP_SearchServiceTable(CurrentService, UUIDList, TotalUUIDs)) { /* Add the listed attributes for the found UUID to the response */ *RespAttributeListSize += SDP_AddListedAttributesToResponse(CurrentService, AttributeList, TotalAttributes, &CurrResponsePos); } /* Select next registered service in the service table list */ CurrentService = CurrentService->NextService; } *RespAttributeListSize = cpu_to_be16(*RespAttributeListSize); /* Write 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_SERVICESEARCHATTRIBUTERESPONSE; 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_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 Search 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_ProcessServiceSearchAttribute(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 Attribute"); /* 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 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); /* Add the outer Data Element Sequence header for all of the retrieved Attributes */ uint16_t* TotalResponseSize = SDP_AddSequence16(&CurrResponsePos); /* 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"); /* Add the listed attributes for the found UUID to the response */ *TotalResponseSize += SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes, &CurrResponsePos); } /* 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(3 + *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)); /* 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); BT_SDP_DEBUG(1, ">> Service Search 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); }
/** 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); }