Merge pull request #43 from ty4tw/gateway

BugFix of Issue #41

MQTTSNGateway/src/MQTTSNGWBrokerRecvTask.cpp

@@ -16,6 +16,7 @@
 
 #include "MQTTSNGWBrokerRecvTask.h"
 #include "MQTTSNGWClient.h"
+#include <unistd.h>
 
 using namespace std;
 using namespace MQTTSNGW;
@@ -90,7 +91,11 @@ void BrokerRecvTask::run(void)
 			client = client->getNextClient();
 		}
 
-		if (maxSock > 0)
+		if (maxSock == 0)
+		{
+			usleep(500 * 1000);
+		}
+		else
 		{
 			/* Check sockets is ready to read */
 			int activity = select(maxSock + 1, &rset, 0, 0, &timeout);
@@ -114,7 +119,6 @@ void BrokerRecvTask::run(void)
 							if ( rc > 0 )
 							{
 								if ( log(client, packet) == -1 )
-
 								{
 									delete packet;
 									goto nextClient;
@@ -127,7 +131,21 @@ void BrokerRecvTask::run(void)
 							}
 							else
 							{
-								if (rc == -1)
+								if ( rc == 0 )  // Disconnected
+								{
+									client->getNetwork()->close();
+									delete packet;
+
+									/* delete client when the client is not authorized on & session is clean */
+									_gateway->getClientList()->erase(client);
+
+									if ( client )
+									{
+										client = client->getNextClient();
+									}
+									continue;
+								}
+								else if (rc == -1)
 								{
 									WRITELOG("%s BrokerRecvTask can't receive a packet from the broker errno=%d %s%s\n", ERRMSG_HEADER, errno, client->getClientId(), ERRMSG_FOOTER);
 								}

MQTTSNGateway/src/MQTTSNGWClient.cpp

@@ -125,30 +125,34 @@ bool ClientList::authorize(const char* fileName)
 
 void ClientList::erase(Client* client)
 {
-	_mutex.lock();
-	Client* prev = client->_prevClient;
-	Client* next = client->_nextClient;
+	if ( !_authorize && client->erasable())
+	{
+		_mutex.lock();
+		Client* prev = client->_prevClient;
+		Client* next = client->_nextClient;
 
-	if (prev)
-	{
-		prev->_nextClient = next;
-	}
-	else
-	{
-		_firstClient = next;
+		if (prev)
+		{
+			prev->_nextClient = next;
+		}
+		else
+		{
+			_firstClient = next;
 
+		}
+		if (next)
+		{
+			next->_prevClient = prev;
+		}
+		else
+		{
+			_endClient = prev;
+		}
+		_clientCnt--;
+		delete client;
+		client = 0;
+		_mutex.unlock();
 	}
-	if (next)
-	{
-		next->_prevClient = prev;
-	}
-	else
-	{
-		_endClient = prev;
-	}
-	_clientCnt--;
-	_authorize = false;
-	_mutex.unlock();
 }
 
 Client* ClientList::getClient(SensorNetAddress* addr)
@@ -280,6 +284,8 @@ Client::Client(bool secure)
 	_sensorNetype = true;
 	_connAck = 0;
 	_waitWillMsgFlg = false;
+	_sessionStatus = false;
+	_otaClient = 0;
 	_prevClient = 0;
 	_nextClient = 0;
 }
@@ -389,6 +395,16 @@ void Client::setKeepAlive(MQTTSNPacket* packet)
 	}
 }
 
+void Client::setSessionStatus(bool status)
+{
+	_sessionStatus = status;
+}
+
+bool Client::erasable(void)
+{
+	return _sessionStatus;
+}
+
 void Client::updateStatus(MQTTSNPacket* packet)
 {
 	if (((_status == Cstat_Disconnected) || (_status == Cstat_Lost)) && packet->getType() == MQTTSN_CONNECT)

MQTTSNGateway/src/MQTTSNGWClient.h

@@ -271,6 +271,8 @@ public:
 	char* getWillMsg(void);
 	const char* getStatus(void);
 	void setWaitWillMsgFlg(bool);
+	void setSessionStatus(bool);  // true: clean session
+	bool erasable(void);
 
 	bool isDisconnect(void);
 	bool isActive(void);
@@ -312,6 +314,8 @@ private:
 	bool _sensorNetype;     // false: unstable network like a G3
 	SensorNetAddress _sensorNetAddr;
 
+	bool _sessionStatus;
+
 	Client* _nextClient;
 	Client* _prevClient;
 	Client* _otaClient;

MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp

@@ -55,14 +55,14 @@ void MQTTSNConnectionHandler::handleSearchgw(MQTTSNPacket* packet)
 {
 	if (packet->getType() == MQTTSN_SEARCHGW)
 	{
-		if (_gateway->getClientList()->getClientCount() < MAX_CLIENTS)
-		{
+		//if (_gateway->getClientList()->getClientCount() < MAX_CLIENTS)
+		//{
 			MQTTSNPacket* gwinfo = new MQTTSNPacket();
 			gwinfo->setGWINFO(_gateway->getGWParams()->gatewayId);
 			Event* ev1 = new Event();
 			ev1->setBrodcastEvent(gwinfo);
 			_gateway->getClientSendQue()->post(ev1);
-		}
+		//}
 	}
 }
 
@@ -94,6 +94,7 @@ void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet
 		connectData->flags.bits.username = 1;
 	}
 
+	client->setSessionStatus(false);
 	if (data.cleansession)
 	{
 		connectData->flags.bits.cleanstart = 1;
@@ -108,6 +109,7 @@ void MQTTSNConnectionHandler::handleConnect(Client* client, MQTTSNPacket* packet
 		}
 		topics = new Topics();
 		client->setTopics(topics);
+		client->setSessionStatus(true);
 	}
 
 	if (data.willFlag)

MQTTSNGateway/src/MQTTSNGWProcess.h

@@ -253,6 +253,729 @@ private:
 	QueElement<T>* _tail;
 };
 
