Make sure that the NVM bus/controller busy waits in the AVRISP MKII clone project...

[pub/USBasp.git] / Demos / Host / Incomplete / BluetoothHost / Lib / BluetoothACLPackets.c

diff --git a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
index 3292a53..8c417ff 100644 (file)
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
@@ -28,16 +28,90 @@
   this software.\r
 */\r
 \r
   this software.\r
 */\r
 \r

+/*\r
+       TODO: Make SendPacket respect receiver's MTU\r
+       TODO: Make ReceivePacket stitch together MTU fragments (?)\r
+       TODO: Add channel opened/closed callbacks\r
+ */\r
+\r

 #define  INCLUDE_FROM_BLUETOOTH_ACLPACKETS_C\r
 #include "BluetoothACLPackets.h"\r
 \r
 #define  INCLUDE_FROM_BLUETOOTH_ACLPACKETS_C\r
 #include "BluetoothACLPackets.h"\r
 \r

-void Bluetooth_ProcessACLPackets(void)\r
+/** Bluetooth ACL processing task. This task should be called repeatedly the main Bluetooth\r
+ *  stack task to manage the ACL processing state.\r
+ */\r
+void Bluetooth_ACLTask(void)\r

 {\r
 {\r

-       Bluetooth_ACL_Header_t ACLPacketHeader;\r
-       Bluetooth_DataPacket_Header_t DataHeader;\r
+       /* Process incomming ACL packets, if any */\r
+       Bluetooth_ProcessIncommingACLPackets();\r
+       \r
+       /* Check for any half-open channels, send configuration details to the remote device if found */\r
+       for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++)\r
+       {\r
+               Bluetooth_Channel_t* ChannelData = &Bluetooth_Connection.Channels[i];\r
+       \r
+               bool MustSendConfigReq = true;\r
+       \r
+               /* Check if we are in a channel state which requires a configuration request to be sent */\r
+               switch (ChannelData->State)\r
+               {\r
+                       case Channel_Config_WaitConfig:\r
+                               ChannelData->State = Channel_Config_WaitReqResp;\r
+                               break;\r
+                       case Channel_Config_WaitSendConfig:\r
+                               ChannelData->State = Channel_Config_WaitResp;\r
+                               break;\r
+                       default:\r
+                               MustSendConfigReq  = false;\r
+                               break;\r
+               }\r
+               \r
+               /* Only send a configuration request if it the channel was in a state which required it */\r
+               if (MustSendConfigReq)\r
+               {\r
+                       struct\r
+                       {\r
+                               BT_Signal_Header_t           SignalCommandHeader;\r
+                               BT_Signal_ConfigurationReq_t ConfigurationRequest;\r
+                               \r
+                               struct\r
+                               {\r
+                                       BT_Config_Option_Header_t Header;\r
+                                       uint16_t Value;\r
+                               } Option_LocalMTU;\r
+                       } PacketData;\r
+                       \r
+                       /* Fill out the Signal Command header in the response packet */\r
+                       PacketData.SignalCommandHeader.Code            = BT_SIGNAL_CONFIGURATION_REQUEST;\r
+                       PacketData.SignalCommandHeader.Identifier      = ++Bluetooth_Connection.SignallingIdentifier;\r
+                       PacketData.SignalCommandHeader.Length          = sizeof(PacketData.ConfigurationRequest) +\r
+                                                                        sizeof(PacketData.Option_LocalMTU);\r
+\r
+                       /* Fill out the Configuration Request in the response packet, including local MTU information */\r
+                       PacketData.ConfigurationRequest.DestinationChannel = ChannelData->RemoteNumber;\r
+                       PacketData.ConfigurationRequest.Flags          = 0;\r
+                       PacketData.Option_LocalMTU.Header.Type         = BT_CONFIG_OPTION_MTU;\r
+                       PacketData.Option_LocalMTU.Header.Length       = sizeof(PacketData.Option_LocalMTU.Value);\r
+                       PacketData.Option_LocalMTU.Value               = ChannelData->LocalMTU;\r
+\r
+                       Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL);\r
+                       \r
+                       BT_ACL_DEBUG(1, ">> L2CAP Configuration Request");\r
+                       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", PacketData.ConfigurationRequest.DestinationChannel);\r
+               }\r
+       }\r
+}\r
+\r
+/** Incomming ACL packet processing task. This task is called by the main ACL processing task to read in and process\r
+ *  any incomming ACL packets to the device, handling signal requests as they are received or passing along channel\r
+ *  data to the user application.\r
+ */\r
+static void Bluetooth_ProcessIncommingACLPackets(void)\r
+{\r
+       BT_ACL_Header_t        ACLPacketHeader;\r
+       BT_DataPacket_Header_t DataHeader;\r

 \r
        Pipe_SelectPipe(BLUETOOTH_DATA_IN_PIPE);\r
 \r
        Pipe_SelectPipe(BLUETOOTH_DATA_IN_PIPE);\r

-       Pipe_SetPipeToken(PIPE_TOKEN_IN);\r

        Pipe_Unfreeze();\r
        \r
        if (!(Pipe_IsReadWriteAllowed()))\r
        Pipe_Unfreeze();\r
        \r
        if (!(Pipe_IsReadWriteAllowed()))\r


@@ -46,38 +120,65 @@ void Bluetooth_ProcessACLPackets(void)
                return;\r
        }\r
          \r
                return;\r
        }\r
          \r

+       /* Read in the received ACL packet headers when it has been discovered that a packet has been received */\r

        Pipe_Read_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader));\r
        Pipe_Read_Stream_LE(&DataHeader, sizeof(DataHeader));\r
 \r
        Pipe_Read_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader));\r
        Pipe_Read_Stream_LE(&DataHeader, sizeof(DataHeader));\r
 \r

