/*************************************************************************
* (C) 2011 AITNET ltd - Sofia/Bulgaria - <misho@aitbg.com>
*  by Michael Pounov <misho@openbsd-bg.org>
*
* $Author: misho $
* $Id: aitmqtt.c,v 1.1 2012/01/26 13:07:33 misho Exp $
*
**************************************************************************
The ELWIX and AITNET software is distributed under the following
terms:

All of the documentation and software included in the ELWIX and AITNET
Releases is copyrighted by ELWIX - Sofia/Bulgaria <info@elwix.org>

Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011
	by Michael Pounov <misho@elwix.org>.  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.
3. All advertising materials mentioning features or use of this software
   must display the following acknowledgement:
This product includes software developed by Michael Pounov <misho@elwix.org>
ELWIX - Embedded LightWeight unIX and its contributors.
4. Neither the name of AITNET nor the names of its contributors
   may be used to endorse or promote products derived from this software
   without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
*/
#include "global.h"


#pragma GCC visibility push(hidden)

int mqtt_Errno;
char mqtt_Error[STRSIZ];

#pragma GCC visibility pop

//
// Error maintenance functions ...
//

// mqtt_GetErrno() Get error code of last operation
inline int
mqtt_GetErrno()
{
	return mqtt_Errno;
}

// mqtt_GetError() Get error text of last operation
inline const char *
mqtt_GetError()
{
	return mqtt_Error;
}

// mqtt_SetErr() Set error to variables for internal use!!!
inline void
mqtt_SetErr(int eno, char *estr, ...)
{
	va_list lst;

	mqtt_Errno = eno;
	memset(mqtt_Error, 0, sizeof mqtt_Error);
	va_start(lst, estr);
	vsnprintf(mqtt_Error, sizeof mqtt_Error, estr, lst);
	va_end(lst);
}

#pragma GCC visibility push(hidden)
// _mqtt_readHEADER() read fixed header from MQTT message
inline struct mqtthdr *
_mqtt_readHEADER(mqtt_msg_t * __restrict buf, u_char cmd, int *bytes, int *len)
{
	struct mqtthdr *hdr;

	if (!buf || !buf->msg_base || !buf->msg_len)
		return NULL;

	hdr = (struct mqtthdr*) buf->msg_base;
	if (hdr->mqtt_msg.type != cmd) {
		mqtt_SetErr(EINVAL, "Error:: wrong command #%d should be %d", 
				hdr->mqtt_msg.type, cmd);
		return NULL;
	}

	*len = mqtt_decodeLen(hdr->mqtt_len, bytes);
	return hdr;
}
#pragma GCC visibility pop

// ----------------------------------------------------------

/*
 * mqtt_msgFree() Free MQTT message
 *
 * @msg = Message buffer
 * @all = !=0 Destroy entire message, if MQTT Message allocated with mqtt_msgAlloc()
 * return: none
 */
inline void
mqtt_msgFree(mqtt_msg_t ** __restrict msg, int all)
{
	if (msg && *msg) {
		if ((*msg)->msg_base) {
			free((*msg)->msg_base);
			(*msg)->msg_base = NULL;
		}
		if (all) {
			free(*msg);
			*msg = NULL;
		} else
			(*msg)->msg_len ^= (*msg)->msg_len;
	}
}

/*
 * mqtt_msgAlloc() Allocate memory for MQTT Message
 *
 * @len = >0 Allocate buffer with length
 * return: NULL error or Message, after use must call mqtt_msgFree() with all!=0
 */
inline mqtt_msg_t *
mqtt_msgAlloc(u_short len)
{
	mqtt_msg_t *m = NULL;

	m = malloc(sizeof(mqtt_msg_t));
	if (!m) {
		LOGERR;
		return NULL;
	} else
		memset(m, 0, sizeof(mqtt_msg_t));

	if (len) {
		m->msg_len = len;
		m->msg_base = malloc(m->msg_len);
		if (!m->msg_base) {
			LOGERR;
			free(m);
			return NULL;
		} else
			memset(m->msg_base, 0, m->msg_len);
	}

	return m;
}

/*
 * mqtt_msgRealloc() Reallocate MQTT message buffer
 *
 * @msg = MQTT message
 * @len = new length
 * return: -1 error or >-1 old buffer length
 */
inline int
mqtt_msgRealloc(mqtt_msg_t * __restrict msg, u_short len)
{
	void *p = NULL;
	int ret = 0;

	if (!msg)
		return -1;

	if (len == msg->msg_len)
		return len;

	p = realloc(msg->msg_base, len);
	if (!p) {
		LOGERR;
		return -1;
	}

	ret = msg->msg_len;
	msg->msg_len = len;
	msg->msg_base = p;

	return ret;
}

/*
 * mqtt_encodeLen() Encode number to MQTT length field
 *
 * @num = number for encode
 * return: -1 error or >-1 length
 */
