/**
 * MQTT outgoing connect processing for a client
 * @param ip_address the TCP address:port to connect to
 * @param clientID the MQTT client id to use
 * @param cleansession MQTT cleansession flag
 * @param keepalive MQTT keepalive timeout in seconds
 * @param willMessage pointer to the will message to be used, if any
 * @param username MQTT 3.1 username, or NULL
 * @param password MQTT 3.1 password, or NULL
 * @return the new client structure
 */
int MQTTProtocol_connect(char* ip_address, Clients* aClient)
{
	int rc, port;
	char* addr;

	FUNC_ENTRY;
	aClient->good = 1;
	time(&(aClient->lastContact));

	addr = MQTTProtocol_addressPort(ip_address, &port);
	rc = Socket_new(addr, port, &(aClient->socket));
	if (rc == EINPROGRESS || rc == EWOULDBLOCK)
		aClient->connect_state = 1; /* TCP connect called */
	else if (rc == 0)
	{
		if ((rc = MQTTPacket_send_connect(aClient)) == 0)
			aClient->connect_state = 2; /* TCP connect completed, in which case send the MQTT connect packet */
		else
			aClient->connect_state = 0;
	}

	FUNC_EXIT_RC(rc);
	return rc;
}

int MQTTSPacket_send_subAck(Clients* client, MQTTS_Subscribe* sub, int topicId, int qos, char returnCode)
{
	MQTTS_SubAck packet;
	int rc = 0;
	char *buf, *ptr;
	int datalen = 6;

	FUNC_ENTRY;
	packet.header.len = 8;
	packet.header.type = MQTTS_SUBACK;

	ptr = buf = malloc(datalen);
	packet.flags.QoS = qos;
	writeChar(&ptr, packet.flags.all);
	writeInt(&ptr, topicId);
	writeInt(&ptr, sub->msgId);
	writeChar(&ptr, returnCode);
	rc = MQTTSPacket_send(client->socket, client->addr, packet.header, buf, datalen);
	free(buf);

	Log(LOG_PROTOCOL, 68, NULL, client->socket, client->addr, client->clientID, sub->msgId, topicId, returnCode, rc);
	FUNC_EXIT_RC(rc);
	return rc;
}

int MQTTClient_publishMessage(MQTTClient handle, const char* topicName, MQTTClient_message* message,
															 MQTTClient_deliveryToken* deliveryToken)
{
	int rc = MQTTCLIENT_SUCCESS;

	FUNC_ENTRY;
	if (message == NULL)
	{
		rc = MQTTCLIENT_NULL_PARAMETER;
		goto exit;
	}

	if (strncmp(message->struct_id, "MQTM", 4) != 0 || message->struct_version != 0)
	{
		rc = MQTTCLIENT_BAD_STRUCTURE;
		goto exit;
	}

	rc = MQTTClient_publish(handle, topicName, message->payloadlen, message->payload,
								message->qos, message->retained, deliveryToken);
exit:
	FUNC_EXIT_RC(rc);
	return rc;
}

/**
  * Serializes the ack packet into the supplied buffer.
  * @param buf the buffer into which the packet will be serialized
  * @param buflen the length in bytes of the supplied buffer
  * @param type the MQTT packet type
  * @param dup the MQTT dup flag
  * @param packetid the MQTT packet identifier
  * @return serialized length, or error if 0
  */
int MQTTSerialize_ack(unsigned char* buf, int buflen, unsigned char packettype, unsigned char dup, unsigned short packetid)
{
	MQTTHeader header = {0};
	int rc = 0;
	unsigned char *ptr = buf;

	FUNC_ENTRY;
	if (buflen < 4)
	{
		rc = MQTTPACKET_BUFFER_TOO_SHORT;
		goto exit;
	}
	header.bits.type = packettype;
	header.bits.dup = dup;
	header.bits.qos = 0;
	writeChar(&ptr, header.byte); /* write header */

	ptr += MQTTPacket_encode(ptr, 2); /* write remaining length */
	writeInt(&ptr, packetid);
	rc = ptr - buf;
exit:
	FUNC_EXIT_RC(rc);
	return rc;
}

