#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._size = strlen(csStr[i]);
		v[i].sub_topic._base = 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._base; v++) {
		free(v->sub_topic._base);
		v->sub_topic._base = NULL;
		v->sub_topic._size = 0;

		if (v->sub_value._base) {
			free(v->sub_value._base);
			v->sub_value._base = NULL;
			v->sub_value._size = 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;
}