+/*=====================================
+ Class Tree23
+ ====================================*/
+#define TREE23_INSERT_ACTIVE  (1)
+#define TREE23_DELETE_ACTIVE  (2)
+#define TREE23_BI_NODE        (3)
+#define TREE23_TRI_NODE       (4)
+
+template <typename K, typename V>
+class Tree23Elm{
+	template<typename T, typename U> friend class Tree23;
+public:
+	Tree23Elm()
+	{
+		_key = 0;
+		_val = 0;
+	}
+
+	Tree23Elm(K* key, V* val)
+	{
+		_key = key;
+		_val = val;
+	}
+
+	~Tree23Elm()
+	{
+
+	}
+
+	int compare(Tree23Elm<K, V>* elm)
+	{
+		return _key->compare(elm->_key);
+	}
+private:
+	K* _key;
+	V* _val;
+};
+
+
+template <typename K, typename V>
+class Tree23Node{
+	template<typename S, typename W> friend class Tree23;
+public:
+	Tree23Node(const int type)
+	{
+		_type = type;
+		_telm0 = _telm1 = NULL;
+		_left = _midle = _right = NULL;
+	}
+
+	Tree23Node(const int type, Tree23Node<K, V>* midle)
+	{
+		_type = type;
+		_telm0 = _telm1 = NULL;
+		_left = _right = NULL;
+		_midle = midle;
+	}
+
+	Tree23Node(const int type, Tree23Elm<K, V>* telm)
+	{
+		_type = type;
+		_telm0 = telm;
+		_telm1 = NULL;
+		_left = _midle = _right = NULL;
+	}
+
+	Tree23Node(const int type, Tree23Elm<K, V>* telm, Tree23Node<K, V>* left, Tree23Node<K, V>* right)
+	{
+		_type = type;
+		_telm0 = telm;
+		_telm1 = NULL;
+		_left = left;
+		_midle = NULL;
+		_right = right;
+	}
+
+	Tree23Node(const int type, Tree23Elm<K, V>* telm0, Tree23Elm<K, V>* telm1, Tree23Node<K, V>* left, Tree23Node<K, V>* midle, Tree23Node<K, V>* right)
+	{
+		_type = type;
+		_telm0 = telm0;
+		_telm1 = telm1;
+		_left = left;
+		_midle = midle;
+		_right = right;
+	}
+
+	~Tree23Node()
+	{
+
+	}
+
+private:
+	int   _type;
+	Tree23Elm<K, V>* _telm0;
+	Tree23Elm<K, V>* _telm1;
+	Tree23Node<K, V>* _left;
+	Tree23Node<K, V>* _midle;
+	Tree23Node<K, V>* _right;
+};
+
+template <typename K, typename V>
+class Tree23{
+public:
+	Tree23()
+	{
+		_root = NULL;
+	}
+
+	~Tree23()
+	{
+		if ( _root )
+		{
+			delete _root;
+		}
+	}
+
+	void add(K* key, V* val)
+	{
+		_root = add( _root, new Tree23Elm<K, V>(key, val));
+		_root->_type = abs(_root->_type);
+	}
+
+	Tree23Node<K, V>* add(Tree23Node<K, V>* n, Tree23Elm<K, V>* elm)
+	{
+		if ( n == 0 )
+		{
+			return new Tree23Node<K, V>(TREE23_INSERT_ACTIVE, elm);
+		}
+
+		int cmp0 = elm->compare(n->_telm0);
+		int cmp1 = 0;
+		switch ( n->_type )
+		{
+		case 2:
+			if ( cmp0 < 0 )
+			{
+				n->_left = add(n->_left, elm);
+				return addLeft2(n);
+			}
+			else if ( cmp0 == 0 )
+			{
+				n->_telm0 = elm;
+				return n;
+			}
+			else
+			{
+				n->_right = add(n->_right, elm);
+				return addRight2(n);
+			}
+			break;
+		case 3:
+			cmp1 = elm->compare(n->_telm1);
+			if ( cmp0 < 0 )
+			{
+				n->_left = add(n->_left, elm);
+				return addLeft3(n);
+			}
+			else if ( cmp0 == 0 )
+			{
+				n->_telm0 = elm;
+				return n;
+			}
+			else if ( cmp1 < 0 )
+			{
+				n->_midle = add(n->_midle, elm);
+				return addMidle3(n);
+			}
+			else if ( cmp1 == 0 )
+			{
+				n->_telm1 = elm;
+				return n;
+			}
+			else
+			{
+				n->_right = add(n->_right, elm);
+				return addRight3(n);
+			}
+			break;
+		default:
+			break;
+		}
+		return 0;
+	}
+
+	void remove(K* k)
+	{
+		_root = remove(_root, k);
+		if ( _root != NULL && _root->_type == TREE23_DELETE_ACTIVE )
+		{
+			_root = _root->_midle;
+		}
+	}
+
+	Tree23Node<K, V>* remove(Tree23Node<K, V>* node, K* k)
+	{
+		if ( node == NULL )
+		{
+			return NULL;
+		}
+		int cmp0 = k->compare(node->_telm0->_key);
+		int cmp1 = 0;
+		switch ( node->_type )
+		{
+		case 2:
+			if ( cmp0 < 0 )
+			{
+				node->_left = remove( node->_left, k);
+				return removeLeft2(node);
+			}
+			else if ( cmp0 == 0 )
+			{
+				if ( node->_left == NULL)
+				{
+					return new Tree23Node<K, V>(TREE23_DELETE_ACTIVE);
+				}
+				Tree23Elm<K, V>* maxLeft = new Tree23Elm<K, V>();
+				node->_left = removeMax(node->_left, maxLeft);
+				node->_telm0 = maxLeft;
+				return removeLeft2(node);
+			}
+			else
+			{
+				node->_right = remove(node->_right, k);
+				return removeRight2(node);
+			}
+		case 3:
+			cmp1 = k->compare(node->_telm1->_key);
+			if ( cmp0 < 0 )
+			{
+				node->_left = remove(node->_left, k);
+				return removeLeft3(node);
+			}
+			else if ( cmp0 == 0 )
+			{
+				if ( node->_left == NULL )
+				{
+					return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1);
+				}
+				Tree23Elm<K, V>* maxLeft = new Tree23Elm<K, V>();
+				node->_left = removeMax(node->_left, maxLeft);
+				node->_telm0 = maxLeft;
+				return removeLeft3(node);
+			}
+			else if ( cmp1 < 0 )
+			{
+				node->_midle = remove(node->_midle, k);
+				return removeMidle3(node);
+			}
+			else if ( cmp1 == 0 )
+			{
+				if ( node->_midle == NULL )
+				{
+					return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0);
+				}
+				Tree23Elm<K, V>* maxMidle = new Tree23Elm<K, V>();
+				node->_midle = removeMax(node->_midle, maxMidle);
+				node->_telm1 = maxMidle;
+				return removeMidle3(node);
+			}
+			else
+			{
+				node->_right = remove(node->_right, k);
+				return removeRight3(node);
+			}
+		default:
+			break;
+		}
+		return NULL;
+	}
+
+	bool find(K* key)
+	{
+		Tree23Node<K, V>* node = _root;
+		while (node != NULL)
+		{
+			int cmp0 = key->compare(node->_telm0->_key);
+			int cmp1 = 0;
+			switch (node->_type)
+			{
+			case 2:
+				if ( cmp0 <  0 ) node = node->_left;
+				else if ( cmp0 == 0 )
+				{
+				return true;
+				}
+				else
+				{
+					node = node->_right;
+				}
+				break;
+			case 3:
+				cmp1 = key->compare(node->_telm1->_key);
+				if ( cmp0 <  0 )
+				{
+					node = node->_left;
+				}
+				else if ( cmp0 == 0 )
+				{
+					return true;
+				}
+				else if ( cmp1 <  0 )
+				{
+					node = node->_midle;
+				}
+				else if ( cmp1 == 0 )
+				{
+					return true;
+				}
+				else
+				{
+					node = node->_right;
+				}
+				break;
+			default:
+				break;
+			}
+		}
+		return false;
+	}
+
+
+	V* getVal(K* key)
+	{
+		Tree23Node<K, V>* node = _root;
+		while (node != NULL)
+		{
+			int cmp0 = key->compare(node->_telm0->_key);
+			int cmp1 = 0;
+			switch (node->_type)
+			{
+			case 2:
+				if ( cmp0 <  0 )
+				{
+					node = node->_left;
+				}
+				else if ( cmp0 == 0 )
+				{
+					return node->_telm0->_val;
+				}
+				else
+				{
+					node = node->_right;
+				}
+				break;
+			case 3:
+				cmp1 = key->compare(node->_telm1->_key);
+				if ( cmp0 < 0 )
+				{
+					node = node->_left;
+				}
+				else if ( cmp0 == 0 )
+				{
+					return node->_telm0->_val;
+				}
+				else if ( cmp1 <  0 )
+				{
+					node = node->_midle;
+				}
+				else if ( cmp1 == 0 )
+				{
+					return node->_telm1->_val;
+				}
+				else
+				{
+					node = node->_right;
+				}
+				break;
+			default:
+				break;
+			}
+		}
+		return NULL;
+	}
+
+private:
+	Tree23Node<K, V>* addLeft2(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n = node->_left;
+		if ( n != NULL && n->_type == TREE23_INSERT_ACTIVE )
+		{
+			return new Tree23Node<K, V>(TREE23_TRI_NODE, n->_telm0, node->_telm0, n->_left, n->_right, node->_right);
+		}
+		return node;
+	}
+
+	Tree23Node<K, V>* addLeft3(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n = node->_left;
+		if ( n != NULL && n->_type == TREE23_INSERT_ACTIVE)
+		{
+			n->_type = TREE23_BI_NODE;
+			Tree23Node<K, V>* nn = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, node->_midle, node->_right);
+			return new Tree23Node<K, V>(TREE23_INSERT_ACTIVE, node->_telm0, n, nn);
+		}
+		return node;
+	}
+
+	Tree23Node<K, V>* addRight2(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n = node->_right;
+		if (n != NULL &&  n->_type == TREE23_INSERT_ACTIVE)
+		{
+			return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, n->_telm0, node->_left, n->_left, n->_right);
+		}
+		return node;
+	}
+
+	Tree23Node<K, V>* addRight3(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n = node->_right;
+		if (n != NULL &&  n->_type == TREE23_INSERT_ACTIVE) {
+			n->_type = TREE23_BI_NODE;
+			Tree23Node<K, V>* nn = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, node->_left, node->_midle);
+			return new Tree23Node<K, V>(TREE23_INSERT_ACTIVE, node->_telm1, nn, n);
+		}
+		return node;
+	}
+
+	Tree23Node<K, V>* addMidle3(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n = node->_midle;
+		if ( n != NULL && n->_type == TREE23_INSERT_ACTIVE )
+		{
+			n->_left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, node->_left, n->_left);
+			n->_right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, n->_right, node->_right);
+			return n;
+		}
+		return node;
+	}
+
+
+	Tree23Node<K, V>* removeMax(Tree23Node<K, V>* node, Tree23Elm<K, V>* elm)
+	{
+		if (node->_right == NULL)
+		{
+			switch (node->_type)
+			{
+				case 2:
+					elm->_key = node->_telm0->_key;
+					elm->_val = node->_telm0->_val;
+					return new Tree23Node<K, V>(TREE23_DELETE_ACTIVE);
+				case 3:
+					elm->_key = node->_telm1->_key;
+					elm->_val = node->_telm1->_val;
+					return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0);
+				default:
+					break;
+			}
+		}
+		else
+		{
+			node->_right = removeMax(node->_right, elm);
+			switch (node->_type)
+			{
+			case 2:
+				return removeRight2(node);
+			case 3:
+				return removeRight3(node);
+			default:
+			   break;
+			}
+		}
+		return NULL;
+	}
+
+
+	Tree23Node<K, V>* removeLeft2(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n  = node->_left;
+		if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+		{
+			Tree23Node<K, V>* r = node->_right;
+			Tree23Node<K, V>* midle;
+			Tree23Node<K, V>* left;
+			Tree23Node<K, V>* right;
+
+			switch ( r->_type )
+			{
+			case 2:
+				midle = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, r->_telm0, n->_midle, r->_left, r->_right);
+				return new Tree23Node<K, V>(TREE23_DELETE_ACTIVE, midle);
+			case 3:
+				left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, n->_midle, r->_left);
+				right = new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm1, r->_midle, r->_right);
+				return new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm0, left, right);
+			default:
+				break;
+			}
+		}
+		return node;
+	}
+
+	Tree23Node<K, V>* removeRight2(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n  = node->_right;
+		if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+		{
+			Tree23Node<K, V>* l = node->_left;
+			Tree23Node<K, V>* midle;
+			Tree23Node<K, V>* left;
+			Tree23Node<K, V>* right;
+
+			switch (l->_type)
+			{
+			case 2:
+				midle = new Tree23Node<K, V>(TREE23_TRI_NODE, l->_telm0, node->_telm0, l->_left, l->_right, n->_midle);
+				return new Tree23Node<K, V>(-1, midle);
+			case 3:
+				right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, l->_right, n->_midle);
+				left = new Tree23Node<K, V>(TREE23_BI_NODE, l->_telm0, l->_left, l->_midle);
+				return new Tree23Node<K, V>(TREE23_BI_NODE, l->_telm1, left, right);
+			default:
+				break;
+			}
+		}
+		return node;
+	}
+
+	Tree23Node<K, V>* removeLeft3(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n  = node->_left;
+		if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+		{
+			Tree23Node<K, V>* m = node->_midle;
+			Tree23Node<K, V>* r = node->_right;
+			Tree23Node<K, V>* left;
+			Tree23Node<K, V>* midle;
+
+			switch (m->_type) {
+			case 2:
+				left = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, m->_telm0, n->_midle, m->_left, m->_right);
+				return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, left, r);
+			case 3:
+				left = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, n->_midle, m->_left);
+				midle = new Tree23Node<K, V>(TREE23_BI_NODE, m->_telm1, m->_midle, m->_right);
+				return new Tree23Node<K, V>(TREE23_TRI_NODE, m->_telm0, node->_telm1, left, midle, r);
+			default:
+				break;
+			}
+		}
+		return node;
+	}
+
+	Tree23Node<K, V>* removeMidle3(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n  = node->_midle;
+	        if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+	        {
+	        	Tree23Node<K, V>* l = node->_left;
+	        	Tree23Node<K, V>* r = node->_right;
+	        	Tree23Node<K, V>* midle;
+	        	Tree23Node<K, V>* right;
+	            switch (r->_type)
+	            {
+				case 2:
+					right = new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm1, r->_telm0, n->_midle, r->_left, r->_right);
+					return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, l, right);
+				case 3:
+					midle = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, n->_midle, r->_left);
+					right = new Tree23Node<K, V>(TREE23_BI_NODE, r->_telm1, r->_midle, r->_right);
+					return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, r->_telm0, l, midle, right);
+				default:
+					break;
+	            }
+	        }
+	        return node;
+	}
+
+	Tree23Node<K, V>* removeRight3(Tree23Node<K, V>* node)
+	{
+		Tree23Node<K, V>* n = node->_right;
+	        if ( n != NULL && n->_type == TREE23_DELETE_ACTIVE )
+	        {
+	        	Tree23Node<K, V>* l = node->_left;
+				Tree23Node<K, V>* m = node->_midle;
+				Tree23Node<K, V>* midle;
+				Tree23Node<K, V>* right;
+	            switch (m->_type)
+	            {
+				case 2:
+					right = new Tree23Node<K, V>(TREE23_TRI_NODE, m->_telm0, node->_telm1, m->_left, m->_right, n->_midle);
+					return new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm0, l, right);
+				case 3:
+					right = new Tree23Node<K, V>(TREE23_BI_NODE, node->_telm1, m->_right, n->_midle);
+					midle = new Tree23Node<K, V>(TREE23_BI_NODE, m->_telm0, m->_left, m->_midle);
+					return new Tree23Node<K, V>(TREE23_TRI_NODE, node->_telm0, m->_telm1, l, midle, right);
+				default:
+					break;
+	            }
+	        }
+	        return node;
+	}
+
+
+	Tree23Node<K, V>* _root;
+};
+
+/*=====================================
+ Class List
+ =====================================*/
+template <typename T>
+class ListElm
+{
+	template<typename U> friend class List;
+public:
+	ListElm()
+	{
+		_elm = 0;
+		_prev = _next = 0;
+	}
+	ListElm(T* elm)
+	{
+		_elm = elm;
+		_prev = _next = 0;
+	}
+	T* getContent(void)
+	{
+		return _elm;
+	}
+	~ListElm(){}
+
+private:
+	ListElm<T>* getNext(void){return _next;}
+	T* _elm;
+	ListElm<T>* _prev;
+	ListElm<T>* _next;
+};
+
+
+template <typename T>
+class List{
+public:
+	List()
+	{
+		_head = _tail = 0;
+		_size = 0;
+	}
+	~List()
+	{
+		clear();
+	}
+
+	int add(T* t)
+	{
+		ListElm<T>* elm = new ListElm<T>(t);
+		if ( elm == 0 )
+		{
+			return 0;
+		}
+		if ( _head == 0 )
+		{
+			_head = elm;
+			_tail = elm;
+		}
+		else
+		{
+			elm->_prev = _tail;
+			_tail->_next = elm;
+			_tail = elm;
+		}
+		_size++;
+		return 1;
+	}
+
+	void erase(ListElm<T>* elm)
+	{
+		if ( _head == elm )
+		{
+			_head = elm->_next;
+			_size--;
+			delete elm;
+		}
+		else if ( _tail == elm )
+		{
+			_tail = elm->_prev;
+			elm->_prev->_next = 0;
+			_size--;
+			delete elm;
+		}
+		else
+		{
+			elm->_prev->_next = elm->_next;
+			elm->_next->_prev = elm->_prev;
+			_size--;
+			delete elm;
+		}
+	}
+	void clear(void)
+	{
+		ListElm<T>* p = _head;
+		while ( p )
+		{
+			ListElm<T>* q = p->_next;
+			delete p;
+			p = q;
+		}
+		_head = 0;
+		_tail = 0;
+		_size = 0;
+	}
+
+	ListElm<T>* getElm(void)
+	{
+		return _head;
+	}
+
+	ListElm<T>* getNext(ListElm<T>* elm)
+	{
+		return elm->getNext();
+	}
+
+	int getSize(void)
+	{
+		return _size;
+	}
+
+
+private:
+	ListElm<T>* _head;
+	ListElm<T>* _tail;
+	int _size;
+};
+
 
 extern Process* theProcess;
 extern MultiTaskProcess* theMultiTaskProcess;