int MQTTSPacket_send_register(Clients* client, int topicId, char* topicName, int msgId)
{
	MQTTS_Register packet;
	int rc = 0;
	char *buf, *ptr;
	int datalen = 4 + strlen(topicName);

	FUNC_ENTRY;
	packet.header.len = datalen+2;
	packet.header.type = MQTTS_REGISTER;

	ptr = buf = malloc(datalen);

	writeInt(&ptr, topicId);
	writeInt(&ptr, msgId);
	memcpy(ptr, topicName, strlen(topicName));

	rc = MQTTSPacket_send(client->socket, client->addr, packet.header, buf, datalen);
	free(buf);

	Log(LOG_PROTOCOL, 50, NULL, client->socket, client->addr, client->clientID, msgId, topicId, topicName, rc);
	FUNC_EXIT_RC(rc);
	return rc;
}

/**
  * Serializes the supplied publish data into the supplied buffer, ready for sending
  * @param buf the buffer into which the packet will be serialized
  * @param buflen the length in bytes of the supplied buffer
  * @param dup integer - the MQTT dup flag
  * @param qos integer - the MQTT QoS value
  * @param retained integer - the MQTT retained flag
  * @param packetid integer - the MQTT packet identifier
  * @param topicName MQTTString - the MQTT topic in the publish
  * @param payload byte buffer - the MQTT publish payload
  * @param payloadlen integer - the length of the MQTT payload
  * @return the length of the serialized data.  <= 0 indicates error
  */
int MQTTSerialize_publish(unsigned char* buf, int buflen, unsigned char dup, int qos, unsigned char retained, unsigned short packetid,
                          MQTTString topicName, unsigned char* payload, int payloadlen)
{
	unsigned char *ptr = buf;
	MQTTHeader header = {0};
	int rem_len = 0;
	int rc = 0;

	FUNC_ENTRY;
	if (MQTTPacket_len(rem_len = MQTTSerialize_publishLength(qos, topicName, payloadlen)) > buflen) {
		rc = MQTTPACKET_BUFFER_TOO_SHORT;
		goto exit;
	}

	header.bits.type = PUBLISH;
	header.bits.dup = dup;
	header.bits.qos = qos;
	header.bits.retain = retained;
	writeChar(&ptr, header.byte); /* write header */

	ptr += MQTTPacket_encode(ptr, rem_len); /* write remaining length */;

	writeMQTTString(&ptr, topicName);

	if (qos > 0)
		writeInt(&ptr, packetid);

	memcpy(ptr, payload, payloadlen);
	ptr += payloadlen;

	rc = ptr - buf;

exit:
	FUNC_EXIT_RC(rc);
	return rc;
}

/**
  * Deserializes the supplied (wire) buffer into advertise data
  * @param gatewayid the returned gateway id
  * @param duration the returned duration - the time interval until the next advertise will be sent
  * @param buf the raw buffer data, of the correct length determined by the remaining length field
  * @param buflen the length in bytes of the data in the supplied buffer
  * @return error code.  1 is success
  */
int MQTTSNDeserialize_advertise(unsigned char* gatewayid, unsigned short* duration,	unsigned char* buf, int buflen)
{
	unsigned char* curdata = buf;
	unsigned char* enddata = NULL;
	int rc = 0;
	int mylen = 0;

	FUNC_ENTRY;
	curdata += (rc = MQTTSNPacket_decode(curdata, buflen, &mylen)); /* read length */
	enddata = buf + mylen;
	if (enddata - curdata > buflen)
		goto exit;

	if (readChar(&curdata) != MQTTSN_ADVERTISE)
		goto exit;

	*gatewayid = readChar(&curdata);
	*duration = readInt(&curdata);

	rc = 1;
exit:
	FUNC_EXIT_RC(rc);
	return rc;
}

/**
  * Deserializes the supplied (wire) buffer into an ack
  * @param packettype returned integer - the MQTT packet type
  * @param packetid returned integer - the MQTT packet identifier
  * @param buf the raw buffer data, of the correct length determined by the remaining length field
  * @param buflen the length in bytes of the data in the supplied buffer
  * @return error code.  1 is success, 0 is failure
  */
