X-Git-Url: http://git.linex4red.de/pub/lufa.git/blobdiff_plain/559af022041942be21d7cabd398639f4d6db52ea..2eff731ecfbcfec4f3152992e5ae5602a3694424:/Demos/Host/Incomplete/BluetoothHost/Lib/RFCOMMControl.c

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/RFCOMMControl.c b/Demos/Host/Incomplete/BluetoothHost/Lib/RFCOMMControl.c
index 3a6bf67c5..1d1863348 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/RFCOMMControl.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/RFCOMMControl.c
@@ -47,7 +47,7 @@ void RFCOMM_ProcessControlCommand(const uint8_t* Command, Bluetooth_Channel_t* c
 	switch (CommandHeader->Command)
 	{
 		case RFCOMM_Control_Test:
-			RFCOMM_ProcessTestCommand(CommandHeader, CommandData, Channel);
+			RFCOMM_ProcessTestCommand(CommandHeader, CommandDataLen, CommandData, Channel);
 			break;
 		case RFCOMM_Control_FlowControlEnable:
 			RFCOMM_ProcessFCECommand(CommandHeader, CommandData, Channel);
@@ -73,10 +73,29 @@ void RFCOMM_ProcessControlCommand(const uint8_t* Command, Bluetooth_Channel_t* c
 	}
 }
 
-static void RFCOMM_ProcessTestCommand(const RFCOMM_Command_t* const CommandHeader, const uint8_t* CommandData,
-			                          Bluetooth_Channel_t* const Channel)
+static void RFCOMM_ProcessTestCommand(const RFCOMM_Command_t* const CommandHeader, const uint8_t CommandDataLen,
+                                      const uint8_t* CommandData, Bluetooth_Channel_t* const Channel)
 {
+	const uint8_t* Params = (const uint8_t*)CommandData;
+
 	BT_RFCOMM_DEBUG(1, "<< TEST Command");
+	
+	struct
+	{
+		RFCOMM_Command_t CommandHeader;
+		uint8_t          Length;
+		uint8_t          TestData[CommandDataLen];
+	} TestResponse;
+
+	/* Fill out the Test response data */
+	TestResponse.CommandHeader = (RFCOMM_Command_t){.Command = RFCOMM_Control_Test, .EA = true};
+	TestResponse.Length        = (CommandDataLen << 1) | 0x01;
+	memcpy(TestResponse.TestData, Params, CommandDataLen);
+	
+	BT_RFCOMM_DEBUG(1, ">> TEST Response");
+
+	/* Send the PDN response to acknowledge the command */
+	RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, false, RFCOMM_Frame_UIH, sizeof(TestResponse), &TestResponse, Channel);
 }
 
 static void RFCOMM_ProcessFCECommand(const RFCOMM_Command_t* const CommandHeader, const uint8_t* CommandData,
@@ -190,7 +209,6 @@ static void RFCOMM_ProcessDPNCommand(const RFCOMM_Command_t* const CommandHeader
 	/* Save the new channel configuration */
 	RFCOMMChannel->State       = RFCOMM_Channel_Open;
 	RFCOMMChannel->Priority    = Params->Priority;
-	RFCOMMChannel->UseUIFrames = (Params->FrameType != 0);
 	RFCOMMChannel->MTU         = Params->MaximumFrameSize;
 	
 	struct