MQTTSNGateway/src/MQTTSNGateway.h

@@ -25,7 +25,7 @@ namespace MQTTSNGW
 /*=================================
  *    Starting prompt
  ==================================*/
-#define GATEWAY_VERSION " * Version: 0.9.5"
+#define GATEWAY_VERSION " * Version: 0.9.6"
 
 #define PAHO_COPYRIGHT0 " * MQTT-SN Transparent Gateway"
 #define PAHO_COPYRIGHT1 " * Part of Project Paho in Eclipse"

MQTTSNGateway/src/linux/Network.cpp

@@ -503,7 +503,8 @@ int Network::recv(uint8_t* buf, uint16_t len)
 	}
 
 	_busy = true;
-	loop: do
+loop:
+	do
 	{
 		readBlockedOnWrite = false;
 		readBlocked = false;

MQTTSNGateway/src/linux/xbee/SensorNetwork.cpp

@@ -400,7 +400,7 @@ SerialPort::SerialPort()
 	_tio.c_iflag = IGNBRK | IGNPAR;
 	_tio.c_cflag = CS8 | CLOCAL | CRTSCTS;
 	_tio.c_cc[VINTR] = 0;
-	_tio.c_cc[VTIME] = 0;
+	_tio.c_cc[VTIME] = 10;   // 1 sec.
 	_tio.c_cc[VMIN] = 1;
 	_fd = 0;
 }

