Fixed MIDI_Host_Flush() not aborting early when the specified MIDI host interface...

[pub/USBasp.git] / Demos / Host / Incomplete / BluetoothHost / Lib / SDP.c

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/SDP.c b/Demos/Host/Incomplete/BluetoothHost/Lib/SDP.c
index 0daf8e2..19d42e2 100644 (file)
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/SDP.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/SDP.c
@@ -52,18 +52,12 @@ const ServiceAttributeTable_t* SDP_Services_Table[] PROGMEM =
 /** Base UUID value common to all standardized Bluetooth services */
 const UUID_t BaseUUID PROGMEM = {0x00000000, BASE_80BIT_UUID};
 
 /** Base UUID value common to all standardized Bluetooth services */
 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
+/** Main Service Discovery Protocol packet processing routine. This function processes incoming SDP packets from

  *  a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the
  *  services the local device exposes.
  *
  *  a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the
  *  services the local device exposes.
  *

- *  \param[in]  Data     Incomming packet data containing the SDP request
- *  \param[in]  Channel  Channel the request was issued to by the remote device
+ *  \param[in] Data     Incoming packet data containing the SDP request
+ *  \param[in] Channel  ACL channel the request was issued to by the remote device

  */
 void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
 {
  */
 void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
 {


@@ -94,9 +88,10 @@ void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
  *  \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
  */
  *  \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)
+static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader,
+                                     Bluetooth_Channel_t* const Channel)

 {
 {

-       const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
+       const void* CurrentParameter = ((const void*)SDPHeader + sizeof(SDP_PDUHeader_t));

 
        BT_SDP_DEBUG(1, "<< Service Search");
 
 
        BT_SDP_DEBUG(1, "<< Service Search");
 


@@ -105,7 +100,7 @@ static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Blu
        uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
        BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
        
        uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
        BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
        

-       /* Retrieve the maximum service record reponse count from the request */
+       /* 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);
        
        uint16_t MaxServiceRecordCount = SDP_ReadData16(&CurrentParameter);
        BT_SDP_DEBUG(2, "-- Max Return Service Count: 0x%04X", MaxServiceRecordCount);
        


@@ -175,9 +170,10 @@ static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Blu
  *  \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
  */
  *  \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)
+static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader,
+                                        Bluetooth_Channel_t* const Channel)

 {
 {

-       const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
+       const void* CurrentParameter = ((const void*)SDPHeader + sizeof(SDP_PDUHeader_t));

 
        BT_SDP_DEBUG(1, "<< Service Attribute");
 
 
        BT_SDP_DEBUG(1, "<< Service Attribute");
 


@@ -185,7 +181,7 @@ static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader,
        uint32_t ServiceHandle = SDP_ReadData32(&CurrentParameter);
        BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
        
        uint32_t ServiceHandle = SDP_ReadData32(&CurrentParameter);
        BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
        

-       /* Retrieve the maximum Attribute reponse size from the request */
+       /* 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);
        
        uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
        BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
        


@@ -265,9 +261,10 @@ static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader,
  *  \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
  */
  *  \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)
+static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHeader,
+                                              Bluetooth_Channel_t* const Channel)

 {
 {

-       const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
+       const void* CurrentParameter = ((const void*)SDPHeader + sizeof(SDP_PDUHeader_t));

        
        BT_SDP_DEBUG(1, "<< Service Search Attribute");
 
        
        BT_SDP_DEBUG(1, "<< Service Search Attribute");
 


@@ -276,7 +273,7 @@ static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHe
        uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
        BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
        
        uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
        BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
        

-       /* Retrieve the maximum Attribute reponse size from the request */
+       /* 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);
        
        uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
        BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
        


@@ -354,8 +351,10 @@ static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHe
  *
  *  \return Number of bytes added to the output buffer
  */
  *
  *  \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)
+static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t* AttributeTable,
+                                                  uint16_t AttributeList[][2],
+                                                  const uint8_t TotalAttributes,
+                                                  void** const BufferPos)

 {
        uint16_t TotalResponseSize;
 
 {
        uint16_t TotalResponseSize;
 


@@ -394,7 +393,7 @@ static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t*
        return TotalResponseSize;
 }
 
        return TotalResponseSize;
 }
 

-/** Adds the given attribute ID and value to the reponse buffer, and advances the response buffer pointer past the added data.
+/** 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] AttributeID          Attribute ID to add to the response buffer
  *  \param[in] AttributeValue       Pointer to the start of the Attribute's value, located in PROGMEM


@@ -402,7 +401,9 @@ static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t*
  *
  *  \return Number of bytes added to the response buffer
  */
  *
  *  \return Number of bytes added to the response buffer
  */

