--- libaitmqtt/src/conn.c 2022/09/14 14:32:48 1.3.12.3 +++ libaitmqtt/src/conn.c 2022/09/14 18:36:23 1.3.12.4 @@ -3,7 +3,7 @@ * by Michael Pounov * * $Author: misho $ -* $Id: conn.c,v 1.3.12.3 2022/09/14 14:32:48 misho Exp $ +* $Id: conn.c,v 1.3.12.4 2022/09/14 18:36:23 misho Exp $ * ************************************************************************** The ELWIX and AITNET software is distributed under the following @@ -292,300 +292,4 @@ mqtt_msg_t * mqtt_msgDISCONNECT() { return _mqtt_msgSIMPLE_(MQTT_TYPE_DISCONNECT); -} - -/* ============= decode ============ */ - -//#pragma GCC visibility push(hidden) -/* _mqtt_readHEADER() read fixed header from MQTT message */ -static struct mqtthdr * -_mqtt_readHEADER(mqtt_msg_t * __restrict buf, u_char cmd, int *len, caddr_t *next) -{ - struct mqtthdr *hdr; - int bytes; - - 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; - } - - if (len) - *len = mqtt_decodeLen(hdr->mqtt_len, &bytes); - - if (next) - *next = buf->msg_base + bytes + 1; - - return hdr; -} -//#pragma GCC visibility pop - -/* - * mqtt_readCONNECT() Read elements from CONNECT message - * - * @buf = Message buffer - * @KASec = Keep Alive in seconds for current connection - * @psConnID = ConnectID - * @connLen = ConnectID length - * @psUser = Username if !=NULL - * @userLen = Username length - * @psPass = Password for Username, only if csUser is set - * @passLen = Password length - * @psWillTopic = Will Topic if !=NULL Will Flags set into message and must be e_free() - * @psWillMessage = Will Message, may be NULL if !NULL must be e_free() after use! - * return: .reserved == 1 is error or == 0 connection flags & msg ok - */ -mqtthdr_connack_t -mqtt_readCONNECT(mqtt_msg_t * __restrict buf, u_short *KASec, char * __restrict psConnID, int connLen, - char * __restrict psUser, int userLen, char * __restrict psPass, int passLen, - char ** __restrict psWillTopic, char ** __restrict psWillMessage) -{ - mqtthdr_connflgs_t flg = { MQTT_CONNFLGS_INIT }; - mqtthdr_connack_t cack = { 1, MQTT_RETCODE_DENIED }; - struct mqtthdr *hdr; - mqtthdr_var_t *var; - mqtt_len_t *ka; - int len; - caddr_t pos; - - if (!buf || !buf->msg_base || !buf->msg_len || !psConnID || !connLen) - return cack; - - hdr = _mqtt_readHEADER(buf, MQTT_TYPE_CONNECT, &len, &pos); - if (!hdr) - return cack; - if (len < 12) { - mqtt_SetErr(EINVAL, "Short message length %d", len); - return cack; - } else - var = (mqtthdr_var_t*) pos; - - /* check init string & protocol */ - if (var->var_sb.sb.l == 4 && !strcmp((char*) var->var_data, MQTT_PROTO_STR)) - pos += var->var_sb.sb.l + sizeof(mqtt_len_t); - else if (var->var_sb.sb.l == 6 || strcmp((char*) var->var_data, MQTT_CONN_STR)) - pos += var->var_sb.sb.l + sizeof(mqtt_len_t); - else { - mqtt_SetErr(EINVAL, "Invalid init string %.6s(%d)", - var->var_data, var->var_sb.sb.l); - cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL; - return cack; - } - switch (*pos) { - case MQTT_PROTO_VER_3: - case MQTT_PROTO_VER_311: - case MQTT_PROTO_VER_5: - pos++; - break; - default: - mqtt_SetErr(EINVAL, "Invalid protocol version %d", *pos); - cack.retcode = MQTT_RETCODE_REFUSE_VER; - return cack; - } - flg = *(mqtthdr_connflgs_t*) pos; - pos++; - ka = (mqtt_len_t*) pos; - *KASec = ntohs(ka->val); - pos += sizeof(mqtt_len_t); - - len -= pos - (caddr_t) var; - - /* get ConnID */ - var = (mqtthdr_var_t*) pos; - len -= MQTTHDR_VAR_SIZEOF(var); - if (len < 0 || var->var_sb.sb.l >= MQTT_CONNID_MAX) { - mqtt_SetErr(EINVAL, "Unexpected EOM at Connection ID %d", len); - cack.retcode = MQTT_RETCODE_REFUSE_ID; - return cack; - } else { - memset(psConnID, 0, connLen--); - memcpy(psConnID, var->var_data, - ntohs(var->var_sb.val) > connLen ? connLen : ntohs(var->var_sb.val)); - pos += MQTTHDR_VAR_SIZEOF(var); - } - - /* get Willz */ - if (flg.will_flg) { - var = (mqtthdr_var_t*) pos; - len -= MQTTHDR_VAR_SIZEOF(var); - if (len < 0) { - mqtt_SetErr(EINVAL, "Unexpected EOM at Will Topic %d", len); - cack.retcode = MQTT_RETCODE_REFUSE_ID; - return cack; - } else { - if (psWillTopic) { - *psWillTopic = e_malloc(ntohs(var->var_sb.val) + 1); - if (!*psWillTopic) { - LOGERR; - cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL; - return cack; - } else - memset(*psWillTopic, 0, ntohs(var->var_sb.val) + 1); - memcpy(*psWillTopic, var->var_data, ntohs(var->var_sb.val)); - } - pos += MQTTHDR_VAR_SIZEOF(var); - } - - var = (mqtthdr_var_t*) pos; - len -= MQTTHDR_VAR_SIZEOF(var); - if (len < 0) { - mqtt_SetErr(EINVAL, "Unexpected EOM at Will Message %d", len); - e_free(psWillTopic); - cack.retcode = MQTT_RETCODE_REFUSE_ID; - return cack; - } else { - if (psWillMessage) { - *psWillMessage = e_malloc(ntohs(var->var_sb.val) + 1); - if (!*psWillMessage) { - LOGERR; - e_free(psWillTopic); - cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL; - return cack; - } else - memset(*psWillMessage, 0, ntohs(var->var_sb.val) + 1); - memcpy(*psWillMessage, var->var_data, ntohs(var->var_sb.val)); - } - pos += MQTTHDR_VAR_SIZEOF(var); - } - } - - /* get User/Pass */ - if (flg.username) { - var = (mqtthdr_var_t*) pos; - len -= MQTTHDR_VAR_SIZEOF(var); - if (len < 0 || var->var_sb.sb.l > 12) { - mqtt_SetErr(EINVAL, "Unexpected EOM at Username %d", len); - if (flg.will_flg) { - if (psWillTopic) - e_free(psWillTopic); - if (psWillMessage) - e_free(psWillMessage); - } - cack.retcode = MQTT_RETCODE_REFUSE_USERPASS; - return cack; - } else { - if (psUser && userLen) { - memset(psUser, 0, userLen--); - memcpy(psUser, var->var_data, - ntohs(var->var_sb.val) > userLen ? userLen : ntohs(var->var_sb.val)); - } - pos += MQTTHDR_VAR_SIZEOF(var); - } - } - if (flg.password) { - var = (mqtthdr_var_t*) pos; - len -= MQTTHDR_VAR_SIZEOF(var); - if (len < 0 || var->var_sb.sb.l > 12) { - mqtt_SetErr(EINVAL, "Unexpected EOM at Password %d", len); - if (flg.will_flg) { - if (psWillTopic) - e_free(psWillTopic); - if (psWillMessage) - e_free(psWillMessage); - } - cack.retcode = MQTT_RETCODE_REFUSE_USERPASS; - return cack; - } else { - if (psPass && passLen) { - memset(psPass, 0, passLen--); - memcpy(psPass, var->var_data, - ntohs(var->var_sb.val) > passLen ? passLen : ntohs(var->var_sb.val)); - } - pos += MQTTHDR_VAR_SIZEOF(var); - } - } - - flg.reserved = 0; - cack.reserved = flg.flags; - cack.retcode = MQTT_RETCODE_ACCEPTED; - return cack; -} - -/* - * mqtt_readCONNACK() Read CONNACK message - * - * @buf = Message buffer - * return: -1 error or >-1 CONNECT message return code - */ -u_char -mqtt_readCONNACK(mqtt_msg_t * __restrict buf) -{ - int len; - struct mqtthdr *hdr; - mqtthdr_connack_t *ack; - caddr_t pos; - - if (!buf || !buf->msg_base || !buf->msg_len) - return (u_char) -1; - - hdr = _mqtt_readHEADER(buf, MQTT_TYPE_CONNACK, &len, &pos); - if (!hdr) - return (u_char) -1; - if (len < sizeof(mqtthdr_connack_t)) { - mqtt_SetErr(EINVAL, "Short message length %d", len); - return (u_char) -1; - } else - ack = (mqtthdr_connack_t*) pos; - - if (ack->retcode > MQTT_RETCODE_DENIED) { - mqtt_SetErr(EINVAL, "Invalid retcode %u", ack->retcode); - return (u_char) -1; - } - - return ack->retcode; -} - -/* - * mqtt_readDISCONNECT() Read DISCONNECT message - * - * @buf = Message buffer - * return: -1 error, 0 ok, >0 undefined result - */ -int -mqtt_readDISCONNECT(mqtt_msg_t * __restrict buf) -{ - int len; - - if (!_mqtt_readHEADER(buf, MQTT_TYPE_DISCONNECT, &len, NULL)) - return -1; - - return len; -} - -/* - * mqtt_readPINGREQ() Read PINGREQ message - * - * @buf = Message buffer - * return: -1 error, 0 ok, >0 undefined result - */ -int -mqtt_readPINGREQ(mqtt_msg_t * __restrict buf) -{ - int len; - - if (!_mqtt_readHEADER(buf, MQTT_TYPE_PINGREQ, &len, NULL)) - return -1; - - return len; -} - -/* - * mqtt_readPINGRESP() Read PINGRESP message - * - * @buf = Message buffer - * return: -1 error, 0 ok, >0 undefined result - */ -int -mqtt_readPINGRESP(mqtt_msg_t * __restrict buf) -{ - int len; - - if (!_mqtt_readHEADER(buf, MQTT_TYPE_PINGRESP, &len, NULL)) - return -1; - - return len; }