this software.
*/
+/** \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.
+ */
+
+/*
+ TODO: Honor remote device's buffer size constraints via continuation state
+ */
+
#define INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C
#include "ServiceDiscoveryProtocol.h"
-/** 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_80BIT_UUID, {0x00, 0x00, 0x00, 0x00, 0x00, 0x01}},
- .AttributeTable = SDP_Attribute_Table,
- },
- { // 128-bit UUID for the RFCOMM service
- .UUID = {BASE_80BIT_UUID, {0x00, 0x00, 0x00, 0x00, 0x00, 0x03}},
- .AttributeTable = RFCOMM_Attribute_Table,
- },
+ SerialPort_Attribute_Table,
};
/** Base UUID value common to all standardized Bluetooth services */
-const UUID_t BaseUUID PROGMEM = {BASE_80BIT_UUID, {0, 0, 0, 0, 0, 0}};
+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
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:
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++)
{
- ServiceAttributeTable_t* AttributeTable;
+ /* Read in a pointer to the current UUID table entry's Attribute table */
+ ServiceAttributeTable_t* CurrAttributeTable = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]);
- /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
- if ((AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem])) == NULL)
+ 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++;
}
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);
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);
if (ServiceHandle == CurrServiceHandle)
{
/* Add the listed attributes for the found UUID to the response */
- *TotalResponseSize += SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes,
- &CurrResponsePos);
+ TotalResponseSize = SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes,
+ &CurrResponsePos);
/* Requested service found, abort the search through the service table */
break;
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);
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);
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++)
{
- ServiceAttributeTable_t* AttributeTable;
+ /* Read in a pointer to the current UUID table entry's Attribute table */
+ ServiceAttributeTable_t* CurrAttributeTable = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]);
- /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
- if ((AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem])) == NULL)
+ 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");
/* Add the listed attributes for the found UUID to the response */
- *TotalResponseSize += SDP_AddListedAttributesToResponse(AttributeTable, AttributeList, TotalAttributes,
+ *TotalResponseSize += SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes,
&CurrResponsePos);
}
(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);
static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t* AttributeTable, uint16_t AttributeList[][2],
const uint8_t TotalAttributes, void** const BufferPos)
{
- uint16_t TotalResponseSize = 0;
+ uint16_t TotalResponseSize;
/* Add an inner Data Element Sequence header for the current services's found Attributes */
- uint16_t* AttributeListSize = SDP_AddDataElementHeader16(BufferPos, SDP_DATATYPE_Sequence);
+ 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++)
void* AttributeValue;
/* Look through the current service's attribute list, examining all the attributes */
- while ((AttributeValue = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
+ 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]))
{
- BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
-
/* Increment the current UUID's returned Attribute container size by the number of added bytes */
*AttributeListSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, BufferPos);
}
AttributeTable++;
}
-
- /* Increment the outer container size by the number of added bytes */
- TotalResponseSize += 3 + *AttributeListSize;
}
+ /* 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);
{
/* 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 */
SDP_WriteData8(ResponseBuffer, (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit));
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)
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;
+ /* 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);
- /* Retrieve the list of the service's Class UUIDs from its Attribute table */
- void* ClassUUIDs = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICECLASSIDS);
+ 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;
-
- /* 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, &((ItemUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
- return CurrAttributeTable;
-
- ClassUUIDListSize -= sizeof(ItemUUID_t);
- ClassUUIDs += sizeof(ItemUUID_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
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 */
- memcpy(&CurrentUUID[UUID_SIZE_BYTES - UUIDLength], *CurrentParameter, UUIDLength);
+ if (UUIDLength <= 4)
+ {
+ /* 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
+ {
+ /* 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[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]);
+ 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;
switch (SizeIndex)
{
case SDP_DATASIZE_Variable8Bit:
- ElementValueSize = SDP_ReadData8(DataElementHeader);
*ElementHeaderSize = (1 + sizeof(uint8_t));
+ ElementValueSize = SDP_ReadData8(DataElementHeader);
break;
case SDP_DATASIZE_Variable16Bit:
- ElementValueSize = SDP_ReadData16(DataElementHeader);
*ElementHeaderSize = (1 + sizeof(uint16_t));
+ ElementValueSize = SDP_ReadData16(DataElementHeader);
break;
case SDP_DATASIZE_Variable32Bit:
- ElementValueSize = SDP_ReadData32(DataElementHeader);
*ElementHeaderSize = (1 + sizeof(uint32_t));
+ ElementValueSize = SDP_ReadData32(DataElementHeader);
break;
default:
- ElementValueSize = (1 << SizeIndex);
*ElementHeaderSize = 1;
+ ElementValueSize = (1 << SizeIndex);
break;
}
projects / pub / USBasp.git / blobdiff