MQTTSNGateway/src/tests/TestProcess.cpp

@@ -19,6 +19,7 @@
 #include "TestProcess.h"
 #include "TestTopics.h"
 #include "TestQue.h"
+#include "TestTree23.h"
 #include "TestTopicIdMap.h"
 #include "MQTTSNGWProcess.h"
 #include "MQTTSNGWClient.h"
@@ -90,6 +91,11 @@ void TestProcess::run(void)
 	tque->test();
 	delete tque;
 
+	/* Test Tree23 */
+	TestTree23* tree23 = new TestTree23();
+	tree23->test();
+	delete tree23;
+
 	/* Test TopicTable */
 	TestTopics* testTopic = new TestTopics();
 	testTopic->test();
@@ -103,7 +109,7 @@ void TestProcess::run(void)
 
 
 	/* Test EventQue */
-	printf("EventQue test start.\n");
+	printf("Test  EventQue       ");
 	Client* client = new Client();
 	_evQue.setMaxSize(EVENT_CNT);
 	for ( int i = 0; i < EVENT_CNT + 4; i++ )
@@ -116,6 +122,4 @@ void TestProcess::run(void)
 	}
 
 	MultiTaskProcess::run();
-
-	printf("\n\nAll Tests completed.\n");
 }

MQTTSNGateway/src/tests/TestQue.cpp

