diff --git a/MQTTSNGateway/src/MQTTGWPublishHandler.cpp b/MQTTSNGateway/src/MQTTGWPublishHandler.cpp
index c6ebcef..12ad28e 100644
--- a/MQTTSNGateway/src/MQTTGWPublishHandler.cpp
+++ b/MQTTSNGateway/src/MQTTGWPublishHandler.cpp
@@ -12,6 +12,7 @@
  *
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
  **************************************************************************************/
 
 #include "MQTTGWPublishHandler.h"
@@ -113,6 +114,8 @@ void MQTTGWPublishHandler::handlePublish(Client* client, MQTTGWPacket* packet)
 				{
 					replyACK(client, &pub, PUBREC);
 				}
+
+				delete snPacket;
 				return;
 			}
 
@@ -126,6 +129,7 @@ void MQTTGWPublishHandler::handlePublish(Client* client, MQTTGWPacket* packet)
 				MQTTSNPacket* regPacket = new MQTTSNPacket();
 
 				MQTTSNString topicName;
+				topicName.cstring = 0;
 				topicName.lenstring.len = topicId.data.long_.len;
 				topicName.lenstring.data = topicId.data.long_.name;
 
diff --git a/MQTTSNGateway/src/MQTTSNGWClient.cpp b/MQTTSNGateway/src/MQTTSNGWClient.cpp
index 9a8a043..c0a7d03 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClient.cpp
@@ -12,6 +12,7 @@
  *
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
  **************************************************************************************/
 
 #include "MQTTSNGWClient.h"
@@ -123,7 +124,7 @@ bool ClientList::authorize(const char* fileName)
 	return _authorize;
 }
 
