cmdu.c

/*
 * cmdu.c - IEEE1905 CMDU and TLV handling
 *
 * Copyright (C) 2021 IOPSYS Software Solutions AB. All rights reserved.
 *
 * Author: anjan.chanda@iopsys.eu
 *
 * See LICENSE file for license related information.
 *
 */


#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>

#include "timer.h"
#include "util.h"
#include "bufutil.h"
//#include "1905_defs.h"
#include "1905_tlvs.h"
#include "cmdu.h"


struct tlv *tlv_alloc(uint16_t datalen)
{
	struct tlv *n = calloc(1, sizeof(*n) + datalen);

	return n;
}

void tlv_zero(struct tlv *t)
{
	if (t)
		memset(t, 0, t->len + sizeof(*t));
}

void tlv_free_linear(struct tlv *t)
{
	if (t)
		free(t);
}

int tlv_ok(struct tlv *t, int rem)
{
	uint16_t l = buf_get_be16(&((uint8_t *)t)[1]);
	if (rem >= sizeof(struct tlv) && l <= rem - 3)
		return 1;

	return 0;
}

struct tlv *tlv_next(struct tlv *t, int *rem)
{
	uint16_t l = buf_get_be16(&((uint8_t *)t)[1]);

	*rem -= (l + 3);
	return (struct tlv *)((uint8_t *)t + l + 3);
}

uint16_t tlv_length(struct tlv *t)
{
	return buf_get_be16(&((uint8_t *)t)[1]);
}

uint16_t tlv_total_length(struct tlv *t)
{
	return tlv_length(t) + 3;
}

int is_cmdu_type_valid(uint16_t type)
{
	return (type >= CMDU_TYPE_TOPOLOGY_DISCOVERY &&
		type <= CMDU_TYPE_MAX);
}

int is_cmdu_tlv_required(uint16_t type)
{
	return !(type == CMDU_TYPE_TOPOLOGY_QUERY ||
		type == CMDU_TYPE_HIGHER_LAYER_QUERY ||
		type == CMDU_TYPE_GENERIC_PHY_QUERY);
}

int cmdu_should_relay(uint16_t type)
{
	return (type == CMDU_TYPE_TOPOLOGY_NOTIFICATION ||
		type == CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH ||
		type == CMDU_TYPE_AP_AUTOCONFIGURATION_RENEW ||
		type == CMDU_TYPE_PUSH_BUTTON_EVENT_NOTIFICATION ||
		type == CMDU_TYPE_PUSH_BUTTON_JOIN_NOTIFICATION);
}

/* for unicast request types only */
int is_cmdu_type_response(uint16_t type)
{
	return (type == CMDU_TYPE_TOPOLOGY_RESPONSE ||
		type == CMDU_TYPE_LINK_METRIC_RESPONSE ||
		/* type == CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE || */
		type == CMDU_TYPE_HIGHER_LAYER_RESPONSE ||
		type == CMDU_TYPE_INTERFACE_POWER_CHANGE_RESPONSE);
}

uint16_t cmdu_expect_response(uint16_t req_type)
{
	switch (req_type) {
		case CMDU_TYPE_TOPOLOGY_QUERY:
			return CMDU_TYPE_TOPOLOGY_RESPONSE;
		case CMDU_TYPE_LINK_METRIC_QUERY:
			return CMDU_TYPE_LINK_METRIC_RESPONSE;
		case CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH:
			return CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE;
		case CMDU_TYPE_AP_AUTOCONFIGURATION_WSC:
			return CMDU_TYPE_AP_AUTOCONFIGURATION_WSC;
		case CMDU_TYPE_AP_AUTOCONFIGURATION_RENEW:
			return CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE;
		case CMDU_TYPE_HIGHER_LAYER_QUERY:
			return CMDU_TYPE_HIGHER_LAYER_RESPONSE;
		case CMDU_TYPE_INTERFACE_POWER_CHANGE_REQUEST:
			return CMDU_TYPE_INTERFACE_POWER_CHANGE_RESPONSE;
		case CMDU_TYPE_GENERIC_PHY_QUERY:
			return CMDU_TYPE_GENERIC_PHY_RESPONSE;
		default:
			break;
	}

	return CMDU_TYPE_NONE;
}

