Fix Bluetooth stack CONNECTION RESPONSE packets - for this command and this command...

[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 45fad7b..449d57f 100644 (file)
--- a/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
+++ b/Demos/Host/Incomplete/BluetoothHost/Lib/BluetoothACLPackets.c
@@ -141,6 +141,17 @@ static void Bluetooth_ProcessACLPackets(void)
                        case BT_SIGNAL_INFORMATION_REQUEST:\r
                                Bluetooth_Signal_InformationReq(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);\r
                                break;\r
                        case BT_SIGNAL_INFORMATION_REQUEST:\r
                                Bluetooth_Signal_InformationReq(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);\r
                                break;\r

+                       case BT_SIGNAL_COMMAND_REJECT:\r
+                               BT_ACL_DEBUG(1, "<< Command Reject", NULL);\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
                                BT_ACL_DEBUG(1, "<< Unknown Signaling Command 0x%02X", SignalCommandHeader.Code);\r
        \r
                        default:\r
                                BT_ACL_DEBUG(1, "<< Unknown Signaling Command 0x%02X", SignalCommandHeader.Code);\r
        \r


@@ -315,9 +326,10 @@ static inline void Bluetooth_Signal_ConnectionReq(BT_ACL_Header_t*        ACLPac
        ResponsePacket.SignalCommandHeader.Code              = BT_SIGNAL_CONNECTION_RESPONSE;\r
        ResponsePacket.SignalCommandHeader.Identifier        = SignalCommandHeader->Identifier;\r
        ResponsePacket.SignalCommandHeader.Length            = sizeof(ResponsePacket.ConnectionResponse);\r
        ResponsePacket.SignalCommandHeader.Code              = BT_SIGNAL_CONNECTION_RESPONSE;\r
        ResponsePacket.SignalCommandHeader.Identifier        = SignalCommandHeader->Identifier;\r
        ResponsePacket.SignalCommandHeader.Length            = sizeof(ResponsePacket.ConnectionResponse);\r

-       ResponsePacket.ConnectionResponse.Result             = (ChannelData == NULL) ? BT_CONNECTION_REFUSED_RESOURCES : BT_CONNECTION_SUCCESSFUL;\r
-       ResponsePacket.ConnectionResponse.DestinationChannel = ChannelData->RemoteNumber;\r
-       ResponsePacket.ConnectionResponse.SourceChannel      = ChannelData->LocalNumber;\r
+       ResponsePacket.ConnectionResponse.DestinationChannel = ChannelData->LocalNumber;\r
+       ResponsePacket.ConnectionResponse.SourceChannel      = ChannelData->RemoteNumber;\r
+       ResponsePacket.ConnectionResponse.Result             = (ChannelData == NULL) ? BT_CONNECTION_REFUSED_RESOURCES :\r
+                                                                                      BT_CONNECTION_SUCCESSFUL;\r

        ResponsePacket.ConnectionResponse.Status             = 0x00;\r
        \r
        Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);\r
        ResponsePacket.ConnectionResponse.Status             = 0x00;\r
        \r
        Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);\r