inline u_int
mqtt_encodeLen(u_int num)
{
	register u_int dig, i;
	u_int ret = 0;

	if (num > 268435455)
		return (u_int) -1;

	for (i = 0; i < sizeof ret && num > 0; i++) {
		dig = num % 0x80;
		num /= 0x80;
		if (num > 0)
			dig |= 0x80;

		*((u_char*) &ret + i) = (u_char) dig;
	}

	return ret;
}

/*
 * mqtt_decodeLen() Decode length from MQTT packet
 *
 * @len = length from MQTT header
 * @n = sizeof bytes, if !=NULL
 * return: -1 error, >-1 length of message
 */
inline u_int
mqtt_decodeLen(void * __restrict len, int * __restrict n)
{
	register u_int i, dig, mul;
	u_int ret = 0;
	u_char *p = (u_char*) len;

	if (!len)
		return (u_int) -1;

	for (mul = 1, i = 0; i < sizeof ret; i++, mul *= 0x80) {
		dig = p[i];
		ret += (dig & 0x7f) * mul;

		if (!(dig & 0x80))
			break;
	}

	if (n)
		*n = (char) (i & 0x7f) + 1;
	return ret;
}

/*
 * mqtt_sizeLen Return sizeof len field
 *
 * @len = length
 * return: -1 error, >-1 sizeof len in bytes
 */
inline char
mqtt_sizeLen(u_int len)
{
	register char i;
	u_char *p = (u_char*) &len;

	if (len > 0xffffff7f)
		return -1;

	for (i = 0; i < sizeof len; i++)
		if (!(*(p + i) & 0x80))
			break;

	return ++i;
}

/*
 * mqtt_str2sub Create MQTT subscribe variable from string(s)
 *
 * @csStr = strings
 * @strnum = number of strings elements
 * @qoses = QoS elements applied to subscribe variable, 
 * 		count of elements must be equal with csStr elements
 * return: NULL error or != subscribe variables array, must be free after use with mqtt_freeSub()
 */
inline mqtt_subscr_t *
mqtt_str2sub(const char **csStr, u_short strnum, u_char *qoses)
{
	mqtt_subscr_t *v;
	register int i, items;
	const char **strs;

	if (!csStr)
		return NULL;
	for (items = 0, strs = csStr; *strs; items++, strs++)
		if (strnum && items >= strnum) {
			items = strnum;
			break;
		}

	if (!(v = malloc((items + 1) * sizeof(mqtt_subscr_t)))) {
		LOGERR;
		return NULL;
	} else
		memset(v, 0, (items + 1) * sizeof(mqtt_subscr_t));

	for (i = 0; i < items; i++) {
		v[i].sub_topic.msg_len = strlen(csStr[i]);
		v[i].sub_topic.msg_base = (u_char*) strdup(csStr[i]);
		if (qoses && qoses[i] < MQTT_QOS_RESERVED)
			v[i].sub_ret = qoses[i];
	}

	return v;
}

/*
 * mqtt_subFree() Free array from subscribe variables
 *
 * @subs = Subscribe variables
 * return: none
 */
inline void
mqtt_subFree(mqtt_subscr_t ** __restrict subs)
{
	mqtt_subscr_t *v;

	if (!subs)
		return;

	for (v = *subs; v->sub_topic.msg_base; v++) {
		free(v->sub_topic.msg_base);
		v->sub_topic.msg_base = NULL;
		v->sub_topic.msg_len = 0;

		if (v->sub_value.msg_base) {
			free(v->sub_value.msg_base);
			v->sub_value.msg_base = NULL;
			v->sub_value.msg_len = 0;
		}
	}

	free(*subs);
	*subs = NULL;
}

/*
 * mqtt_subAlloc() Create array from subscribe variables
 *
 * @num = Number of elements
 * return: NULL error or subscribe array, after use must call mqtt_subFree()
 */
inline mqtt_subscr_t *
mqtt_subAlloc(u_short num)
{
	mqtt_subscr_t *s = NULL;

	s = malloc((num + 1) * sizeof(mqtt_subscr_t));
	if (!s) {
		LOGERR;
		return NULL;
	} else
		memset(s, 0, (num + 1) * sizeof(mqtt_subscr_t));

	return s;
}

/*
 * mqtt_subRealloc() Reallocate array from subscribe variables
 *
 * @subs = Subscribe array
 * @num = Number of elements
 * return: NULL error or subscribe array, after use must call mqtt_subFree()
 */
inline mqtt_subscr_t *
mqtt_subRealloc(mqtt_subscr_t * __restrict subs, u_short num)
{
	mqtt_subscr_t *s = NULL;

	s = realloc(subs, (num + 1) * sizeof(mqtt_subscr_t));
	if (!s) {
		LOGERR;
		return NULL;
	}

	return s;
}