--- mqtt/src/Attic/conn.c	2011/11/21 12:47:15	1.1.2.2
+++ mqtt/src/Attic/conn.c	2011/12/13 15:23:43	1.1.2.14
@@ -8,6 +8,7 @@
  *
  * @buf = Message buffer
  * @csConnID = ConnectID
+ * @kasec = Keep alive timeout
  * @csUser = Username if !=NULL
  * @csPass = Password for Username, only if csUser is set
  * @csWillTopic = Will Topic if !=NULL Will Flags set into message
@@ -19,9 +20,9 @@
  */
 int
 mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const char *csConnID, 
-		const char *csUser, const char *csPass, 
+		u_short kasec, const char *csUser, const char *csPass, 
 		const char *csWillTopic, const char *csWillMessage, 
-		char ClrSess, char WillQOS, char WillRetain)
+		u_char ClrSess, u_char WillQOS, u_char WillRetain)
 {
 	int siz = 0;
 	struct mqtthdr *hdr;
@@ -32,19 +33,19 @@ mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const cha
 
 	if (!buf || !csConnID)
 		return -1;
-	if (strlen(csConnID) > 23) {
+	if (strlen(csConnID) > 24) {
 		mqtt_SetErr(EINVAL, "Error:: invalid argument ConnID is too long (max 23 bytes)");
 		return -1;
 	}
-	if (csUser && strlen(csUser) > 12) {
+	if (csUser && strlen(csUser) > 13) {
 		mqtt_SetErr(EINVAL, "Error:: invalid argument Username is too long (max 12 bytes)");
 		return -1;
 	}
-	if (csPass && strlen(csPass) > 12) {
+	if (csPass && strlen(csPass) > 13) {
 		mqtt_SetErr(EINVAL, "Error:: invalid argument Password is too long (max 12 bytes)");
 		return -1;
 	}
-	if (WillQOS < MQTT_QOS_ONCE && WillQOS > MQTT_QOS_EXACTLY) {
+	if (WillQOS > MQTT_QOS_EXACTLY) {
 		mqtt_SetErr(EINVAL, "Error:: invalid argument WillQOS - unknown QOS value");
 		return -1;
 	}
@@ -65,6 +66,7 @@ mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const cha
 	}
 
 	/* fixed header */
+	MQTTHDR_MSGINIT(hdr);
 	hdr->mqtt_msg.type = MQTT_TYPE_CONNECT;
 	*hdr->mqtt_len = 0;
 
@@ -76,14 +78,14 @@ mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const cha
 
 	/* CONNECT header */
 	flags->clean_sess = ClrSess ? 1 : 0;
-	if (csUser) {
+	if (csUser && *csUser) {
 		flags->username = 1;
 		flags->password = csPass ? 1 : 0;
 	} else {
 		flags->username = 0;
 		flags->password = 0;
 	}
-	if (csWillTopic) {
+	if (csWillTopic && *csWillTopic) {
 		flags->will_flg = 1;
 		flags->will_qos = WillQOS;
 		flags->will_retain = WillRetain ? 1 : 0;
@@ -93,7 +95,7 @@ mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const cha
 		flags->will_retain = 0;
 	}
 
-	ka->sb.l = MQTT_KEEPALIVE;
+	ka->val = kasec ? htons(kasec) : htons(MQTT_KEEPALIVE);
 
 	/* ConnID */
 	cid = (mqtthdr_var_t*) (buf->msg_base + siz);
@@ -102,14 +104,14 @@ mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const cha
 	memcpy(cid->var_data, csConnID, ntohs(cid->var_sb.val));
 
 	/* If Will Flags setup */
-	if (csWillTopic) {
+	if (csWillTopic && *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) {
+		if (csWillMessage && *csWillMessage) {
 			wmsg->var_sb.val = htons(strlen(csWillMessage));
 			memcpy(wmsg->var_data, csWillMessage, ntohs(wmsg->var_sb.val));
 		} else
@@ -118,13 +120,13 @@ mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const cha
 	}
 
 	/* If defined Username & Password */
-	if (csUser) {
+	if (csUser && *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) {
+		if (csPass && *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));
@@ -132,7 +134,7 @@ mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const cha
 		}
 	}
 
-	*hdr->mqtt_len = mqtt_encodeLen(siz - 2);
+	*hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr));
 	mqtt_msgRealloc(buf, siz);
 	return siz;
 }
@@ -168,6 +170,7 @@ mqtt_msgCONNACK(mqtt_msg_t * __restrict buf, u_char re
 	}
 
 	/* fixed header */
+	MQTTHDR_MSGINIT(hdr);
 	hdr->mqtt_msg.type = MQTT_TYPE_CONNACK;
 	*hdr->mqtt_len = sizeof(mqtthdr_connack_t);
 
@@ -176,4 +179,321 @@ mqtt_msgCONNACK(mqtt_msg_t * __restrict buf, u_char re
 	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 and must be free()
+ * @psWillMessage = Will Message, may be NULL if !NULL must be 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_v_t *ka;
+	mqtthdr_protover_t *proto;
+	int len, ret;
+	caddr_t pos;
+
+	if (!buf || !buf->msg_base || !buf->msg_len || !psConnID || !connLen)
+		return cack;
+
+	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_CONNECT, &ret, &len);
+	if (!hdr)
+		return cack;
+	if (len < 12) {
+		mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
+		return cack;
+	} else {
+		pos = buf->msg_base + ret + 1;
+		var = (mqtthdr_var_t*) pos;
+	}
+	/* check init string & protocol */
+	if (var->var_sb.sb.l != 6 || strncmp((char*) var->var_data, MQTT_CONN_STR, 6)) {
+		mqtt_SetErr(EINVAL, "Error:: invalid init string %.6s(%d)", 
+				var->var_data, var->var_sb.sb.l);
+		cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL;
+		return cack;
+	} 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);
+		cack.retcode = MQTT_RETCODE_REFUSE_VER;
+		return cack;
+	} 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);
+		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, "Error:: unexpected EOM at Will Topic %d", len);
+			cack.retcode = MQTT_RETCODE_REFUSE_ID;
+			return cack;
+		} else {
+			if (psWillTopic) {
+				*psWillTopic = 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, "Error:: unexpected EOM at Will Message %d", len);
+			cack.retcode = MQTT_RETCODE_REFUSE_ID;
+			return cack;
+		} else {
+			if (psWillMessage) {
+				*psWillMessage = malloc(ntohs(var->var_sb.val) + 1);
+				if (!*psWillMessage) {
+					LOGERR;
+					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) {
+			mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Username %d", len);
+			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) {
+			mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Password %d", len);
+			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, 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;
+}
+
+/*
+ * 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, ret;
+	struct mqtthdr *hdr;
+
+	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_DISCONNECT, &ret, &len);
+	if (!hdr || ret != 1)
+		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, ret;
+	struct mqtthdr *hdr;
+
+	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_PINGREQ, &ret, &len);
+	if (!hdr || ret != 1)
+		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, ret;
+	struct mqtthdr *hdr;
+
+	hdr = _mqtt_readHEADER(buf, MQTT_TYPE_PINGRESP, &ret, &len);
+	if (!hdr || ret != 1)
+		return -1;
+
+	return len;
 }