From 26421b38cd59191cd42d8e6f0d1499c3775e75d7 Mon Sep 17 00:00:00 2001
From: Dean Camera <dean@fourwalledcubicle.com>
Date: Sun, 11 Apr 2010 02:45:42 +0000
Subject: [PATCH] Fix up Bluetooth Demo's options processing on incoming
channel configuration requests.
---
.../Incomplete/BluetoothHost/BluetoothHost.c | 4 +-
.../BluetoothHost/Lib/BluetoothACLPackets.c | 41 ++++++++++---------
2 files changed, 23 insertions(+), 22 deletions(-)
diff --git a/Demos/Host/Incomplete/BluetoothHost/BluetoothHost.c b/Demos/Host/Incomplete/BluetoothHost/BluetoothHost.c
index fbacc2537..d2bd07589 100644
--- a/Demos/Host/Incomplete/BluetoothHost/BluetoothHost.c
+++ b/Demos/Host/Incomplete/BluetoothHost/BluetoothHost.c
@@ -251,8 +251,8 @@ void Bluetooth_PacketReceived(uint16_t* PacketLength, Bluetooth_Channel_t* Chann
Pipe_Read_Stream_LE(&DataPayload, *PacketLength);
*PacketLength = 0;
- printf_P(PSTR("L2CAP Packet Recetion on channel %02X:\r\n"), Channel->LocalNumber);
- for (uint16_t Byte = 0; Byte < *PacketLength; Byte++)
+ printf_P(PSTR("Packet Received (Channel 0x%04X, PSM: 0x%02x):\r\n"), Channel->LocalNumber, Channel->PSM);
+ for (uint16_t Byte = 0; Byte < sizeof(DataPayload); Byte++)
printf_P(PSTR("0x%02X "), DataPayload[Byte]);
puts_P(PSTR("\r\n"));
}
diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
index 50fba7c5d..df831fd0d 100644
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
@@ -360,34 +360,35 @@ static inline void Bluetooth_Signal_ConfigurationReq(BT_ACL_Header_t* ACL
BT_Signal_Header_t* SignalCommandHeader)
{
BT_Signal_ConfigurationReq_t ConfigurationRequest;
- uint8_t OptionsLen;
+ uint8_t OptionsLen = (SignalCommandHeader->Length - sizeof(ConfigurationRequest));
+ uint8_t Options[OptionsLen];
- Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest));
- OptionsLen = (DataHeader->PayloadLength - sizeof(*SignalCommandHeader));
-
- Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, false);
-
- while (OptionsLen)
- {
- BT_Config_Option_Header_t OptionHeader;
-
- Pipe_Read_Stream_LE(&OptionHeader, sizeof(OptionHeader));
-
- if ((OptionHeader.Type == BT_CONFIG_OPTION_MTU) && (ChannelData != NULL))
- Pipe_Read_Stream_LE(&ChannelData->RemoteMTU, sizeof(ChannelData->RemoteMTU));
- else
- Pipe_Discard_Stream(OptionHeader.Length);
-
- OptionsLen -= (sizeof(OptionHeader) + OptionHeader.Length);
- }
+ Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest));
+ Pipe_Read_Stream_LE(&Options, sizeof(Options));
Pipe_ClearIN();
Pipe_Freeze();
+ Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, false);
+
BT_ACL_DEBUG(1, "<< L2CAP Configuration Request", NULL);
BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ConfigurationRequest.DestinationChannel);
- BT_ACL_DEBUG(2, "-- Options Len: 0x%04X", (DataHeader->PayloadLength - sizeof(*SignalCommandHeader)));
BT_ACL_DEBUG(2, "-- Remote MTU: 0x%04X", ChannelData->RemoteMTU);
+ BT_ACL_DEBUG(2, "-- Options Len: 0x%04X", OptionsLen);
+
+ uint8_t OptionPos = 0;
+ while (OptionPos < OptionsLen)
+ {
+ BT_Config_Option_Header_t* OptionHeader = (BT_Config_Option_Header_t*)&Options[OptionPos];
+
+ BT_ACL_DEBUG(2, "-- Option Type: 0x%04X", OptionHeader->Type);
+ BT_ACL_DEBUG(2, "-- Option Length: 0x%04X", (sizeof(*OptionHeader) + OptionHeader->Length));
+
+ if ((OptionHeader->Type == BT_CONFIG_OPTION_MTU) && (ChannelData != NULL))
+ ChannelData->RemoteMTU = *((uint16_t*)&Options[OptionPos + sizeof(*OptionHeader)]);
+
+ OptionPos += (sizeof(*OptionHeader) + OptionHeader->Length);
+ }
struct
{
--
GitLab