Fix up Bluetooth Demo's options processing on incoming channel configuration requests.

diff --git a/Demos/Host/Incomplete/BluetoothHost/BluetoothHost.c b/Demos/Host/Incomplete/BluetoothHost/BluetoothHost.c
index fbacc25..d2bd075 100644 (file)
--- 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);\r
        *PacketLength = 0;\r
 \r
-       printf_P(PSTR("L2CAP Packet Recetion on channel %02X:\r\n"), Channel->LocalNumber);\r
-       for (uint16_t Byte = 0; Byte < *PacketLength; Byte++)\r
+       printf_P(PSTR("Packet Received (Channel 0x%04X, PSM: 0x%02x):\r\n"), Channel->LocalNumber, Channel->PSM);\r
+       for (uint16_t Byte = 0; Byte < sizeof(DataPayload); Byte++)\r
          printf_P(PSTR("0x%02X "), DataPayload[Byte]);\r
        puts_P(PSTR("\r\n"));\r
 }\r

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
index 50fba7c..df831fd 100644 (file)
--- 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)\r
 {\r
        BT_Signal_ConfigurationReq_t ConfigurationRequest;\r
-       uint8_t OptionsLen;\r
+       uint8_t OptionsLen = (SignalCommandHeader->Length - sizeof(ConfigurationRequest));\r
+       uint8_t Options[OptionsLen];\r
 \r
-       Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest));\r
-       OptionsLen = (DataHeader->PayloadLength - sizeof(*SignalCommandHeader));\r
-       \r
-       Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, false);\r
-\r
-       while (OptionsLen)\r
-       {\r
-               BT_Config_Option_Header_t OptionHeader;\r
-               \r
-               Pipe_Read_Stream_LE(&OptionHeader, sizeof(OptionHeader));\r
-       \r
-               if ((OptionHeader.Type == BT_CONFIG_OPTION_MTU) && (ChannelData != NULL))\r
-                 Pipe_Read_Stream_LE(&ChannelData->RemoteMTU, sizeof(ChannelData->RemoteMTU));\r
-               else\r
-                 Pipe_Discard_Stream(OptionHeader.Length);\r
-\r
-               OptionsLen -= (sizeof(OptionHeader) + OptionHeader.Length);\r
-       }\r
+       Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest));       \r
+       Pipe_Read_Stream_LE(&Options, sizeof(Options));\r
 \r
        Pipe_ClearIN();\r
        Pipe_Freeze();\r
 \r
+       Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, false);\r
+\r
        BT_ACL_DEBUG(1, "<< L2CAP Configuration Request", NULL);\r
        BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ConfigurationRequest.DestinationChannel);\r
-       BT_ACL_DEBUG(2, "-- Options Len: 0x%04X", (DataHeader->PayloadLength - sizeof(*SignalCommandHeader)));\r
        BT_ACL_DEBUG(2, "-- Remote MTU: 0x%04X", ChannelData->RemoteMTU);\r
+       BT_ACL_DEBUG(2, "-- Options Len: 0x%04X", OptionsLen);\r
+\r
+       uint8_t OptionPos = 0;\r
+       while (OptionPos < OptionsLen)\r
+       {\r
+               BT_Config_Option_Header_t* OptionHeader = (BT_Config_Option_Header_t*)&Options[OptionPos];\r
+\r
+               BT_ACL_DEBUG(2, "-- Option Type: 0x%04X", OptionHeader->Type);\r
+               BT_ACL_DEBUG(2, "-- Option Length: 0x%04X", (sizeof(*OptionHeader) + OptionHeader->Length));\r
+               \r
+               if ((OptionHeader->Type == BT_CONFIG_OPTION_MTU) && (ChannelData != NULL))\r
+                 ChannelData->RemoteMTU = *((uint16_t*)&Options[OptionPos + sizeof(*OptionHeader)]);\r
+\r
+               OptionPos += (sizeof(*OptionHeader) + OptionHeader->Length);\r
+       }\r
        \r
        struct\r
        {\r