-       BT_DEBUG("(ACL) Packet Received", NULL);\r
-       BT_DEBUG("(ACL) -- Connection Handle: 0x%04X", ACLPacketHeader.ConnectionHandle);\r
-       BT_DEBUG("(ACL) -- Data Length: 0x%04X", ACLPacketHeader.DataLength);\r
-       BT_DEBUG("(ACL) -- Destination Channel: 0x%04X", DataHeader.DestinationChannel);\r
-       BT_DEBUG("(ACL) -- Payload Length: 0x%04X", DataHeader.PayloadLength);\r
+       BT_ACL_DEBUG(2, "");\r
+       BT_ACL_DEBUG(2, "Packet Received");\r
+       BT_ACL_DEBUG(2, "-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF));\r
+       BT_ACL_DEBUG(2, "-- Data Length: 0x%04X", ACLPacketHeader.DataLength);\r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DataHeader.DestinationChannel);\r
+       BT_ACL_DEBUG(2, "-- Payload Length: 0x%04X", DataHeader.PayloadLength);\r

 \r
 \r

-       if (DataHeader.DestinationChannel == BLUETOOTH_CHANNEL_SIGNALING)\r
+       /* Check the packet's destination channel - signalling channel should be processed by the stack internally */\r
+       if (DataHeader.DestinationChannel == BT_CHANNEL_SIGNALING)\r

        {\r
        {\r

-               Bluetooth_SignalCommand_Header_t SignalCommandHeader;\r
+               /* Read in the Signal Command header of the incomming packet */\r
+               BT_Signal_Header_t SignalCommandHeader;\r

                Pipe_Read_Stream_LE(&SignalCommandHeader, sizeof(SignalCommandHeader));\r
                Pipe_Read_Stream_LE(&SignalCommandHeader, sizeof(SignalCommandHeader));\r

-\r
+               \r
+               /* Dispatch to the appropriate handler function based on the Signal message code */\r

                switch (SignalCommandHeader.Code)\r
                {\r
                switch (SignalCommandHeader.Code)\r
                {\r

-                       case BLUETOOTH_SIGNAL_CONNECTION_REQUEST:\r
-                               Bluetooth_ProcessSignalPacket_ConnectionRequest(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);\r
+                       case BT_SIGNAL_CONNECTION_REQUEST:\r
+                               Bluetooth_Signal_ConnectionReq(&SignalCommandHeader);\r

                                break;\r
                                break;\r

-                       case BLUETOOTH_SIGNAL_CONFIGURATION_REQUEST:\r
-                               Bluetooth_ProcessSignalPacket_ConfigurationRequest(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);\r
+                       case BT_SIGNAL_CONNECTION_RESPONSE:\r
+                               Bluetooth_Signal_ConnectionResp(&SignalCommandHeader);\r

                                break;\r
                                break;\r

-                       case BLUETOOTH_SIGNAL_INFORMATION_REQUEST:\r
-                               BT_DEBUG("(ACL) -- Information Request, Discarded", NULL);\r
-\r
-                               Pipe_Discard_Stream(ACLPacketHeader.DataLength);\r
-                               Pipe_ClearIN();         \r
-                               Pipe_Freeze();\r
+                       case BT_SIGNAL_CONFIGURATION_REQUEST:\r
+                               Bluetooth_Signal_ConfigurationReq(&SignalCommandHeader);\r
+                               break;\r
+                       case BT_SIGNAL_CONFIGURATION_RESPONSE:\r
+                               Bluetooth_Signal_ConfigurationResp(&SignalCommandHeader);\r
+                               break;\r
+                       case BT_SIGNAL_DISCONNECTION_REQUEST:\r
+                               Bluetooth_Signal_DisconnectionReq(&SignalCommandHeader);\r
+                               break;\r
+                       case BT_SIGNAL_DISCONNECTION_RESPONSE:\r
+                               Bluetooth_Signal_DisconnectionResp(&SignalCommandHeader);\r
+                               break;\r
+                       case BT_SIGNAL_ECHO_REQUEST:\r
+                               Bluetooth_Signal_EchoReq(&SignalCommandHeader);\r
+                               break;\r
+                       case BT_SIGNAL_INFORMATION_REQUEST:\r
+                               Bluetooth_Signal_InformationReq(&SignalCommandHeader);\r
+                               break;\r
+                       case BT_SIGNAL_COMMAND_REJECT:\r
+                               BT_ACL_DEBUG(1, "<< Command Reject");\r
+                               \r
+                               uint16_t RejectReason;\r
+                               Pipe_Read_Stream_LE(&RejectReason, sizeof(RejectReason));\r
+                               Pipe_Discard_Stream(ACLPacketHeader.DataLength - sizeof(RejectReason));\r
+                               Pipe_ClearIN();\r
+                               Pipe_Freeze();                          \r
+                       \r
+                               BT_ACL_DEBUG(2, "-- Reason: %d", RejectReason);\r

                                break;\r
                        default:\r
                                break;\r
                        default:\r

-                               BT_DEBUG("(ACL) >> Unknown Signaling Command 0x%02X", SignalCommandHeader.Code);\r
-                                       \r
+                               BT_ACL_DEBUG(1, "<< Unknown Signaling Command 0x%02X", SignalCommandHeader.Code);\r
+       \r

                                Pipe_Discard_Stream(ACLPacketHeader.DataLength);\r
                                Pipe_ClearIN();         \r
                                Pipe_Freeze();\r
                                Pipe_Discard_Stream(ACLPacketHeader.DataLength);\r
                                Pipe_ClearIN();         \r
                                Pipe_Freeze();\r


@@ -86,121 +187,570 @@ void Bluetooth_ProcessACLPackets(void)
        }\r
        else\r
        {\r
        }\r
        else\r
        {\r

-               uint8_t DataPayload[DataHeader.PayloadLength];\r
-               Pipe_Read_Stream_LE(&DataPayload, sizeof(DataPayload));\r
-               DataHeader.PayloadLength = 0;\r
+               /* Non-signalling packet received, read in the packet contents and pass to the user application */\r
+               uint8_t PacketData[DataHeader.PayloadLength];\r
+               Pipe_Read_Stream_LE(PacketData, DataHeader.PayloadLength);\r
+               Pipe_ClearIN();\r
+               Pipe_Freeze();\r
+\r
+               Bluetooth_PacketReceived(PacketData, DataHeader.PayloadLength,\r
+                                        Bluetooth_GetChannelData(DataHeader.DestinationChannel, CHANNEL_SEARCH_LOCALNUMBER));\r
+       }\r
+}\r
+\r
+/** Sends a packet to the remote device on the specified channel.\r
+ *\r
+ * \param[in] Data     Pointer to a buffer where the data is to be sourced from\r
+ * \param[in] DataLen  Length of the data to send\r
+ * \param[in] Channel  Channel information structure containing the destination channel's information, NULL to send\r
+ *                     to the remote device's signalling channel\r
+ *\r
+ * \return A value from the \ref BT_SendPacket_ErrorCodes_t enum\r
+ */\r
+uint8_t Bluetooth_SendPacket(void* Data, const uint16_t DataLen, Bluetooth_Channel_t* const Channel)\r
+{\r
+       BT_ACL_Header_t        ACLPacketHeader;\r
+       BT_DataPacket_Header_t DataHeader;\r
+\r
+       /* A remote device must be connected before a packet transmission is attempted */\r
+       if (!(Bluetooth_Connection.IsConnected))\r
+         return BT_SENDPACKET_NotConnected;\r
+\r
+       /* If the destination channel is not the signalling channel and it is not currently fully open, abort */\r
+       if ((Channel != NULL) && (Channel->State != Channel_Open))\r
+         return BT_SENDPACKET_ChannelNotOpen;\r
+\r
+       /* Fill out the packet's header from the remote device connection information structure */\r
+       ACLPacketHeader.ConnectionHandle      = (Bluetooth_Connection.ConnectionHandle | BT_ACL_FIRST_AUTOFLUSH);\r
+       ACLPacketHeader.DataLength            = sizeof(DataHeader) + DataLen;\r
+       DataHeader.PayloadLength              = DataLen;\r
+       DataHeader.DestinationChannel         = (Channel == NULL) ? BT_CHANNEL_SIGNALING : Channel->RemoteNumber;\r
+\r
+       Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);\r
+       Pipe_Unfreeze();\r

        \r
        \r

-               BT_DEBUG("(ACL) -- Data Payload: ", NULL);\r
-               for (uint16_t B = 0; B < sizeof(DataPayload); B++)\r
-                 printf("0x%02X ", DataPayload[B]);\r
-               BT_DEBUG("", NULL);\r
+       /* Write the packet contents to the pipe so that it can be sent to the remote device */\r
+       Pipe_Write_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader));\r
+       Pipe_Write_Stream_LE(&DataHeader, sizeof(DataHeader));\r
+       Pipe_Write_Stream_LE(Data, DataLen);\r
+       Pipe_ClearOUT();\r
+       \r
+       Pipe_Freeze();\r
+       \r
+       BT_ACL_DEBUG(2, "");\r
+       BT_ACL_DEBUG(2, "Packet Sent");\r
+       BT_ACL_DEBUG(2, "-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF));\r
+       BT_ACL_DEBUG(2, "-- Data Length: 0x%04X", ACLPacketHeader.DataLength);\r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DataHeader.DestinationChannel);\r
+       BT_ACL_DEBUG(2, "-- Payload Length: 0x%04X", DataHeader.PayloadLength);\r

 \r
 \r