@@ -35,7 +35,7 @@ void TestQue::test(void)
 	int* v = 0;
 	int i = 0;
 
-	printf("Que Test start.\n");
+	printf("Test  Que            ");
 	for ( i = 0; i < 10; i++ )
 	{
 		v = new int(i);
@@ -73,7 +73,7 @@ void TestQue::test(void)
 			delete p;
 		}
 	}
-	printf("Que test completed.\n\n");
+	printf("[ OK ]\n");
 }
 
 int* TestQue::front(void)

MQTTSNGateway/src/tests/TestTask.cpp

@@ -52,7 +52,7 @@ void TestTask::run(void)
 		{
 			assert(EVENT_CNT + 1 == evcnt);
 			delete ev;
-			printf("EventQue test complete.\n\n");
+			printf("[ OK ]\n");
 			break;
 		}
 		MQTTSNPacket* packet = ev->getMQTTSNPacket();
@@ -64,10 +64,10 @@ void TestTask::run(void)
 	{
 		if ( CHK_SIGINT)
 		{
-			printf("Task stopped.\n");
+			printf("\nTest  Task           [ OK ]\n");
 			return;
 		}
-		printf("Task is running. Enter CTRL+C\n");
+		printf("Enter CTRL+C\n");
 		sleep(1);
 	}
 }

