#include "global.h"
/* ------------------------------------------------------------------- */
/*
* mqtt_msgCONNECT() Create CONNECT message
*
* @buf = Message buffer
* @csConnID = ConnectID
* @csUser = Username if !=NULL
* @csPass = Password for Username, only if csUser is set
* @csWillTopic = Will Topic if !=NULL Will Flags set into message
* @csWillMessage = Will Message, may be NULL
* @ClrSess = Clear Session subscriptions after disconnect
* @WillQOS = Will QOS if csWillTopic is set
* @WillRetain = Will Retain Will Message if csWillTopic is set
* return: -1 error or >-1 message size for send
*/
int
mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const char *csConnID,
const char *csUser, const char *csPass,
const char *csWillTopic, const char *csWillMessage,
u_char ClrSess, u_char WillQOS, u_char WillRetain)
{
int siz = 0;
struct mqtthdr *hdr;
mqtthdr_var_t *var, *cid, *topic, *wmsg, *user, *pass;
mqtthdr_protover_t *proto;
mqtthdr_connflgs_t *flags;
mqtt_v_t *ka;
if (!buf || !csConnID)
return -1;
if (strlen(csConnID) > 23) {
mqtt_SetErr(EINVAL, "Error:: invalid argument ConnID is too long (max 23 bytes)");
return -1;
}
if (csUser && strlen(csUser) > 12) {
mqtt_SetErr(EINVAL, "Error:: invalid argument Username is too long (max 12 bytes)");
return -1;
}
if (csPass && strlen(csPass) > 12) {
mqtt_SetErr(EINVAL, "Error:: invalid argument Password is too long (max 12 bytes)");
return -1;
}
if (WillQOS > MQTT_QOS_EXACTLY) {
mqtt_SetErr(EINVAL, "Error:: invalid argument WillQOS - unknown QOS value");
return -1;
}
if (mqtt_msgRealloc(buf, BUFSIZ) == -1)
return -1;
else {
hdr = (struct mqtthdr *) (buf->msg_base + siz);
siz += sizeof(struct mqtthdr);
var = (mqtthdr_var_t*) (buf->msg_base + siz);
siz += 8;
proto = buf->msg_base + siz;
siz++;
flags = (mqtthdr_connflgs_t*) (buf->msg_base + siz);
siz++;
ka = (mqtt_v_t*) (buf->msg_base + siz);
siz += sizeof(mqtt_v_t);
}
/* fixed header */
MQTTHDR_MSGINIT(hdr);
hdr->mqtt_msg.type = MQTT_TYPE_CONNECT;
*hdr->mqtt_len = 0;
/* variable header */
var->var_sb.sb.l = 6;
memcpy(var->var_data, MQTT_CONN_STR, 6);
*proto = MQTT_PROTO_VER;
/* CONNECT header */
flags->clean_sess = ClrSess ? 1 : 0;
if (csUser) {
flags->username = 1;
flags->password = csPass ? 1 : 0;
} else {
flags->username = 0;
flags->password = 0;
}
if (csWillTopic) {
flags->will_flg = 1;
flags->will_qos = WillQOS;
flags->will_retain = WillRetain ? 1 : 0;
} else {
flags->will_flg = 0;
flags->will_qos = 0;
flags->will_retain = 0;
}
ka->sb.l = MQTT_KEEPALIVE;
/* ConnID */
cid = (mqtthdr_var_t*) (buf->msg_base + siz);
cid->var_sb.val = htons(strlen(csConnID));
siz += MQTTHDR_VAR_SIZEOF(cid);
memcpy(cid->var_data, csConnID, ntohs(cid->var_sb.val));
/* If Will Flags setup */
if (csWillTopic) {
topic = (mqtthdr_var_t*) (buf->msg_base + siz);
topic->var_sb.val = htons(strlen(csWillTopic));
memcpy(topic->var_data, csWillTopic, ntohs(topic->var_sb.val));
siz += MQTTHDR_VAR_SIZEOF(topic);
wmsg = (mqtthdr_var_t*) (buf->msg_base + siz);
if (csWillMessage) {
wmsg->var_sb.val = htons(strlen(csWillMessage));
memcpy(wmsg->var_data, csWillMessage, ntohs(wmsg->var_sb.val));
} else
wmsg->var_sb.val = 0;
siz += MQTTHDR_VAR_SIZEOF(wmsg);
}
/* If defined Username & Password */
if (csUser) {
user = (mqtthdr_var_t*) (buf->msg_base + siz);
user->var_sb.val = htons(strlen(csUser));
memcpy(user->var_data, csUser, ntohs(user->var_sb.val));
siz += MQTTHDR_VAR_SIZEOF(user);
if (csPass) {
pass = (mqtthdr_var_t*) (buf->msg_base + siz);
pass->var_sb.val = htons(strlen(csPass));
memcpy(pass->var_data, csPass, ntohs(pass->var_sb.val));
siz += MQTTHDR_VAR_SIZEOF(pass);
}
}
*hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr));
mqtt_msgRealloc(buf, siz);
return siz;
}
/*
* mqtt_msgCONNACK() Create CONNACK message
*
* @buf = Message buffer
* @retcode = Return code
* return: -1 error or >-1 message size for send
*/
int
mqtt_msgCONNACK(mqtt_msg_t * __restrict buf, u_char retcode)
{
int siz = 0;
struct mqtthdr *hdr;
mqtthdr_connack_t *ack;
if (!buf)
return -1;
if (retcode > MQTT_RETCODE_DENIED) {
mqtt_SetErr(EINVAL, "Error:: invalid retcode");
return -1;
}
if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr) + sizeof(mqtthdr_connack_t)) == -1)
return -1;
else {
hdr = (struct mqtthdr *) (buf->msg_base + siz);
siz += sizeof(struct mqtthdr);
ack = (mqtthdr_connack_t*) (buf->msg_base + siz);
siz += sizeof(mqtthdr_connack_t);
}
/* fixed header */
MQTTHDR_MSGINIT(hdr);
hdr->mqtt_msg.type = MQTT_TYPE_CONNACK;
*hdr->mqtt_len = sizeof(mqtthdr_connack_t);
/* CONNACK header */
ack->reserved = 0;
ack->retcode = retcode;
return siz;
}
static int
_mqtt_msgSIMPLE_(mqtt_msg_t * __restrict buf, u_char cmd)
{
int siz = 0;
struct mqtthdr *hdr;
if (!buf)
return -1;
if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr)) == -1)
return -1;
else {
hdr = (struct mqtthdr *) (buf->msg_base + siz);
siz += sizeof(struct mqtthdr);
}
/* fixed header */
MQTTHDR_MSGINIT(hdr);
hdr->mqtt_msg.type = cmd;
*hdr->mqtt_len = 0;
return siz;
}
/*
* mqtt_msgPINGREQ() Create PINGREQ message
*
* @buf = Message buffer
* return: -1 error or >-1 message size for send
*/
int
mqtt_msgPINGREQ(mqtt_msg_t * __restrict buf)
{
return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_PINGREQ);
}
/*
* mqtt_msgPINGRESP() Create PINGRESP message
*
* @buf = Message buffer
* return: -1 error or >-1 message size for send
*/
int
mqtt_msgPINGRESP(mqtt_msg_t * __restrict buf)
{
return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_PINGRESP);
}
/*
* mqtt_msgDISCONNECT() Create DISCONNECT message
*
* @buf = Message buffer
* return: -1 error or >-1 message size for send
*/
int
mqtt_msgDISCONNECT(mqtt_msg_t * __restrict buf)
{
return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_DISCONNECT);
}
/* ============= decode ============ */
/*
* 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
* @topicLen = Will Topic length
* @psWillMessage = Will Message, may be NULL
* @msgLen = Will Message length
* return: .reserved == 1 is error or == 0 connection flags & msg ok
*/
mqtthdr_connflgs_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, int topicLen, char * __restrict psWillMessage, int msgLen)
{
mqtthdr_connflgs_t flg = MQTT_CONNFLGS_INIT;
struct mqtthdr *hdr;
mqtthdr_var_t *var;
mqtt_v_t *ka;
mqtthdr_protover_t *proto;
int len, ret;
caddr_t pos;
if (!buf || !buf->msg_base || !buf->msg_len || !psConnID || !connLen)
return flg;
hdr = _mqtt_readHEADER(buf, MQTT_TYPE_CONNECT, &ret, &len);
if (!hdr)
return flg;
if (len < 12) {
mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
return flg;
} else {
pos = buf->msg_base + ret + 1;
var = (mqtthdr_var_t*) pos;
}
/* check init string & protocol */
if (var->var_sb.sb.l != 6 || strncmp(var->var_data, MQTT_CONN_STR, 6)) {
mqtt_SetErr(EINVAL, "Error:: invalid init string %.6s(%d)",
var->var_data, var->var_sb.sb.l);
return flg;
} else {
pos += var->var_sb.sb.l + sizeof(mqtt_v_t);
proto = (mqtthdr_protover_t*) pos;
}
if (*proto != MQTT_PROTO_VER) {
mqtt_SetErr(EINVAL, "Error:: invalid protocol version %d", *pos);
return flg;
} else
pos++;
flg = *(mqtthdr_connflgs_t*) pos;
pos++;
ka = (mqtt_v_t*) pos;
*kasec = ntohs(ka->val);
pos += sizeof(mqtt_v_t);
len -= pos - (caddr_t) var;
/* get ConnID */
var = (mqtthdr_var_t*) pos;
len -= MQTTHDR_VAR_SIZEOF(var);
if (len < 0) {
mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Connection ID %d", len);
flg.reserved = 1;
return flg;
} 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, "Error:: unexpected EOM at Will Topic %d", len);
flg.reserved = 1;
return flg;
} else {
if (psWillTopic && topicLen) {
memset(psWillTopic, 0, topicLen--);
memcpy(psWillTopic, var->var_data,
ntohs(var->var_sb.val) > topicLen ? topicLen : 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, "Error:: unexpected EOM at Will Message %d", len);
flg.reserved = 1;
return flg;
} else {
if (psWillMessage && msgLen) {
memset(psWillMessage, 0, msgLen--);
memcpy(psWillMessage, var->var_data,
ntohs(var->var_sb.val) > msgLen ? msgLen : 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) {
mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Username %d", len);
flg.reserved = 1;
return flg;
} 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) {
mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Password %d", len);
flg.reserved = 1;
return flg;
} 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);
}
}
return flg;
}
/*
* 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, ret;
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, &ret, &len);
if (!hdr)
return (u_char) -1;
if (len < sizeof(mqtthdr_connack_t)) {
mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
return (u_char) -1;
} else {
pos = buf->msg_base + ret + 1;
ack = (mqtthdr_connack_t*) pos;
}
if (ack->retcode > MQTT_RETCODE_DENIED) {
mqtt_SetErr(EINVAL, "Error:: invalid retcode %u", ack->retcode);
return (u_char) -1;
}
return ack->retcode;
}
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>