-static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const void* AttributeValue, void** ResponseBuffer)
+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;
 {
        /* Retrieve the size of the attribute value from its container header */
        uint8_t  AttributeHeaderLength;


@@ -430,7 +431,8 @@ static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const voi
  *
  *  \return Pointer to the start of the Attribute's value if found within the table, NULL otherwise
  */
  *
  *  \return Pointer to the start of the Attribute's value if found within the table, NULL otherwise
  */

-static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable, const uint16_t AttributeID)
+static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable,
+                                   const uint16_t AttributeID)

 {
        void* CurrTableItemData;
        
 {
        void* CurrTableItemData;
        


@@ -455,46 +457,42 @@ static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable
  *
  *  \return True if all the UUIDs given in the UUID list appear in the given attribute table, false otherwise
  */
  *
  *  \return True if all the UUIDs given in the UUID list appear in the given attribute table, false otherwise
  */

-static bool SDP_SearchServiceTable(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs,
+static bool SDP_SearchServiceTable(uint8_t UUIDList[][UUID_SIZE_BYTES],
+                                   const uint8_t TotalUUIDs,

                                               const ServiceAttributeTable_t* CurrAttributeTable)
 {
                                               const ServiceAttributeTable_t* CurrAttributeTable)
 {

-       bool UUIDMatch[TotalUUIDs];     
-       
-       /* Set all the match flags to false (not matched) before starting the search */
-       memset(UUIDMatch, false, sizeof(UUIDMatch));
-

        const void* CurrAttribute;
        const void* CurrAttribute;

+       uint16_t    UUIDMatchFlags = 0;

        
        /* Search through the current attribute table, checking each attribute value for UUID matches */
        while ((CurrAttribute = pgm_read_ptr(&CurrAttributeTable->Data)) != NULL)
        {
        
        /* Search through the current attribute table, checking each attribute value for UUID matches */
        while ((CurrAttribute = pgm_read_ptr(&CurrAttributeTable->Data)) != NULL)
        {

-               SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, UUIDMatch, CurrAttribute);
+               SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, &UUIDMatchFlags, CurrAttribute);

                CurrAttributeTable++;
        }
 
        /* Determine how many UUID matches in the list we have found */
                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++;
-       }
+       uint8_t UUIDMatches;
+       for (UUIDMatches = 0; UUIDMatchFlags; UUIDMatches++)
+         UUIDMatchFlags &= (UUIDMatchFlags - 1);

        
        /* If all UUIDs have been matched to the current service, return true */
        return (UUIDMatches == TotalUUIDs);
 }
 
        
        /* 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
+/** 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.
  *
  *  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
+ *  \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
  */
  *
  *  \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[],
+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);
                                const void* CurrAttribute)
 {
        uint8_t CurrAttributeType = (pgm_read_byte(CurrAttribute) & ~0x07);


@@ -502,16 +500,20 @@ static void SDP_CheckUUIDMatch(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_
        /* Check the data type of the current attribute value - if UUID, compare, if Sequence, unwrap and recurse */
        if (CurrAttributeType == SDP_DATATYPE_UUID)
        {
        /* 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 */
                /* 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)))
+                       if (!(*UUIDMatchFlags & CurrUUIDMatchMask) && !(memcmp_P(UUIDList[i], (CurrAttribute + 1), UUID_SIZE_BYTES)))

                        {
                                /* Indicate match found for the current attribute UUID and early-abort */
                        {
                                /* Indicate match found for the current attribute UUID and early-abort */

-                               UUIDMatch[i] = true;
+                               *UUIDMatchFlags |= CurrUUIDMatchMask;

                                break;
                        }
                                break;
                        }

+                       
+                       CurrUUIDMatchMask <<= 1;

                }
        }
        else if (CurrAttributeType == SDP_DATATYPE_Sequence)
                }
        }
        else if (CurrAttributeType == SDP_DATATYPE_Sequence)


@@ -527,8 +529,10 @@ static void SDP_CheckUUIDMatch(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_
                        uint8_t  InnerHeaderSize;
                        uint16_t InnerSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &InnerHeaderSize);
                        
                        uint8_t  InnerHeaderSize;
                        uint16_t InnerSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &InnerHeaderSize);
                        

-                       SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, UUIDMatch, CurrAttribute);
-                                               
+                       /* 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;
                }
                        SequenceSize  -= InnerHeaderSize + InnerSize;
                        CurrAttribute += InnerHeaderSize + InnerSize;
                }


@@ -543,7 +547,8 @@ static void SDP_CheckUUIDMatch(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_
  *
  *  \return Total number of Attribute ranges stored in the Data Element Sequence
  */
  *
  *  \return Total number of Attribute ranges stored in the Data Element Sequence
  */

-static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2], const void** const CurrentParameter)
+static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2],
+                                    const void** const CurrentParameter)

 {
        uint8_t ElementHeaderSize;
        uint8_t TotalAttributes = 0;
 {
        uint8_t ElementHeaderSize;
        uint8_t TotalAttributes = 0;


@@ -585,7 +590,8 @@ static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2], const void** co
  *
  *  \return Total number of UUIDs stored in the Data Element Sequence
  */
  *
  *  \return Total number of UUIDs stored in the Data Element Sequence
  */

-static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** const CurrentParameter)
+static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES],
+                               const void** const CurrentParameter)

 {
        uint8_t ElementHeaderSize;
        uint8_t TotalUUIDs = 0;
 {
        uint8_t ElementHeaderSize;
        uint8_t TotalUUIDs = 0;


@@ -636,7 +642,8 @@ static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void**
  *
  *  \return Size in bytes of the entire attribute container, including the header
  */
  *
  *  \return Size in bytes of the entire attribute container, including the header
  */

-static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
+static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData,
+                                                   uint8_t* const HeaderSize)

 {
        /* Fetch the size of the Data Element structure from the header */
        uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 {
        /* Fetch the size of the Data Element structure from the header */
        uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);


@@ -675,7 +682,8 @@ static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeDa
  *
  *  \return Size in bytes of the Data Element container's contents, minus the header
  */
  *
  *  \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)
+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); 
 {
        /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
        uint8_t  SizeIndex = (SDP_ReadData8(DataElementHeader) & 0x07);