struct cmdu_buff *cmdu_alloc(int size)
{
#define CMDU_RESERVE_HEADSPACE	32

	struct cmdu_buff *n;
	uint8_t *p;


	p = calloc(1, sizeof(*n) + size + CMDU_RESERVE_HEADSPACE);
	if (!p)
		return NULL;

	n = (struct cmdu_buff *)p;
	n->head = (uint8_t *)(n + 1) + CMDU_RESERVE_HEADSPACE;
	n->end = n->head + size;
	n->data = n->head;
	n->cdata = NULL;
	//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

	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;


	f = cmdu_alloc_default();
	if (!f) {
		fprintf(stderr, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	f->cdata = (struct cmdu_linear *)(f->head + 18);
	f->data = (uint8_t *)(f->cdata + 1);
	f->tail = f->data;

	cmdu_set_type(f, type);

	if (*mid == 0)
		*mid = cmdu_get_next_mid();

	cmdu_set_mid(f, *mid);
	CMDU_SET_LAST_FRAGMENT(f->cdata);

	return f;
}

void cmdu_free(struct cmdu_buff *c)
{
	if (c) {
		list_flush(&c->fraglist, struct cmdu_frag, list);
		free(c);
	}
}

int cmdu_size(struct cmdu_buff *c)
{
	return c ? c->datalen + sizeof(struct cmdu_header) : 0;
}

#if 0
int cmdu_reserve(struct cmdu_buff *c, size_t s)
{
	if (!c)
		return -1;

	if (c->head - (uint8_t *)(c + 1) < s)
		return -1;

	c->head -= s;

	return 0;
}
#endif


int cmdu_copy_tlvs_linear(struct cmdu_buff *c, uint8_t *tlvs, uint32_t tlvslen)
{
	if (c->end - c->tail < tlvslen)
		return -1;

	memcpy(c->tail, tlvs, tlvslen);
	c->tail += tlvslen;
	c->datalen += tlvslen;

	return 0;
}

struct cmdu_buff *cmdu_alloc_custom(uint16_t type, uint16_t *mid, char *ifname,
				    uint8_t *origin, uint8_t *tlvs,
				    uint32_t tlvslen)
{
	struct cmdu_buff *f;
	int ret;


	f = cmdu_alloc_frame(tlvslen + 128);
	if (!f) {
		fprintf(stderr, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	cmdu_set_type(f, type);
	cmdu_set_mid(f, *mid);

	ret = cmdu_copy_tlvs_linear(f, tlvs, tlvslen);
	if (ret) {
		fprintf(stderr, "%s: tlv-length > max cmdu size!\n", __func__);
		cmdu_free(f);
		return NULL;
	}

	memcpy(f->dev_macaddr, origin, 6);
	if (ifname)
		strncpy(f->dev_ifname, ifname, 15);

	return f;
}

struct cmdu_buff *cmdu_clone(struct cmdu_buff *frm)
{
	struct cmdu_buff *f;
	int len;


	if (!frm || !frm->cdata) {
		fprintf(stderr, "%s: cmdu for cloning is invalid!\n", __func__);
		return NULL;
	}

	len = cmdu_size(frm);
	f = cmdu_alloc(len);
	if (!f) {
		fprintf(stderr, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	f->cdata = (struct cmdu_linear *)(f->head + 18);
	f->data = (uint8_t *)(f->cdata + 1);
	f->datalen = frm->datalen;
	f->tail = f->data + f->datalen;
	memcpy(f->cdata, frm->cdata, len);

	memcpy(f->dev_macaddr, frm->dev_macaddr, 6);
	strncpy(f->dev_ifname, frm->dev_ifname, 15);
	memcpy(f->origin, frm->origin, 6);

	return f;
}

int cmdu_copy_tlvs(struct cmdu_buff *c, struct tlv *tv[], int tv_arrsize)
{
	uint16_t tlvslen = 0;
	int i;


	for (i = 0; i < tv_arrsize; i++)
		tlvslen += tv[i]->len;


	if (c->end - c->tail < tlvslen)
		return -1;


	for (i = 0; i < tv_arrsize; i++) {
		uint16_t tlen = tv[i]->len;

		*c->tail = tv[i]->type;
		buf_put_be16(c->tail + 1, tlen);
		memcpy(c->tail + 3, tv[i]->data, tlen);
		c->tail += tlen + sizeof(struct tlv);
		c->datalen += tlen + sizeof(struct tlv);
	}

	return 0;
}

int cmdu_put_tlv(struct cmdu_buff *c, struct tlv *t)
{
	uint16_t tlen;


	if (!c || !t)
		return -1;


	tlen = t->len;

	if (c->end - c->tail < tlen) {
		fprintf(stderr, "%s: %d: c->end = %p c->tail = %p\n",
			__func__, __LINE__, c->end, c->tail);

		return -1;
	}

	if ((uint8_t *)t != c->tail) {
		fprintf(stderr, "%s: tlv outside cmdu buffer; use cmdu_copy_tlv() instead\n",
			__func__);
		return -1;
	}


	buf_put_be16(c->tail + 1, tlen);
	c->tail += tlen + sizeof(*t);
	c->datalen += tlen + sizeof(*t);

	return 0;
}

int cmdu_put(struct cmdu_buff *c, uint8_t *bytes, int len)
{
	if (!c || !bytes)
		return -1;

	if (c->end - c->tail < len) {
		fprintf(stderr, "%s: %d: c->end = %p c->tail = %p\n",
			__func__, __LINE__, c->end, c->tail);

		return -1;
	}

	memcpy(c->tail, bytes, len);
	c->tail += len;
	c->datalen += len;

	return 0;
}

int cmdu_put_eom(struct cmdu_buff *c)
{
	uint8_t eom[3] = {0};

	return cmdu_put(c, eom, sizeof(eom));
}

struct tlv *cmdu_reserve_tlv(struct cmdu_buff *c, uint16_t tlv_datalen)
{
	uint16_t len = tlv_datalen + TLV_HLEN;

	if (!c)
		return NULL;

	if (c->end - c->tail < len) {
		fprintf(stderr, "%s: Failed to reserve %hu! Allocate new cmdu fragment\n",
			__func__, tlv_datalen);

		return NULL;
	}

	return (struct tlv *)c->tail;
}

int cmdu_parse_tlvs(struct cmdu_buff *c, struct tlv *tv[][16],
		    struct tlv_policy *policy, int policy_len)
{
	int idx[policy_len];
	struct tlv *t;
	int len;
	int i;


	if (!c)
		return -1;

	for (i = 0; i < policy_len; i++) {
		memset(tv[i], 0, 16 * sizeof(struct tlv *));
		idx[i] = 0;
	}
	len = c->datalen;

	cmdu_for_each_tlv(t, c->data, len) {
		for (i = 0; i < policy_len; i++) {
			if (policy[i].type != t->type)
				continue;

			if (policy[i].minlen > 0 &&
			    tlv_length(t) < policy[i].minlen)
				continue;

			if (policy[i].maxlen > 0 &&
			    tlv_length(t) > policy[i].maxlen)
				continue;

			if (tv[i][0]) {
				if (policy[i].present == TLV_PRESENT_ONE)
					continue;
			}

			tv[i][idx[i]++] = t;
		}
	}

	return 0;
}

/* Extracts the first matching tlv from tlv-stream.
 * This function is destructive, i.e. it modifies the passed cmdu buffer.
 * Use cmdu_peek_tlv() for the non-destructive version.
 */
struct tlv *cmdu_extract_tlv(struct cmdu_buff *c, uint8_t tlv_type)
{
	struct tlv *t, *tmp;
	int found = 0;
	int inlen;


	if (!c)
		return NULL;

	inlen = c->datalen;

	cmdu_for_each_tlv(t, c->data, inlen) {
		if (t->type == tlv_type) {
			found = 1;
			break;
		}
	}

	if (found) {
		uint16_t tlen = tlv_total_length(t);

		tmp = tlv_alloc(tlv_length(t));
		if (tmp)
			memcpy(tmp, t, tlen);

		inlen -= tlen;
		memmove(t, (uint8_t *)t + tlen, inlen);
		c->datalen -= tlen;
		return tmp;
	}

	return NULL;
}

struct tlv *cmdu_peek_tlv(struct cmdu_buff *c, uint8_t tlv_type)
{
	struct tlv *t;
	int len;

	if (!c)
		return NULL;

	len = c->datalen;

	cmdu_for_each_tlv(t, c->data, len) {
		if (t->type == tlv_type)
			return t;
	}

	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;


	bufprintf(data, datalen, "Data to fragment");

	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_frame(FRAG_DATA_SIZE_TLV);
				if (!cmdu) {
					printf("-ENOMEM\n");
					return NULL;
				}

				cmdu_put(cmdu, data, FRAG_DATA_SIZE_TLV);
				cmdu->num_frags = 0;
				buf_put_be16(&cmdu->data[FRAG_DATA_SIZE_TLV - tfraglen + 1],
					     tfraglen - TLV_HLEN);

				bufprintf(cmdu->data, cmdu->datalen, "Fragment-0");
			} 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;
}


static struct cmdu_frag_rx *cmdufrag_lookup(void *rxfq, uint16_t type,
					    uint16_t mid, uint8_t fid,
					    uint8_t *origin)
{
	struct cmdufrag_queue *q = (struct cmdufrag_queue *)rxfq;
	int idx = cmdu_frag_hash(type, mid, origin);
	struct cmdu_frag_rx *frag = NULL;


	pthread_mutex_lock(&q->qlock);
	hlist_for_each_entry(frag, &q->table[idx], hlist) {
		if (frag->type == type && frag->mid == mid &&
		    !memcmp(frag->origin, origin, 6)) {
			if (frag->fid == fid) {
				pthread_mutex_unlock(&q->qlock);
				return frag;
			}
		}
	}
	pthread_mutex_unlock(&q->qlock);

	return NULL;
}

static void cmdu_fragqueue_delete_chain(struct cmdu_frag_rx *frag)
{
	struct cmdu_frag_rx *ee = NULL, *e;

	if (!frag)
		return;

	for (e = frag; e; e = e->next) {
		if (ee) {
			printf("freeing ee ..fid = %d ....\n", ee->fid);
			free(ee);
		}
		ee = e;
	}

	if (ee) {
		printf("freeing ee ..fid = %d ...\n", ee->fid);
		free(ee);
	}
}

static void cmdu_fragqueue_free_entry(struct cmdu_frag_rx *frag)
{
	if (frag)
		free(frag);
}

static struct cmdu_frag_rx *cmdu_create_rxfrag(struct cmdu_buff *cmdu,
					       uint32_t timeout)
{
	struct cmdu_frag_rx *frag;
	struct timeval tsp = { 0 };

	frag = calloc(1, sizeof(*frag));
	if (!frag) {
		fprintf(stderr, "calloc failed. err = NOMEM\n");
		return NULL;
	}

	frag->cmdu = cmdu;
	frag->type = cmdu_get_type(cmdu);
	frag->mid = cmdu_get_mid(cmdu);
	frag->fid = cmdu_get_fid(cmdu);
	frag->last_frag = IS_CMDU_LAST_FRAGMENT(cmdu->cdata) ? true : false;
	memcpy(frag->origin, cmdu_get_origin(cmdu), 6);
	frag->next = NULL;
	frag->last = frag;
	frag->tlen = cmdu->datalen;	//0;
	frag->numfrags = 1;
	gettimeofday(&tsp, NULL);
	frag->ageing_time = timeout;
	timeradd_msecs(&tsp, frag->ageing_time, &frag->ageing_tmo);
	frag->ageing_tmo.tv_usec = (frag->ageing_tmo.tv_usec / 1000) * 1000;
	fprintf(stderr,
		"CREATE frag: type = 0x%04x  mid = %hu (%d) datalen = %d  origin = " MACFMT " timeout = { %u (%lu:%lu) }\n",
		cmdu_get_type(cmdu),
		cmdu_get_mid(cmdu),
		cmdu_get_fid(cmdu),
		frag->tlen,
		MAC2STR(cmdu_get_origin(cmdu)),
		frag->ageing_time,
		frag->ageing_tmo.tv_sec,
		frag->ageing_tmo.tv_usec / 1000);

	return frag;
}

int cmdufrag_queue_enqueue(void *rxfq, struct cmdu_buff *cmdu, uint32_t timeout)
{
	struct cmdufrag_queue *q = (struct cmdufrag_queue *)rxfq;
	struct cmdu_frag_rx *frag = NULL;
	uint8_t *origin;
	uint16_t type;
	uint16_t mid;
	uint8_t fid;


	type = cmdu_get_type(cmdu);
	mid = cmdu_get_mid(cmdu);
	fid = cmdu_get_fid(cmdu);
	origin = cmdu_get_origin(cmdu);

	frag = cmdufrag_lookup(rxfq, type, mid, fid, origin);
	if (frag) {
		fprintf(stderr,
			"Duplicate: type = 0x%04x mid = %hu fid = %d origin = " MACFMT "\n",
			type, mid, fid, MAC2STR(origin));

		return -1;
	}

	frag = cmdu_create_rxfrag(cmdu, timeout);
	if (frag) {
		int idx = cmdu_frag_hash(type, mid, origin);

		pthread_mutex_lock(&q->qlock);
		q->pending_cnt++;
		pthread_mutex_unlock(&q->qlock);

		fprintf(stderr,
			"ENQ: type = 0x%04x  mid = %hu fid = %d  origin = " MACFMT "\n",
			type, mid, fid, MAC2STR(origin));

		if (fid > 0) {
			struct cmdu_frag_rx *firstfrag = NULL;

			firstfrag = cmdufrag_lookup(rxfq, type, mid, 0, origin);
			if (!firstfrag) {
				fprintf(stderr,
					"First fragment missing for mid = %hu\n", mid);
				cmdu_fragqueue_free_entry(frag);
				return -1;
			}

			pthread_mutex_lock(&q->qlock);
			firstfrag->last->next = frag;
			firstfrag->last = frag;
			firstfrag->tlen += frag->cmdu->datalen;
			firstfrag->numfrags++;
			fprintf(stderr, "%s: tlen = %d   numfrags = %d\n", __func__, firstfrag->tlen, firstfrag->numfrags);

			/* do not ageout fragments other than the first.
			 * If the first one ages-out, then all the related
			 * fragments will be cleaned up.
			 */
			pthread_mutex_unlock(&q->qlock);
			return 0;
		}

		pthread_mutex_lock(&q->qlock);
		hlist_add_head(&frag->hlist, &q->table[idx]);

		if (timer_pending(&q->ageing_timer)) {
			if (timercmp(&q->next_tmo, &frag->ageing_tmo, >)) {
				q->next_tmo.tv_sec = frag->ageing_tmo.tv_sec;
				q->next_tmo.tv_usec = frag->ageing_tmo.tv_usec;

				timer_set(&q->ageing_timer, frag->ageing_time);
			}
		} else {
			q->next_tmo.tv_sec = frag->ageing_tmo.tv_sec;
			q->next_tmo.tv_usec = frag->ageing_tmo.tv_usec;
			timer_set(&q->ageing_timer, frag->ageing_time);
		}

		pthread_mutex_unlock(&q->qlock);
		return 0;
	}

	return -1;
}

static void cmdu_frag_ageout(struct cmdufrag_queue *st, struct hlist_head *head,
			     struct timeval *min_next_tmo)
{
	struct cmdu_frag_rx *frag;
	struct hlist_node *tmp;
	struct timeval now = { 0 };


	gettimeofday(&now, NULL);
	now.tv_usec = (now.tv_usec / 1000) * 1000;

	hlist_for_each_entry_safe(frag, tmp, head, hlist) {
		if (timercmp(&frag->ageing_tmo, &now, <=)) {
			st->pending_cnt--;
			hlist_del(&frag->hlist, head);
			fprintf(stderr, "Fragments from " MACFMT " aged out.\n",
				MAC2STR(frag->origin));
			cmdu_fragqueue_delete_chain(frag);
		} else {
			struct timeval new_next_tmo = { 0 };

			timersub(&frag->ageing_tmo, &now, &new_next_tmo);
			if (!timercmp(min_next_tmo, &new_next_tmo, <)) {
				min_next_tmo->tv_sec = new_next_tmo.tv_sec;
				min_next_tmo->tv_usec = new_next_tmo.tv_usec;
			}
		}
	}
}

static void cmdu_fragqueue_ageing_timer_run(atimer_t *t)
{
	struct cmdufrag_queue *st = container_of(t, struct cmdufrag_queue, ageing_timer);
	struct timeval min_next_tmo = { .tv_sec = 999999 };
	int remain_cnt = 0;
	struct timeval nu;
	int i;

	gettimeofday(&nu, NULL);

	fprintf(stderr, "\n Timer now = %lu.%lu   cnt = %d\n",
		nu.tv_sec, nu.tv_usec, st->pending_cnt);

	for (i = 0; i < NUM_FRAGMENTS; i++) {
		if (hlist_empty(&st->table[i]))
			continue;

		cmdu_frag_ageout(st, &st->table[i], &min_next_tmo);
	}

	remain_cnt = st->pending_cnt;
	st->next_tmo.tv_sec = min_next_tmo.tv_sec;
	st->next_tmo.tv_usec = min_next_tmo.tv_usec;

	if (remain_cnt) {
		uint32_t tmo_msecs = st->next_tmo.tv_sec * 1000 +
					st->next_tmo.tv_usec / 1000;

		if (tmo_msecs > 0)
			timer_set(&st->ageing_timer, tmo_msecs);
	}
}

struct cmdu_buff *cmdu_defrag(void *rxfq, struct cmdu_buff *lastfrag)
{
	struct cmdufrag_queue *q = (struct cmdufrag_queue *)rxfq;
	struct cmdu_frag_rx *ee = NULL, *e;
	struct cmdu_frag_rx *frag = NULL;
	struct cmdu_buff *cmdu = NULL;
	struct hlist_node *tmp;
	uint32_t fidsum = 0;
	bool is_lastfrag;
	uint8_t *origin;
	uint16_t type;
	uint16_t mid;
	uint8_t fid;
	int idx;


	if (!lastfrag || !lastfrag->cdata)
		return NULL;

	type = cmdu_get_type(lastfrag);
	mid = cmdu_get_mid(lastfrag);
	fid = cmdu_get_fid(lastfrag);
	is_lastfrag = IS_CMDU_LAST_FRAGMENT(lastfrag->cdata);
	origin = cmdu_get_origin(lastfrag);

	if (!is_lastfrag)
		return NULL;

	idx = cmdu_frag_hash(type, mid, origin);

	pthread_mutex_lock(&q->qlock);
	hlist_for_each_entry_safe(frag, tmp, &q->table[idx], hlist) {
		if (frag->type == type && frag->mid == mid &&
		    !memcmp(frag->origin, origin, 6)) {

			fprintf(stderr, "DEFRAG: type: %hu mid = %hu  fid = %d\n",
				frag->type, frag->mid, frag->fid);

			hlist_del(&frag->hlist, &q->table[idx]);
			q->pending_cnt -= frag->numfrags;
			break;
		}
	}
	pthread_mutex_unlock(&q->qlock);

	/* alloc unfragmented cmdu */
	cmdu = cmdu_alloc_frame(frag->tlen + 3);	/* including EOM */
	if (!cmdu) {
		printf("-ENOMEM\n");
		return NULL;
	}

	fprintf(stderr, "%s: reassembled CMDU datalen = %d\n", __func__, frag->tlen + 3);
	cmdu_set_type(cmdu, type);
	cmdu_set_mid(cmdu, mid);
	memcpy(cmdu->origin, origin, 6);

	for (e = frag; e; e = e->next) {
		if (ee) {
			fprintf(stderr, "freeing fid = %d\n", ee->fid);
			free(ee);
		}
		fidsum += e->fid;
		memcpy(cmdu->tail, e->cmdu->data, e->cmdu->datalen);
		cmdu->datalen += e->cmdu->datalen;
		cmdu->tail += e->cmdu->datalen;
		ee = e;
	}

	if (ee) {
		fprintf(stderr, "freeing fid = %d\n", ee->fid);
		free(ee);
	}

	fprintf(stderr, "frags sum = %d\n", fidsum);
	if (fid * (fid + 1) != 2 * fidsum) {
		fprintf(stderr, "Defrag Failure!\n");
		//TODO: cmdu_free(cmdu);
		return NULL;
	}


	bufprintf(cmdu->data, cmdu->datalen, "=== DEFRAG CMDU ===");

	return cmdu;
}

int cmdufrag_queue_init(void *rxfq)
{
	struct cmdufrag_queue *q = (struct cmdufrag_queue *)rxfq;

	memset(q, 0, sizeof(*q));
	pthread_mutex_init(&q->qlock, NULL);