Rename FunctionAttributes.h to Attributes.h, as some attributes are applicable to...

[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 1ee4c84..124b129 100644 (file)
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c
@@ -28,14 +28,98 @@
   this software.\r
 */\r
 \r
+#define  INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C\r
 #include "ServiceDiscoveryProtocol.h"\r
 \r
-void ServiceDiscovery_ProcessPacket(void* Data, uint16_t Length, Bluetooth_Channel_t* Channel)\r
+void ServiceDiscovery_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)\r
 {\r
        SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data;\r
        SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength);\r
 \r
-       BT_SDP_DEBUG(1, "<< Service Discovery Packet", NULL);\r
+       BT_SDP_DEBUG(1, "SDP Packet Received", NULL);\r
        BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU);\r
        BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength);\r
+       \r
+       switch (SDPHeader->PDU)\r
+       {\r
+               case SDP_PDU_SERVICESEARCHREQUEST:\r
+                       ServiceDiscovery_ProcessServiceSearch(SDPHeader);\r
+                       break;          \r
+               case SDP_PDU_SERVICEATTRIBUTEREQUEST:\r
+                       ServiceDiscovery_ProcessServiceAttribute(SDPHeader);\r
+                       break;\r
+               case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:\r
+                       ServiceDiscovery_ProcessServiceSearchAttribute(SDPHeader);\r
+                       break;\r
+       }\r
+}\r
+\r
+static void ServiceDiscovery_ProcessServiceSearch(SDP_PDUHeader_t* SDPHeader)\r
+{\r
+       BT_SDP_DEBUG(1, "<< Service Search", NULL);\r
+}\r
+\r
+static void ServiceDiscovery_ProcessServiceAttribute(SDP_PDUHeader_t* SDPHeader)\r
+{\r
+       BT_SDP_DEBUG(1, "<< Service Attribute", NULL);\r
+}\r
+\r
+static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPHeader)\r
+{\r
+       uint8_t* CurrentParameter = ((uint8_t*)SDPHeader + sizeof(SDP_PDUHeader_t));\r
+       \r
+       BT_SDP_DEBUG(1, "<< Service Search Attribute", NULL);\r
+\r
+       uint8_t ServicePatternLength = ServiceDiscovery_GetDataElementSize(CurrentParameter);\r
+       while (ServicePatternLength)\r
+       {\r
+               uint8_t UUIDLength = ServiceDiscovery_GetDataElementSize(CurrentParameter);\r
+               uint8_t UUID[16];\r
+               \r
+               memset(UUID, 0x00, sizeof(UUID));\r
+               memcpy(&UUID[sizeof(UUID) - UUIDLength], 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
+       \r
+               ServicePatternLength -= UUIDLength;\r
+       }\r
+       \r
+       uint16_t MaxAttributeSize = ServiceDiscovery_Read16BitParameter(CurrentParameter);\r
+\r
+       uint8_t AttributeIDListLength = ServiceDiscovery_GetDataElementSize(CurrentParameter);\r
+       while (AttributeIDListLength)\r
+       {\r
+               uint8_t AttributeLength = ServiceDiscovery_GetDataElementSize(CurrentParameter);\r
+               \r
+               BT_SDP_DEBUG(2, "-- Attribute Length: 0x%04X", AttributeLength);\r
+       \r
+               AttributeIDListLength -= AttributeLength;\r
+       }\r
+}\r
+\r
+static uint32_t ServiceDiscovery_GetDataElementSize(void* DataElementHeader)\r
+{\r
+       uint8_t SizeIndex = (*((uint8_t*)DataElementHeader++) & 0x07);\r
+\r
+       switch (SizeIndex)\r
+       {\r
+               case 0:\r
+                       return 1;\r
+               case 1:\r
+                       return 2;\r
+               case 2:\r
+                       return 4;\r
+               case 3:\r
+                       return 8;\r
+               case 4:\r
+                       return 16;\r
+               case 5:\r
+                       return *((uint8_t*)DataElementHeader++);\r
+               case 6:\r
+                       return *((uint16_t*)DataElementHeader++);\r
+               default:\r
+                       return *((uint32_t*)DataElementHeader++);\r
+       }\r
 }\r