-               Pipe_Discard_Stream(ACLPacketHeader.DataLength);\r
-               Pipe_ClearIN();         \r
-               Pipe_Freeze();\r
+       return BT_SENDPACKET_NoError;\r
+}\r
+\r
+/** Opens a bluetooth channel to the currently connected remote device, so that data can be exchanged.\r
+ *\r
+ *  \note The channel is not immediately opened when this function returns - it must undergo a two way\r
+ *        connection and configuration process first as the main Bluetooth stack processing task is\r
+ *        repeatedly called. The returned channel is unusable by the user application until its State\r
+ *        element has progressed to the Open state.\r
+ *\r
+ *  \param[in] PSM  PSM of the service that the channel is to be opened for\r
+ *\r
+ *  \return Pointer to the channel information structure of the opened channel, or NULL if no free channels\r
+ */\r
+Bluetooth_Channel_t* Bluetooth_OpenChannel(const uint16_t PSM)\r
+{\r
+       Bluetooth_Channel_t* ChannelData = NULL;\r
+\r
+       /* Search through the channel information list for a free channel item */\r
+       for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++)\r
+       {\r
+               if (Bluetooth_Connection.Channels[i].State == Channel_Closed)\r
+               {\r
+                       ChannelData = &Bluetooth_Connection.Channels[i];\r
+                       \r
+                       /* Set the new channel structure's local channel number to a unique value within the connection orientated\r
+                          channel address space */\r
+                       ChannelData->LocalNumber = (BT_CHANNELNUMBER_BASEOFFSET + i);\r
+                       break;\r
+               }\r

        }\r
        }\r