int MQTTSNDeserialize_ack(unsigned char* type, unsigned short* packetid, unsigned char* buf, int buflen)
{
	unsigned char* curdata = buf;
	unsigned char* enddata = NULL;
	int rc = 0;
	int mylen = 0;

	FUNC_ENTRY;
	curdata += (rc = MQTTSNPacket_decode(curdata, buflen, &mylen)); /* read length */
	enddata = buf + mylen;
	if (enddata - curdata > buflen)
		goto exit;

	*type = readChar(&curdata);
	if (*type != MQTTSN_PUBREL && *type != MQTTSN_PUBREC && *type != MQTTSN_PUBCOMP)
		goto exit;

	*packetid = readInt(&curdata);

	rc = 1;
exit:
	FUNC_EXIT_RC(rc);
	return rc;
}

/**
 * Process an incoming publish packet for a socket
 * @param pack pointer to the publish packet
 * @param sock the socket on which the packet was received
 * @return completion code
 */
int MQTTProtocol_handlePublishes(void* pack, int sock, Clients* client)
{
	Publish* publish = (Publish*)pack;
	char* clientid = NULL;
	int rc = TCPSOCKET_COMPLETE;

	FUNC_ENTRY;
	if (client == NULL)
		clientid = INTERNAL_CLIENTID; /* this is an internal client */
	else
	{
		clientid = client->clientID;
		Log(LOG_PROTOCOL, 11, NULL, sock, clientid, publish->msgId, publish->header.bits.qos,
				publish->header.bits.retain);
	}
#if defined(MQTTS)
	rc = Protocol_handlePublishes(publish, sock, client, clientid, 0);
#else
	rc = Protocol_handlePublishes(publish, sock, client, clientid);
#endif

	FUNC_EXIT_RC(rc);
	return rc;
}

/**
  * Deserializes the supplied (wire) buffer into suback data
  * @param packetid returned integer - the MQTT packet identifier
  * @param maxcount - the maximum number of members allowed in the grantedQoSs array
  * @param count returned integer - number of members in the grantedQoSs array
  * @param grantedQoSs returned array of integers - the granted qualities of service
  * @param buf the raw buffer data, of the correct length determined by the remaining length field
  * @param buflen the length in bytes of the data in the supplied buffer
  * @return error code.  1 is success, 0 is failure
  */
int MQTTDeserialize_suback(unsigned short* packetid, int maxcount, int* count, int grantedQoSs[], unsigned char* buf, int buflen)
{
	MQTTHeader header = {0};
	unsigned char* curdata = buf;
	unsigned char* enddata = NULL;
	int rc = 0;
	int mylen;

	FUNC_ENTRY;
	header.byte = readChar(&curdata);
	if (header.bits.type != SUBACK)
		goto exit;

	curdata += (rc = MQTTPacket_decodeBuf(curdata, &mylen)); /* read remaining length */
	enddata = curdata + mylen;
	if (enddata - curdata < 2)
		goto exit;

	*packetid = readInt(&curdata);

	*count = 0;
	while (curdata < enddata)
	{
		if (*count > maxcount)
		{
			rc = -1;
			goto exit;
		}
		grantedQoSs[(*count)++] = readChar(&curdata);
	}

	rc = 1;
exit:
	FUNC_EXIT_RC(rc);
	return rc;
}

/**
 * Sends an MQTT packet in one system call write
 * @param socket the socket to which to write the data
 * @param header the one-byte MQTT header
 * @param buffer the rest of the buffer to write (not including remaining length)
 * @param buflen the length of the data in buffer to be written
 * @return the completion code (TCPSOCKET_COMPLETE etc)
 */
int MQTTPacket_send(networkHandles* net, Header header, char* buffer, size_t buflen, int freeData)
{
	int rc;
	size_t buf0len;
	char *buf;

	FUNC_ENTRY;
	buf = malloc(10);
	buf[0] = header.byte;
	buf0len = 1 + MQTTPacket_encode(&buf[1], buflen);
#if !defined(NO_PERSISTENCE)
	if (header.bits.type == PUBREL)
	{
		char* ptraux = buffer;
		int msgId = readInt(&ptraux);
		rc = MQTTPersistence_put(net->socket, buf, buf0len, 1, &buffer, &buflen,
			header.bits.type, msgId, 0);
	}
#endif

#if defined(OPENSSL)
	if (net->ssl)
		rc = SSLSocket_putdatas(net->ssl, net->socket, buf, buf0len, 1, &buffer, &buflen, &freeData);
	else
#endif
		rc = Socket_putdatas(net->socket, buf, buf0len, 1, &buffer, &buflen, &freeData);
		
	if (rc == TCPSOCKET_COMPLETE)
		time(&(net->lastSent));
	
	if (rc != TCPSOCKET_INTERRUPTED)
	  free(buf);

	FUNC_EXIT_RC(rc);
	return rc;
}

