Make bluetooth SDP code correctly unpack the search UUIDs and parameters.

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

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
index 3937a97..0d96542 100644 (file)
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
@@ -40,6 +40,11 @@ void ServiceDiscovery_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)
        BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU);\r
        BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength);\r
        \r
+       printf("\r\n");\r
+       for (uint8_t i = 0; i < SDPHeader->ParameterLength; i++)\r
+         printf("0x%02X ", *((uint8_t*)Data + sizeof(SDP_PDUHeader_t) + i));\r
+       printf("\r\n");\r
+\r
        switch (SDPHeader->PDU)\r
        {\r
                case SDP_PDU_SERVICESEARCHREQUEST:\r
@@ -66,60 +71,92 @@ static void ServiceDiscovery_ProcessServiceAttribute(SDP_PDUHeader_t* SDPHeader)
 \r
 static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPHeader)\r
 {\r
-       uint8_t* CurrentParameter = ((uint8_t*)SDPHeader + sizeof(SDP_PDUHeader_t));\r
+       void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));\r
        \r
        BT_SDP_DEBUG(1, "<< Service Search Attribute", NULL);\r
+       \r
+       uint8_t ElementHeaderSize;\r
 \r
-       uint8_t ServicePatternLength = ServiceDiscovery_GetDataElementSize(CurrentParameter);\r
+       uint16_t ServicePatternLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);\r
+       BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);\r
        while (ServicePatternLength)\r
        {\r
-               uint8_t UUIDLength = ServiceDiscovery_GetDataElementSize(CurrentParameter);\r
+               uint8_t UUIDLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);\r
                uint8_t UUID[16];\r
                \r
                memset(UUID, 0x00, sizeof(UUID));\r
-               memcpy(&UUID[sizeof(UUID) - UUIDLength], CurrentParameter, UUIDLength);\r
+               memcpy(UUID, CurrentParameter, UUIDLength);\r
+               CurrentParameter += UUIDLength;\r
                \r
-               BT_SDP_DEBUG(2, "-- UUID: 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",\r
-                               UUID[0], UUID[1], UUID[2],  UUID[3],  UUID[4],  UUID[5],  UUID[6],  UUID[7],\r
-                                               UUID[8], UUID[9], UUID[10], UUID[11], UUID[12], UUID[13], UUID[14], UUID[15]);\r
+               BT_SDP_DEBUG(2, "-- UUID (%d): 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",\r
+                                               UUIDLength,\r
+                               UUID[15], UUID[14], UUID[13], UUID[12], UUID[11], UUID[10], UUID[9], UUID[8],\r
+                                               UUID[7],  UUID[6],  UUID[5],  UUID[4],  UUID[3],  UUID[2],  UUID[1], UUID[0]);\r
        \r
-               ServicePatternLength -= UUIDLength;\r
+               ServicePatternLength -= (UUIDLength + ElementHeaderSize);\r
        }\r
        \r
-       uint16_t MaxAttributeSize = ServiceDiscovery_Read16BitParameter(CurrentParameter);\r
+       uint16_t MaxAttributeSize = ServiceDiscovery_Read16BitParameter(&CurrentParameter);\r
+       BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);\r
 \r
-       uint8_t AttributeIDListLength = ServiceDiscovery_GetDataElementSize(CurrentParameter);\r
+       uint16_t AttributeIDListLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);\r
+       BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);\r
        while (AttributeIDListLength)\r
        {\r
-               uint8_t AttributeLength = ServiceDiscovery_GetDataElementSize(CurrentParameter);\r
+               uint8_t  AttributeLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);\r
+               uint32_t Attribute = 0;\r
                \r
-               BT_SDP_DEBUG(2, "-- Attribute Length: 0x%04X", AttributeLength);\r
+               memcpy(&Attribute, CurrentParameter, AttributeLength);\r
+               CurrentParameter += AttributeLength;\r
+               \r
+               BT_SDP_DEBUG(2, "-- Attribute(%d): 0x%08lX", AttributeLength, Attribute);\r
        \r
-               AttributeIDListLength -= AttributeLength;\r
+               AttributeIDListLength -= (AttributeLength + ElementHeaderSize);\r
        }\r
 }\r
 \r
-static uint32_t ServiceDiscovery_GetDataElementSize(void* DataElementHeader)\r
+static uint32_t ServiceDiscovery_GetDataElementSize(void** DataElementHeader, uint8_t* ElementHeaderSize)\r
 {\r
-       uint8_t SizeIndex = (*((uint8_t*)DataElementHeader++) & 0x07);\r
-\r
+       uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);\r
+       *DataElementHeader += sizeof(uint8_t);\r
+       \r
+       *ElementHeaderSize = 1;\r
+       \r
+       uint32_t ElementValue;\r
+       \r
        switch (SizeIndex)\r
        {\r
                case 0:\r
-                       return 1;\r
+                       ElementValue = 1;\r
+                       break;\r
                case 1:\r
-                       return 2;\r
+                       ElementValue = 2;\r
+                       break;\r
                case 2:\r
-                       return 4;\r
+                       ElementValue = 4;\r
+                       break;\r
                case 3:\r
-                       return 8;\r
+                       ElementValue = 8;\r
+                       break;\r
                case 4:\r
-                       return 16;\r
+                       ElementValue = 16;\r
+                       break;\r
                case 5:\r
-                       return *((uint8_t*)DataElementHeader++);\r
+                       ElementValue = *((uint8_t*)*DataElementHeader);\r
+                       *DataElementHeader += sizeof(uint8_t);\r
+                       *ElementHeaderSize  = (1 + sizeof(uint8_t));\r
+                       break;\r
                case 6:\r
-                       return *((uint16_t*)DataElementHeader++);\r
+                       ElementValue = *((uint16_t*)*DataElementHeader);\r
+                       *DataElementHeader += sizeof(uint16_t);\r
+                       *ElementHeaderSize  = (1 + sizeof(uint16_t));\r
+                       break;\r
                default:\r
-                       return *((uint32_t*)DataElementHeader++);\r
+                       ElementValue = *((uint32_t*)*DataElementHeader);\r
+                       *DataElementHeader += sizeof(uint32_t);\r
+                       *ElementHeaderSize  = (1 + sizeof(uint32_t));\r
+                       break;\r
        }\r
+       \r
+       return ElementValue;\r
 }\r