diff --git a/src/cmdu.c b/src/cmdu.c index 9f35afa11ea3486b40dd3230b8f29a040fcc3438..2d095424946d036ae1b1fcb1aafb629ac3d307e6 100644 --- a/src/cmdu.c +++ b/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; diff --git a/src/cmdu.h b/src/cmdu.h index 4a112e0bb41138caccea97282494d88f6d009ecc..b2ff75bf7cda9774bb72da4e90958981a1a2ab84 100644 --- a/src/cmdu.h +++ b/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 diff --git a/src/i1905.c b/src/i1905.c index 6d841266ff33f04e8e7ba2ce37b820575b58e8ed..c7880d59a6af0cb63d4b2f5dd610f6ca123424f5 100644 --- a/src/i1905.c +++ b/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);