diff --git a/MQTTSNGateway/src/MQTTSNGWClient.cpp b/MQTTSNGateway/src/MQTTSNGWClient.cpp
index f5ab22c..ad7f48b 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClient.cpp
@@ -1155,8 +1155,12 @@ int WaitREGACKPacketList::setPacket(MQTTSNPacket* packet, uint16_t REGACKMsgId)
 		_first = elm;
 		_end = elm;
 	}
-	elm->_prev = _end;
-	_end->_next = elm;
+	else
+	{
+		_end->_next = elm;
+		elm->_prev = _end;
+		_end = elm;
+	}
 	return 1;
 }