+\r
+       /* If no free channel item was found in the list, all channels are occupied - abort */\r
+       if (ChannelData == NULL)\r
+         return NULL;\r
+\r
+       /* Reset and fill out the allocated channel's information structure with defaults */\r
+       ChannelData->RemoteNumber = 0;\r
+       ChannelData->PSM          = PSM;\r
+       ChannelData->LocalMTU     = MAXIMUM_CHANNEL_MTU;\r
+       ChannelData->State        = Channel_WaitConnectRsp;\r
+         \r
+       struct\r
+       {\r
+               BT_Signal_Header_t        SignalCommandHeader;\r
+               BT_Signal_ConnectionReq_t ConnectionRequest;\r
+       } PacketData;\r
+\r
+       /* Fill out the Signal Command header in the response packet */\r
+       PacketData.SignalCommandHeader.Code              = BT_SIGNAL_CONNECTION_REQUEST;\r
+       PacketData.SignalCommandHeader.Identifier        = ++Bluetooth_Connection.SignallingIdentifier;\r
+       PacketData.SignalCommandHeader.Length            = sizeof(PacketData.ConnectionRequest);\r
+       \r
+       /* Fill out the Connection Request in the response packet */\r
+       PacketData.ConnectionRequest.PSM                 = PSM;\r
+       PacketData.ConnectionRequest.SourceChannel       = ChannelData->LocalNumber;\r
+       \r
+       Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL);\r
+\r
+       BT_ACL_DEBUG(1, ">> L2CAP Connection Request");\r
+       BT_ACL_DEBUG(2, "-- PSM 0x%04X", PacketData.ConnectionRequest.PSM);\r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", PacketData.ConnectionRequest.SourceChannel);\r
+\r
+       return ChannelData;\r

 }\r
 \r
 }\r
 \r

-static inline void Bluetooth_ProcessSignalPacket_ConnectionRequest(Bluetooth_ACL_Header_t* ACLPacketHeader,\r
-                                                                   Bluetooth_DataPacket_Header_t* DataHeader,\r
-                                                                   Bluetooth_SignalCommand_Header_t* SignalCommandHeader)\r
+/** Closes a bluetooth channel that is open to the currently connected remote device, so that no further data\r
+ *  can be exchanged.\r
+ *\r
+ *  \note The channel is not immediately closed when this function returns - it must undergo an asynchronous\r
+ *        disconnection process first as the main Bluetooth stack processing task is repeatedly called. The\r
+ *        returned channel is unusable by the user application upon return however the channel is not completely\r
+ *        closed until its State element has progressed to the Closed state.\r
+ *\r
+ * \param[in,out] Channel  Channel information structure of the channel to close\r
+ */\r
+void Bluetooth_CloseChannel(Bluetooth_Channel_t* const Channel)\r

 {\r
 {\r

-       Bluetooth_SignalCommand_ConnectionRequest_t ConnectionRequest;\r
+       /* Don't try to close a non-existing or already closed channel */\r
+       if ((Channel == NULL) || (Channel->State == Channel_Closed))\r
+         return;\r
+\r
+       /* Set the channel's state to the start of the teardown process */\r
+       Channel->State = Channel_WaitDisconnect;\r
+\r
+       struct\r
+       {\r
+               BT_Signal_Header_t           SignalCommandHeader;\r
+               BT_Signal_DisconnectionReq_t DisconnectionRequest;\r
+       } PacketData;\r

        \r
        \r

-       Pipe_Read_Stream_LE(&ConnectionRequest, sizeof(ConnectionRequest));\r
+       /* Fill out the Signal Command header in the response packet */\r
+       PacketData.SignalCommandHeader.Code            = BT_SIGNAL_DISCONNECTION_REQUEST;\r
+       PacketData.SignalCommandHeader.Identifier      = ++Bluetooth_Connection.SignallingIdentifier;\r
+       PacketData.SignalCommandHeader.Length          = sizeof(PacketData.DisconnectionRequest);\r
+\r
+       /* Fill out the Disconnection Request in the response packet */\r
+       PacketData.DisconnectionRequest.DestinationChannel = Channel->RemoteNumber;\r
+       PacketData.DisconnectionRequest.SourceChannel = Channel->LocalNumber;\r

 \r
 \r

-       BT_DEBUG("(ACL) >> L2CAP Connection Request", NULL);\r
-       BT_DEBUG("(ACL) -- PSM: 0x%04X", ConnectionRequest.PSM);\r
-       BT_DEBUG("(ACL) -- Source Channel: 0x%04X", ConnectionRequest.SourceChannel);\r
+       Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL);\r

        \r
        \r

+       BT_ACL_DEBUG(1, ">> L2CAP Disconnection Request");\r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", PacketData.DisconnectionRequest.DestinationChannel);  \r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", PacketData.DisconnectionRequest.SourceChannel);    \r
+}\r
+\r
+/** Internal Bluetooth stack Signal Command processing routine for a Connection Request command.\r
+ *\r
+ *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header\r
+ */\r
+static inline void Bluetooth_Signal_ConnectionReq(const BT_Signal_Header_t* const SignalCommandHeader)\r
+{\r
+       BT_Signal_ConnectionReq_t ConnectionRequest;\r
+       \r
+       Pipe_Read_Stream_LE(&ConnectionRequest, sizeof(ConnectionRequest));\r
+\r

        Pipe_ClearIN();\r
        Pipe_Freeze();\r
        Pipe_ClearIN();\r
        Pipe_Freeze();\r

-       Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);\r
-       Pipe_SetPipeToken(PIPE_TOKEN_OUT);\r
-       Pipe_Unfreeze();\r
+\r
+       BT_ACL_DEBUG(1, "<< L2CAP Connection Request");\r
+       BT_ACL_DEBUG(2, "-- PSM: 0x%04X", ConnectionRequest.PSM);\r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConnectionRequest.SourceChannel);\r
+       \r
+       /* Try to retrieve the existing channel's information structure if it exists */\r
+       Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConnectionRequest.SourceChannel, CHANNEL_SEARCH_REMOTENUMBER);\r
+\r
+       /* If an existing channel item with the correct remote channel number was not found, find a free channel entry */\r
+       if (ChannelData == NULL)\r
+       {\r
+               /* Look through the channel information list for a free entry */\r
+               for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++)\r
+               {\r
+                       if (Bluetooth_Connection.Channels[i].State == Channel_Closed)\r
+                       {\r
+                               ChannelData = &Bluetooth_Connection.Channels[i];\r
+\r
+                               /* Set the new channel structure's local channel number to a unique value within the connection orientated\r
+                                  channel address space */\r
+                               ChannelData->LocalNumber = (BT_CHANNELNUMBER_BASEOFFSET + i);\r
+                               break;\r
+                       }\r
+               }\r
+       }\r
+       \r
+       uint8_t ChannelStatus = BT_CONNECTION_REFUSED_RESOURCES;\r
+\r
+       /* Reset the channel item contents only if a channel entry was found for it */\r
+       if (ChannelData != NULL)\r
+       {\r
+               /* Check if the user application will allow the connection based on its PSM */\r
+               if (Bluetooth_ChannelConnectionRequest(ConnectionRequest.PSM))\r
+               {\r
+                       ChannelData->RemoteNumber = ConnectionRequest.SourceChannel;\r
+                       ChannelData->PSM          = ConnectionRequest.PSM;\r
+                       ChannelData->LocalMTU     = MAXIMUM_CHANNEL_MTU;\r
+                       ChannelData->State        = Channel_Config_WaitConfig;\r
+                       
+                       ChannelStatus = BT_CONNECTION_SUCCESSFUL;\r
+               }\r
+               else\r
+               {\r
+                       ChannelStatus = BT_CONNECTION_REFUSED_PSM;              \r
+               }\r
+       }\r
+       \r
+       struct\r
+       {\r
+               BT_Signal_Header_t         SignalCommandHeader;\r
+               BT_Signal_ConnectionResp_t ConnectionResponse;\r
+       } ResponsePacket;\r
+\r
+       /* Fill out the Signal Command header in the response packet */\r
+       ResponsePacket.SignalCommandHeader.Code              = BT_SIGNAL_CONNECTION_RESPONSE;\r
+       ResponsePacket.SignalCommandHeader.Identifier        = SignalCommandHeader->Identifier;\r
+       ResponsePacket.SignalCommandHeader.Length            = sizeof(ResponsePacket.ConnectionResponse);\r
+\r
+       /* Fill out the Connection Response in the response packet */\r
+       ResponsePacket.ConnectionResponse.DestinationChannel = ChannelData->LocalNumber;\r
+       ResponsePacket.ConnectionResponse.SourceChannel      = ChannelData->RemoteNumber;\r
+       ResponsePacket.ConnectionResponse.Result             = ChannelStatus;\r
+       ResponsePacket.ConnectionResponse.Status             = 0x00;\r

        \r
        \r