/**
 * Sends an MQTT packet from multiple buffers in one system call write
 * @param socket the socket to which to write the data
 * @param header the one-byte MQTT header
 * @param count the number of buffers
 * @param buffers the rest of the buffers to write (not including remaining length)
 * @param buflens the lengths of the data in the array of buffers to be written
 * @return the completion code (TCPSOCKET_COMPLETE etc)
 */
int MQTTPacket_sends(networkHandles* net, Header header, int count, char** buffers, size_t* buflens, int* frees)
{
	int i, rc;
	size_t buf0len, total = 0;
	char *buf;

	FUNC_ENTRY;
	buf = malloc(10);
	buf[0] = header.byte;
	for (i = 0; i < count; i++)
		total += buflens[i];
	buf0len = 1 + MQTTPacket_encode(&buf[1], total);
#if !defined(NO_PERSISTENCE)
	if (header.bits.type == PUBLISH && header.bits.qos != 0)
	{   /* persist PUBLISH QoS1 and Qo2 */
		char *ptraux = buffers[2];
		int msgId = readInt(&ptraux);
		rc = MQTTPersistence_put(net->socket, buf, buf0len, count, buffers, buflens,
			header.bits.type, msgId, 0);
	}
#endif
#if defined(OPENSSL)
	if (net->ssl)
		rc = SSLSocket_putdatas(net->ssl, net->socket, buf, buf0len, count, buffers, buflens, frees);
	else
#endif
		rc = Socket_putdatas(net->socket, buf, buf0len, count, buffers, buflens, frees);
		
	if (rc == TCPSOCKET_COMPLETE)
		time(&(net->lastSent));
	
	if (rc != TCPSOCKET_INTERRUPTED)
	  free(buf);
	FUNC_EXIT_RC(rc);
	return rc;
}

int MQTTSProtocol_startRegistration(Clients* client, char* topic)
{
	int rc = 0;

	FUNC_ENTRY;
	if (client->outbound)
		rc = MQTTSProtocol_startClientRegistration(client,topic);
	else
	{
		PendingRegistration* pendingReg = malloc(sizeof(PendingRegistration));
		Registration* reg;
		int msgId = MQTTProtocol_assignMsgId(client);
		char* regTopicName = malloc(strlen(topic)+1);
		strcpy(regTopicName,topic);
		reg = MQTTSProtocol_registerTopic(client, regTopicName);
		pendingReg->msgId = msgId;
		pendingReg->registration = reg;
		time(&(pendingReg->sent));
		client->pendingRegistration = pendingReg;
		rc = MQTTSPacket_send_register(client, reg->id, regTopicName, msgId);
	}
	FUNC_EXIT_RC(rc);
	return rc;
}

int MQTTSProtocol_handleRegisters(void* pack, int sock, char* clientAddr, Clients* client)
{
	int rc = 0;
	MQTTS_Register* registerPack = (MQTTS_Register*)pack;
	ListElement* elem = NULL;
	int topicId = 0;

	FUNC_ENTRY;
	Log(LOG_PROTOCOL, 51, NULL, sock, clientAddr, client ? client->clientID : "",
			registerPack->msgId, registerPack->topicId, registerPack->topicName);
	if ((elem = ListFindItem(client->registrations, registerPack->topicName, registeredTopicNameCompare)) == NULL)
	{
		topicId = (MQTTSProtocol_registerTopic(client, registerPack->topicName))->id;
		registerPack->topicName = NULL;
	}
	else
		topicId = ((Registration*)(elem->content))->id;

	rc = MQTTSPacket_send_regAck(client, registerPack->msgId, topicId, MQTTS_RC_ACCEPTED);
	time( &(client->lastContact) );
	MQTTSPacket_free_packet(pack);
	FUNC_EXIT_RC(rc);
	return rc;
}

/**
  * Serializes the connect options into the buffer.
  * @param buf the buffer into which the packet will be serialized
  * @param len the length in bytes of the supplied buffer
  * @param options the options to be used to build the connect packet
  * @return serialized length, or error if 0
  */
