X-Git-Url: http://git.linex4red.de/pub/USBasp.git/blobdiff_plain/c77f136661ae0fa779c02ef6efeab95aa4b92068..19ecd04f37f68c0674f1194aa8d8a4fc94d6168b:/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 d26ba1962..d18e643b4 100644 --- a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c +++ b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c @@ -31,16 +31,22 @@ #define INCLUDE_FROM_BLUETOOTH_ACLPACKETS_C #include "BluetoothACLPackets.h" +/** Bluetooth ACL processing task. This task should be called repeatedly the main Bluetooth + * stack task to manage the ACL processing state. + */ void Bluetooth_ACLTask(void) { - Bluetooth_ProcessACLPackets(); + /* Process incomming ACL packets, if any */ + Bluetooth_ProcessIncommingACLPackets(); + /* Check for any half-open channels, send configuration details to the remote device if found */ for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++) { Bluetooth_Channel_t* ChannelData = &Bluetooth_Connection.Channels[i]; bool MustSendConfigReq = true; + /* Check if we are in a channel state which requires a configuration request to be sent */ switch (ChannelData->State) { case Channel_Config_WaitConfig: @@ -54,52 +60,47 @@ void Bluetooth_ACLTask(void) break; } + /* Only send a configuration request if it the channel was in a state which required it */ if (MustSendConfigReq) { - BT_ACL_Header_t ACLPacketHeader; - BT_DataPacket_Header_t DataHeader; - BT_Signal_Header_t SignalCommandHeader; - BT_Signal_ConfigurationReq_t ConfigurationRequest; - - ACLPacketHeader.ConnectionHandle = Bluetooth_Connection.ConnectionHandle; - ACLPacketHeader.DataLength = sizeof(DataHeader) + sizeof(SignalCommandHeader) + sizeof(ConfigurationRequest); - DataHeader.PayloadLength = sizeof(SignalCommandHeader) + sizeof(ConfigurationRequest); - DataHeader.DestinationChannel = BT_CHANNEL_SIGNALING; - SignalCommandHeader.Code = BT_SIGNAL_CONFIGURATION_REQUEST; - SignalCommandHeader.Identifier = ++Bluetooth_Connection.SignallingIdentifier; - SignalCommandHeader.Length = sizeof(ConfigurationRequest); + struct + { + BT_Signal_Header_t SignalCommandHeader; + BT_Signal_ConfigurationReq_t ConfigurationRequest; + + struct + { + BT_Config_Option_Header_t Header; + uint16_t Value; + } Option_LocalMTU; + } PacketData; - ConfigurationRequest.DestinationChannel = ChannelData->RemoteNumber; - ConfigurationRequest.Flags = 0; - - Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE); - Pipe_Unfreeze(); + /* Fill out the Signal Command header in the response packet */ + PacketData.SignalCommandHeader.Code = BT_SIGNAL_CONFIGURATION_REQUEST; + PacketData.SignalCommandHeader.Identifier = ++Bluetooth_Connection.SignallingIdentifier; + PacketData.SignalCommandHeader.Length = sizeof(PacketData.ConfigurationRequest) + + sizeof(PacketData.Option_LocalMTU); + + /* Fill out the Configuration Request in the response packet, including local MTU information */ + PacketData.ConfigurationRequest.DestinationChannel = ChannelData->RemoteNumber; + PacketData.ConfigurationRequest.Flags = 0; + PacketData.Option_LocalMTU.Header.Type = BT_CONFIG_OPTION_MTU; + PacketData.Option_LocalMTU.Header.Length = sizeof(PacketData.Option_LocalMTU.Value); + PacketData.Option_LocalMTU.Value = ChannelData->LocalMTU; + + Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL); - Pipe_Write_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader)); - Pipe_Write_Stream_LE(&DataHeader, sizeof(DataHeader)); - Pipe_Write_Stream_LE(&SignalCommandHeader, sizeof(SignalCommandHeader)); - Pipe_Write_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest)); - - Pipe_Freeze(); - - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("Packet Sent", NULL); - BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF)); - BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader.DataLength); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader.DestinationChannel); - BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader.PayloadLength); - #endif - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG(">> L2CAP Configuration Request", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", ConfigurationRequest.DestinationChannel); - #endif + BT_ACL_DEBUG(1, ">> L2CAP Configuration Request", NULL); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", PacketData.ConfigurationRequest.DestinationChannel); } } } -static void Bluetooth_ProcessACLPackets(void) +/** Incomming ACL packet processing task. This task is called by the main ACL processing task to read in and process + * any incomming ACL packets to the device, handling signal requests as they are received or passing along channel + * data to the user application. + */ +static void Bluetooth_ProcessIncommingACLPackets(void) { BT_ACL_Header_t ACLPacketHeader; BT_DataPacket_Header_t DataHeader; @@ -113,43 +114,64 @@ static void Bluetooth_ProcessACLPackets(void) return; } + /* Read in the received ACL packet headers when it has been discovered that a packet has been received */ Pipe_Read_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader)); Pipe_Read_Stream_LE(&DataHeader, sizeof(DataHeader)); - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("Packet Received", NULL); - BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF)); - BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader.DataLength); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader.DestinationChannel); - BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader.PayloadLength); - #endif + BT_ACL_DEBUG(2, "", NULL); + BT_ACL_DEBUG(2, "Packet Received", NULL); + BT_ACL_DEBUG(2, "-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF)); + BT_ACL_DEBUG(2, "-- Data Length: 0x%04X", ACLPacketHeader.DataLength); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DataHeader.DestinationChannel); + BT_ACL_DEBUG(2, "-- Payload Length: 0x%04X", DataHeader.PayloadLength); + /* Check the packet's destination channel - signalling channel should be processed by the stack internally */ if (DataHeader.DestinationChannel == BT_CHANNEL_SIGNALING) { + /* Read in the Signal Command header of the incomming packet */ BT_Signal_Header_t SignalCommandHeader; Pipe_Read_Stream_LE(&SignalCommandHeader, sizeof(SignalCommandHeader)); - + + /* Dispatch to the appropriate handler function based on the Signal message code */ switch (SignalCommandHeader.Code) { case BT_SIGNAL_CONNECTION_REQUEST: - Bluetooth_Signal_ConnectionReq(&ACLPacketHeader, &DataHeader, &SignalCommandHeader); + Bluetooth_Signal_ConnectionReq(&SignalCommandHeader); + break; + case BT_SIGNAL_CONNECTION_RESPONSE: + Bluetooth_Signal_ConnectionResp(&SignalCommandHeader); break; case BT_SIGNAL_CONFIGURATION_REQUEST: - Bluetooth_Signal_ConfigurationReq(&ACLPacketHeader, &DataHeader, &SignalCommandHeader); + Bluetooth_Signal_ConfigurationReq(&SignalCommandHeader); + break; + case BT_SIGNAL_CONFIGURATION_RESPONSE: + Bluetooth_Signal_ConfigurationResp(&SignalCommandHeader); break; case BT_SIGNAL_DISCONNECTION_REQUEST: - Bluetooth_Signal_DisconnectionReq(&ACLPacketHeader, &DataHeader, &SignalCommandHeader); - break; + Bluetooth_Signal_DisconnectionReq(&SignalCommandHeader); + break; + case BT_SIGNAL_DISCONNECTION_RESPONSE: + Bluetooth_Signal_DisconnectionResp(&SignalCommandHeader); + break; case BT_SIGNAL_ECHO_REQUEST: - Bluetooth_Signal_EchoReq(&ACLPacketHeader, &DataHeader, &SignalCommandHeader); + Bluetooth_Signal_EchoReq(&SignalCommandHeader); break; case BT_SIGNAL_INFORMATION_REQUEST: - Bluetooth_Signal_InformationReq(&ACLPacketHeader, &DataHeader, &SignalCommandHeader); + Bluetooth_Signal_InformationReq(&SignalCommandHeader); + break; + case BT_SIGNAL_COMMAND_REJECT: + BT_ACL_DEBUG(1, "<< Command Reject", NULL); + + uint16_t RejectReason; + Pipe_Read_Stream_LE(&RejectReason, sizeof(RejectReason)); + Pipe_Discard_Stream(ACLPacketHeader.DataLength - sizeof(RejectReason)); + Pipe_ClearIN(); + Pipe_Freeze(); + + BT_ACL_DEBUG(2, "-- Reason: %d", RejectReason); break; default: - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG("<< Unknown Signaling Command 0x%02X", SignalCommandHeader.Code); - #endif + BT_ACL_DEBUG(1, "<< Unknown Signaling Command 0x%02X", SignalCommandHeader.Code); Pipe_Discard_Stream(ACLPacketHeader.DataLength); Pipe_ClearIN(); @@ -159,134 +181,338 @@ static void Bluetooth_ProcessACLPackets(void) } else { - Bluetooth_PacketReceived(&DataHeader.PayloadLength, Bluetooth_GetChannelData(DataHeader.DestinationChannel, true)); - - Pipe_SelectPipe(BLUETOOTH_DATA_IN_PIPE); - Pipe_Discard_Stream(DataHeader.PayloadLength); + /* Non-signalling packet received, read in the packet contents and pass to the user application */ + uint8_t PacketData[DataHeader.PayloadLength]; + Pipe_Read_Stream_LE(PacketData, DataHeader.PayloadLength); Pipe_ClearIN(); Pipe_Freeze(); + + Bluetooth_PacketReceived(PacketData, DataHeader.PayloadLength, Bluetooth_GetChannelData(DataHeader.DestinationChannel, false)); } } -uint8_t Bluetooth_SendPacket(uint8_t* Data, uint16_t DataLen, Bluetooth_Channel_t* Channel) +/** Sends a packet to the remote device on the specified channel. + * + * \param Data Pointer to a buffer where the data is to be sourced from + * \param DataLen Length of the data to send + * \param Channel Channel information structure containing the destination channel's information, NULL to send + * to the remote device's signalling channel + * + * \return A value from the \ref BT_SendPacket_ErrorCodes_t enum + */ +uint8_t Bluetooth_SendPacket(void* Data, uint16_t DataLen, Bluetooth_Channel_t* Channel) { BT_ACL_Header_t ACLPacketHeader; BT_DataPacket_Header_t DataHeader; - if (Bluetooth_Connection.IsConnected) + /* A remote device must be connected before a packet transmission is attempted */ + if (!(Bluetooth_Connection.IsConnected)) return BT_SENDPACKET_NotConnected; - if (Channel->State != Channel_Open) + /* If the destination channel is not the signalling channel and it is not currently fully open, abort */ + if ((Channel != NULL) && (Channel->State != Channel_Open)) return BT_SENDPACKET_ChannelNotOpen; - ACLPacketHeader.ConnectionHandle = Bluetooth_Connection.ConnectionHandle; + /* Fill out the packet's header from the remote device connection information structure */ + ACLPacketHeader.ConnectionHandle = (Bluetooth_Connection.ConnectionHandle | BT_ACL_FIRST_AUTOFLUSH); ACLPacketHeader.DataLength = sizeof(DataHeader) + DataLen; - DataHeader.DestinationChannel = Channel->RemoteNumber; DataHeader.PayloadLength = DataLen; + DataHeader.DestinationChannel = (Channel == NULL) ? BT_CHANNEL_SIGNALING : Channel->RemoteNumber; Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE); Pipe_Unfreeze(); + /* Write the packet contents to the pipe so that it can be sent to the remote device */ Pipe_Write_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader)); Pipe_Write_Stream_LE(&DataHeader, sizeof(DataHeader)); Pipe_Write_Stream_LE(Data, DataLen); - + Pipe_ClearOUT(); + Pipe_Freeze(); + BT_ACL_DEBUG(2, "", NULL); + BT_ACL_DEBUG(2, "Packet Sent", NULL); + BT_ACL_DEBUG(2, "-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF)); + BT_ACL_DEBUG(2, "-- Data Length: 0x%04X", ACLPacketHeader.DataLength); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DataHeader.DestinationChannel); + BT_ACL_DEBUG(2, "-- Payload Length: 0x%04X", DataHeader.PayloadLength); + return BT_SENDPACKET_NoError; } -static inline void Bluetooth_Signal_ConnectionReq(BT_ACL_Header_t* ACLPacketHeader, - BT_DataPacket_Header_t* DataHeader, - BT_Signal_Header_t* SignalCommandHeader) +/** Opens a bluetooth channel to the currently connected remote device, so that data can be exchanged. + * + * \note The channel is not immediately opened when this function returns - it must undergo a two way + * connection and configuration process first as the main Bluetooth stack processing task is + * repeatedly called. The returned channel is unusable by the user application until its State + * element has progressed to the Open state. + * + * \param PSM PSM of the service that the channel is to be opened for + * + * \return Pointer to the channel information structure of the opened channel, or NULL if no free channels + */ +Bluetooth_Channel_t* Bluetooth_OpenChannel(uint16_t PSM) +{ + Bluetooth_Channel_t* ChannelData = NULL; + + /* Search through the channel information list for a free channel item */ + for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++) + { + if (Bluetooth_Connection.Channels[i].State == Channel_Closed) + { + ChannelData = &Bluetooth_Connection.Channels[i]; + + /* Set the new channel structure's local channel number to a unique value within the connection orientated + channel address space */ + ChannelData->LocalNumber = (BT_CHANNELNUMBER_BASEOFFSET + i); + break; + } + } + + /* If no free channel item was found in the list, all channels are occupied - abort */ + if (ChannelData == NULL) + return NULL; + + /* Reset and fill out the allocated channel's information structure with defaults */ + ChannelData->RemoteNumber = 0; + ChannelData->PSM = PSM; + ChannelData->LocalMTU = MAXIMUM_CHANNEL_MTU; + ChannelData->State = Channel_WaitConnectRsp; + + struct + { + BT_Signal_Header_t SignalCommandHeader; + BT_Signal_ConnectionReq_t ConnectionRequest; + } PacketData; + + /* Fill out the Signal Command header in the response packet */ + PacketData.SignalCommandHeader.Code = BT_SIGNAL_CONNECTION_REQUEST; + PacketData.SignalCommandHeader.Identifier = ++Bluetooth_Connection.SignallingIdentifier; + PacketData.SignalCommandHeader.Length = sizeof(PacketData.ConnectionRequest); + + /* Fill out the Connection Request in the response packet */ + PacketData.ConnectionRequest.PSM = PSM; + PacketData.ConnectionRequest.SourceChannel = ChannelData->LocalNumber; + + Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL); + + BT_ACL_DEBUG(1, ">> L2CAP Connection Request", NULL); + BT_ACL_DEBUG(2, "-- PSM 0x%04X", PacketData.ConnectionRequest.PSM); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", PacketData.ConnectionRequest.SourceChannel); + + return ChannelData; +} + +/** Closes a bluetooth channel that is open to the currently connected remote device, so that no further data + * can be exchanged. + * + * \note The channel is not immediately closed when this function returns - it must undergo an asynchronous + * disconnection process first as the main Bluetooth stack processing task is repeatedly called. The + * returned channel is unusable by the user application upon return however the channel is not completely + * closed until its State element has progressed to the Closed state. + * + * \param Channel Channel information structure of the channel to close + */ +void Bluetooth_CloseChannel(Bluetooth_Channel_t* Channel) +{ + /* Don't try to close a non-existing or already closed channel */ + if ((Channel == NULL) || (Channel->State == Channel_Closed)) + return; + + /* Set the channel's state to the start of the teardown process */ + Channel->State = Channel_WaitDisconnect; + + struct + { + BT_Signal_Header_t SignalCommandHeader; + BT_Signal_DisconnectionReq_t DisconnectionRequest; + } PacketData; + + /* Fill out the Signal Command header in the response packet */ + PacketData.SignalCommandHeader.Code = BT_SIGNAL_DISCONNECTION_REQUEST; + PacketData.SignalCommandHeader.Identifier = ++Bluetooth_Connection.SignallingIdentifier; + PacketData.SignalCommandHeader.Length = sizeof(PacketData.DisconnectionRequest); + + /* Fill out the Disconnection Request in the response packet */ + PacketData.DisconnectionRequest.DestinationChannel = Channel->RemoteNumber; + PacketData.DisconnectionRequest.SourceChannel = Channel->LocalNumber; + + Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL); + + BT_ACL_DEBUG(1, ">> L2CAP Disconnection Request", NULL); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", PacketData.DisconnectionRequest.DestinationChannel); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", PacketData.DisconnectionRequest.SourceChannel); +} + +/** Internal Bluetooth stack Signal Command processing routine for a Connection Request command. + * + * \param SignalCommandHeader Pointer to the start of the received packet's Signal Command header + */ +static inline void Bluetooth_Signal_ConnectionReq(BT_Signal_Header_t* SignalCommandHeader) { BT_Signal_ConnectionReq_t ConnectionRequest; Pipe_Read_Stream_LE(&ConnectionRequest, sizeof(ConnectionRequest)); - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG("<< L2CAP Connection Request", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- PSM: 0x%04X", ConnectionRequest.PSM); - BT_ACL_DEBUG("-- Source Channel: 0x%04X", ConnectionRequest.SourceChannel); - #endif - Pipe_ClearIN(); Pipe_Freeze(); - Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE); - Pipe_Unfreeze(); - - BT_Signal_ConnectionResp_t ConnectionResponse; - ACLPacketHeader->DataLength = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(ConnectionResponse); - DataHeader->PayloadLength = sizeof(*SignalCommandHeader) + sizeof(ConnectionResponse); - DataHeader->DestinationChannel = BT_CHANNEL_SIGNALING; - SignalCommandHeader->Code = BT_SIGNAL_CONNECTION_RESPONSE; - SignalCommandHeader->Length = sizeof(ConnectionResponse); + BT_ACL_DEBUG(1, "<< L2CAP Connection Request", NULL); + BT_ACL_DEBUG(2, "-- PSM: 0x%04X", ConnectionRequest.PSM); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConnectionRequest.SourceChannel); - Bluetooth_Channel_t* ChannelData = Bluetooth_InitChannelData(ConnectionRequest.SourceChannel, ConnectionRequest.PSM); + /* Try to retrieve the existing channel's information structure if it exists */ + Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConnectionRequest.SourceChannel, true); + + /* If an existing channel item with the correct remote channel number was not found, find a free channel entry */ + if (ChannelData == NULL) + { + /* Look through the channel information list for a free entry */ + for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++) + { + if (Bluetooth_Connection.Channels[i].State == Channel_Closed) + { + ChannelData = &Bluetooth_Connection.Channels[i]; + + /* Set the new channel structure's local channel number to a unique value within the connection orientated + channel address space */ + ChannelData->LocalNumber = (BT_CHANNELNUMBER_BASEOFFSET + i); + break; + } + } + } + + /* Reset the channel item contents only if a channel entry was found for it */ + if (ChannelData != NULL) + { + ChannelData->RemoteNumber = ConnectionRequest.SourceChannel; + ChannelData->PSM = ConnectionRequest.PSM; + ChannelData->LocalMTU = MAXIMUM_CHANNEL_MTU; + ChannelData->State = Channel_Config_WaitConfig; + } - ConnectionResponse.Result = (ChannelData == NULL) ? BT_CONNECTION_REFUSED_RESOURCES : BT_CONNECTION_SUCCESSFUL; - ConnectionResponse.DestinationChannel = ChannelData->LocalNumber; - ConnectionResponse.SourceChannel = ChannelData->RemoteNumber; - ConnectionResponse.Status = 0x00; + struct + { + BT_Signal_Header_t SignalCommandHeader; + BT_Signal_ConnectionResp_t ConnectionResponse; + } ResponsePacket; + + /* Fill out the Signal Command header in the response packet */ + ResponsePacket.SignalCommandHeader.Code = BT_SIGNAL_CONNECTION_RESPONSE; + ResponsePacket.SignalCommandHeader.Identifier = SignalCommandHeader->Identifier; + ResponsePacket.SignalCommandHeader.Length = sizeof(ResponsePacket.ConnectionResponse); + + /* Fill out the Connection Response in the response packet */ + ResponsePacket.ConnectionResponse.DestinationChannel = ChannelData->LocalNumber; + ResponsePacket.ConnectionResponse.SourceChannel = ChannelData->RemoteNumber; + ResponsePacket.ConnectionResponse.Result = (ChannelData == NULL) ? BT_CONNECTION_REFUSED_RESOURCES : + BT_CONNECTION_SUCCESSFUL; + ResponsePacket.ConnectionResponse.Status = 0x00; + + Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL); + + BT_ACL_DEBUG(1, ">> L2CAP Connection Response", NULL); + BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.ConnectionResponse.Result); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ResponsePacket.ConnectionResponse.DestinationChannel); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.ConnectionResponse.SourceChannel); +} - Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader)); - Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader)); - Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader)); - Pipe_Write_Stream_LE(&ConnectionResponse, sizeof(ConnectionResponse)); +/** Internal Bluetooth stack Signal Command processing routine for a Connection Response command. + * + * \param SignalCommandHeader Pointer to the start of the received packet's Signal Command header + */ +static inline void Bluetooth_Signal_ConnectionResp(BT_Signal_Header_t* SignalCommandHeader) +{ + BT_Signal_ConnectionResp_t ConnectionResponse; - Pipe_ClearOUT(); + Pipe_Read_Stream_LE(&ConnectionResponse, sizeof(ConnectionResponse)); + + Pipe_ClearIN(); Pipe_Freeze(); - - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("Packet Sent", NULL); - BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF)); - BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel); - BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength); - #endif - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG(">> L2CAP Connection Response", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- Source Channel: 0x%04X", ConnectionResponse.SourceChannel); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", ConnectionResponse.DestinationChannel); - #endif + + BT_ACL_DEBUG(1, "<< L2CAP Connection Response", NULL); + BT_ACL_DEBUG(2, "-- Result: 0x%02X", ConnectionResponse.Result); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConnectionResponse.SourceChannel); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ConnectionResponse.DestinationChannel); + + /* Search for the referenced channel in the channel information list */ + Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConnectionResponse.SourceChannel, false); + + /* Only progress if the referenced channel data was found */ + if (ChannelData != NULL) + { + /* Set the channel structure's remote channel number to the channel allocated on the remote device */ + ChannelData->RemoteNumber = ConnectionResponse.SourceChannel; + ChannelData->State = (ConnectionResponse.Result == BT_CONNECTION_SUCCESSFUL) ? + Channel_Config_WaitConfig : Channel_Closed; + } } -static inline void Bluetooth_Signal_ConfigurationReq(BT_ACL_Header_t* ACLPacketHeader, - BT_DataPacket_Header_t* DataHeader, - BT_Signal_Header_t* SignalCommandHeader) +/** Internal Bluetooth stack Signal Command processing routine for a Configuration Request command. + * + * \param SignalCommandHeader Pointer to the start of the received packet's Signal Command header + */ +static inline void Bluetooth_Signal_ConfigurationReq(BT_Signal_Header_t* SignalCommandHeader) { BT_Signal_ConfigurationReq_t ConfigurationRequest; - Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest)); - // TODO: Process/Discard configuration options here - Pipe_Discard_Stream(DataHeader->PayloadLength - sizeof(*SignalCommandHeader)); + /* Allocate a buffer large enough to hold the variable number of configuration options in the request */ + uint8_t OptionsLen = (SignalCommandHeader->Length - sizeof(ConfigurationRequest)); + uint8_t Options[OptionsLen]; + + Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest)); + Pipe_Read_Stream_LE(&Options, sizeof(Options)); - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG("<< L2CAP Configuration Request", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", ConfigurationRequest.DestinationChannel); - #endif - Pipe_ClearIN(); Pipe_Freeze(); - Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE); - Pipe_Unfreeze(); - - BT_Signal_ConfigurationResp_t ConfigurationResponse; - ACLPacketHeader->DataLength = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(ConfigurationResponse); - DataHeader->PayloadLength = sizeof(*SignalCommandHeader) + sizeof(ConfigurationResponse); - DataHeader->DestinationChannel = BT_CHANNEL_SIGNALING; - SignalCommandHeader->Code = BT_SIGNAL_CONFIGURATION_RESPONSE; - SignalCommandHeader->Length = sizeof(ConfigurationResponse); + /* Search for the referenced channel in the channel information list */ + 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, "-- Remote MTU: 0x%04X", ChannelData->RemoteMTU); + BT_ACL_DEBUG(2, "-- Options Len: 0x%04X", OptionsLen); + + /* Only look at the channel configuration options if a valid channel entry for the local channel number was found */ + if (ChannelData != NULL) + { + /* Iterate through each option in the configuration request to look for ones which can be processed */ + uint8_t OptionPos = 0; + while (OptionPos < OptionsLen) + { + BT_Config_Option_Header_t* OptionHeader = (BT_Config_Option_Header_t*)&Options[OptionPos]; + void* OptionData = &Options[OptionPos + sizeof(*OptionHeader)]; + + BT_ACL_DEBUG(2, "-- Option Type: 0x%04X", OptionHeader->Type); + BT_ACL_DEBUG(2, "-- Option Length: 0x%04X", (sizeof(*OptionHeader) + OptionHeader->Length)); + + /* Store the remote MTU option's value if present */ + if (OptionHeader->Type == BT_CONFIG_OPTION_MTU) + ChannelData->RemoteMTU = *((uint16_t*)OptionData); + + /* Progress to the next option in the packet */ + OptionPos += (sizeof(*OptionHeader) + OptionHeader->Length); + } + } - Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, false); + struct + { + BT_Signal_Header_t SignalCommandHeader; + BT_Signal_ConfigurationResp_t ConfigurationResponse; + } ResponsePacket; + + /* Fill out the Signal Command header in the response packet */ + ResponsePacket.SignalCommandHeader.Code = BT_SIGNAL_CONFIGURATION_RESPONSE; + ResponsePacket.SignalCommandHeader.Identifier = SignalCommandHeader->Identifier; + ResponsePacket.SignalCommandHeader.Length = sizeof(ResponsePacket.ConfigurationResponse); + + /* Fill out the Configuration Response in the response packet */ + ResponsePacket.ConfigurationResponse.SourceChannel = ChannelData->RemoteNumber; + ResponsePacket.ConfigurationResponse.Flags = 0x00; + ResponsePacket.ConfigurationResponse.Result = (ChannelData != NULL) ? BT_CONFIGURATION_SUCCESSFUL : BT_CONFIGURATION_REJECTED; + + Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL); if (ChannelData != NULL) { @@ -303,206 +529,210 @@ static inline void Bluetooth_Signal_ConfigurationReq(BT_ACL_Header_t* ACLPacketH break; } } - - // TODO: Add channel config data to the tail of ConfigurationResponse - ConfigurationResponse.SourceChannel = ChannelData->RemoteNumber; - ConfigurationResponse.Flags = 0x00; - ConfigurationResponse.Result = (ChannelData != NULL) ? BT_CONFIGURATION_SUCCESSFUL : BT_CONFIGURATION_REJECTED; + BT_ACL_DEBUG(1, ">> L2CAP Configuration Response", NULL); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.ConfigurationResponse.SourceChannel); + BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.ConfigurationResponse.Result); +} - Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader)); - Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader)); - Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader)); - Pipe_Write_Stream_LE(&ConfigurationResponse, sizeof(ConfigurationResponse)); - - Pipe_ClearOUT(); +/** Internal Bluetooth stack Signal Command processing routine for a Configuration Response command. + * + * \param SignalCommandHeader Pointer to the start of the received packet's Signal Command header + */ +static inline void Bluetooth_Signal_ConfigurationResp(BT_Signal_Header_t* SignalCommandHeader) +{ + BT_Signal_ConfigurationResp_t ConfigurationResponse; + + Pipe_Read_Stream_LE(&ConfigurationResponse, sizeof(ConfigurationResponse)); + + Pipe_ClearIN(); Pipe_Freeze(); - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("Packet Sent", NULL); - BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF)); - BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel); - BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength); - #endif - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG(">> L2CAP Configuration Response", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- Result: 0x%02X", ConfigurationResponse.Result); - #endif + BT_ACL_DEBUG(1, "<< L2CAP Configuration Response", NULL); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConfigurationResponse.SourceChannel); + BT_ACL_DEBUG(2, "-- Result: 0x%02X", ConfigurationResponse.Result); + + /* Search for the referenced channel in the channel information list */ + Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationResponse.SourceChannel, true); + + /* Only update the channel's state if it was found in the channel list */ + if (ChannelData != NULL) + { + /* Check if the channel configuration completed successfuly */ + if (ConfigurationResponse.Result == BT_CONFIGURATION_SUCCESSFUL) + { + switch (ChannelData->State) + { + case Channel_Config_WaitReqResp: + ChannelData->State = Channel_Config_WaitReq; + break; + case Channel_Config_WaitResp: + ChannelData->State = Channel_Open; + break; + } + } + else + { + /* Configuration failed - close the channel */ + ChannelData->State = Channel_Closed; + } + } } -static inline void Bluetooth_Signal_DisconnectionReq(BT_ACL_Header_t* ACLPacketHeader, - BT_DataPacket_Header_t* DataHeader, - BT_Signal_Header_t* SignalCommandHeader) +/** Internal Bluetooth stack Signal Command processing routine for a Disconnection Request command. + * + * \param SignalCommandHeader Pointer to the start of the received packet's Signal Command header + */ +static inline void Bluetooth_Signal_DisconnectionReq(BT_Signal_Header_t* SignalCommandHeader) { BT_Signal_DisconnectionReq_t DisconnectionRequest; Pipe_Read_Stream_LE(&DisconnectionRequest, sizeof(DisconnectionRequest)); - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG("<< L2CAP Disconnection Request", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DisconnectionRequest.DestinationChannel); - BT_ACL_DEBUG("-- Source Channel: 0x%04X", DisconnectionRequest.SourceChannel); - #endif + BT_ACL_DEBUG(1, "<< L2CAP Disconnection Request", NULL); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DisconnectionRequest.DestinationChannel); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", DisconnectionRequest.SourceChannel); Pipe_ClearIN(); Pipe_Freeze(); - Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE); - Pipe_Unfreeze(); - BT_Signal_DisconnectionResp_t DisconnectionResponse; + /* Search for the referenced channel in the channel information list */ + Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionRequest.SourceChannel, true); - ACLPacketHeader->DataLength = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(DisconnectionResponse); - DataHeader->PayloadLength = sizeof(*SignalCommandHeader) + sizeof(DisconnectionResponse); - DataHeader->DestinationChannel = BT_CHANNEL_SIGNALING; - SignalCommandHeader->Code = BT_SIGNAL_DISCONNECTION_RESPONSE; - SignalCommandHeader->Length = sizeof(DisconnectionResponse); - - Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionRequest.SourceChannel, true); + struct + { + BT_Signal_Header_t SignalCommandHeader; + BT_Signal_DisconnectionResp_t DisconnectionResponse; + } ResponsePacket; + + /* Fill out the Signal Command header in the response packet */ + ResponsePacket.SignalCommandHeader.Code = BT_SIGNAL_DISCONNECTION_RESPONSE; + ResponsePacket.SignalCommandHeader.Identifier = SignalCommandHeader->Identifier; + ResponsePacket.SignalCommandHeader.Length = sizeof(ResponsePacket.DisconnectionResponse); + /* Fill out the Disconnection Response in the response packet */ + ResponsePacket.DisconnectionResponse.DestinationChannel = ChannelData->RemoteNumber; + ResponsePacket.DisconnectionResponse.SourceChannel = ChannelData->LocalNumber; + + Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL); + + /* If the channel was found in the channel list, close it */ if (ChannelData != NULL) ChannelData->State = Channel_Closed; + + BT_ACL_DEBUG(1, ">> L2CAP Disconnection Response", NULL); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.DisconnectionResponse.SourceChannel); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ResponsePacket.DisconnectionResponse.DestinationChannel); +} + +/** Internal Bluetooth stack Signal Command processing routine for a Disconnection Response command. + * + * \param SignalCommandHeader Pointer to the start of the received packet's Signal Command header + */ +static inline void Bluetooth_Signal_DisconnectionResp(BT_Signal_Header_t* SignalCommandHeader) +{ + BT_Signal_DisconnectionResp_t DisconnectionResponse; - DisconnectionResponse.DestinationChannel = ChannelData->LocalNumber; - DisconnectionResponse.SourceChannel = ChannelData->RemoteNumber; + Pipe_Read_Stream_LE(&DisconnectionResponse, sizeof(DisconnectionResponse)); - Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader)); - Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader)); - Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader)); - Pipe_Write_Stream_LE(&DisconnectionResponse, sizeof(DisconnectionResponse)); + BT_ACL_DEBUG(1, "<< L2CAP Disconnection Response", NULL); + BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DisconnectionResponse.DestinationChannel); + BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", DisconnectionResponse.SourceChannel); - Pipe_ClearOUT(); + Pipe_ClearIN(); Pipe_Freeze(); - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("Packet Sent", NULL); - BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF)); - BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel); - BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength); - #endif - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG(">> L2CAP Disconnection Response", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- Source Channel: 0x%04X", DisconnectionResponse.SourceChannel); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DisconnectionResponse.DestinationChannel); - #endif + /* Search for the referenced channel in the channel information list */ + Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionResponse.SourceChannel, true); + + /* If the channel was found in the channel list, close it */ + if (ChannelData != NULL) + ChannelData->State = Channel_Closed; } -static inline void Bluetooth_Signal_EchoReq(BT_ACL_Header_t* ACLPacketHeader, - BT_DataPacket_Header_t* DataHeader, - BT_Signal_Header_t* SignalCommandHeader) +/** Internal Bluetooth stack Signal Command processing routine for an Echo Request command. + * + * \param SignalCommandHeader Pointer to the start of the received packet's Signal Command header + */ +static inline void Bluetooth_Signal_EchoReq(BT_Signal_Header_t* SignalCommandHeader) { - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG("<< L2CAP Echo Request", NULL); - #endif + BT_ACL_DEBUG(1, "<< L2CAP Echo Request", NULL); Pipe_ClearIN(); Pipe_Freeze(); - Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE); - Pipe_Unfreeze(); - ACLPacketHeader->DataLength = sizeof(*DataHeader) + sizeof(*SignalCommandHeader); - DataHeader->PayloadLength = sizeof(*SignalCommandHeader); - DataHeader->DestinationChannel = BT_CHANNEL_SIGNALING; - SignalCommandHeader->Code = BT_SIGNAL_ECHO_RESPONSE; - SignalCommandHeader->Length = 0; - - Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader)); - Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader)); - Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader)); - - Pipe_ClearOUT(); - Pipe_Freeze(); + struct + { + BT_Signal_Header_t SignalCommandHeader; + } ResponsePacket; + + /* Fill out the Signal Command header in the response packet */ + ResponsePacket.SignalCommandHeader.Code = BT_SIGNAL_ECHO_RESPONSE; + ResponsePacket.SignalCommandHeader.Identifier = SignalCommandHeader->Identifier; + ResponsePacket.SignalCommandHeader.Length = 0; - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("Packet Sent", NULL); - BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF)); - BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel); - BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength); - #endif - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG(">> L2CAP Echo Response", NULL); - #endif + Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL); + + BT_ACL_DEBUG(1, ">> L2CAP Echo Response", NULL); } -static inline void Bluetooth_Signal_InformationReq(BT_ACL_Header_t* ACLPacketHeader, - BT_DataPacket_Header_t* DataHeader, - BT_Signal_Header_t* SignalCommandHeader) +/** Internal Bluetooth stack Signal Command processing routine for an Information Request command. + * + * \param SignalCommandHeader Pointer to the start of the received packet's Signal Command header + */ +static inline void Bluetooth_Signal_InformationReq(BT_Signal_Header_t* SignalCommandHeader) { BT_Signal_InformationReq_t InformationRequest; Pipe_Read_Stream_LE(&InformationRequest, sizeof(InformationRequest)); - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG("<< Information Request", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- Info Type: 0x%04X", InformationRequest.InfoType); - #endif + BT_ACL_DEBUG(1, "<< L2CAP Information Request", NULL); + BT_ACL_DEBUG(2, "-- Info Type: 0x%04X", InformationRequest.InfoType); Pipe_ClearIN(); Pipe_Freeze(); - Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE); - Pipe_Unfreeze(); - - BT_Signal_InformationResp_t InformationResponse; - uint8_t ResponseData[4]; - uint8_t ResponseLen; + struct + { + BT_Signal_Header_t SignalCommandHeader; + BT_Signal_InformationResp_t InformationResponse; + + uint8_t Data[4]; + } ResponsePacket; + + uint8_t DataLen = 0; + + /* Retrieve the requested information and store it in the outgoing packet, if found */ switch (InformationRequest.InfoType) { case BT_INFOREQ_MTU: - InformationResponse.Result = BT_INFORMATION_SUCCESSFUL; - ResponseLen = 2; + ResponsePacket.InformationResponse.Result = BT_INFORMATION_SUCCESSFUL; + DataLen = 2; - *((uint16_t*)&ResponseData) = 65533; + *((uint16_t*)&ResponsePacket.Data) = MAXIMUM_CHANNEL_MTU; break; case BT_INFOREQ_EXTENDEDFEATURES: - InformationResponse.Result = BT_INFORMATION_SUCCESSFUL; - ResponseLen = 4; + ResponsePacket.InformationResponse.Result = BT_INFORMATION_SUCCESSFUL; + DataLen = 4; - *((uint32_t*)&ResponseData) = 0; + *((uint32_t*)&ResponsePacket.Data) = 0; break; default: - InformationResponse.Result = BT_INFORMATION_NOTSUPPORTED; - ResponseLen = 0; + ResponsePacket.InformationResponse.Result = BT_INFORMATION_NOTSUPPORTED; + DataLen = 0; break; } - ACLPacketHeader->DataLength = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(InformationResponse) + - ResponseLen; - DataHeader->PayloadLength = sizeof(*SignalCommandHeader) + sizeof(InformationResponse) + ResponseLen; - DataHeader->DestinationChannel = BT_CHANNEL_SIGNALING; - SignalCommandHeader->Code = BT_SIGNAL_INFORMATION_RESPONSE; - SignalCommandHeader->Length = sizeof(InformationResponse) + ResponseLen; - - Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader)); - Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader)); - Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader)); - Pipe_Write_Stream_LE(&InformationResponse, sizeof(InformationResponse)); - Pipe_Write_Stream_LE(ResponseData, ResponseLen); + /* Fill out the Signal Command header in the response packet */ + ResponsePacket.SignalCommandHeader.Code = BT_SIGNAL_INFORMATION_RESPONSE; + ResponsePacket.SignalCommandHeader.Identifier = SignalCommandHeader->Identifier; + ResponsePacket.SignalCommandHeader.Length = sizeof(ResponsePacket.InformationResponse) + DataLen; + + /* Fill out the Information Response in the response packet */ + ResponsePacket.InformationResponse.InfoType = InformationRequest.InfoType; - Pipe_ClearOUT(); - Pipe_Freeze(); + Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket) - sizeof(ResponsePacket.Data) + DataLen), NULL); - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("Packet Sent", NULL); - BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF)); - BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength); - BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel); - BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength); - #endif - #if (ACL_DEBUG_LEVEL > 0) - BT_ACL_DEBUG(">> L2CAP Information Response", NULL); - #endif - #if (ACL_DEBUG_LEVEL > 1) - BT_ACL_DEBUG("-- Result: 0x%02X", InformationResponse.Result); - #endif + BT_ACL_DEBUG(1, ">> L2CAP Information Response", NULL); + BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.InformationResponse.Result); }