fix i1905_cmdu_fragment_and_tx()

src/cmdu.c

@@ -145,18 +145,33 @@ struct cmdu_buff *cmdu_alloc(int size)
 	n->end = n->head + size;
 	n->data = n->head;
 	n->cdata = NULL;
-	//n->cdata = (struct cmdu_linear *)n->head;
-	//n->cdata->hdr.version = 0;
+	//n->cdata = (struct cmdu_linear *)(n->head + 18);
 	//n->data = (uint8_t *)(n->cdata + 1);
 	n->tail = n->data;
 	n->num_frags = 0;
 	n->datalen = 0;
 	n->len = 0;
 	n->head -= 18;
+	INIT_LIST_HEAD(&n->fraglist);
 
 	return n;
 }
 
+struct cmdu_buff *cmdu_alloc_frame(int size)
+{
+	struct cmdu_buff *f;
+
+	f = cmdu_alloc(size + sizeof(struct cmdu_header));
+	if (!f)
+		return NULL;
+
+	f->cdata = (struct cmdu_linear *)(f->head + 18);
+	f->data = (uint8_t *)(f->cdata + 1);
+	f->tail = f->data;
+
+	return f;
+}
+
 struct cmdu_buff *cmdu_alloc_default(void)
 {
 #define ETH_FRAME_SZ	1500
@@ -164,6 +179,20 @@ struct cmdu_buff *cmdu_alloc_default(void)
 	return cmdu_alloc(ETH_FRAME_SZ);
 }
 
+struct cmdu_buff *cmdu_alloc_nohdr(void)
+{
+	struct cmdu_buff *f;
+
+	f = cmdu_alloc(ETH_FRAME_SZ);
+	if (f) {
+		f->cdata = NULL;
+		f->data = (uint8_t *)(f->head + 18);
+		f->tail = f->data;
+	}
+
+	return f;
+}
+
 struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t *mid)
 {
 	struct cmdu_buff *f;
@@ -176,7 +205,9 @@ struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t *mid)
 	}
 
 	f->cdata = (struct cmdu_linear *)(f->head + 18);
-	//if (type <= CMDU_TYPE_MAX)
+	f->data = (uint8_t *)(f->cdata + 1);
+	f->tail = f->data;
+
 	cmdu_set_type(f, type);
 
 	if (*mid == 0)
@@ -185,17 +216,14 @@ struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t *mid)
 	cmdu_set_mid(f, *mid);
 	CMDU_SET_LAST_FRAGMENT(f->cdata);
 
-	f->data = (uint8_t *)(f->cdata + 1);
-	f->tail = f->data;
-
 	return f;
 }
 
 void cmdu_free(struct cmdu_buff *c)
 {
 	if (c) {
+		list_flush(&c->fraglist, struct cmdu_frag, list);
 		free(c);
-		// TODO: fraglist
 	}
 }
 
@@ -509,11 +537,12 @@ struct cmdu_buff *cmdu_fragment(uint8_t *data, int datalen)
 	int len = 0;
 
 