MQTTSNGateway/src/tests/TestTopicIdMap.cpp

@@ -36,7 +36,7 @@ TestTopicIdMap::~TestTopicIdMap()
 void TestTopicIdMap::test(void)
 {
 	uint16_t id[MAXID];
-	printf("Test TopicIdMat start.\n");
+	printf("Test  TopicIdMat     ");
 
 	for ( int i = 0; i < MAXID; i++ )
 	{
@@ -120,6 +120,6 @@ void TestTopicIdMap::test(void)
 		//printf("TopicId=%d  msgId=%d type=%d\n", topicId, i, type);
 		assert( topicId == 0 );
 	}
-	printf("Test TopicIdMat completed.\n\n");
+	printf("[ OK ]\n");
 }
 

MQTTSNGateway/src/tests/TestTopics.cpp

@@ -34,7 +34,7 @@ TestTopics::~TestTopics()
 
 void TestTopics::test(void)
 {
-	printf("Topics Test start.\n");
+	printf("Test  Topics         ");
 
 	MQTTSN_topicid topic[12];
 	char tp[12][10];
@@ -179,5 +179,5 @@ void TestTopics::test(void)
 */
 		assert( t != 0);
 	}
-	printf("Topics Test complete.\n\n");
+	printf("[ OK ]\n");
 }