-void ClientList::erase(Client* client)
+void ClientList::erase(Client*& client)
 {
 	if ( !_authorize && client->erasable())
 	{
@@ -236,6 +237,8 @@ Client* ClientList::createClient(SensorNetAddress* addr, MQTTSNString* clientId,
 		 free(dummyId.cstring);
 	}
 
+	_mutex.lock();
+
 	/* add the list */
 	if ( _firstClient == 0 )
 	{
@@ -1152,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;
 }
 
diff --git a/MQTTSNGateway/src/MQTTSNGWClient.h b/MQTTSNGateway/src/MQTTSNGWClient.h
index 690f98b..3b52fa1 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.h
+++ b/MQTTSNGateway/src/MQTTSNGWClient.h
@@ -12,6 +12,7 @@
  *
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
  **************************************************************************************/
 
 #ifndef MQTTSNGWCLIENT_H_
@@ -341,7 +342,7 @@ public:
 	ClientList();
 	~ClientList();
 	bool authorize(const char* fileName);
-	void erase(Client*);
+	void erase(Client*&);
 	Client* getClient(SensorNetAddress* addr);
 	Client* getClient(uint8_t* clientId);
 	Client* createClient(SensorNetAddress* addr, MQTTSNString* clientId, bool unstableLine,
diff --git a/MQTTSNGateway/src/MQTTSNGWPacketHandleTask.cpp b/MQTTSNGateway/src/MQTTSNGWPacketHandleTask.cpp
index 43cdd24..02dd9ab 100644
--- a/MQTTSNGateway/src/MQTTSNGWPacketHandleTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWPacketHandleTask.cpp
@@ -12,6 +12,7 @@
  *
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
  **************************************************************************************/
 
 #include "MQTTSNGWDefines.h"
@@ -155,6 +156,7 @@ void PacketHandleTask::run()
 			case MQTTSN_PUBACK:
 				_mqttsnPublish->handlePuback(client, snPacket);
 				break;
+			case MQTTSN_PUBREC:
 				_mqttsnPublish->handleAck(client, snPacket, PUBREC);
 				break;
 			case MQTTSN_PUBREL:
diff --git a/MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp b/MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp
index ea7f9b0..24b077d 100644
--- a/MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWPublishHandler.cpp
@@ -12,6 +12,7 @@
  *
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
  **************************************************************************************/
 
 #include "MQTTSNGWPublishHandler.h"
@@ -180,8 +181,6 @@ void MQTTSNPublishHandler::handlePuback(Client* client, MQTTSNPacket* packet)
 
 	if ( client->isActive() )
 	{
-		MQTTGWPacket* pubAck = new MQTTGWPacket();
-
 		if ( packet->getPUBACK(&topicId, &msgId, &rc) == 0 )
 		{
 			return;
@@ -189,6 +188,7 @@ void MQTTSNPublishHandler::handlePuback(Client* client, MQTTSNPacket* packet)
 
 		if ( rc == MQTTSN_RC_ACCEPTED)
 		{
+			MQTTGWPacket* pubAck = new MQTTGWPacket();
 			pubAck->setAck(PUBACK, msgId);
 			Event* ev1 = new Event();
 			ev1->setBrokerSendEvent(client, pubAck);
@@ -226,10 +226,8 @@ void MQTTSNPublishHandler::handleRegister(Client* client, MQTTSNPacket* packet)
 	MQTTSNString topicName;
 	MQTTSN_topicid topicid;
 
-
 	if ( client->isActive() || client->isAwake())
 	{
-		MQTTSNPacket* regAck = new MQTTSNPacket();
 		if ( packet->getREGISTER(&id, &msgId, &topicName) == 0 )
 		{
 			return;
@@ -240,6 +238,8 @@ void MQTTSNPublishHandler::handleRegister(Client* client, MQTTSNPacket* packet)
 		topicid.data.long_.name = topicName.lenstring.data;
 
 		id = client->getTopics()->add(&topicid)->getTopicId();
+
+		MQTTSNPacket* regAck = new MQTTSNPacket();
 		regAck->setREGACK(id, msgId, MQTTSN_RC_ACCEPTED);
 		Event* ev = new Event();
 		ev->setClientSendEvent(client, regAck);
diff --git a/MQTTSNGateway/src/MQTTSNGWSubscribeHandler.cpp b/MQTTSNGateway/src/MQTTSNGWSubscribeHandler.cpp
index 3b93e41..5d4aab8 100644
--- a/MQTTSNGateway/src/MQTTSNGWSubscribeHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWSubscribeHandler.cpp
@@ -12,6 +12,7 @@
  *
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation and/or initial documentation
+ *    Tieto Poland Sp. z o.o. - Gateway improvements
  **************************************************************************************/
 
 #include "MQTTSNGWSubscribeHandler.h"
@@ -89,6 +90,7 @@ void MQTTSNSubscribeHandler::handleSubscribe(Client* client, MQTTSNPacket* packe
 		}
 		else
 		{
+			uint16_t topicId = 0;
 			MQTTGWPacket* subscribe = new MQTTGWPacket();
 			topic = client->getTopics()->getTopic(&topicFilter);
 			if (topic == 0)
@@ -96,6 +98,7 @@ void MQTTSNSubscribeHandler::handleSubscribe(Client* client, MQTTSNPacket* packe
 				if (topicFilter.type == MQTTSN_TOPIC_TYPE_NORMAL)
 				{
 					topic = client->getTopics()->add(&topicFilter);
+					topicId = topic->getTopicId();
 					subscribe->setSUBSCRIBE((char*)topic->getTopicName()->c_str(), (uint8_t)qos, (uint16_t)msgId);
 				}
 				else if (topicFilter.type == MQTTSN_TOPIC_TYPE_SHORT)
@@ -104,17 +107,19 @@ void MQTTSNSubscribeHandler::handleSubscribe(Client* client, MQTTSNPacket* packe
 					topic[0] = topicFilter.data.short_name[0];
 					topic[1] = topicFilter.data.short_name[1];
 					topic[2] = 0;
+					topicId = topicFilter.data.id;
 					subscribe->setSUBSCRIBE(topic, (uint8_t)qos, (uint16_t)msgId);
 				}
 			}
 			else
 			{
+				topicId = topic->getTopicId();
 				subscribe->setSUBSCRIBE((char*)topic->getTopicName()->c_str(), (uint8_t)qos, (uint16_t)msgId);
 			}
 
 			if ( msgId > 0 )
 			{
-				client->setWaitedSubTopicId(msgId, topic->getTopicId(), topicFilter.type);
+				client->setWaitedSubTopicId(msgId, topicId, topicFilter.type);
 			}
 
 			Event* ev1 = new Event();
@@ -180,7 +185,6 @@ void MQTTSNSubscribeHandler::handleUnsubscribe(Client* client, MQTTSNPacket* pac
 	}
 	else if (topicFilter.type == MQTTSN_TOPIC_TYPE_SHORT)
 	{
-		MQTTGWPacket* unsubscribe = new MQTTGWPacket();
 		char shortTopic[3];
 		shortTopic[0] = topicFilter.data.short_name[0];
 		shortTopic[1] = topicFilter.data.short_name[1];