-       Bluetooth_SignalCommand_ConnectionResponse_t ConnectionResponse;\r
+       Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);\r

 \r
 \r

-       ACLPacketHeader->DataLength           = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(ConnectionResponse);\r
-       DataHeader->PayloadLength             = sizeof(*SignalCommandHeader) + sizeof(ConnectionResponse);\r
-       DataHeader->DestinationChannel        = BLUETOOTH_CHANNEL_SIGNALING;\r
-       SignalCommandHeader->Code             = BLUETOOTH_SIGNAL_CONNECTION_RESPONSE;\r
-       SignalCommandHeader->Length           = sizeof(ConnectionResponse);\r
+       BT_ACL_DEBUG(1, ">> L2CAP Connection Response");\r
+       BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.ConnectionResponse.Result);\r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ResponsePacket.ConnectionResponse.DestinationChannel);\r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.ConnectionResponse.SourceChannel);\r
+}\r
+\r
+/** Internal Bluetooth stack Signal Command processing routine for a Connection Response command.\r
+ *\r
+ *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header\r
+ */\r
+static inline void Bluetooth_Signal_ConnectionResp(const BT_Signal_Header_t* const SignalCommandHeader)\r
+{\r
+       BT_Signal_ConnectionResp_t ConnectionResponse;\r

        \r
        \r

-       Bluetooth_Channel_t* ChannelData      = Bluetooth_InitChannelData(ConnectionRequest.SourceChannel, ConnectionRequest.PSM);\r
+       Pipe_Read_Stream_LE(&ConnectionResponse, sizeof(ConnectionResponse));\r
+\r
+       Pipe_ClearIN();\r
+       Pipe_Freeze();\r
+\r
+       BT_ACL_DEBUG(1, "<< L2CAP Connection Response");\r
+       BT_ACL_DEBUG(2, "-- Result: 0x%02X", ConnectionResponse.Result);        \r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConnectionResponse.SourceChannel); \r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ConnectionResponse.DestinationChannel);       \r
+\r
+       /* Search for the referenced channel in the channel information list */\r
+       Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConnectionResponse.SourceChannel, CHANNEL_SEARCH_LOCALNUMBER);\r
+\r
+       /* Only progress if the referenced channel data was found */\r
+       if (ChannelData != NULL)\r
+       {\r
+               /* Set the channel structure's remote channel number to the channel allocated on the remote device */\r
+               ChannelData->RemoteNumber = ConnectionResponse.SourceChannel;\r
+               ChannelData->State        = (ConnectionResponse.Result == BT_CONNECTION_SUCCESSFUL) ?\r
+                                            Channel_Config_WaitConfig : Channel_Closed;\r
+       }\r
+}\r
+\r
+/** Internal Bluetooth stack Signal Command processing routine for a Configuration Request command.\r
+ *\r
+ *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header\r
+ */\r
+static inline void Bluetooth_Signal_ConfigurationReq(const BT_Signal_Header_t* const SignalCommandHeader)\r
+{\r
+       BT_Signal_ConfigurationReq_t ConfigurationRequest;\r

        \r
        \r

-       ConnectionResponse.Result             = (ChannelData == NULL) ? BLUETOOTH_CONNECTION_REFUSED_RESOURCES :\r
-                                                                                                                                   BLUETOOTH_CONNECTION_SUCCESSFUL;\r
-       ConnectionResponse.DestinationChannel = ChannelData->LocalNumber;\r
-       ConnectionResponse.SourceChannel      = ChannelData->RemoteNumber;\r
-       ConnectionResponse.Status             = 0x00;\r
+       /* Allocate a buffer large enough to hold the variable number of configuration options in the request */\r
+       uint8_t OptionsLen = (SignalCommandHeader->Length - sizeof(ConfigurationRequest));\r
+       uint8_t Options[OptionsLen];\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
+       /* Search for the referenced channel in the channel information list */\r
+       Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, CHANNEL_SEARCH_LOCALNUMBER);\r
+\r
+       BT_ACL_DEBUG(1, "<< L2CAP Configuration Request");\r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ConfigurationRequest.DestinationChannel);\r
+       BT_ACL_DEBUG(2, "-- Remote MTU: 0x%04X", ChannelData->RemoteMTU);\r
+       BT_ACL_DEBUG(2, "-- Options Len: 0x%04X", OptionsLen);\r
+\r
+       /* Only look at the channel configuration options if a valid channel entry for the local channel number was found */\r
+       if (ChannelData != NULL)\r
+       {\r
+               /* Iterate through each option in the configuration request to look for ones which can be processed */\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
+                       void*                      OptionData   = &Options[OptionPos + sizeof(BT_Config_Option_Header_t)];\r

 \r
 \r

-       Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader));\r
-       Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader));\r
-       Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader));\r
-       Pipe_Write_Stream_LE(&ConnectionResponse, sizeof(ConnectionResponse));\r
+                       BT_ACL_DEBUG(2, "-- Option Type: 0x%04X", OptionHeader->Type);\r
+                       BT_ACL_DEBUG(2, "-- Option Length: 0x%04X", (sizeof(BT_Config_Option_Header_t) + OptionHeader->Length));\r
+                       \r
+                       /* Store the remote MTU option's value if present */\r
+                       if (OptionHeader->Type == BT_CONFIG_OPTION_MTU)\r
+                         ChannelData->RemoteMTU = *((uint16_t*)OptionData);\r
+\r
+                       /* Progress to the next option in the packet */\r
+                       OptionPos += (sizeof(BT_Config_Option_Header_t) + OptionHeader->Length);\r
+               }\r
+       }\r

        \r
        \r