int MQTTSerialize_connect(unsigned char* buf, int buflen, MQTTPacket_connectData* options)
{
	unsigned char *ptr = buf;
	MQTTHeader header = {0};
	MQTTConnectFlags flags = {0};
	int len = 0;
	int rc = -1;

	FUNC_ENTRY;
	if (MQTTPacket_len(len = MQTTSerialize_connectLength(options)) > buflen)
	{
		rc = MQTTPACKET_BUFFER_TOO_SHORT;
		goto exit;
	}

	header.byte = 0;
	header.bits.type = CONNECT;
	writeChar(&ptr, header.byte); /* write header */

	ptr += MQTTPacket_encode(ptr, len); /* write remaining length */

	if (options->MQTTVersion == 4)
	{
		writeCString(&ptr, "MQTT");
		writeChar(&ptr, (char) 4);
	}
	else
	{
		writeCString(&ptr, "MQIsdp");
		writeChar(&ptr, (char) 3);
	}

	flags.all = 0;
	flags.bits.cleansession = options->cleansession;
	flags.bits.will = (options->willFlag) ? 1 : 0;
	if (flags.bits.will)
	{
		flags.bits.willQoS = options->will.qos;
		flags.bits.willRetain = options->will.retained;
	}

	if (options->username.cstring || options->username.lenstring.data)
		flags.bits.username = 1;
	if (options->password.cstring || options->password.lenstring.data)
		flags.bits.password = 1;

	writeChar(&ptr, flags.all);
	writeInt(&ptr, options->keepAliveInterval);
	writeMQTTString(&ptr, options->clientID);
	if (options->willFlag)
	{
		writeMQTTString(&ptr, options->will.topicName);
		writeMQTTString(&ptr, options->will.message);
	}
	if (flags.bits.username)
		writeMQTTString(&ptr, options->username);
	if (flags.bits.password)
		writeMQTTString(&ptr, options->password);

	rc = ptr - buf;

	exit: FUNC_EXIT_RC(rc);
	return rc;
}

/**
 * Initialize the message module
 * @param bstate pointer to the broker state structure
 * @return completion code, success = 0
 */
int Messages_initialize(BrokerStates* bstate)
{
	FILE* rfile = NULL;
	char buf[max_msg_len];
	int count = 0;
	int rc = -99;
	char fn[30] = "Messages_en"; /* default to English in all cases */
	char* loc;

	FUNC_ENTRY;
	if ((loc = setlocale(LC_CTYPE, "")) == NULL)
		Log(LOG_WARNING, 9989, "Can't set the native locale");
    else
    {
    	int i;
    	/* select messages file on the basis of the locale, and whether utf-8 or utf-16 is needed */
		for (i = 0; i < ARRAY_SIZE(locale_map); ++i)
		{
			if (strncmp(locale_map[i][0], loc, strlen(locale_map[i][0])) == 0)
			{
				strncpy(&fn[9], locale_map[i][1], strlen(locale_map[i][1]));
				break;
			}
		}
	}
	strcat(fn, ".");
	strcat(fn, utf_choice);
	
	if ((rfile = fopen(fn, "r")) == NULL)
	{
		char fullfn[256];
		sprintf(fullfn, "..%cmessages%c%s", sep, sep, fn);
		if ((rfile = fopen(fullfn, "r")) == NULL)
		{
			if (Messages_findMyLocation(fullfn, sizeof(fullfn)) == 0)
			{
				int dirlength = strlen(fullfn);
				
				snprintf(&fullfn[dirlength], sizeof(fullfn) - dirlength, "%c%s", sep, fn);
				rfile = fopen(fullfn, "r");
				if (rfile == NULL)
				{
					snprintf(&fullfn[dirlength + 1], sizeof(fullfn) - dirlength, "..%cmessages%c%s", sep, sep, fn);
					rfile = fopen(fullfn, "r");
				}
			}
		}
	}

	if (rfile == NULL)
		Log(LOG_WARNING, 9989, "Could not find or open message file %s", fn);
	else
	{
		char* msg;
		memset(message_list, '\0', sizeof(message_list));
		while (fgets(buf, max_msg_len, rfile) != NULL && count < MESSAGE_COUNT)
		{
			int msgindex = 0;

			if (buf[0] == '#')
				continue; /* it's a comment */
			msgindex = atoi(buf);
			if (msgindex < ARRAY_SIZE(message_list))
			{
				char* start = strchr(buf, '=');
				int msglen = strlen(buf);

				if (start == NULL)
					continue;
				if (buf[msglen - 1] == '\n')
					buf[--msglen] = '\0';
				if (buf[msglen - 1] == '\r') /* this can happen if we read a messages file in with gcc with windows */
					buf[--msglen] = '\0';				/* end of line markers */
				msglen -= ++start - buf;
				msg = (char*)malloc(msglen + 1);
				strcpy(msg, start);
				message_list[msgindex] = msg;
				count++;
			}
		}
		fclose(rfile);
		if (count != MESSAGE_COUNT)
			Log(LOG_WARNING, 9988, "Found %d instead of %d messages in file %s", count, MESSAGE_COUNT, fn);
		else
			rc = 0;
	}
	FUNC_EXIT_RC(rc);
	return rc;
}

