--- mqtt/src/Attic/conn.c	2011/11/22 14:13:04	1.1.2.5
+++ mqtt/src/Attic/conn.c	2011/12/05 14:11:47	1.1.2.6
@@ -239,3 +239,156 @@ 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 = (struct mqtthdr*) buf->msg_base;
+	if (hdr->mqtt_msg.type != MQTT_TYPE_CONNECT) {
+		mqtt_SetErr(EINVAL, "Error:: wrong command #%d", hdr->mqtt_msg.type);
+		return flg;
+	} else
+		len = mqtt_decodeLen(hdr->mqtt_len, &ret);
+	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;
+}