-       Pipe_ClearOUT();                \r
+       struct\r
+       {\r
+               BT_Signal_Header_t            SignalCommandHeader;\r
+               BT_Signal_ConfigurationResp_t ConfigurationResponse;\r
+       } ResponsePacket;\r
+\r
+       /* Fill out the Signal Command header in the response packet */\r
+       ResponsePacket.SignalCommandHeader.Code              = BT_SIGNAL_CONFIGURATION_RESPONSE;\r
+       ResponsePacket.SignalCommandHeader.Identifier        = SignalCommandHeader->Identifier;\r
+       ResponsePacket.SignalCommandHeader.Length            = sizeof(ResponsePacket.ConfigurationResponse);\r
+\r
+       /* Fill out the Configuration Response in the response packet */\r
+       ResponsePacket.ConfigurationResponse.SourceChannel   = ChannelData->RemoteNumber;\r
+       ResponsePacket.ConfigurationResponse.Flags           = 0x00;\r
+       ResponsePacket.ConfigurationResponse.Result          = (ChannelData != NULL) ? BT_CONFIGURATION_SUCCESSFUL : BT_CONFIGURATION_REJECTED;\r
+\r
+       Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);\r
+\r
+       if (ChannelData != NULL)\r
+       {\r
+               switch (ChannelData->State)\r
+               {\r
+                       case Channel_Config_WaitConfig:\r
+                               ChannelData->State = Channel_Config_WaitSendConfig;\r
+                               break;\r
+                       case Channel_Config_WaitReqResp:\r
+                               ChannelData->State = Channel_Config_WaitResp;\r
+                               break;\r
+                       case Channel_Config_WaitReq:\r
+                               ChannelData->State = Channel_Open;\r
+                               break;\r
+               }\r
+       }\r
+\r
+       BT_ACL_DEBUG(1, ">> L2CAP Configuration Response");\r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.ConfigurationResponse.SourceChannel);\r
+       BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.ConfigurationResponse.Result);\r
+}\r
+\r
+/** Internal Bluetooth stack Signal Command processing routine for a Configuration Response command.\r
+ *\r
+ *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header\r
+ */\r
+static inline void Bluetooth_Signal_ConfigurationResp(const BT_Signal_Header_t* const SignalCommandHeader)\r
+{\r
+       BT_Signal_ConfigurationResp_t ConfigurationResponse;\r
+\r
+       Pipe_Read_Stream_LE(&ConfigurationResponse, sizeof(ConfigurationResponse));\r
+\r
+       Pipe_ClearIN();\r

        Pipe_Freeze();\r
        \r
        Pipe_Freeze();\r
        \r

-       BT_DEBUG("(ACL) Packet Sent", NULL);\r
-       BT_DEBUG("(ACL) -- Connection Handle: 0x%04X", ACLPacketHeader->ConnectionHandle);\r
-       BT_DEBUG("(ACL) -- Data Length: 0x%04X", ACLPacketHeader->DataLength);\r
-       BT_DEBUG("(ACL) -- Destination Channel: 0x%04X", DataHeader->DestinationChannel);\r
-       BT_DEBUG("(ACL) -- Payload Length: 0x%04X", DataHeader->PayloadLength);                 \r
-       BT_DEBUG("(ACL) >> L2CAP Connection Response", NULL);\r
-       BT_DEBUG("(ACL) -- Source Channel: 0x%04X", ConnectionResponse.SourceChannel);\r
-       BT_DEBUG("(ACL) -- Destination Channel: 0x%04X", ConnectionResponse.DestinationChannel);\r
+       BT_ACL_DEBUG(1, "<< L2CAP Configuration Response");\r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConfigurationResponse.SourceChannel);\r
+       BT_ACL_DEBUG(2, "-- Result: 0x%02X", ConfigurationResponse.Result);\r
+\r
+       /* Search for the referenced channel in the channel information list */\r
+       Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationResponse.SourceChannel, CHANNEL_SEARCH_REMOTENUMBER);\r
+       \r
+       /* Only update the channel's state if it was found in the channel list */\r
+       if (ChannelData != NULL)\r
+       {\r
+               /* Check if the channel configuration completed successfuly */\r
+               if (ConfigurationResponse.Result == BT_CONFIGURATION_SUCCESSFUL)\r
+               {\r
+                       switch (ChannelData->State)\r
+                       {\r
+                               case Channel_Config_WaitReqResp:\r
+                                       ChannelData->State = Channel_Config_WaitReq;\r
+                                       break;\r
+                               case Channel_Config_WaitResp:\r
+                                       ChannelData->State = Channel_Open;\r
+                                       break;\r
+                       }       \r
+               }\r
+               else\r
+               {\r
+                       /* Configuration failed - close the channel */\r
+                       ChannelData->State = Channel_Closed;\r
+               }\r
+       }\r

 }\r
 \r
 }\r
 \r