/**
 * Send an MQTT CONNECT packet down a socket.
 * @param client a structure from which to get all the required values
 * @return the completion code (e.g. TCPSOCKET_COMPLETE)
 */
int MQTTPacket_send_connect(Clients* client)
{
	char *buf, *ptr;
	Connect packet;
	int rc, len;

	FUNC_ENTRY;
	packet.header.byte = 0;
	packet.header.bits.type = CONNECT;
	packet.header.bits.qos = 1;

	len = 12 + strlen(client->clientID)+2;
	if (client->will)
		len += strlen(client->will->topic)+2 + strlen(client->will->msg)+2;
	if (client->username)
		len += strlen(client->username)+2;
	if (client->password)
		len += strlen(client->password)+2;

	ptr = buf = malloc(len);
	writeUTF(&ptr, "MQIsdp");
	if (client->noLocal)
		writeChar(&ptr, (char)-125);
	else
		writeChar(&ptr, (char)3);

	packet.flags.all = 0;
	packet.flags.bits.cleanstart = client->cleansession;
	packet.flags.bits.will = (client->will) ? 1 : 0;
	if (packet.flags.bits.will)
	{
		packet.flags.bits.willQoS = client->will->qos;
		packet.flags.bits.willRetain = client->will->retained;
	}

	if (client->username)
		packet.flags.bits.username = 1;
	if (client->password)
		packet.flags.bits.password = 1;

	writeChar(&ptr, packet.flags.all);
	writeInt(&ptr, client->keepAliveInterval);
	writeUTF(&ptr, client->clientID);
	if (client->will)
	{
		writeUTF(&ptr, client->will->topic);
		writeUTF(&ptr, client->will->msg);
	}
	if (client->username)
		writeUTF(&ptr, client->username);
	if (client->password)
		writeUTF(&ptr, client->password);

	rc = MQTTPacket_send(client->socket, packet.header, buf, len);
	Log(LOG_PROTOCOL, 0, NULL, client->socket, client->clientID, client->cleansession,
			client->noLocal, rc);
	free(buf);
	if (rc == TCPSOCKET_COMPLETE)
		time(&(client->lastContact));
	FUNC_EXIT_RC(rc);
	return rc;
}

