--- libaitmqtt/src/sub.c 2012/01/26 13:07:33 1.1 +++ libaitmqtt/src/sub.c 2012/06/20 15:02:24 1.2 @@ -3,7 +3,7 @@ * by Michael Pounov * * $Author: misho $ -* $Id: sub.c,v 1.1 2012/01/26 13:07:33 misho Exp $ +* $Id: sub.c,v 1.2 2012/06/20 15:02:24 misho Exp $ * ************************************************************************** The ELWIX and AITNET software is distributed under the following @@ -12,7 +12,7 @@ terms: All of the documentation and software included in the ELWIX and AITNET Releases is copyrighted by ELWIX - Sofia/Bulgaria -Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011 +Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012 by Michael Pounov . All rights reserved. Redistribution and use in source and binary forms, with or without @@ -46,8 +46,6 @@ SUCH DAMAGE. #include "global.h" -/* ------------------------------------------------------------------- */ - /* * mqtt_msgSUBSCRIBE() Create SUBSCRIBE message * @@ -62,56 +60,67 @@ int mqtt_msgSUBSCRIBE(mqtt_msg_t * __restrict buf, mqtt_subscr_t * __restrict Topics, u_short msgID, u_char Dup, u_char QOS) { - int siz = 0; + int len, siz = 0; + u_int n; struct mqtthdr *hdr; mqtthdr_var_t *topic; mqtt_len_t *mid; mqtt_subscr_t *t; u_char *qos; + void *data; if (!buf || !Topics) return -1; if (QOS > MQTT_QOS_EXACTLY) { - mqtt_SetErr(EINVAL, "Error:: invalid QoS parameter"); + mqtt_SetErr(EINVAL, "Invalid QoS parameter"); return -1; } if (!msgID && QOS != MQTT_QOS_ONCE) { - mqtt_SetErr(EINVAL, "Error:: invalid MessageID parameter must be >0"); + mqtt_SetErr(EINVAL, "Invalid MessageID parameter must be >0"); return -1; } - if (mqtt_msgRealloc(buf, MQTTMSG_MAX) == -1) + /* calculate message size */ + len = sizeof(mqtt_len_t); /* msgid */ + for (t = Topics; t && t->sub_topic.msg_base; t++) /* subscribes & qos */ + len += sizeof(mqtt_len_t) + t->sub_topic.msg_len + 1; + + /* calculate header size */ + siz = sizeof(struct mqtthdr); /* mqtt fixed header */ + n = mqtt_encodeLen(len); /* message size */ + siz += mqtt_sizeLen(n) - 1; /* length size */ + + if (mqtt_msgRealloc(buf, siz + len) == -1) return -1; else { - hdr = (struct mqtthdr *) (buf->msg_base + siz); - siz += sizeof(struct mqtthdr); + data = buf->msg_base; + hdr = (struct mqtthdr *) data; } + /* fixed header */ + MQTTHDR_MSGINIT(hdr); + hdr->mqtt_msg.type = MQTT_TYPE_SUBSCRIBE; + hdr->mqtt_msg.qos = QOS; + hdr->mqtt_msg.dup = Dup ? 1 : 0; + hdr->mqtt_msg.retain = 0; + *(u_int*) hdr->mqtt_len = n; + data += siz; + /* variable header */ - mid = (mqtt_len_t*) (buf->msg_base + siz); + mid = (mqtt_len_t*) data; mid->val = htons(msgID); - siz += sizeof(mqtt_len_t); + data += sizeof(mqtt_len_t); /* payload with subscriptions */ for (t = Topics; t && t->sub_topic.msg_base; t++) { - topic = (mqtthdr_var_t*) (buf->msg_base + siz); + topic = (mqtthdr_var_t*) data; topic->var_sb.val = htons(t->sub_topic.msg_len); memcpy(topic->var_data, t->sub_topic.msg_base, ntohs(topic->var_sb.val)); - siz += MQTTHDR_VAR_SIZEOF(topic); - qos = (buf->msg_base + siz); + data += MQTTHDR_VAR_SIZEOF(topic); + qos = data++; *qos = t->sub_ret; - siz++; } - /* fixed header */ - MQTTHDR_MSGINIT(hdr); - hdr->mqtt_msg.type = MQTT_TYPE_SUBSCRIBE; - hdr->mqtt_msg.qos = QOS; - hdr->mqtt_msg.dup = Dup ? 1 : 0; - hdr->mqtt_msg.retain = 0; - *hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr)); - - mqtt_msgRealloc(buf, siz); return siz; } @@ -159,7 +168,6 @@ mqtt_msgSUBACK(mqtt_msg_t * __restrict buf, mqtt_subsc hdr->mqtt_msg.type = MQTT_TYPE_SUBACK; *hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr)); - mqtt_msgRealloc(buf, siz); return siz; } @@ -177,52 +185,64 @@ int mqtt_msgUNSUBSCRIBE(mqtt_msg_t * __restrict buf, mqtt_subscr_t * __restrict Topics, u_short msgID, u_char Dup, u_char QOS) { - int siz = 0; + int len, siz = 0; + u_int n; struct mqtthdr *hdr; mqtthdr_var_t *topic; mqtt_len_t *mid; mqtt_subscr_t *t; + void *data; if (!buf || !Topics) return -1; if (QOS > MQTT_QOS_EXACTLY) { - mqtt_SetErr(EINVAL, "Error:: invalid QoS parameter"); + mqtt_SetErr(EINVAL, "Invalid QoS parameter"); return -1; } if (!msgID && QOS != MQTT_QOS_ONCE) { - mqtt_SetErr(EINVAL, "Error:: invalid MessageID parameter must be >0"); + mqtt_SetErr(EINVAL, "Invalid MessageID parameter must be >0"); return -1; } - if (mqtt_msgRealloc(buf, MQTTMSG_MAX) == -1) + /* calculate message size */ + len = sizeof(mqtt_len_t); /* msgid */ + for (t = Topics; t && t->sub_topic.msg_base; t++) /* subscribes */ + len += sizeof(mqtt_len_t) + t->sub_topic.msg_len; + + /* calculate header size */ + siz = sizeof(struct mqtthdr); /* mqtt fixed header */ + n = mqtt_encodeLen(len); /* message size */ + siz += mqtt_sizeLen(n) - 1; /* length size */ + + if (mqtt_msgRealloc(buf, siz + len) == -1) return -1; else { - hdr = (struct mqtthdr *) (buf->msg_base + siz); - siz += sizeof(struct mqtthdr); + data = buf->msg_base; + hdr = (struct mqtthdr *) data; } + /* fixed header */ + MQTTHDR_MSGINIT(hdr); + hdr->mqtt_msg.type = MQTT_TYPE_UNSUBSCRIBE; + hdr->mqtt_msg.qos = QOS; + hdr->mqtt_msg.dup = Dup ? 1 : 0; + hdr->mqtt_msg.retain = 0; + *(u_int*) hdr->mqtt_len = n; + data += siz; + /* variable header */ mid = (mqtt_len_t*) (buf->msg_base + siz); mid->val = htons(msgID); - siz += sizeof(mqtt_len_t); + data += sizeof(mqtt_len_t); /* payload with subscriptions */ for (t = Topics; t && t->sub_topic.msg_base; t++) { - topic = (mqtthdr_var_t*) (buf->msg_base + siz); + topic = (mqtthdr_var_t*) data; topic->var_sb.val = htons(t->sub_topic.msg_len); memcpy(topic->var_data, t->sub_topic.msg_base, ntohs(topic->var_sb.val)); - siz += MQTTHDR_VAR_SIZEOF(topic); + data += MQTTHDR_VAR_SIZEOF(topic); } - /* fixed header */ - MQTTHDR_MSGINIT(hdr); - hdr->mqtt_msg.type = MQTT_TYPE_UNSUBSCRIBE; - hdr->mqtt_msg.qos = QOS; - hdr->mqtt_msg.dup = Dup ? 1 : 0; - hdr->mqtt_msg.retain = 0; - *hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr)); - - mqtt_msgRealloc(buf, siz); return siz; } @@ -272,9 +292,9 @@ mqtt_msgUNSUBACK(mqtt_msg_t * __restrict buf, u_short * @buf = Message buffer * @msgID = MessageID * @subscr = Subscriptions, must be free after use with mqtt_subFree() - * return: NULL error or !=NULL MQTT fixed header + * return: -1 error or >-1 elements into subscr */ -struct mqtthdr * +int mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_short *msgID, mqtt_subscr_t **subscr) { register int i; @@ -286,19 +306,19 @@ mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_shor caddr_t pos; if (!buf || !msgID || !subscr) - return NULL; + return -1; hdr = _mqtt_readHEADER(buf, MQTT_TYPE_SUBSCRIBE, &ret, &len); if (!hdr) - return NULL; + return -1; pos = buf->msg_base + ret + 1; v = (mqtt_len_t*) pos; /* MessageID */ len -= sizeof(mqtt_len_t); if (len < 0) { - mqtt_SetErr(EINVAL, "Error:: short message length %d", len); - return NULL; + mqtt_SetErr(EINVAL, "Short message length %d", len); + return -1; } else { *msgID = ntohs(v->val); pos += sizeof(mqtt_len_t); @@ -306,7 +326,7 @@ mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_shor subs = mqtt_subAlloc(0); if (!subs) - return NULL; + return -1; else *subscr = subs; @@ -316,32 +336,33 @@ mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_shor len -= MQTTHDR_VAR_SIZEOF(var) + 1; if (len < 0) { mqtt_subFree(subscr); - mqtt_SetErr(EINVAL, "Error:: short message length %d", len); - return NULL; + mqtt_SetErr(EINVAL, "Short message length %d", len); + return -1; } - subs = mqtt_subRealloc(subs, i + 1); - if (!subs) { + if (!mqtt_subRealloc(&subs, i + 1)) { mqtt_subFree(subscr); - return NULL; + return -1; } else *subscr = subs; memset(&subs[i], 0, sizeof subs[i]); subs[i].sub_topic.msg_len = ntohs(var->var_sb.val); - subs[i].sub_topic.msg_base = malloc(subs[i].sub_topic.msg_len); + subs[i].sub_topic.msg_base = malloc(subs[i].sub_topic.msg_len + 1); if (!subs[i].sub_topic.msg_base) { LOGERR; mqtt_subFree(subscr); - return NULL; - } else + return -1; + } else { memcpy(subs[i].sub_topic.msg_base, var->var_data, subs[i].sub_topic.msg_len); + ((char*) subs[i].sub_topic.msg_base)[subs[i].sub_topic.msg_len] = 0; + } pos += MQTTHDR_VAR_SIZEOF(var); subs[i].sub_ret = *pos; pos++; } - return hdr; + return i; } /* @@ -372,7 +393,7 @@ mqtt_readSUBACK(mqtt_msg_t * __restrict buf, u_short * /* MessageID */ len -= sizeof(mqtt_len_t); if (len < 0) { - mqtt_SetErr(EINVAL, "Error:: short message length %d", len); + mqtt_SetErr(EINVAL, "Short message length %d", len); return -1; } else { *msgID = ntohs(v->val); @@ -396,9 +417,9 @@ mqtt_readSUBACK(mqtt_msg_t * __restrict buf, u_short * * @buf = Message buffer * @msgID = MessageID * @subscr = Subscriptions, must be free after use with mqtt_subFree() - * return: NULL error or !=NULL MQTT fixed header + * return: -1 error or >-1 elements into subscr */ -struct mqtthdr * +int mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_short *msgID, mqtt_subscr_t **subscr) { register int i; @@ -410,19 +431,19 @@ mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_sh caddr_t pos; if (!buf || !msgID || !subscr) - return NULL; + return -1; hdr = _mqtt_readHEADER(buf, MQTT_TYPE_UNSUBSCRIBE, &ret, &len); if (!hdr) - return NULL; + return -1; pos = buf->msg_base + ret + 1; v = (mqtt_len_t*) pos; /* MessageID */ len -= sizeof(mqtt_len_t); if (len < 0) { - mqtt_SetErr(EINVAL, "Error:: short message length %d", len); - return NULL; + mqtt_SetErr(EINVAL, "Short message length %d", len); + return -1; } else { *msgID = ntohs(v->val); pos += sizeof(mqtt_len_t); @@ -430,7 +451,7 @@ mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_sh subs = mqtt_subAlloc(0); if (!subs) - return NULL; + return -1; else *subscr = subs; @@ -440,29 +461,30 @@ mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_sh len -= MQTTHDR_VAR_SIZEOF(var); if (len < 0) { mqtt_subFree(subscr); - mqtt_SetErr(EINVAL, "Error:: short message length %d", len); - return NULL; + mqtt_SetErr(EINVAL, "Short message length %d", len); + return -1; } - subs = mqtt_subRealloc(subs, i + 1); - if (!subs) { + if (!mqtt_subRealloc(&subs, i + 1)) { mqtt_subFree(subscr); - return NULL; + return -1; } else *subscr = subs; memset(&subs[i], 0, sizeof subs[i]); subs[i].sub_topic.msg_len = ntohs(var->var_sb.val); - subs[i].sub_topic.msg_base = malloc(subs[i].sub_topic.msg_len); + subs[i].sub_topic.msg_base = malloc(subs[i].sub_topic.msg_len + 1); if (!subs[i].sub_topic.msg_base) { LOGERR; mqtt_subFree(subscr); - return NULL; - } else + return -1; + } else { memcpy(subs[i].sub_topic.msg_base, var->var_data, subs[i].sub_topic.msg_len); + ((char*) subs[i].sub_topic.msg_base)[subs[i].sub_topic.msg_len] = 0; + } pos += MQTTHDR_VAR_SIZEOF(var); } - return hdr; + return i; } /* @@ -483,7 +505,7 @@ mqtt_readUNSUBACK(mqtt_msg_t * __restrict buf) if (!hdr) return (u_short) -1; if (len < sizeof(mqtt_len_t)) { - mqtt_SetErr(EINVAL, "Error:: short message length %d", len); + mqtt_SetErr(EINVAL, "Short message length %d", len); return (u_short) -1; } else { pos = buf->msg_base + ret + 1;