-static inline void Bluetooth_ProcessSignalPacket_ConfigurationRequest(Bluetooth_ACL_Header_t* ACLPacketHeader,\r
-                                                                      Bluetooth_DataPacket_Header_t* DataHeader,\r
-                                                                      Bluetooth_SignalCommand_Header_t* SignalCommandHeader)\r
+/** Internal Bluetooth stack Signal Command processing routine for a Disconnection Request command.\r
+ *\r
+ *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header\r
+ */\r
+static inline void Bluetooth_Signal_DisconnectionReq(const BT_Signal_Header_t* const SignalCommandHeader)\r

 {\r
 {\r

-       Bluetooth_SignalCommand_ConfigurationRequest_t ConfigurationRequest;\r
+       BT_Signal_DisconnectionReq_t DisconnectionRequest;\r

        \r
        \r

-       Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest));\r
+       Pipe_Read_Stream_LE(&DisconnectionRequest, sizeof(DisconnectionRequest));\r

 \r
 \r

-       BT_DEBUG("(ACL) >> L2CAP Configuration Request", NULL);\r
-       BT_DEBUG("(ACL) -- Destination Channel: 0x%04X", ConfigurationRequest.DestinationChannel);\r
+       BT_ACL_DEBUG(1, "<< L2CAP Disconnection Request");\r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DisconnectionRequest.DestinationChannel);\r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", DisconnectionRequest.SourceChannel);\r

        \r
        Pipe_ClearIN();\r
        Pipe_Freeze();\r
        \r
        Pipe_ClearIN();\r
        Pipe_Freeze();\r

-       Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);\r
-       Pipe_SetPipeToken(PIPE_TOKEN_OUT);\r
-       Pipe_Unfreeze();\r

        \r
        \r

-       Bluetooth_SignalCommand_ConfigurationResponse_t ConfigurationResponse;\r
+       /* Search for the referenced channel in the channel information list */\r
+       Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionRequest.SourceChannel, CHANNEL_SEARCH_REMOTENUMBER);\r
+\r
+       struct\r
+       {\r
+               BT_Signal_Header_t            SignalCommandHeader;\r
+               BT_Signal_DisconnectionResp_t DisconnectionResponse;\r
+       } ResponsePacket;\r
+\r
+       /* Fill out the Signal Command header in the response packet */\r
+       ResponsePacket.SignalCommandHeader.Code                 = BT_SIGNAL_DISCONNECTION_RESPONSE;\r
+       ResponsePacket.SignalCommandHeader.Identifier           = SignalCommandHeader->Identifier;\r
+       ResponsePacket.SignalCommandHeader.Length               = sizeof(ResponsePacket.DisconnectionResponse);\r
+\r
+       /* Fill out the Disconnection Response in the response packet */\r
+       ResponsePacket.DisconnectionResponse.DestinationChannel = ChannelData->RemoteNumber;\r
+       ResponsePacket.DisconnectionResponse.SourceChannel      = ChannelData->LocalNumber;\r

 \r
 \r

-       ACLPacketHeader->DataLength           = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(ConfigurationResponse);\r
-       DataHeader->PayloadLength             = sizeof(*SignalCommandHeader) + sizeof(ConfigurationResponse);\r
-       DataHeader->DestinationChannel        = BLUETOOTH_CHANNEL_SIGNALING;\r
-       SignalCommandHeader->Code             = BLUETOOTH_SIGNAL_CONFIGURATION_RESPONSE;\r
-       SignalCommandHeader->Length           = sizeof(ConfigurationResponse);\r
+       Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);\r
+\r
+       /* If the channel was found in the channel list, close it */\r
+       if (ChannelData != NULL)\r
+         ChannelData->State = Channel_Closed;\r
+\r
+       BT_ACL_DEBUG(1, ">> L2CAP Disconnection Response");\r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.DisconnectionResponse.SourceChannel);\r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ResponsePacket.DisconnectionResponse.DestinationChannel);\r
+}\r
+\r
+/** Internal Bluetooth stack Signal Command processing routine for a Disconnection Response command.\r
+ *\r
+ *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header\r
+ */\r
+static inline void Bluetooth_Signal_DisconnectionResp(const BT_Signal_Header_t* const SignalCommandHeader)\r
+{\r
+       BT_Signal_DisconnectionResp_t DisconnectionResponse;\r

        \r
        \r

-       Bluetooth_Channel_t* ChannelData      = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, CHANNEL_LOOKUP_BY_DESTINATION);\r
+       Pipe_Read_Stream_LE(&DisconnectionResponse, sizeof(DisconnectionResponse));\r

 \r
 \r

+       BT_ACL_DEBUG(1, "<< L2CAP Disconnection Response");\r
+       BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DisconnectionResponse.DestinationChannel);\r
+       BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", DisconnectionResponse.SourceChannel);\r
+       \r
+       Pipe_ClearIN();\r
+       Pipe_Freeze();\r
+       \r
+       /* Search for the referenced channel in the channel information list */\r
+       Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionResponse.SourceChannel, CHANNEL_SEARCH_REMOTENUMBER);\r
+       \r
+       /* If the channel was found in the channel list, close it */    \r

        if (ChannelData != NULL)\r
        if (ChannelData != NULL)\r