int Protocol_handlePublishes(Publish* publish, int sock, Clients* client, char* clientid)
{
	int rc = TCPSOCKET_COMPLETE;
#if !defined(SINGLE_LISTENER)
	Listener* listener = NULL;
#endif

	FUNC_ENTRY;
	if (Protocol_isClientQuiescing(client))
		goto exit; /* don't accept new work */
#if !defined(SINGLE_LISTENER)
	listener = Socket_getParentListener(sock);
	if (listener && listener->mount_point)
	{
		char* temp = malloc(strlen(publish->topic) + strlen(listener->mount_point) + 1);
		strcpy(temp, listener->mount_point);
		strcat(temp, publish->topic);
		free(publish->topic);
		publish->topic = temp;
	}
#endif

#if !defined(NO_BRIDGE)
	if (client && client->outbound)
		Bridge_handleInbound(client, publish);
#endif

	if (publish->header.bits.qos == 0)
	{
		if (strlen(publish->topic) < 5 || strncmp(publish->topic, sysprefix, strlen(sysprefix)) != 0)
		{
			++(bstate->msgs_received);
			bstate->bytes_received += publish->payloadlen;
		}
		Protocol_processPublication(publish, clientid);
	}
	else if (publish->header.bits.qos == 1)
	{
		/* send puback before processing the publications because a lot of return publications could fill up the socket buffer */
#if defined(MQTTS)
		if (client->protocol == PROTOCOL_MQTTS)
			rc = MQTTSPacket_send_puback(client, publish->msgId, MQTTS_RC_ACCEPTED);
		else
#endif
			rc = MQTTPacket_send_puback(publish->msgId, sock, clientid);
		/* if we get a socket error from sending the puback, should we ignore the publication? */
		Protocol_processPublication(publish, clientid);
		++(bstate->msgs_received);
		bstate->bytes_received += publish->payloadlen;
	}
	else if (publish->header.bits.qos == 2 && client->inboundMsgs->count < bstate->max_inflight_messages)
	{
		/* store publication in inbound list - if list is full, ignore and rely on client retry */
		int len;
		ListElement* listElem = NULL;
		Messages* m = NULL;
		Publications* p = MQTTProtocol_storePublication(publish, &len);

		if ((listElem = ListFindItem(client->inboundMsgs, &publish->msgId, messageIDCompare)) != NULL)
		{
			m = (Messages*)(listElem->content);
			MQTTProtocol_removePublication(m->publish); /* remove old publication data - could be different */
		}
		else
			m = malloc(sizeof(Messages));

		m->publish = p;
		m->msgid = publish->msgId;
		m->qos = publish->header.bits.qos;
		m->retain = publish->header.bits.retain;
		m->nextMessageType = PUBREL;

		if (listElem == NULL)
			ListAppend(client->inboundMsgs, m, sizeof(Messages) + len);
#if defined(MQTTS)
		if (client->protocol == PROTOCOL_MQTTS)
			rc = MQTTSPacket_send_pubrec(client, publish->msgId);
		else
#endif
			rc = MQTTPacket_send_pubrec(publish->msgId, sock, clientid);
	}
	else if (publish->header.bits.qos == 3) /* only applies to MQTT-S */
	{
		publish->header.bits.qos = 0;
		Protocol_processPublication(publish, clientid);
	}
exit:
	if (sock > 0)
		MQTTPacket_freePublish(publish);
	FUNC_EXIT_RC(rc);
	return rc;
}

void* MQTTSPacket_Factory(int sock, char** clientAddr, struct sockaddr* from, uint8_t** wlnid , uint8_t *wlnid_len , int* error)
{
	static MQTTSHeader header;
	void* pack = NULL;
	/*struct sockaddr_in cliAddr;*/
	int n;
	char* data = msg;
	socklen_t len = sizeof(struct sockaddr_in6);
	*wlnid = NULL ;
	*wlnid_len = 0 ;

	FUNC_ENTRY;
/* #if !defined(NO_BRIDGE)
	client = Protocol_getoutboundclient(sock);
	FUNC_ENTRY;
	if (client!=NULL)
		n = recv(sock,msg,512,0);
	else
 #endif */

	/* max message size from global parameters, as we lose the packet if we don't receive it.  Default is
	 * 65535, so the parameter can be used to decrease the memory usage.
	 * The message memory area must be allocated on the heap so that this memory can be not allocated
	 * on reduced-memory systems.
	 */
	n = recvfrom(sock, msg, max_packet_size, 0, from, &len);
	if (n == SOCKET_ERROR)
	{
		int en = Socket_error("UDP read error", sock);
		if (en == EINVAL)
			Log(LOG_WARNING, 0, "EINVAL");

		*error = SOCKET_ERROR;
		goto exit;
	}

	*clientAddr = Socket_getaddrname(from, sock);
/*
	printf("%d bytes of data on socket %d from %s\n",n,sock,*clientAddr);
	if (n>0) {
		for (i=0;i<n;i++) {
			printf("%d ",msg[i]);
		}
		printf("\n");
	}
*/
	*error = SOCKET_ERROR;  // indicate whether an error occurred, or not
	if (n < 2)
		goto exit;

	data = MQTTSPacket_parse_header( &header, data ) ;

	/* In case of Forwarder Encapsulation packet, Length: 1-octet long, specifies the number of octets up to the end
	 * of the “Wireless Node Id” field (incl. the Length octet itself). Length does not include length of payload
	 * (encapsulated MQTT-SN message itself).
	 */
	if (header.type != MQTTS_FRWDENCAP && header.len != n)
    {
		*error = UDPSOCKET_INCOMPLETE;
		goto exit;
    }
	else
	{
		// Forwarder Encapsulation packet. Extract Wireless Node Id and MQTT-SN message
		if ( header.type == MQTTS_FRWDENCAP )
		{
			// Skip Crt(1) field
			data++ ;
			// Wireless Node Id
			*wlnid = data ;
			// Wireless Node Id length is packet length - 3 octet (Length(1) + MsgType(1) + Crt(1))
			*wlnid_len = header.len - 3 ;
			data += *wlnid_len ;

			// Read encapsulated packet and set header and shift data to beginning of payload
			data = MQTTSPacket_parse_header( &header, data ) ;
		}

		uint8_t ptype = header.type;
		if (ptype < MQTTS_ADVERTISE || ptype > MQTTS_WILLMSGRESP || new_mqtts_packets[ptype] == NULL)
			Log(TRACE_MAX, 17, NULL, ptype);
		else if ((pack = (*new_mqtts_packets[ptype])(header, data)) == NULL)
			*error = BAD_MQTTS_PACKET;
	}
exit:
   	FUNC_EXIT_RC(*error);
   	return pack;
}

