diff --git a/MQTTnet.Core/Client/MqttClient.cs b/MQTTnet.Core/Client/MqttClient.cs
index 3239f548..31d20daa 100644
--- a/MQTTnet.Core/Client/MqttClient.cs
+++ b/MQTTnet.Core/Client/MqttClient.cs
@@ -249,21 +249,25 @@ namespace MQTTnet.Core.Client
                 if (mqttPacket is MqttPingReqPacket)
                 {
                     await SendAsync(new MqttPingRespPacket());
+                    return;
                 }
 
                 if (mqttPacket is MqttDisconnectPacket)
                 {
                     await DisconnectAsync();
+                    return;
                 }
 
                 if (mqttPacket is MqttPublishPacket publishPacket)
                 {
                     await ProcessReceivedPublishPacket(publishPacket);
+                    return;
                 }
 
                 if (mqttPacket is MqttPubRelPacket pubRelPacket)
                 {
                     await ProcessReceivedPubRelPacket(pubRelPacket);
+                    return;
                 }
 
                 _packetDispatcher.Dispatch(mqttPacket);
@@ -336,8 +340,9 @@ namespace MQTTnet.Core.Client
 
         private async Task<TResponsePacket> SendAndReceiveAsync<TResponsePacket>(MqttBasePacket requestPacket) where TResponsePacket : MqttBasePacket
         {
+            var wait = _packetDispatcher.WaitForPacketAsync(requestPacket, typeof(TResponsePacket), _options.DefaultCommunicationTimeout);
             await _adapter.SendPacketsAsync(_options.DefaultCommunicationTimeout, requestPacket).ConfigureAwait(false);
-            return (TResponsePacket)await _packetDispatcher.WaitForPacketAsync(requestPacket, typeof(TResponsePacket), _options.DefaultCommunicationTimeout).ConfigureAwait(false);
+            return (TResponsePacket)await wait.ConfigureAwait(false);
         }
 
         private ushort GetNewPacketIdentifier()
diff --git a/MQTTnet.Core/Server/MqttClientMessageQueue.cs b/MQTTnet.Core/Server/MqttClientMessageQueue.cs
index a8845e05..ae7cb7a7 100644
--- a/MQTTnet.Core/Server/MqttClientMessageQueue.cs
+++ b/MQTTnet.Core/Server/MqttClientMessageQueue.cs
@@ -54,6 +54,12 @@ namespace MQTTnet.Core.Server
             var consumable = _pendingPublishPackets.GetConsumingEnumerable();
             while (!cancellationToken.IsCancellationRequested)
             {
+                if (_pendingPublishPackets.Count == 0)
+                {
+                    await Task.Delay(TimeSpan.FromMilliseconds(5)).ConfigureAwait(false);
+                    continue;
+                }
+
                 var packets = consumable.Take(_pendingPublishPackets.Count).ToList();
                 try
                 {