-         ChannelData->State = Channel_Open;\r
-         \r
-        // TODO: Add channel config data to the tail of ConfigurationResponse\r
+         ChannelData->State = Channel_Closed;\r
+}\r
+\r
+/** Internal Bluetooth stack Signal Command processing routine for an Echo Request command.\r
+ *\r
+ *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header\r
+ */\r
+static inline void Bluetooth_Signal_EchoReq(const BT_Signal_Header_t* const SignalCommandHeader)\r
+{\r
+       BT_ACL_DEBUG(1, "<< L2CAP Echo Request");\r
+       \r
+       Pipe_ClearIN();\r
+       Pipe_Freeze();\r
+       \r
+       struct\r
+       {\r
+               BT_Signal_Header_t SignalCommandHeader;\r
+       } ResponsePacket;\r
+\r
+       /* Fill out the Signal Command header in the response packet */\r
+       ResponsePacket.SignalCommandHeader.Code                 = BT_SIGNAL_ECHO_RESPONSE;\r
+       ResponsePacket.SignalCommandHeader.Identifier           = SignalCommandHeader->Identifier;\r
+       ResponsePacket.SignalCommandHeader.Length               = 0;\r
+       \r
+       Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);\r
+\r
+       BT_ACL_DEBUG(1, ">> L2CAP Echo Response");\r
+}\r

 \r
 \r

-       ConfigurationResponse.SourceChannel   = ChannelData->RemoteNumber;\r
-       ConfigurationResponse.Flags           = 0x00;\r
-       ConfigurationResponse.Result          = (ChannelData != NULL) ? BLUETOOTH_CONFIGURATION_SUCCESSFUL : BLUETOOTH_CONFIGURATION_REJECTED;\r
+/** Internal Bluetooth stack Signal Command processing routine for an Information Request command.\r
+ *\r
+ *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header\r
+ */\r
+static inline void Bluetooth_Signal_InformationReq(const BT_Signal_Header_t* const SignalCommandHeader)\r
+{\r
+       BT_Signal_InformationReq_t InformationRequest;\r
+\r
+       Pipe_Read_Stream_LE(&InformationRequest, sizeof(InformationRequest));\r

 \r
 \r

-       Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader));\r
-       Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader));\r
-       Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader));\r
-       Pipe_Write_Stream_LE(&ConfigurationResponse, sizeof(ConfigurationResponse));\r
+       BT_ACL_DEBUG(1, "<< L2CAP Information Request");\r
+       BT_ACL_DEBUG(2, "-- Info Type: 0x%04X", InformationRequest.InfoType);\r

        \r
        \r

-       Pipe_ClearOUT();                \r
+       Pipe_ClearIN();\r

        Pipe_Freeze();\r
        Pipe_Freeze();\r

+\r
+       struct\r
+       {\r
+               BT_Signal_Header_t          SignalCommandHeader;\r
+               BT_Signal_InformationResp_t InformationResponse;\r
+               \r
+               uint8_t Data[4];\r
+       } ResponsePacket;\r
+       \r
+       uint8_t DataLen = 0;\r

        \r
        \r

-       BT_DEBUG("(ACL) Packet Sent", NULL);\r
-       BT_DEBUG("(ACL) -- Connection Handle: 0x%04X", ACLPacketHeader->ConnectionHandle);\r
-       BT_DEBUG("(ACL) -- Data Length: 0x%04X", ACLPacketHeader->DataLength);\r
-       BT_DEBUG("(ACL) -- Destination Channel: 0x%04X", DataHeader->DestinationChannel);\r
-       BT_DEBUG("(ACL) -- Payload Length: 0x%04X", DataHeader->PayloadLength);                 \r
-       BT_DEBUG("(ACL) >> L2CAP Configuration Response", NULL);\r
+       /* Retrieve the requested information and store it in the outgoing packet, if found */\r
+       switch (InformationRequest.InfoType)\r
+       {\r
+               case BT_INFOREQ_MTU:            \r
+                       ResponsePacket.InformationResponse.Result = BT_INFORMATION_SUCCESSFUL;\r
+                       DataLen = 2;\r
+                       \r
+                       *((uint16_t*)&ResponsePacket.Data) = MAXIMUM_CHANNEL_MTU;\r
+                       break;\r
+               case BT_INFOREQ_EXTENDEDFEATURES:\r
+                       ResponsePacket.InformationResponse.Result = BT_INFORMATION_SUCCESSFUL;\r
+                       DataLen = 4;\r
+                       \r
+                       *((uint32_t*)&ResponsePacket.Data) = 0;\r
+                       break;\r
+               default:\r
+                       ResponsePacket.InformationResponse.Result = BT_INFORMATION_NOTSUPPORTED;\r
+                       DataLen = 0;\r
+                       break;\r
+       }\r
+       \r
+       /* Fill out the Signal Command header in the response packet */\r
+       ResponsePacket.SignalCommandHeader.Code                 = BT_SIGNAL_INFORMATION_RESPONSE;\r
+       ResponsePacket.SignalCommandHeader.Identifier           = SignalCommandHeader->Identifier;\r
+       ResponsePacket.SignalCommandHeader.Length               = sizeof(ResponsePacket.InformationResponse) + DataLen;\r
+\r
+       /* Fill out the Information Response in the response packet */\r
+       ResponsePacket.InformationResponse.InfoType = InformationRequest.InfoType;\r
+       \r
+       Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket) - sizeof(ResponsePacket.Data) + DataLen), NULL);\r
+\r
+       BT_ACL_DEBUG(1, ">> L2CAP Information Response");       \r
+       BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.InformationResponse.Result);\r

 }\r
 }\r