int MQTTSProtocol_handleSubscribes(void* pack, int sock, char* clientAddr, Clients* client)
{
	int rc = 0;
	MQTTS_Subscribe* sub = (MQTTS_Subscribe*)pack;
	int isnew;
	int topicId = 0;
	char* topicName = NULL;

	FUNC_ENTRY;
	Log(LOG_PROTOCOL, 67, NULL, sock, clientAddr, client ? client->clientID : "",
		sub->msgId,
		(sub->flags.QoS == 3) ? -1: sub->flags.QoS,
		sub->flags.topicIdType);

	// NORMAL (topic name is in subscribe packet) or SHORT topic name
	if (sub->flags.topicIdType == MQTTS_TOPIC_TYPE_NORMAL || sub->flags.topicIdType == MQTTS_TOPIC_TYPE_SHORT)
	{
		topicName = sub->topicName;
		sub->topicName = NULL;
	}
	// Pre-defined topic
	else if (sub->flags.topicIdType == MQTTS_TOPIC_TYPE_PREDEFINED && client != NULL && sub->topicId != 0)
	{
		char *predefinedTopicName = MQTTSProtocol_getPreDefinedTopicName(client, sub->topicId) ;
		// copy the topic name as it will be freed by subscription engine
		topicName = malloc(strlen(predefinedTopicName)+1);
		strcpy(topicName, predefinedTopicName);
		topicId = sub->topicId;
	}

	// If topic name not found send SubAck with Rejected - Invalid topic ID
	if (topicName == NULL)
		rc = MQTTSPacket_send_subAck(client, sub, 0, sub->flags.QoS, MQTTS_RC_REJECTED_INVALID_TOPIC_ID);
	else
	{
		// Topic name
		if (sub->flags.topicIdType == MQTTS_TOPIC_TYPE_NORMAL && !Topics_hasWildcards(topicName))
		{
			char* regTopicName = malloc(strlen(topicName)+1);
			strcpy(regTopicName, topicName);
			topicId = (MQTTSProtocol_registerTopic(client, regTopicName))->id;
		}
		// Pre-defined topic
		else if (sub->flags.topicIdType == MQTTS_TOPIC_TYPE_PREDEFINED)
		{
			char* regTopicName = malloc(strlen(topicName)+1);
			strcpy(regTopicName, topicName);
			MQTTSProtocol_registerPreDefinedTopic(client, topicId, regTopicName);
		}
		isnew = SubscriptionEngines_subscribe(bstate->se, client->clientID,
				topicName, sub->flags.QoS, client->noLocal, (client->cleansession == 0), PRIORITY_NORMAL);

		if ( (rc = MQTTSPacket_send_subAck(client, sub, topicId, sub->flags.QoS, MQTTS_RC_ACCEPTED)) == 0)
			if ((client->noLocal == 0) || isnew)
				MQTTProtocol_processRetaineds(client, topicName,sub->flags.QoS, PRIORITY_NORMAL);
	}
	time( &(client->lastContact) );
	MQTTSPacket_free_packet(pack);
	FUNC_EXIT_RC(rc);
	return rc;
}