-	/* tlv_printf(data, datalen); */
+	//bufprintf(data, datalen, "Passed Data");
 
 	cmdu_for_each_tlv(p, data, remlen) {
 		ptr = (uint8_t *)p;
 
+
 		if (len + tlv_total_length(p) <= FRAG_DATA_SIZE_TLV) {
 			len += tlv_total_length(p);
 		} else {
@@ -526,7 +555,7 @@ struct cmdu_buff *cmdu_fragment(uint8_t *data, int datalen)
 			}
 
 			if (!cmdu) {
-				cmdu = cmdu_alloc(FRAG_DATA_SIZE_TLV);
+				cmdu = cmdu_alloc_frame(FRAG_DATA_SIZE_TLV);
 				if (!cmdu) {
 					printf("-ENOMEM\n");
 					return NULL;
@@ -537,6 +566,8 @@ struct cmdu_buff *cmdu_fragment(uint8_t *data, int datalen)
 				cmdu->num_frags = 0;
 				buf_put_be16(&cmdu->data[FRAG_DATA_SIZE_TLV - tfraglen + 1],
 					     tfraglen - TLV_HLEN);
+
+				bufprintf(cmdu->data, FRAG_DATA_SIZE_TLV, "Fragment-0");
 			} else {
 				if (frag_res) {
 					frag->data[frag_res->len] = p->type;
@@ -916,7 +947,7 @@ struct cmdu_buff *cmdu_defrag(void *rxfq, struct cmdu_buff *lastfrag)
 	pthread_mutex_unlock(&q->qlock);
 
 	/* alloc unfragmented cmdu */
-	cmdu = cmdu_alloc(frag->tlen);
+	cmdu = cmdu_alloc(frag->tlen + sizeof(struct cmdu_header));
 	if (!cmdu) {
 		printf("-ENOMEM\n");
 		return NULL;

src/cmdu.h

@@ -143,8 +143,11 @@ uint16_t tlv_length(struct tlv *t);
 uint16_t tlv_total_length(struct tlv *t);
 
 
+/* Allocates cmdu_buff to hold 'size' length cmdu payload */
+struct cmdu_buff *cmdu_alloc(int size);		// XXX: internal use
+
 /** Allocates cmdu_buff to hold 'size' length cmdu payload */
-struct cmdu_buff *cmdu_alloc(int size);
+struct cmdu_buff *cmdu_alloc_frame(int size);
 
 /**
  * Allocates cmdu_buff to hold full-size ethernet frame of 1500 bytes
@@ -156,6 +159,16 @@ struct cmdu_buff *cmdu_alloc(int size);
  */
 struct cmdu_buff *cmdu_alloc_default(void);
 
+/**
+ * Allocates full-sized cmdu_buff without CMDU header info
+ *
+ * @return newly allocated cmdu_buff on success, or NULL if failed.
+ *
+ * This function is useful to allocate cmdu_buff for tx/rx of LLDP frames.
+ */
+struct cmdu_buff *cmdu_alloc_nohdr(void);
+
+
 /**
  * Convenient function to allocate cmdu_buff that can hold a full-size CMDU
  * @param[in] type  CMDU type

src/i1905.c

@@ -480,7 +480,7 @@ int i1905_send_cmdu(struct i1905_interface_private *ifpriv,
 	sa.sll_halen = ETH_ALEN;
 	memcpy(sa.sll_addr, dst, 6);
 
-	//bufprintf(frm->head, frm->len, "i1905_send_cmdu");
+	bufprintf(frm->head, frm->len, "i1905_send_cmdu");
 
 	ret = sendto(ifpriv->sock_1905, frm->head, frm->len, 0, (struct sockaddr*)&sa,
 		     sizeof(struct sockaddr_ll));
@@ -561,6 +561,114 @@ int i1905_send_cmdu_relay_mcast(struct i1905_private *priv, const char *ifname,
 	return 0;
 }
 
+int i1905_cmdu_fragment_and_tx(struct i1905_interface_private *ifpriv,
+			       uint8_t *dst, uint8_t *src, uint16_t type,
+			       uint16_t *mid, uint8_t *data, int datalen)
+{
+	struct i1905_interface *iface;
+	struct cmdu_buff *frm = NULL;
+	struct cmdu_frag *frag = NULL;
+	struct i1905_private *priv;
+	uint16_t resp_type;
+	int pending = 0;
+	int ret = 0;
+	int i = 0;
+
+
+	priv = (struct i1905_private *)ifpriv->i1905private;
+
+	/* re-enqueue for receive if dst = self */
+	if (hwaddr_equal(dst, iface->aladdr)) {
+		frm = cmdu_alloc_frame(datalen);
+		if (!frm) {
+			err("%s: -ENOMEM\n", __func__);
+			return -1;
+		}
+
+		cmdu_set_type(frm, type);
+		if (*mid == 0)
+			*mid = cmdu_get_next_mid();
+
+		cmdu_set_mid(frm, *mid);
+		CMDU_SET_LAST_FRAGMENT(frm->cdata);
+
+		ret = cmdu_put(frm, data, datalen) ||
+		      cmdu_put_eom(frm);
+
+		if (ret) {
+			cmdu_free(frm);
+			return ret;
+		}
+
+		cmdu_enqueue(&priv->rxqueue, frm);
+		cmdu_queue_signal(&priv->rxqueue);
+		return 0;
+	}
+
+
+	frm = cmdu_fragment(data, datalen);
+	if (!frm) {
+		err("%s: -ENOMEM\n", __func__);
+		return -1;
+	}
+
+	if (*mid == 0)
+		*mid = cmdu_get_next_mid();
+
+	cmdu_set_mid(frm, *mid);
+	cmdu_set_type(frm, type);
+
+	err("TX Frag %d >>>>>>>>>>>>>>>>>>>>>>>>\n", i);
+	ret = i1905_send_cmdu(ifpriv, dst, src, ETHERTYPE_1905, frm);
+	if (ret < 0) {
+		err("%s: failed to send cmdu!\n", __func__);
+		goto out;
+	}
+
+	list_for_each_entry(frag, &frm->fraglist, list) {
+		struct cmdu_buff *ffrm = NULL;
+
+		bufprintf(frag->data, frag->len, "CMDU Fragment:");
+
+		ffrm = cmdu_alloc_frame(frag->len);
+		if (!ffrm) {
+			err("%s: -ENOMEM\n", __func__);
+			goto out;
+		}
+
+		ret = cmdu_put(ffrm, frag->data, frag->len);
+		if (!ret) {
+			cmdu_set_mid(ffrm, *mid);
+			cmdu_set_type(ffrm, type);
+			cmdu_set_fid(ffrm, ++i);
+			if (i == frm->num_frags) {
+				CMDU_SET_LAST_FRAGMENT(ffrm->cdata);
+				cmdu_put_eom(ffrm);
+			}
+
+			err("TX Frag-%d >>>>>>>>>>>>>>>>>>>>>>>>\n", i);
+			ret = i1905_send_cmdu(ifpriv, dst, src, ETHERTYPE_1905, ffrm);
+			if (ret < 0) {
+				err("%s: failed to send cmdu!\n", __func__);
+				cmdu_free(ffrm);
+				goto out;
+			}
+			cmdu_free(ffrm);
+		}
+	}
+
+	//TODO: when dst = mcast, skip matching dst on recv'd cmdu in DEQ
+	resp_type = cmdu_expect_response(type);
+	if (resp_type != CMDU_TYPE_NONE) {
+		cmdu_ackq_enqueue(&priv->txack_q, resp_type, *mid, dst,
+				  CMDU_DEFAULT_TIMEOUT, NULL);
+	}
+
+out:
+	cmdu_free(frm);
+	return ret;
+}
+
 int i1905_cmdu_tx(struct i1905_interface_private *ifpriv,
 		  uint8_t *dst, uint8_t *src, uint16_t type, uint16_t *mid,
 		  uint8_t *data, int datalen)
@@ -584,6 +692,9 @@ int i1905_cmdu_tx(struct i1905_interface_private *ifpriv,
 		return -1;
 	}
 
+	if (datalen > FRAG_DATA_SIZE)
+		return i1905_cmdu_fragment_and_tx(ifpriv, dst, src, type, mid,
+						  data, datalen);
 
 	frm = cmdu_alloc_simple(type, mid);
 	if (!frm) {
@@ -591,11 +702,8 @@ int i1905_cmdu_tx(struct i1905_interface_private *ifpriv,
 		return -1;
 	}
 
-	if (data && datalen > 0)
-		ret = cmdu_put(frm, data, datalen);
-
-	if (!ret)
-		ret = cmdu_put_eom(frm);
+	ret = cmdu_put(frm, data, datalen) ||
+	      cmdu_put_eom(frm);
 
 	if (ret) {
 		cmdu_free(frm);