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