--- libaitmqtt/src/sub.c 2012/04/27 15:15:12 1.1.1.1.2.5 +++ libaitmqtt/src/sub.c 2022/09/16 04:14:03 1.3.12.4 @@ -3,7 +3,7 @@ * by Michael Pounov * * $Author: misho $ -* $Id: sub.c,v 1.1.1.1.2.5 2012/04/27 15:15:12 misho Exp $ +* $Id: sub.c,v 1.3.12.4 2022/09/16 04:14:03 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, 2012 +Copyright 2004 - 2022 by Michael Pounov . All rights reserved. Redistribution and use in source and binary forms, with or without @@ -49,95 +49,97 @@ SUCH DAMAGE. /* * mqtt_msgSUBSCRIBE() Create SUBSCRIBE message * - * @buf = Message buffer * @Topics = MQTT subscription topics * @msgID = MessageID - * @Dup = Duplicate message - * @QOS = QoS - * return: -1 error or >-1 message size for send + * return: NULL error or allocated SUBSCRIBE message */ -int -mqtt_msgSUBSCRIBE(mqtt_msg_t * __restrict buf, mqtt_subscr_t * __restrict Topics, - u_short msgID, u_char Dup, u_char QOS) +mqtt_msg_t * +mqtt_msgSUBSCRIBE(mqtt_subscr_t * __restrict Topics, u_short msgID) { - int siz = 0; + int len, siz; + u_int n, *l; struct mqtthdr *hdr; mqtthdr_var_t *topic; mqtt_len_t *mid; mqtt_subscr_t *t; - u_char *qos; + void *data; + mqtt_msg_t *msg = NULL; - if (!buf || !Topics) - return -1; - if (QOS > MQTT_QOS_EXACTLY) { - mqtt_SetErr(EINVAL, "Invalid QoS parameter"); - return -1; - } - if (!msgID && QOS != MQTT_QOS_ONCE) { + if (!Topics) + return NULL; + if (!msgID) { mqtt_SetErr(EINVAL, "Invalid MessageID parameter must be >0"); - return -1; + return NULL; } - if (mqtt_msgRealloc(buf, MQTTMSG_MAX) == -1) - return -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 (!(msg = mqtt_msgAlloc(siz + len))) + return NULL; else { - hdr = (struct mqtthdr *) (buf->msg_base + siz); - siz += sizeof(struct mqtthdr); + data = msg->msg_base; + hdr = (struct mqtthdr *) data; } + /* fixed header */ + hdr->mqtt_msg.type = MQTT_TYPE_SUBSCRIBE; + hdr->mqtt_msg.qos = MQTT_QOS_ACK; + l = (u_int*) hdr->mqtt_len; + *l = 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); - *qos = t->sub_ret; - siz++; + data += MQTTHDR_VAR_SIZEOF(topic); + *((char*) data) = t->sub_qos; + 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; - *hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr)); - - return siz; + return msg; } /* * mqtt_msgSUBACK() Create SUBACK message * - * @buf = Message buffer * @Topics = MQTT subscription topics * @msgID = MessageID - * return: -1 error or >-1 message size for send + * return: NULL error or allocated SUBACK message */ -int -mqtt_msgSUBACK(mqtt_msg_t * __restrict buf, mqtt_subscr_t * __restrict Topics, u_short msgID) +mqtt_msg_t * +mqtt_msgSUBACK(mqtt_subscr_t * __restrict Topics, u_short msgID) { int siz = 0; struct mqtthdr *hdr; mqtt_len_t *v; mqtt_subscr_t *t; u_char *qos; + mqtt_msg_t *msg = NULL; - if (!buf || !Topics) - return -1; + if (!Topics) + return NULL; - if (mqtt_msgRealloc(buf, MQTTMSG_MAX) == -1) - return -1; + if (!(msg = mqtt_msgAlloc(MQTTMSG_MAX))) + return NULL; else { - hdr = (struct mqtthdr *) (buf->msg_base + siz); - siz += sizeof(struct mqtthdr); - v = (mqtt_len_t*) (buf->msg_base + siz); + hdr = (struct mqtthdr *) msg->msg_base; + siz = sizeof(struct mqtthdr); + v = (mqtt_len_t*) (msg->msg_base + siz); siz += sizeof(mqtt_len_t); } @@ -145,347 +147,119 @@ mqtt_msgSUBACK(mqtt_msg_t * __restrict buf, mqtt_subsc v->val = htons(msgID); /* QoS payload from subscriptions */ - for (t = Topics; t && t->sub_topic.msg_base; t++) { - qos = (buf->msg_base + siz); - *qos = t->sub_ret; - siz++; + for (t = Topics; t && t->sub_topic.msg_base; t++, siz++) { + qos = (msg->msg_base + siz); + *qos = t->sub_qos; } /* fixed header */ - MQTTHDR_MSGINIT(hdr); hdr->mqtt_msg.type = MQTT_TYPE_SUBACK; *hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr)); - return siz; + return msg; } /* * mqtt_msgUNSUBSCRIBE() Create UNSUBSCRIBE message * - * @buf = Message buffer * @Topics = MQTT subscription topics * @msgID = MessageID * @Dup = Duplicate message * @QOS = QoS - * return: -1 error or >-1 message size for send + * return: NULL error or allocated UNSUBSCRIBE message */ -int -mqtt_msgUNSUBSCRIBE(mqtt_msg_t * __restrict buf, mqtt_subscr_t * __restrict Topics, - u_short msgID, u_char Dup, u_char QOS) +mqtt_msg_t * +mqtt_msgUNSUBSCRIBE(mqtt_subscr_t * __restrict Topics, u_short msgID, + u_char Dup, u_char QOS) { - int siz = 0; + int len, siz = 0; + u_int n, *l; struct mqtthdr *hdr; mqtthdr_var_t *topic; mqtt_len_t *mid; mqtt_subscr_t *t; + void *data; + mqtt_msg_t *msg = NULL; - if (!buf || !Topics) - return -1; + if (!Topics) + return NULL; if (QOS > MQTT_QOS_EXACTLY) { mqtt_SetErr(EINVAL, "Invalid QoS parameter"); - return -1; + return NULL; } if (!msgID && QOS != MQTT_QOS_ONCE) { mqtt_SetErr(EINVAL, "Invalid MessageID parameter must be >0"); - return -1; + return NULL; } - if (mqtt_msgRealloc(buf, MQTTMSG_MAX) == -1) - return -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 (!(msg = mqtt_msgAlloc(siz + len))) + return NULL; else { - hdr = (struct mqtthdr *) (buf->msg_base + siz); - siz += sizeof(struct mqtthdr); + data = msg->msg_base; + hdr = (struct mqtthdr *) data; } + /* fixed header */ + hdr->mqtt_msg.type = MQTT_TYPE_UNSUBSCRIBE; + hdr->mqtt_msg.qos = QOS; + hdr->mqtt_msg.dup = Dup ? 1 : 0; + hdr->mqtt_msg.retain = 0; + l = (u_int*) hdr->mqtt_len; + *l = n; + data += siz; + /* variable header */ - mid = (mqtt_len_t*) (buf->msg_base + siz); + mid = (mqtt_len_t*) (msg->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)); - - return siz; + return msg; } /* * mqtt_msgUNSUBACK() Create UNSUBACK message * - * @buf = Message buffer * @msgID = MessageID - * return: -1 error or >-1 message size for send + * return: NULL error or allocated UNSUBACK message */ -int -mqtt_msgUNSUBACK(mqtt_msg_t * __restrict buf, u_short msgID) +mqtt_msg_t * +mqtt_msgUNSUBACK(u_short msgID) { - int siz = 0; struct mqtthdr *hdr; mqtt_len_t *v; + mqtt_msg_t *msg = NULL; - if (!buf) - return -1; - - if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr) + sizeof(mqtt_len_t)) == -1) - return -1; + if (!(msg = mqtt_msgAlloc(sizeof(struct mqtthdr) + sizeof(mqtt_len_t)))) + return NULL; else { - hdr = (struct mqtthdr *) (buf->msg_base + siz); - siz += sizeof(struct mqtthdr); - v = (mqtt_len_t*) (buf->msg_base + siz); - siz += sizeof(mqtt_len_t); + hdr = (struct mqtthdr *) msg->msg_base; + v = (mqtt_len_t*) (msg->msg_base + sizeof(struct mqtthdr)); } /* fixed header */ - MQTTHDR_MSGINIT(hdr); hdr->mqtt_msg.type = MQTT_TYPE_UNSUBACK; *hdr->mqtt_len = sizeof(mqtt_len_t); /* MessageID */ v->val = htons(msgID); - return siz; -} - - -/* ============= decode ============ */ - -/* - * mqtt_readSUBSCRIBE() Read SUBSCRIBE message - * - * @buf = Message buffer - * @msgID = MessageID - * @subscr = Subscriptions, must be free after use with mqtt_subFree() - * return: -1 error or >-1 elements into subscr - */ -int -mqtt_readSUBSCRIBE(mqtt_msg_t * __restrict buf, u_short *msgID, mqtt_subscr_t **subscr) -{ - register int i; - int len, ret; - struct mqtthdr *hdr; - mqtthdr_var_t *var; - mqtt_subscr_t *subs; - mqtt_len_t *v; - caddr_t pos; - - if (!buf || !msgID || !subscr) - return -1; - - hdr = _mqtt_readHEADER(buf, MQTT_TYPE_SUBSCRIBE, &ret, &len); - if (!hdr) - 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, "Short message length %d", len); - return -1; - } else { - *msgID = ntohs(v->val); - pos += sizeof(mqtt_len_t); - } - - subs = mqtt_subAlloc(0); - if (!subs) - return -1; - else - *subscr = subs; - - /* Subscribes */ - for (i = 0; len > 0; i++) { - var = (mqtthdr_var_t*) pos; - len -= MQTTHDR_VAR_SIZEOF(var) + 1; - if (len < 0) { - mqtt_subFree(subscr); - mqtt_SetErr(EINVAL, "Short message length %d", len); - return -1; - } - if (!mqtt_subRealloc(&subs, i + 1)) { - mqtt_subFree(subscr); - 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 + 1); - if (!subs[i].sub_topic.msg_base) { - LOGERR; - mqtt_subFree(subscr); - 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 i; -} - -/* - * mqtt_readSUBACK() Read SUBACK message - * - * @buf = Message buffer - * @msgID = MessageID - * @subqos = Subscribes QoS, must be free after use with free() - * return: -1 error or >-1 readed subscribes QoS elements - */ -int -mqtt_readSUBACK(mqtt_msg_t * __restrict buf, u_short *msgID, u_char **subqos) -{ - int len, ret; - struct mqtthdr *hdr; - mqtt_len_t *v; - caddr_t pos; - - if (!buf || !msgID || !subqos) - return -1; - - hdr = _mqtt_readHEADER(buf, MQTT_TYPE_SUBACK, &ret, &len); - if (!hdr) - 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, "Short message length %d", len); - return -1; - } else { - *msgID = ntohs(v->val); - pos += sizeof(mqtt_len_t); - } - - /* Subscribes */ - *subqos = malloc(len); - if (!*subqos) { - LOGERR; - return -1; - } else - memcpy(*subqos, pos, len); - - return len; -} - -/* - * mqtt_readUNSUBSCRIBE() Read UNSUBSCRIBE message - * - * @buf = Message buffer - * @msgID = MessageID - * @subscr = Subscriptions, must be free after use with mqtt_subFree() - * return: -1 error or >-1 elements into subscr - */ -int -mqtt_readUNSUBSCRIBE(mqtt_msg_t * __restrict buf, u_short *msgID, mqtt_subscr_t **subscr) -{ - register int i; - int len, ret; - struct mqtthdr *hdr; - mqtthdr_var_t *var; - mqtt_subscr_t *subs; - mqtt_len_t *v; - caddr_t pos; - - if (!buf || !msgID || !subscr) - return -1; - - hdr = _mqtt_readHEADER(buf, MQTT_TYPE_UNSUBSCRIBE, &ret, &len); - if (!hdr) - 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, "Short message length %d", len); - return -1; - } else { - *msgID = ntohs(v->val); - pos += sizeof(mqtt_len_t); - } - - subs = mqtt_subAlloc(0); - if (!subs) - return -1; - else - *subscr = subs; - - /* Subscribes */ - for (i = 0; len > 0; i++) { - var = (mqtthdr_var_t*) pos; - len -= MQTTHDR_VAR_SIZEOF(var); - if (len < 0) { - mqtt_subFree(subscr); - mqtt_SetErr(EINVAL, "Short message length %d", len); - return -1; - } - if (!mqtt_subRealloc(&subs, i + 1)) { - mqtt_subFree(subscr); - 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 + 1); - if (!subs[i].sub_topic.msg_base) { - LOGERR; - mqtt_subFree(subscr); - 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 i; -} - -/* - * mqtt_readUNSUBACK() Read UNSUBACK message - * - * @buf = Message buffer - * return: -1 error or MessageID - */ -u_short -mqtt_readUNSUBACK(mqtt_msg_t * __restrict buf) -{ - int len, ret; - struct mqtthdr *hdr; - mqtt_len_t *v; - caddr_t pos; - - hdr = _mqtt_readHEADER(buf, MQTT_TYPE_UNSUBACK, &ret, &len); - if (!hdr) - return (u_short) -1; - if (len < sizeof(mqtt_len_t)) { - mqtt_SetErr(EINVAL, "Short message length %d", len); - return (u_short) -1; - } else { - pos = buf->msg_base + ret + 1; - v = (mqtt_len_t*) pos; - } - - return ntohs(v->val); + return msg; }