コード例 #1
0
ファイル: SDP.c プロジェクト: abcminiuser/explorerbot
/** 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;
		}
	}
}
コード例 #2
0
ファイル: SDP.c プロジェクト: abcminiuser/explorerbot
/** 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);
}
コード例 #3
0
ファイル: SDP.c プロジェクト: abcminiuser/explorerbot
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);
}
コード例 #4
0
ファイル: SDP.c プロジェクト: abcminiuser/explorerbot
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);
}
コード例 #5
0
ファイル: SDP.c プロジェクト: davr/lufa-lib
/** 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);
}
コード例 #6
0
ファイル: SDP.c プロジェクト: davr/lufa-lib
/** 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);
}