diff --git a/src/cmdu.c b/src/cmdu.c index 9fd851e44f2f51ded0687b7d68fa5be9373fd12a..70b93d35d3c62b43ea0044ece81c9942ea9acc1f 100644 --- a/src/cmdu.c +++ b/src/cmdu.c @@ -406,3 +406,143 @@ struct tlv *cmdu_peek_tlv(struct cmdu_buff *c, uint8_t tlv_type) return NULL; } + +struct cmdu_buff *cmdu_fragment(uint8_t *data, int datalen) +{ + struct cmdu_frag *frag_res = NULL; + struct cmdu_frag *frag = NULL; + struct cmdu_buff *cmdu = NULL; + uint16_t tnextfraglen = 0; + uint16_t tfraglen = 0; + int frag_residue = 0; + struct tlv *p = NULL; + int remlen = datalen; + uint8_t *ptr = data; + uint16_t plen; + int ppos = 0; + int len = 0; + int i; + + + /* tlv_printf(data, datalen); */ + + 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 { + + tfraglen = FRAG_DATA_SIZE_TLV - len; + tnextfraglen = len + tlv_total_length(p) - FRAG_DATA_SIZE_TLV; + if (tfraglen <= TLV_HLEN) { + tfraglen = FRAG_DATA_SIZE_TLV; + tnextfraglen = len + tlv_total_length(p) - tfraglen; + } + + if (!cmdu) { + cmdu = cmdu_alloc(FRAG_DATA_SIZE_TLV); + if (!cmdu) { + printf("-ENOMEM\n"); + return NULL; + } + + memcpy(cmdu->data, ptr, FRAG_DATA_SIZE_TLV); + cmdu->datalen += FRAG_DATA_SIZE; + cmdu->num_frags = 0; + buf_put_be16(&cmdu->data[FRAG_DATA_SIZE_TLV - tfraglen + 1], + tfraglen - TLV_HLEN); + } else { + if (frag_res) { + frag->data[frag_res->len] = p->type; + buf_put_be16(&frag_res->data[frag_res->len + 1], + frag_residue - TLV_HLEN); + memcpy(&frag_res->data[frag_res->len + TLV_HLEN], + ptr + TLV_HLEN, frag_residue - TLV_HLEN); + frag->len += frag_residue; + frag_residue = 0; + frag_res = NULL; + } else { + frag = calloc(1, FRAG_DATA_SIZE_TLV + sizeof(*frag)); + if (!frag) + return NULL; + + frag->data = (uint8_t *)(frag + 1); + frag->data[0] = p->type; + buf_put_be16(&frag->data[1], tfraglen - TLV_HLEN); + memcpy(&frag->data[TLV_HLEN], ptr + TLV_HLEN, tfraglen - TLV_HLEN); + frag->len += tfraglen; + list_add_tail(&frag->list, &cmdu->fraglist); + cmdu->num_frags++; + } + } + + ppos = tfraglen; + plen = tnextfraglen; + + while (plen + TLV_HLEN > FRAG_DATA_SIZE_TLV) { + frag = calloc(1, FRAG_DATA_SIZE_TLV + sizeof(*frag)); + if (!frag) + return NULL; + + frag->data = (uint8_t *)(frag + 1); + frag->data[0] = p->type; + buf_put_be16(&frag->data[1], FRAG_DATA_SIZE); + memcpy(&frag->data[TLV_HLEN], &ptr[ppos], FRAG_DATA_SIZE); + frag->len += FRAG_DATA_SIZE_TLV; + list_add_tail(&frag->list, &cmdu->fraglist); + cmdu->num_frags++; + + plen -= FRAG_DATA_SIZE; + ppos += FRAG_DATA_SIZE; + } + + len = 0; + + /* residue */ + if (plen) { + frag = calloc(1, FRAG_DATA_SIZE_TLV + sizeof(*frag)); + if (!frag) + return NULL; + + frag->data = (uint8_t *)(frag + 1); + frag->data[0] = p->type; + buf_put_be16(&frag->data[1], plen); + memcpy(&frag->data[TLV_HLEN], &ptr[ppos], plen); + frag->len += plen + TLV_HLEN; + list_add_tail(&frag->list, &cmdu->fraglist); + cmdu->num_frags++; + if (frag->len + TLV_HLEN < FRAG_DATA_SIZE_TLV) { + frag_res = frag; + frag_residue = FRAG_DATA_SIZE_TLV - frag->len; + } else { + frag_res = NULL; + frag_residue = 0; + } + + if (FRAG_DATA_SIZE_TLV - frag->len > TLV_HLEN) + len = frag->len; + } + } + } + + +#ifdef I1905_DEBUG + printf("\ncmdu fragments\n"); + printf("CMDU: len = %d\n", cmdu->datalen + TLV_HLEN); + for (i = 0; i < cmdu->datalen + TLV_HLEN; i++) + printf("%02x ", cmdu->data[i] & 0xff); + + printf("\n\n"); + + list_for_each_entry(frag, &cmdu->fraglist, list) { + printf("FRAGMENT: fraglen = %d\n", frag->len); + for (i = 0; i < frag->len; i++) { + printf("%02x ", frag->data[i] & 0xff); + } + printf("\n\n"); + } +#endif + + return cmdu; +} diff --git a/src/cmdu.h b/src/cmdu.h index 67507f555e6520de302330ce4eba2da7da176726..2991d0339c9588d043f08a897a15db9345c596f0 100644 --- a/src/cmdu.h +++ b/src/cmdu.h @@ -24,7 +24,7 @@ struct cmdu_header { #define CMDU_SET_LAST_FRAGMENT(c) (c)->hdr.flag |= 0x80 #define CMDU_SET_RELAY_MCAST(c) (c)->hdr.flag |= 0x40 -#define CMDU_DEFAULT_TIMEOUT 2000 +#define CMDU_DEFAULT_TIMEOUT 1000 struct cmdu_linear { @@ -33,6 +33,16 @@ struct cmdu_linear { } __attribute__ ((packed)); +#define TLV_HLEN 3 +#define FRAG_DATA_SIZE_TLV 1492 +#define FRAG_DATA_SIZE (FRAG_DATA_SIZE_TLV - TLV_HLEN) + +struct cmdu_frag { + uint8_t *data; + uint16_t len; + struct list_head list; +}; + struct cmdu_buff { uint8_t *head; uint8_t *data; @@ -110,6 +120,7 @@ int cmdu_reserve(struct cmdu_buff *c, size_t s); int cmdu_expand(struct cmdu_buff *c, size_t newsize); int cmdu_send(struct cmdu_buff *c); +struct cmdu_buff *cmdu_fragment(uint8_t *data, int datalen); const char *cmdu_type2str(uint16_t type);