MQTTSNGateway/src/tests/TestTree23.cpp

@@ -0,0 +1,93 @@
+/**************************************************************************************
+ * Copyright (c) 2016, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation 
+ **************************************************************************************/
+#include "TestTree23.h"
+#include <stdio.h>
+#include <string>
+#include <cassert>
+
+using namespace std;
+using namespace MQTTSNGW;
+
+TestTree23::TestTree23()
+{
+
+}
+
+TestTree23::~TestTree23()
+{
+
+}
+
+void TestTree23::test(void)
+{
+	printf("Test  Tree23         ");
+	int N = 100;
+
+	Key* r1[100];
+	Integer* r2[100];
+
+	for ( int i = 0; i < N; i++)
+	{
+		char buff[5];
+		sprintf(buff,"%d", i);
+		r1[i] = new Key(string(buff));
+		r2[i] = new Integer(i);
+		this->add(r1[i], r2[i]);
+	}
+
+	for ( int i = 0; i < N; i++)
+	{
+		Integer* rc = this->getVal(r1[i]);
+		//printf("key=%d  val=%d\n", i, rc->_val);
+		assert(i == rc->_val);
+	}
+
+	for ( int i = 20; i < 50; i++)
+	{
+		this->remove(r1[i]);
+		//printf("key=%d  str=%s\n", i, r1[i]->_key.c_str());
+	}
+
+	for ( int i = 0; i < 20; i++)
+	{
+		bool rc = this->find(r1[i]);
+		assert(rc == true);
+		//printf("key=%d  find=%d\n", i, rc);
+		Integer* val = this->getVal(r1[i]);
+		//printf("key=%d  val=%d\n", i, val->_val);
+		assert(val->_val == i);
+	}
+	for (  int i = 20; i < 50; i++ )
+	{
+		bool rc = this->find(r1[i]);
+		assert(rc == false);
+		//printf("key=%d  find=%d\n", i, rc);
+		Integer* val = this->getVal(r1[i]);
+		//printf("key=%d  val=%d\n", i, val->_val);
+		assert(val == 0);
+	}
+	for ( int i = 50; i < N; i++)
+	{
+		bool rc = this->find(r1[i]);
+		assert(rc == true);
+		//printf("key=%d  find=%d\n", i, rc);
+		Integer* val = this->getVal(r1[i]);
+		//printf("key=%d  val=%d\n", i, val->_val);
+		assert(val->_val == i);
+	}
+	printf("[ OK ]\n");
+}
+

MQTTSNGateway/src/tests/TestTree23.h

@@ -0,0 +1,74 @@
+/**************************************************************************************
+ * Copyright (c) 2016, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation 
+ **************************************************************************************/
+#ifndef MQTTSNGATEWAY_SRC_TESTS_TESTTREE23_H_
+#define MQTTSNGATEWAY_SRC_TESTS_TESTTREE23_H_
+
+#include "MQTTSNGWProcess.h"
+
+namespace MQTTSNGW
+{
+
+class Integer
+{
+public:
+	int _val;
+	Integer(){_val = 0;}
+	Integer(int val){_val = val;}
+};
+
+class Key
+{
+public:
+	string _key;
+	Key(){};
+	Key(string key){_key = key;}
+	int compare(Key* obj){
+		if ( _key == obj->_key )
+		{
+			return 0;
+		}
+		else if ( _key < obj->_key )
+		{
+			return -1;
+		}
+		else
+		{
+			return 1;
+		}
+	}
+};
+
+
+
+class TestTree23
+{
+public:
+	TestTree23();
+	~TestTree23();
+	void add(Key* key, Integer* val){_tree23.add(key, val);}
+	Tree23Node<Key, Integer>* add(Tree23Node<Key, Integer>* n, Tree23Elm<Key, Integer>* elm){return _tree23.add(n, elm);}
+	void remove(Key* k){_tree23.remove(k);}
+	Tree23Node<Key, Integer>* remove(Tree23Node<Key, Integer>* node, Key* k){return _tree23.remove(node, k);}
+	bool find(Key* key){return _tree23.find(key);}
+	Integer* getVal(Key* key){return _tree23.getVal(key);}
+	void test(void);
+private:
+	Tree23<Key, Integer> _tree23;
+};
+
+}
+
+#endif /* MQTTSNGATEWAY_SRC_TESTS_TESTTREE23_H_ */

MQTTSNGateway/src/tests/mainTestProcess.cpp

@@ -26,6 +26,7 @@ int main(int argc, char** argv)
 	test->initialize(argc, argv);
 	test->run();
 	delete test;
+	printf("\nPass all tests. \n");
 	return 0;
 }
 

Makefile

@@ -41,6 +41,7 @@ $(SRCDIR)/$(OS)/Network.cpp \
 $(SRCDIR)/$(OS)/Threading.cpp \
 $(SRCDIR)/$(TEST)/TestProcess.cpp \
 $(SRCDIR)/$(TEST)/TestQue.cpp \
+$(SRCDIR)/$(TEST)/TestTree23.cpp \
 $(SRCDIR)/$(TEST)/TestTopics.cpp \
 $(SRCDIR)/$(TEST)/TestTopicIdMap.cpp \
 $(SRCDIR)/$(TEST)/TestTask.cpp