/*
 * agent_tlv.c - tlv building functions
 *
 * Copyright (C) 2020 IOPSYS Software Solutions AB. All rights reserved.
 *
 * Author: jakob.olsson@iopsys.eu
 *
 */
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif

#include "agent_tlv.h"

#include <cmdu.h>
#include <dpputils.h>
#include <easy/bufutil.h>
#include <easy/timestamp.h>
#include <easy/utils.h>
#include <easymesh.h>
#include <i1905_wsc.h>
#include <libubox/list.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <timer.h>
#include <uci.h>
#include <wifidefs.h>

#include "agent.h"
#include "agent_map.h"
#include "config.h"
#include "unasta.h"
#include "utils/1905_ubus.h"
#include "utils/debug.h"
#include "utils/utils.h"
#include "wifi.h"
#include "wifi_opclass.h"



#define AP_COLLECTION_INTERVAL (10 * 1000)

int agent_gen_ap_ht_caps(struct agent *a, struct cmdu_buff *cmdu,
			 struct wifi_radio_element *re)
{
	int ret;
	struct tlv *t;
	struct tlv_ap_ht_cap *data;

	if (!(re->ap_caps.wifi_caps.valid & WIFI_CAP_HT_VALID))
		return -1;

	t = cmdu_reserve_tlv(cmdu, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_HT_CAPABILITIES;
	t->len = sizeof(*data);
	data = (struct tlv_ap_ht_cap *)t->data;

	data->cap = re->ap_caps.ht_cap.cap;
	memcpy(data->radio, re->macaddr, 6);

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_he_caps(struct agent *a, struct cmdu_buff *cmdu,
			 struct wifi_radio_element *re)
{
	int ret;
	int offset = 0;
	int mcs_len;
	struct tlv *t;

	if (!(re->ap_caps.wifi_caps.valid & WIFI_CAP_HE_VALID))
		return -1;

	t = cmdu_reserve_tlv(cmdu, 40);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_HE_CAPABILITIES;

	mcs_len = re->ap_caps.he_cap.mcs_len;

	memcpy(&t->data[offset], re->macaddr, 6);
	offset += 6;

	t->data[offset++] = mcs_len;
	memcpy(&t->data[offset], re->ap_caps.he_cap.mcs, mcs_len);

	offset += mcs_len;
	memcpy(&t->data[offset], &re->ap_caps.he_cap.cap[0], 2);
	t->len = offset + 2;

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

#if (EASYMESH_VERSION > 2)
static void fill_wifi6_role(uint8_t *t_data, int *offset,
		const struct agent_wifi_caps *caps_src,
		uint8_t role_id, uint8_t *num_roles)
{
	struct wifi6_agent_role_other_caps *role_other_caps_data;
	struct wifi6_agent_role *role_data;
	uint8_t mcs_len;

	if (!(caps_src->wifi_caps.valid & WIFI_CAP_HE_VALID))
		return;

	role_data = (struct wifi6_agent_role *)(t_data + *offset);
	role_data->caps = caps_src->wifi6_cap.caps;
	role_data->caps |= role_id << 6;

	*offset += sizeof(role_data->caps);

	mcs_len = caps_src->wifi6_cap.mcs_len;
	if (mcs_len > 0) {
		memcpy(role_data->mcs_nss_12, caps_src->wifi6_cap.mcs, mcs_len);
		*offset += mcs_len;
	}

	role_other_caps_data = (struct wifi6_agent_role_other_caps *)(t_data + *offset);
	role_other_caps_data->beamform_caps = caps_src->wifi6_cap.beamform_caps;
	role_other_caps_data->max_mu_mimo_users = caps_src->wifi6_cap.max_mu_mimo_users;
	role_other_caps_data->max_dl_ofdma_users = caps_src->wifi6_cap.max_dl_ofdma_users;
	role_other_caps_data->max_ul_ofdma_users = caps_src->wifi6_cap.max_ul_ofdma_users;
	role_other_caps_data->other_caps = caps_src->wifi6_cap.other_caps;

	*offset += sizeof(struct wifi6_agent_role_other_caps);
	(*num_roles)++;
}

int agent_gen_ap_wifi6_caps(struct agent *a,
		struct cmdu_buff *cmdu, struct wifi_radio_element *radio)
{
	struct tlv_ap_wifi6_caps *caps_data;
	uint8_t num_roles = 0;
	int offset = 0;
	struct tlv *t;

	if (!(radio->ap_caps.wifi_caps.valid & WIFI_CAP_HE_VALID) &&
			!(radio->sta_caps.wifi_caps.valid & WIFI_CAP_HE_VALID))
		return -1;

	t = cmdu_reserve_tlv(cmdu, 64);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_WIFI6_CAPS;
	caps_data = (struct tlv_ap_wifi6_caps *) t->data;

	memcpy(caps_data->ruid, radio->macaddr, sizeof(caps_data->ruid));
	offset += sizeof(caps_data->ruid);

	caps_data->num_roles = 0;
	offset += sizeof(caps_data->num_roles);

	fill_wifi6_role(t->data, &offset, &radio->ap_caps, AGENT_ROLE_AP, &num_roles);
	fill_wifi6_role(t->data, &offset, &radio->sta_caps, AGENT_ROLE_BH_STA, &num_roles);

	caps_data->num_roles = num_roles;
	t->len = offset;

	if (cmdu_put_tlv(cmdu, t)) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}
#endif /* EASYMESH_VERSION > 2 */

int agent_gen_ap_caps(struct agent *a,
		struct cmdu_buff *cmdu)
{
	int ret;
	struct tlv *t;
	struct tlv_ap_cap *data;

	t = cmdu_reserve_tlv(cmdu, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_CAPABILITY;
	t->len = sizeof(*data);
	data = (struct tlv_ap_cap *)t->data;
	/* Support Agent-initiated RCPI-based Steering */
	data->cap |= AGENT_SUPPORTS_RCPI_STEER;
	/* Support Unassociated STA Link Metrics reporting on
	 * the channels its BSSs are currently operating on.
	 */
	data->cap |= UNASSOC_STA_REPORTING_ONCHAN;
	if (a->cfg.off_channel_monitor)
		data->cap |= UNASSOC_STA_REPORTING_OFFCHAN;

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_radio_basic_cap(struct agent *a,
		struct cmdu_buff *frm, struct wifi_radio_element *re)
{
	struct wifi_radio_opclass *opclass;
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass_channel *channel;
	struct tlv *t;
	uint8_t *data;
	int i, j, offset;

	opclass = &re->opclass;
	offset = 0;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_RADIO_BASIC_CAPABILITIES;

	data = (uint8_t *) t->data;
	memcpy(&data[offset], re->macaddr, 6);
	offset += 6;

	data[offset] = WIFI_IFACE_MAX_NUM;
	offset += 1;

	data[offset] = wifi_opclass_num_supported(opclass); /* k */
	offset += 1;

	for (i = 0; i < opclass->entry_num; i++) {
		entry = &opclass->entry[i];

		/* Skip unsupported opclasses */
		if (!wifi_opclass_id_supported(opclass, entry->id))
			continue;

		data[offset] = entry->id;
		offset += 1;
		data[offset] = entry->max_txpower;
		offset += 1;

		data[offset] = wifi_opclass_id_num_channels_unsupported(opclass, entry->id);	/* m */
		offset += 1;

		for (j = 0; j < entry->channel_num; j++) {
			channel = &entry->channel[j];

			if (wifi_opclass_id_channel_supported(opclass, entry->id, channel->channel))
				continue;

			data[offset] = channel->channel;
			offset += 1;
		}
	}

	t->len = offset;

	if (cmdu_put_tlv(frm, t)) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_vht_caps(struct agent *a, struct cmdu_buff *cmdu,
			  struct wifi_radio_element *re)
{
	int ret;
	struct tlv *t;
	struct tlv_ap_vht_cap *data;

	if (!(re->ap_caps.wifi_caps.valid & WIFI_CAP_VHT_VALID))
		return -1;

	t = cmdu_reserve_tlv(cmdu, 30);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_VHT_CAPABILITIES;
	t->len = sizeof(*data);

	data = (struct tlv_ap_vht_cap *)t->data;

	memcpy(data, &re->ap_caps.vht_cap, sizeof(*data));
	memcpy(data->radio, re->macaddr, 6);

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_profile2_ap_cap(struct agent *a, struct cmdu_buff *frm)
{
	struct tlv *t;
	struct tlv_profile2_ap_cap *data;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_PROFILE2_AP_CAPABILITY;
	t->len = 4;

	data = (struct tlv_profile2_ap_cap *) t->data;
#if (EASYMESH_VERSION > 2)
	data->caps = STATS_UNIT_BYTE;
	data->caps |= PRIORITIZATION_SUPPORTED;
	data->caps |= TRAFFIC_SEPARATION_SUPPORTED;
#ifdef USE_LIBDPP
	data->caps |= DPP_ONBOARDING_SUPPORTED;
#endif /* USE_LIBDPP */
#else
	data->unit = STATS_UNIT_BYTE;
#endif
	data->max_vids = (uint8_t) 255;

	if (cmdu_put_tlv(frm, t)) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_radio_adv_cap(struct agent *a,
		struct cmdu_buff *frm, struct wifi_radio_element *radio)
{
	struct tlv *t;
	struct tlv_ap_radio_adv_cap *data;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_RADIO_ADV_CAPABILITY;
	t->len = 7;

	data = (struct tlv_ap_radio_adv_cap *) t->data;
	memcpy(data->radio, radio->macaddr, 6);
#if 0 /* Today don't support R1 agents in TS network */
	if (a->cfg.map_profile == 0x02)
		data->cap |= RADIO_CAP_COMBINED_P1P2;
#endif
	if (!hwaddr_is_zero(radio->bksta.macaddr))
		data->cap |= RADIO_CAP_COMBINED_FHBK;

#if (EASYMESH_VERSION > 2)
	/* we support DSCP-to-UP mapping */
	data->cap |= RADIO_CAP_DSCP_TO_UP_MAPPING;
#endif

	if (cmdu_put_tlv(frm, t)) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_wsc(struct agent *a, struct cmdu_buff *frm,
		struct wifi_radio_element *radio)
{
	struct tlv *t;
	uint8_t *m1;
	uint16_t m1_size = 0;
	struct wsc_key *key;
	struct wps_credential wps = {0};
	int ret;
	struct agent_config_radio *rcfg;

	rcfg = get_agent_config_radio(&a->cfg, radio->name);
	if (!rcfg) {
		dbg("|%s:%d| radio config not found for %s\n", __func__,
		    __LINE__, radio->name);
		return -1;
	}

	wps.auth_type = rcfg->encryption;
	wps.band = radio->band;
	memcpy(wps.macaddr, a->almac, 6);
	strncpy(wps.manufacturer, radio->manufacturer, 64);
	strncpy(wps.model_name, radio->model_name, 32);
	strncpy(wps.device_name, radio->device_name, 32);
	memcpy(wps.model_number, radio->model_number, 32);
	memcpy(wps.serial_number, radio->serial_number, 32);
	memcpy(wps.uuid, radio->uuid, 16);

	ret = wsc_build_m1(&wps, &m1, &m1_size, (void *)&key);
	if (ret)
		return -1;

	t = cmdu_reserve_tlv(frm, 1024);
	if (!t)
		goto error;

	t->type = TLV_TYPE_WSC;
	t->len = m1_size;
	memcpy(t->data, m1, m1_size);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		goto error;
	}

	agent_free_wsc_data(&radio->autconfig);

	radio->autconfig.m1_frame = m1;
	radio->autconfig.m1_size = m1_size;
	radio->autconfig.key = key;

	dbg("size = %d\n", m1_size);
	return 0;
error:
	if (m1)
		free(m1);
	if (key) {
		if (key->key)
			free(key->key);

		free(key);
	}
	return -1;
}

int agent_gen_ch_scan_cap(struct agent *a, struct cmdu_buff *cmdu)
{
	trace("%s ---->\n", __func__);
	struct wifi_radio_opclass_channel *channel;
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass *opclass;
	struct wifi_radio_element *re;
	int channels_supported;
	struct tlv *t;
	uint8_t cap = 0x00;
	int offset = 0;
	int ret;

	if (a->num_radios <= 0)
		return -1;

	t = cmdu_reserve_tlv(cmdu, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_CHANNEL_SCAN_CAPABILITY;
	t->data[offset++] = a->num_radios;

	/* cap */
	if (a->cfg.scan_on_boot_only)
		cap |= SCAN_CAP_ON_BOOT_ONLY; /* On Boot Only field */
		/* If the "On boot only" bit is set to one,
		 * the Scan Impact field shall be set to 0x00.
		 */
	else
		cap |= SCAN_CAP_IMPACT; /* Scan Impact field */
		/* 0x03: Radio unavailable for >= 2 seconds) */

	list_for_each_entry(re, &a->radiolist, list) {
		int i;

		opclass = &re->opclass;

		memcpy(&t->data[offset], re->macaddr, 6);			/* radio */
		offset += 6;
		t->data[offset++] = cap;				/* cap */
		/* Minimum Scan Interval */
		BUF_PUT_BE32(t->data[offset], MIN_SCAN_ITV_SEC);
		offset += 4;

		t->data[offset++] = wifi_opclass_num_supported_bw20(opclass);	/* m */

		for (i = 0; i < opclass->entry_num; i++) {
			int j;

			entry = &opclass->entry[i];

			if (!wifi_opclass_id_supported(opclass, entry->id)
					|| entry->bandwidth != 20)
				continue;

			t->data[offset++] = entry->id;				/* classid */

			/* All channels supported? */
			if (wifi_opclass_id_all_channels_supported(opclass, entry->id)) {
				t->data[offset++] = 0;				/* j = 0 */
				continue;
			}

			channels_supported = wifi_opclass_id_num_channels_supported(opclass, entry->id);
			t->data[offset++] = channels_supported;
			for (j = 0; j < entry->channel_num; j++) {
				channel = &entry->channel[j];
				if (!wifi_opclass_id_channel_supported(opclass, entry->id, channel->channel))
					continue;
				t->data[offset++] = channel->channel;	/* j */
			}
		}
	}

	/* update the tlv length */
	t->len = offset;

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

static inline void put_duration_data(uint8_t *buf, uint32_t val)
{
	/* ignore val MSB data */
	buf[0] = (val >> 16) & 0x0ff;
	buf[1] = (val >> 8) & 0xff;
	buf[2] = val & 0xff;
}

static int agent_num_dfs_radios(struct agent *a)
{
	struct wifi_radio_element *re = NULL;
	int num = 0;

	list_for_each_entry(re, &a->radiolist, list) {
		if (wifi_opclass_dfs_supported(&re->opclass))
			num++;
	}

	return num;
}

int agent_gen_cac_cap(struct agent *a, struct cmdu_buff *cmdu)
{
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass *opclass;
	struct wifi_radio_element *re = NULL;
	struct tlv_cac_cap *data;
	int offset = 0;
	struct tlv *t;
	int ret;
	int cac;

	t = cmdu_reserve_tlv(cmdu, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_CAC_CAPABILITY;
	data = (struct tlv_cac_cap *)t->data;

	list_for_each_entry(re, &a->radiolist, list) {
		memcpy(data->country, re->country_code, 2);
		break;
	}

	data->num_radio = agent_num_dfs_radios(a);
	offset = sizeof(*data);
	list_for_each_entry(re, &a->radiolist, list) {
		struct cac_cap_radio *r = (struct cac_cap_radio *)&t->data[offset];
		uint32_t cac_time[] = {60, 600}; /* Today we have 60/600 seconds */
		int i;

		opclass = &re->opclass;

		if (!wifi_opclass_dfs_supported(opclass))
			continue;

		memcpy(r->radio, re->macaddr, 6);
		r->num_cac = 0;
		offset += sizeof(*r);

		for (i = 0; i < 4; i++) {
			struct cac_cap_cac *c =
				(struct cac_cap_cac *)&t->data[offset];
			int j;

			if ((re->cac_methods & (1U << i)) == 0)
				continue;

			for (cac = 0; cac < ARRAY_SIZE(cac_time); cac++) {
				int num_opclass = 0;

				c = (struct cac_cap_cac *)&t->data[offset];
				num_opclass = 0;

				switch (i) {
				case WIFI_CAC_CONTINUOUS:
					c->supp_method = CAC_METHOD_CONTINUOUS_CAC;
					break;
				case WIFI_CAC_DEDICATED:
					c->supp_method = CAC_METHOD_DEDICATED_RADIO;
					break;
				case WIFI_CAC_MIMO_REDUCED:
					c->supp_method = CAC_METHOD_MIMO_DIM_REDUCED;
					break;
				case WIFI_CAC_TIME_SLICED:
					c->supp_method = CAC_METHOD_TIME_SLICED;
					break;
				default:
					break;
				}

				/* TODO/revisit for putting data in duration buffer, 3 byte */
				put_duration_data(c->duration, cac_time[cac]);
				c->num_opclass = 0;
				offset += sizeof(*c);

				for (j = 0; j < opclass->entry_num; j++) {
					struct cac_cap_opclass *op = (struct cac_cap_opclass *)&t->data[offset];
					int num = 0;
					int k;

					entry = &opclass->entry[j];

					if (!wifi_opclass_id_dfs_supported(opclass, entry->id))
						continue;

					op->classid = entry->id;
					op->num_channel = 0;

					for (k = 0; k < entry->channel_num; k++) {
						if (!wifi_opclass_is_channel_supported(&entry->channel[k]))
							continue;
						if (!wifi_opclass_is_dfs_channel(&entry->channel[k]))
							continue;
						if (entry->channel[k].cac_time != cac_time[cac])
							continue;

						op->channel[num] = entry->channel[k].channel;
						num++;
					}

					if (!num)
						continue;

					op->num_channel = num;
					offset += sizeof(*op) + op->num_channel;

					num_opclass++;
				}

				c->num_opclass = num_opclass;
				r->num_cac++;
			}
		}
	}

	/* update the tlv length */
	t->len = offset;

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

#define AP_COLLECTION_INTERVAL	(10 * 1000)
int agent_gen_metric_collection_interval(struct agent *a, struct cmdu_buff *cmdu)
{
	struct tlv_metric_collection_int *data;
	struct tlv *t;
	int ret;

	t = cmdu_reserve_tlv(cmdu, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_METRIC_COLLECTION_INTERVAL;
	t->len = sizeof(*data);
	data = (struct tlv_metric_collection_int *)t->data;
	BUF_PUT_BE32(data->interval, AP_COLLECTION_INTERVAL);

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_oper_channel_report(struct agent *a,
	struct cmdu_buff *frm, struct wifi_radio_element *re,
	uint32_t channel, uint32_t bw, uint32_t opclass)
{
	uint32_t bws[] = { 20, 40, 80, 160, 320};
	uint8_t *opc = NULL;
	int ret, offset = 0;
	int num_opclass = 0;
	uint8_t txpower = 0;
	struct tlv *t;
	int i;

	t = cmdu_reserve_tlv(frm, 100);
	if (!t)
		return -1;

	t->type = MAP_TLV_OPERATING_CHANNEL_REPORT;
	memcpy(&t->data[offset], re->macaddr, 6);
	offset += 6;
	opc = &t->data[offset];
	t->data[offset++] = 0;	/* num opclass */

	for (i = 0; i < ARRAY_SIZE(bws); i++) {
		uint8_t op_class;
		uint8_t chan;

		if ( bws[i] > bw)
			break;

		op_class = wifi_opclass_find_id_chan_from_channel(&re->opclass,
								  channel,
								  bws[i],
								  &chan);
		if (op_class == 0)
			continue;

		t->data[offset++] = op_class;
		t->data[offset++] = chan;

		num_opclass++;
	}
	*opc = num_opclass;

	/* current transmit power is the (operating class tx power) *
	 *	(current_tx_power_percent)/100
	 */
	for (i = 0; i < re->opclass.entry_num; i++) {
		if (re->opclass.entry[i].id == re->current_opclass) {
			txpower = (re->opclass.entry[i].max_txpower *
				   re->current_txpower_percent) / 100;
			break;
		}
	}

	t->data[offset++] = txpower;

	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_channel_selection_resp(struct agent *a, struct cmdu_buff *frm,
		uint8_t *radio_recvd, uint8_t reason_code)
{
	struct tlv_channel_selection_resp *data;
	struct tlv *t;
	int ret;

	t = cmdu_reserve_tlv(frm, 30);
	if (!t)
		return -1;

	t->type = MAP_TLV_CHANNEL_SELECTION_RESPONSE;
	t->len = sizeof(*data);
	data = (struct tlv_channel_selection_resp *)t->data;

	memcpy(data->radio, radio_recvd, 6);
	data->response = reason_code;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_channel_pref(struct agent *a, struct cmdu_buff *frm,
			   struct wifi_radio_element *re)
{
	struct wifi_radio_opclass_channel *channel;
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass *opclass;
	int opclass_num_offset;
	int ret, offset = 0;
	int opclass_num;
	struct tlv *t;
	int i, j;

	/* Get opclass preferences from lower layer */
	WARN_ON(wifi_radio_update_opclass_preferences(a, re, false));

	opclass = &re->opclass;
	opclass_num = 0;

	t = cmdu_reserve_tlv(frm, 2048);
	if (!t)
		return -1;

	t->type = MAP_TLV_CHANNEL_PREFERENCE;
	memcpy(&t->data[offset], re->macaddr, 6);				/* radio id */
	offset += 6;

	opclass_num_offset = offset;
	t->data[offset++] = 0;							/* m */

	for (i = 0; i < opclass->entry_num; i++) {
		entry = &opclass->entry[i];
		uint8_t preference;

		if (wifi_opclass_id_same_preference(&re->opclass, entry->id, &preference)) {
			t->data[offset++] = entry->id;
			t->data[offset++] = 0;					/* k */
			t->data[offset++] = preference;
			opclass_num++;
			continue;
		}

		for (j = 0; j < entry->channel_num; j++) {
			channel = &entry->channel[j];

			/* skip max pref for non-DFS channels */
			if (wifi_opclass_max_preference(channel->preference) &&
			    channel->dfs == WIFI_RADIO_OPCLASS_CHANNEL_DFS_NONE)
				continue;

			t->data[offset++] = entry->id;
			t->data[offset++] = 1;					/* k */
			t->data[offset++] = channel->channel;
			t->data[offset++] = channel->preference;

			opclass_num++;
		}
	}

	t->data[opclass_num_offset] = opclass_num;				/* m */

	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_radio_oper_restrict(struct agent *a, struct cmdu_buff *frm,
				  struct wifi_radio_element *re)
{
	struct wifi_radio_opclass_channel *channel;
	int ret, offset = 0, opclass_num_offset;
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass *opclass;
	int opclass_num;
	struct tlv *t;
	int i, j;

	opclass = &re->opclass;
	opclass_num = 0;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_RADIO_OPERATION_RESTRICTION;

	memcpy(&t->data[offset], re->macaddr, 6);	/* radio id */
	offset += 6;
	opclass_num_offset = offset;
	t->data[offset++] = 0;				/* m */

	for (i = 0; i < opclass->entry_num; i++) {
		entry = &opclass->entry[i];

		if (wifi_opclass_id_all_channels_supported(opclass, entry->id))
			continue;

		t->data[offset++] = entry->id;
		t->data[offset++] = wifi_opclass_id_num_channels_unsupported(opclass, entry->id);	/* k */

		for (j = 0; j < entry->channel_num; j++) {
			channel = &entry->channel[j];

			if (wifi_opclass_id_channel_supported(opclass, entry->id, channel->channel))
				continue;

			t->data[offset++] = channel->channel;
			t->data[offset++] = 0;	/* Freq separation */
		}

		opclass_num++;
	}

	t->data[opclass_num_offset] = opclass_num;
	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_cac_complete_report(struct agent *a, struct cmdu_buff *frm, uint8_t *radioid)
{
	struct wifi_radio_element *re = NULL;
	uint8_t *num_radios_ptr;
	uint8_t num_radios = 0;
	int ret, offset = 0;
	struct tlv *t;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_CAC_COMPLETION_REPORT;

	num_radios_ptr = &t->data[offset++];
	*num_radios_ptr = 0;

	list_for_each_entry(re, &a->radiolist, list) {
		struct wifi_radio_opclass_channel *channel;

		if (radioid && memcmp(re->macaddr, radioid, 6))
			continue;

		/* Check if cac requested */
		if (!re->cac_request.channel &&
		    !re->cac_request.report_failed)
			continue;

		/* Get channel with dfs_state */
		channel = wifi_opclass_get_channel(&re->opclass, re->cac_request.opclass, re->cac_request.channel);

		/* Now check opclass/channel state */
		if (re->cac_request.report_failed) {
			memcpy(&t->data[offset], re->macaddr, 6);	/* radio id */
			offset += 6;
			t->data[offset++] = re->cac_request.report_failed_opclass;
			t->data[offset++] = re->cac_request.report_failed_channel;

			t->data[offset++] = re->cac_request.report_failed_status;
			t->data[offset++] = 0;

			/* We are done with this request */
			re->cac_request.report_failed_channel = 0;
			re->cac_request.report_failed_opclass = 0;
			re->cac_request.report_failed_status = 0;
			re->cac_request.report_failed = false;
			num_radios++;

			if (re->cac_request.report_failed_status == CAC_COMP_REPORT_STATUS_TOO_BUSY &&
			    timestamp_elapsed_sec(&re->cac_request.time) > 20 &&
			    channel &&
			    channel->dfs != WIFI_RADIO_OPCLASS_CHANNEL_DFS_CAC) {
				/* CAC not started/aborted */
				memset(&re->cac_request, 0, sizeof(re->cac_request));
			}

			continue;
		}

		if (WARN_ON(!channel)) {
			memcpy(&t->data[offset], re->macaddr, 6);	/* radio id */
			offset += 6;
			t->data[offset++] = re->cac_request.opclass;
			t->data[offset++] = re->cac_request.channel;

			t->data[offset++] = CAC_COMP_REPORT_STATUS_NON_CONFORMANT;
			t->data[offset++] = 0;

			/* We are done with this request */
			memset(&re->cac_request, 0, sizeof(re->cac_request));
			num_radios++;
			continue;
		}

		/* Check current dfs state */
		switch (channel->dfs) {
		case WIFI_RADIO_OPCLASS_CHANNEL_DFS_NONE:
			WARN_ON(1);
			break;
		/* Seems someone abort this request */
		case WIFI_RADIO_OPCLASS_CHANNEL_DFS_USABLE:
			/* Give some time driver could start this request */
			if (timestamp_elapsed_sec(&re->cac_request.time) > 3) {
				memcpy(&t->data[offset], re->macaddr, 6);	/* radio id */
				offset += 6;
				t->data[offset++] = re->cac_request.opclass;
				t->data[offset++] = re->cac_request.channel;

				t->data[offset++] = CAC_COMP_REPORT_STATUS_OTHER;
				t->data[offset++] = 0;

				/* We are done with this request */
				memset(&re->cac_request, 0, sizeof(re->cac_request));
				num_radios++;
			}
			break;
		/* Nothing to report */
		case WIFI_RADIO_OPCLASS_CHANNEL_DFS_CAC:
			timer_set(&re->preference_report_timer, (channel->cac_time + 10) * 1000);
			break;
		case WIFI_RADIO_OPCLASS_CHANNEL_DFS_AVAILABLE:
			memcpy(&t->data[offset], re->macaddr, 6);	/* radio id */
			offset += 6;
			t->data[offset++] = re->cac_request.opclass;
			t->data[offset++] = re->cac_request.channel;

			t->data[offset++] = CAC_COMP_REPORT_STATUS_SUCCESSFUL;
			t->data[offset++] = 0;

			/* We are done with this request */
			memset(&re->cac_request, 0, sizeof(re->cac_request));
			num_radios++;
			break;
		case WIFI_RADIO_OPCLASS_CHANNEL_DFS_NOP:
			memcpy(&t->data[offset], re->macaddr, 6);	/* radio id */
			offset += 6;
			t->data[offset++] = re->cac_request.opclass;
			t->data[offset++] = re->cac_request.channel;

			t->data[offset++] = CAC_COMP_REPORT_STATUS_RADAR_DETECTED;

			/* TODO should we report also subclasses? */
			t->data[offset++] = 1;
			t->data[offset++] = re->cac_request.opclass;
			t->data[offset++] = re->cac_request.channel;

			/* We are done with this request */
			memset(&re->cac_request, 0, sizeof(re->cac_request));
			num_radios++;
			break;
		default:
			break;
		}
	}

	*num_radios_ptr = num_radios;
	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_cac_status_report(struct agent *a, struct cmdu_buff *frm, uint8_t *radioid)
{
	struct wifi_radio_opclass_channel *chan;
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass *opclass;
	struct wifi_radio_element *re = NULL;

	int ret, offset = 0;
	struct tlv *t;
	uint8_t *num_ptr;
	uint8_t num;
	uint32_t time;
	int i, j;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_CAC_STATUS_REPORT;

	/* CAC available */
	num = 0;
	num_ptr = &t->data[offset];
	t->data[offset++] = num;

	list_for_each_entry(re, &a->radiolist, list) {
		opclass = &re->opclass;

		/* Report all available channels also non-DFS */
		//if (!wifi_opclass_dfs_supported(opclass))
		//	continue;

		if (radioid && memcmp(re->macaddr, radioid, 6))
			continue;

		for (i = 0; i < opclass->entry_num; i++) {
			entry = &opclass->entry[i];

			for (j = 0; j < entry->channel_num; j++) {
				chan = &entry->channel[j];

				if (!wifi_opclass_is_channel_supported(chan))
					continue;
				if (wifi_opclass_is_dfs_channel(chan) && !wifi_opclass_is_channel_dfs_available(chan))
					continue;

				t->data[offset++] = entry->id;
				t->data[offset++] = chan->channel;

				if (wifi_opclass_is_dfs_channel(chan))
					time = 60;
				else
					time = 0;
				BUF_PUT_BE16(t->data[offset], time);
				offset += 2;
				num++;
			}
		}
	}
	*num_ptr = num;

	/* NOP */
	num = 0;
	num_ptr = &t->data[offset];
	t->data[offset++] = num;

	re = NULL;
	list_for_each_entry(re, &a->radiolist, list) {
		opclass = &re->opclass;

		if (radioid && memcmp(re->macaddr, radioid, 6))
			continue;

		if (!wifi_opclass_dfs_supported(opclass))
			continue;

		for (i = 0; i < opclass->entry_num; i++) {
			entry = &opclass->entry[i];

			for (j = 0; j < entry->channel_num; j++) {
				chan = &entry->channel[j];

				if (!wifi_opclass_is_channel_supported(chan))
					continue;
				if (!wifi_opclass_is_dfs_channel(chan))
					continue;
				if (!wifi_opclass_is_channel_dfs_nop(chan))
					continue;

				t->data[offset++] = entry->id;
				t->data[offset++] = chan->channel;

				time = wifi_opclass_channel_dfs_nop_time(chan);
				BUF_PUT_BE16(t->data[offset], time);
				offset += 2;
				num++;
			}
		}
	}
	*num_ptr = num;

	/* CAC ongoing */
	num = 0;
	num_ptr = &t->data[offset];
	t->data[offset++] = 0;

	re = NULL;
	list_for_each_entry(re, &a->radiolist, list) {
		opclass = &re->opclass;

		if (radioid && memcmp(re->macaddr, radioid, 6))
			continue;

		if (!wifi_opclass_dfs_supported(opclass))
			continue;

		for (i = 0; i < opclass->entry_num; i++) {
			entry = &opclass->entry[i];

			for (j = 0; j < entry->channel_num; j++) {
				chan = &entry->channel[j];

				if (!wifi_opclass_is_channel_supported(chan))
					continue;
				if (!wifi_opclass_is_dfs_channel(chan))
					continue;
				if (!wifi_opclass_is_channel_dfs_cac(chan))
					continue;

				t->data[offset++] = entry->id;
				t->data[offset++] = chan->channel;

				time = wifi_opclass_channel_dfs_cac_time(chan);

				BUF_PUT_BE24(t->data[offset], time);
				offset += 3;
				num++;
			}
		}
	}
	*num_ptr = num;

	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tlv_error_code(struct agent *a, struct cmdu_buff *frm,
			     uint8_t *macaddr, uint8_t reason_code)
{
	struct tlv *t;
	struct tlv_error_code *data;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_ERROR_CODE;
	t->len = 7;

	data = (struct tlv_error_code *)t->data;
	data->reason = reason_code;

	if (macaddr)
		memcpy(data->macaddr, macaddr, 6);

	if (cmdu_put_tlv(frm, t)) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_mac(struct agent *a, struct cmdu_buff *frm, uint8_t *macaddr)
{
	struct tlv *t;
	struct tlv_macaddr *data;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = TLV_TYPE_MAC_ADDRESS_TYPE;
	t->len = 6;

	data = (struct tlv_macaddr *) t->data;
	memcpy(data->macaddr, macaddr, 6);

	if (cmdu_put_tlv(frm, t)) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_al_mac(struct agent *a, struct cmdu_buff *frm, uint8_t *macaddr)
{
	struct tlv *t;
	struct tlv_aladdr *data;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = TLV_TYPE_AL_MAC_ADDRESS_TYPE;
	t->len = 6;

	data = (struct tlv_aladdr *) t->data;
	memcpy(data->macaddr, macaddr, 6);

	if (cmdu_put_tlv(frm, t)) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}


int agent_gen_supported_service(struct agent *a, struct cmdu_buff *frm)
{
	struct tlv *t;
	int ret;
	int num = 0;
	struct tlv_supported_service *t_supp_serv;

	t = cmdu_reserve_tlv(frm, 128);
	if (!t)
		return -1;

	t->type = MAP_TLV_SUPPORTED_SERVICE;
	t_supp_serv = (struct tlv_supported_service *) t->data;

	if (!memcmp(a->almac, a->cntlr_almac, 6))
		t_supp_serv->services[num++] = SUPPORTED_SERVICE_MULTIAP_CONTROLLER;

	t_supp_serv->services[num++] = SUPPORTED_SERVICE_MULTIAP_AGENT;
	t_supp_serv->num_services = num;
	t->len = num + 1;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_searched_service(struct agent *a, struct cmdu_buff *frm, uint8_t service)
{
	struct tlv *t;
	int ret;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_SEARCHED_SERVICE;
	t->len = 2;
	t->data[0] = 0x1;
	t->data[1] = service;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_map_profile(struct agent *a, struct cmdu_buff *frm, uint8_t profile)
{
	struct tlv *t;
	struct tlv_map_profile *data;
	int ret;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_MULTIAP_PROFILE;
	t->len = 1;
	data = (struct tlv_map_profile *) t->data;
	data->profile = profile;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tlv_beacon_metrics_query(struct agent *a,
		struct cmdu_buff *frm, uint8_t *sta_addr,
		uint8_t opclass, uint8_t channel,
		uint8_t *bssid, uint8_t reporting_detail, char *ssid,
		uint8_t num_report, struct sta_channel_report *report,
		uint8_t num_element, const uint8_t *element)
{
	struct tlv *t;
	struct tlv_beacon_metrics_query *data;
	uint8_t *data_p;
	struct ssid_query *ssidq;
	size_t ssid_len = strlen(ssid);
	int i, ret;

	/* TODO: check size */
	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_BEACON_METRICS_QUERY;
	/* It will be increased later for variable params */
	t->len = sizeof(struct tlv_beacon_metrics_query);

	/* Note: this cast holds only till 'reporting_detail' field */
	data = (struct tlv_beacon_metrics_query *) t->data;

	memcpy(data->sta_macaddr, sta_addr, 6);
	data->opclass = opclass;
	data->channel = channel;
	memcpy(data->bssid, bssid, 6);
	data->reporting_detail = reporting_detail;

	/* Flexible array in the middle of the struct - cast to ssid_query */
	ssidq = (struct ssid_query *) &data->ssid;
	ssidq->ssidlen = ssid_len;
	memcpy(ssidq->ssid, ssid, ssid_len);

	t->len += ssid_len;

	/* No more direct use of tlv_beacon_metrics_query structure layout
	 * from here on: data->num_report doesn't point to num_report anymore!
	 * From now on just use the data pointer to pack the data manually.
	 */
	data_p = &(ssidq->ssidlen) + 1 + ssid_len;

	/* Channel reports */
	if (num_report && report) {
		/* data->num_report */
		*data_p = num_report;
		data_p++;

		/* data->report */
		/* -1: one report always counted for in sizeof query struct */
		t->len += (num_report - 1) * sizeof(struct ap_channel_report);

		for (i = 0; i < num_report; i++) {
			struct ap_channel_report *ch_rep =
					(struct ap_channel_report *) data_p;
			int num_channel = report[i].num_channel;

			ch_rep->opclass = report[i].opclass;
			/* opclass + channel[] */
			ch_rep->len = 1 + num_channel;
			memcpy(ch_rep->channel, report[i].channel, num_channel);

			/* Increase t->len by number of channels */
			t->len += num_channel;
			/* (len + opclass) + channel[] */
			data_p += 2 + num_channel;
		}
	}

	/* Request elements */
	if (reporting_detail == 1 && num_element) {
		/* data->num_element */
		*data_p = num_element;
		/* num_element already counted for in len */
		data_p++;

		if (element) {
			/* data->element */
			t->len += num_element;
			for (i = 0; i < num_element; i++) {
				*data_p = element[i];
				data_p++;
			}
		}
	}

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tlv_unassoc_sta_lm_query(struct agent *a,
		struct cmdu_buff *frm, uint8_t opclass,
		uint8_t num_metrics, struct unassoc_sta_metric *metrics)
{
	int ret, i, j;
	struct tlv *t;
	struct tlv_unassoc_sta_link_metrics_query *data;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_UNASSOCIATED_STA_LINK_METRICS_QUERY;
	t->len = sizeof(struct tlv_unassoc_sta_link_metrics_query);

	data = (struct tlv_unassoc_sta_link_metrics_query *) t->data;
	data->opclass = opclass;
	data->num_channel = num_metrics;

	for (i = 0; i < num_metrics; i++) {
		int num_sta;

		t->len += 2; /* two bytes: channel & num_sta */

		data->ch[i].channel = metrics[i].channel;
		num_sta = metrics[i].num_sta;

		if (num_sta > MAX_UNASSOC_STAMACS) {
			dbg("%s: error: num_sta (%d) greater than %d\n",
				__func__, num_sta, MAX_UNASSOC_STAMACS);
			num_sta = MAX_UNASSOC_STAMACS;
		}

		t->len += (num_sta * 6); /* six bytes: macaddr */

		data->ch[i].num_sta = num_sta;
		for (j = 0; j < num_sta; j++)
			memcpy(data->ch[i].sta[j].macaddr,
			       metrics[i].sta[j].macaddr, 6);
	}

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tlv_unassoc_sta_lm_report(struct agent *a,
		struct cmdu_buff *frm, int num_metrics,
		struct wifi_unassoc_sta_element *metrics, uint8_t opclass)
{
	int ret;
	int offset = 0;
	struct tlv *t;
	struct tlv_unassoc_sta_link_metrics_resp *data;
	struct timespec now;
	int i;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_UNASSOCIATED_STA_LINK_METRICS_RESPONSE;
	data = (struct tlv_unassoc_sta_link_metrics_resp *)t->data;
	data->opclass = opclass;
	data->num_sta = 0;

	offset = sizeof(*data);

	timestamp_update(&now);

	for (i = 0; i < num_metrics; i++) {

		if (metrics[i].meas.opclass == opclass &&
				metrics[i].meas.rssi &&
				metrics[i].meas.rcpi != 255) {

			struct unassoc_sta_link_metrics_sta *s =
				(struct unassoc_sta_link_metrics_sta *)&t->data[offset];

			memcpy(s->macaddr, metrics[i].macaddr, 6);
			s->channel = metrics[i].meas.channel;
			BUF_PUT_BE32(s->time_delta, metrics[i].meas.last_seen_ms);
			s->ul_rcpi = metrics[i].meas.rcpi; // reporting average RCPI

			offset += sizeof(*s);
			data->num_sta++;
		}
	}

	t->len = offset;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tlv_beacon_metrics_resp(struct agent *a,
		uint8_t *tlv, uint8_t *sta_addr,
		uint8_t report_elems_nr, uint8_t *report_elem,
		uint16_t elem_len)
{
	struct tlv_beacon_metrics_resp *data;
	struct tlv *t = (struct tlv *) tlv;
	uint16_t len = sizeof(struct tlv_beacon_metrics_resp) + elem_len;

	if (!t)
		return -1;

	t->type = MAP_TLV_BEACON_METRICS_RESPONSE;
	buf_put_be16((uint8_t *)&t->len, len); /* swap */
	data = (struct tlv_beacon_metrics_resp *) t->data;

	memcpy(data->sta_macaddr, sta_addr, 6);
	//data->reserved = 0;
	data->num_element = report_elems_nr;

	if (report_elems_nr) {
		memcpy(data->element, report_elem, elem_len);
	}

	return 0;
}

int agent_gen_steer_btm_report(struct agent *a, struct cmdu_buff *frm,
		uint8_t *target_bssid, uint8_t *src_bssid,
		uint8_t *sta, uint8_t status_code)
{
	int ret;
	struct tlv *t;
	struct tlv_steer_btm_report *data;

	t = cmdu_reserve_tlv(frm, 40);
	if (!t)
		return -1;

	t->type = MAP_TLV_STEERING_BTM_REPORT;
	t->len = sizeof(*data);
	data = (struct tlv_steer_btm_report *)t->data;

	memcpy(data->bssid, src_bssid, 6);
	memcpy(data->sta_macaddr, sta, 6);
	data->status = status_code;

	if (status_code == 0x00) {
		memcpy(data->target_bssid, target_bssid, 6);
		t->len += 6;
	}

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

/**
 * band -
 *	0x00 2.4GHz
 *	0x01 5GHz
 *	0x02 60GHz
 */
int agent_gen_autoconf_freq_band(struct agent *a, struct cmdu_buff *frm,
		uint8_t band)
{
	struct tlv *t;
	struct tlv_autoconfig_band *data;
	int ret;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = TLV_TYPE_AUTOCONFIG_FREQ_BAND;
	t->len = 1;
	data = (struct tlv_autoconfig_band *) t->data;
	data->band = band;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_searched_role(struct agent *a, struct cmdu_buff *frm,
		uint8_t role)
{
	struct tlv *t;
	struct tlv_searched_role *data;
	int ret;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = TLV_TYPE_SEARCHED_ROLE;
	t->len = 1;
	data = (struct tlv_searched_role *) t->data;
	data->role = role;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_radio_metrics(struct agent *a, struct cmdu_buff *frm,
		struct wifi_radio_element *re)
{
	struct tlv_radio_metrics *data;
	struct tlv *t;
	int total = 0;
	int ret;

	t = cmdu_reserve_tlv(frm, 30);
	if (!t)
		return -1;

	t->type = MAP_TLV_RADIO_METRICS;
	t->len = sizeof(*data);
	data = (struct tlv_radio_metrics *) t->data;

	memcpy(data->radio, re->macaddr, 6);
	data->noise = re->anpi;
	data->transmit = re->tx_utilization;
	data->receive_self = re->rx_utilization;
	data->receive_other = re->other_utilization;

	total = data->transmit + data->receive_self +
		data->receive_other;

	/* total airtime should not exceed 255 */
	if (total > 255) {
		float r = (float) 255 / (float) total;

		data->transmit *= r;
		data->receive_self *= r;
		data->receive_other *= r;
	}

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_metrics(struct agent *a, struct cmdu_buff *frm,
			 struct wifi_radio_element *re,
			 struct wifi_bss_element *bss)
{
	int ret;
	int copy_index = 0;
	struct tlv *t;
	struct tlv_ap_metrics *data;
	struct netif_ap *ap;
	struct sta *s = NULL;
	uint16_t num_sta = 0;

	t = cmdu_reserve_tlv(frm, 64);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_METRICS;
	t->len = sizeof(*data);
	data = (struct tlv_ap_metrics *) t->data;

	memcpy(data->bssid, bss->bssid, 6);
	data->channel_utilization = re->total_utilization;
	ap = agent_get_ap(a, bss->bssid);
	if (!ap)
		return -1;

	list_for_each_entry(s, &ap->stalist, list)
		num_sta++;

	BUF_PUT_BE16(data->num_station, num_sta);
	data->esp_ac = ESP_AC_BE;
	memcpy(data->esp_be, bss->est_wmm_be, 3);

	if (bss->is_ac_bk) {
		data->esp_ac |= ESP_AC_BK;
		memcpy(data->esp + copy_index, bss->est_wmm_bk, 3);
		copy_index += 3;
	}

	if (bss->is_ac_vo) {
		data->esp_ac |= ESP_AC_VO;
		memcpy(data->esp + copy_index, bss->est_wmm_vo, 3);
		copy_index += 3;
	}

	if (bss->is_ac_vi) {
		data->esp_ac |= ESP_AC_VI;
		memcpy(data->esp + copy_index, bss->est_wmm_vi, 3);
		copy_index += 3;
	}

	t->len += copy_index;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_ext_metrics(struct agent *a, struct cmdu_buff *frm,
			     struct wifi_bss_element *bss)
{
	int ret;
	struct tlv *t;
	struct tlv_ap_ext_metrics *data;

	t = cmdu_reserve_tlv(frm, 64);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_EXTENDED_METRICS;
	t->len = sizeof(*data);
	data = (struct tlv_ap_ext_metrics *) t->data;

	memcpy(data->bssid, bss->bssid, 6);
	BUF_PUT_BE32(data->tx_bytes_ucast, bss->tx_ucast_bytes);
	BUF_PUT_BE32(data->rx_bytes_ucast, bss->rx_ucast_bytes);
	BUF_PUT_BE32(data->tx_bytes_mcast, bss->tx_mcast_bytes);
	BUF_PUT_BE32(data->rx_bytes_mcast, bss->rx_mcast_bytes);
	BUF_PUT_BE32(data->tx_bytes_bcast, bss->tx_bcast_bytes);
	BUF_PUT_BE32(data->rx_bytes_bcast, bss->rx_bcast_bytes);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_assoc_sta_traffic_stats(struct agent *a,
		struct cmdu_buff *frm, uint8_t *mac, struct sta *s)
{
	int ret;
	struct tlv *t;
	struct tlv_assoc_sta_traffic_stats *data;

	t = cmdu_reserve_tlv(frm, 64);
	if (!t)
		return -1;

	t->type = MAP_TLV_ASSOCIATED_STA_TRAFFIC_STATS;
	t->len = sizeof(*data);
	data = (struct tlv_assoc_sta_traffic_stats *) t->data;

	memcpy(data->macaddr, mac, 6);

	if (s) {
		BUF_PUT_BE32(data->tx_bytes, s->tx_bytes);
		BUF_PUT_BE32(data->rx_bytes, s->rx_bytes);
		BUF_PUT_BE32(data->tx_packets, s->tx_pkts);
		BUF_PUT_BE32(data->rx_packets, s->rx_pkts);
		BUF_PUT_BE32(data->tx_err_packets, s->tx_fail_pkts);
		BUF_PUT_BE32(data->rx_err_packets, s->rx_fail_pkts);
		BUF_PUT_BE32(data->rtx_packets, s->rtx_pkts);
	} else {
		memset(data + 6, 0, 58);
	}

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_assoc_sta_link_metrics(struct agent *a,
		struct cmdu_buff *frm, struct sta *s, uint8_t *bssid)
{
	int ret, i;
	int offset = 0;
	struct tlv *t;
	struct tlv_assoc_sta_link_metrics *data;
	struct timespec curr_ts;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	timestamp_update(&curr_ts);
	t->type = MAP_TLV_ASSOCIATED_STA_LINK_METRICS;
	data = (struct tlv_assoc_sta_link_metrics *) t->data;
	memcpy(data->macaddr, s->macaddr, 6);
	data->num_bss = 1;
	offset = sizeof(*data);

	for (i = 0; i < data->num_bss; i++) {
		struct assoc_sta_link_metrics_bss *b =
			(struct assoc_sta_link_metrics_bss *)&t->data[offset];
		uint64_t mb_tx_thput, mb_rx_thput;

		mb_tx_thput = ((s->tx_thput << 3) / (1000 * 1000));
		mb_rx_thput = ((s->rx_thput << 3) / (1000 * 1000));

		memcpy(b->bssid, bssid, 6);
		BUF_PUT_BE32(b->time_delta,
				timestamp_diff_ms(curr_ts, s->last_update));

		/*
		 * TODO kill mb_tx_thput/mb_rx_thput after libwifi report it
		 * correctly for all drivers
		 */
		if (s->est_rx_thput)
			BUF_PUT_BE32(b->dl_thput, s->est_rx_thput);
		else
			BUF_PUT_BE32(b->dl_thput, mb_rx_thput);

		if (s->est_tx_thput)
			BUF_PUT_BE32(b->ul_thput, s->est_tx_thput);
		else
			BUF_PUT_BE32(b->ul_thput, mb_tx_thput);

		b->ul_rcpi = rssi_to_rcpi(s->rssi_avg[0]); // report last average RCPI
		offset += sizeof(*b);
	}

	t->len = offset;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_assoc_sta_ext_link_metric(struct agent *a,
		struct cmdu_buff *frm, struct sta *s, uint8_t *bssid)
{
	int ret, i;
	int offset = 0;
	struct tlv *t;
	struct tlv_sta_ext_link_metric *data;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_ASSOCIATED_STA_EXT_LINK_METRICS;
	data = (struct tlv_sta_ext_link_metric *) t->data;
	memcpy(data->macaddr, s->macaddr, 6);
	data->num_bss = 1;
	offset = sizeof(*data);

	for (i = 0; i < data->num_bss; i++) {
		struct sta_ext_link_metric_bss *b =
			(struct sta_ext_link_metric_bss *)&t->data[offset];

		memcpy(b->bssid, bssid, 6);
		BUF_PUT_BE32(b->dl_rate, s->tx_rate);
		BUF_PUT_BE32(b->ul_rate, s->rx_rate);
		BUF_PUT_BE32(b->rx_util, s->rx_airtime);
		BUF_PUT_BE32(b->tx_util, s->tx_airtime);
		offset += sizeof(*b);
	}

	t->len = offset;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_radio_identifier(struct agent *a,
		struct cmdu_buff *frm, uint8_t *radio_id)
{
	int ret;
	struct tlv *t;
	struct tlv_ap_radio_identifier *data;

	t = cmdu_reserve_tlv(frm, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_RADIO_IDENTIFIER;
	t->len = sizeof(*data);
	data = (struct tlv_ap_radio_identifier *) t->data;
	memcpy(data->radio, radio_id, 6);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_metric_query(struct agent *a, struct cmdu_buff *frm,
		int num_bss, uint8_t *bsslist)
{
	int i, ret;
	struct tlv *t;
	struct tlv_ap_metric_query *data;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_METRIC_QUERY;
	t->len = sizeof(*data) + (6 * num_bss);
	data = (struct tlv_ap_metric_query *) t->data;

	data->num_bss = num_bss;
	for (i = 0; i < data->num_bss; i++)
		memcpy(data->bss[i].bssid, &bsslist[i * 6], 6);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_source_info(struct agent *a,
		struct cmdu_buff *frm, uint8_t *mac)
{
	int ret;
	struct tlv *t;
	struct tlv_source_info *data;

	if (!mac)
		return -1;

	t = cmdu_reserve_tlv(frm, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_SOURCE_INFO;
	t->len = sizeof(*data);
	data = (struct tlv_source_info *) t->data;

	memcpy(data->macaddr, mac, 6);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tunnel_msg_type(struct agent *a,
		struct cmdu_buff *frm, uint8_t protocol)
{
	int ret;
	struct tlv *t;
	struct tlv_tunnel_msg_type *data;

	t = cmdu_reserve_tlv(frm, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_TUNNELED_MSG_TYPE;
	t->len = sizeof(*data);
	data = (struct tlv_tunnel_msg_type *) t->data;

	data->type = protocol;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tunneled(struct agent *a, struct cmdu_buff *frm,
		int frame_len, uint8_t *frame_body)
{
	int ret;
	struct tlv *t;
	struct tlv_tunneled *data;

	if ((!frame_body) || (frame_len <= 0))
		return -1;

	t = cmdu_reserve_tlv(frm, 1024);
	if (!t)
		return -1;

	t->type = MAP_TLV_TUNNELED;
	t->len = frame_len;
	data = (struct tlv_tunneled *) t->data;

	memcpy(data->frame, frame_body, frame_len);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int fill_steering_policy_from_tlv(struct agent *a,
		struct tlv_steering_policy *p,
		struct uci_context *ctx, struct uci_package *pkg,
		int skip_offset)
{
	int i, offset = 0;
	uint8_t *data = (uint8_t *)p;
	int num_radio;

	offset = skip_offset;
	num_radio = data[offset++];
	for (i = 0; i < num_radio; i++) {
		struct wifi_radio_element *re;
		uint8_t macaddr[6] = {0};
		uint8_t util_threshold;
		uint8_t rcpi_threshold;
		uint8_t steer_policy;
		char buf[4] = {0};

		memcpy(macaddr, data + offset, 6);
		offset += 6;
		steer_policy = data[offset++];
		util_threshold = data[offset++];
		rcpi_threshold = data[offset++];

		re = agent_get_radio(a, macaddr);
		if (!re)
			continue;

		snprintf(buf, sizeof(buf), "%d", steer_policy);
		uci_set_wireless_interface_option("mapagent", "radio", "device",
			re->name, "steer_policy", buf);

		snprintf(buf, sizeof(buf), "%d", util_threshold);
		uci_set_wireless_interface_option("mapagent", "radio", "device",
			re->name, "util_threshold", buf);

		snprintf(buf, sizeof(buf), "%d", rcpi_threshold);
		uci_set_wireless_interface_option("mapagent", "radio", "device",
			re->name, "rcpi_threshold", buf);
	}

	return 0;
}

/* for tlv_steering_policy:
 * num_radios = 1 && radio_id == 'ff:ff:ff:ff:ff:ff'
 * Means the config need to apply for all ap section;
 */
int fill_steering_policy_all(struct agent *a,
		struct tlv_steering_policy *p,
		struct uci_context *ctx, struct uci_package *pkg,
		int skip_offset)
{
	int ret, offset = 0;
	struct uci_element *e;
	struct uci_ptr ptr = {0};
	char buf[64] = {0};
	uint8_t *data = (uint8_t *)p;
	uint8_t steer_policy;
	uint8_t util_threshold;
	uint8_t rcpi_threshold;

	/* add the configuration in each iface section */
	offset = skip_offset + 1 + 6;
	steer_policy = data[offset++];
	util_threshold = data[offset++];
	rcpi_threshold = data[offset++];

	uci_foreach_element(&pkg->sections, e) {
		struct uci_section *s = uci_to_section(e);

		if (strcmp(s->type, "radio"))
			continue;

		/* Add the policy config params */
		memset(buf, 0, sizeof(buf));
		snprintf(buf, sizeof(buf) - 1,
				"%s.%s.steer_policy=%d",
				pkg->e.name, s->e.name,
				steer_policy);
		ret = uci_lookup_ptr(ctx, &ptr, buf, true);
		if (ret != UCI_OK)
			return -1;

		uci_set(ctx, &ptr);
		uci_save(ctx, ptr.p);

		memset(buf, 0, sizeof(buf));
		snprintf(buf, sizeof(buf) - 1,
				"%s.%s.util_threshold=%d",
				pkg->e.name, s->e.name,
				util_threshold);
		ret = uci_lookup_ptr(ctx, &ptr, buf, true);
		if (ret != UCI_OK)
			return -1;

		uci_set(ctx, &ptr);
		uci_save(ctx, ptr.p);

		memset(buf, 0, sizeof(buf));
		snprintf(buf, sizeof(buf) - 1,
				"%s.%s.rcpi_threshold=%d",
				pkg->e.name, s->e.name,
				rcpi_threshold);
		ret = uci_lookup_ptr(ctx, &ptr, buf, true);
		if (ret != UCI_OK)
			return -1;

		uci_set(ctx, &ptr);
		uci_save(ctx, ptr.p);
	}

	return 0;
}

int agent_fill_steering_policy(struct agent *a,
		struct tlv_steering_policy *p,
		struct uci_context *ctx, struct uci_package *pkg)
{
	int ret, i, num_radio;
	int offset = 0, skip_offset;
	int local_disallowed_sta_nr;
	int btm_disallowed_sta_nr;
	struct uci_element *e;
	struct uci_ptr ptr;
	char buf[64] = {0};
	char addr[18] = {0};
	uint8_t mac[6] = {0};
	uint8_t radio_id[6] = {0};
	uint8_t *data = (uint8_t *)p;
	uint8_t generic_id[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
	struct uci_section *s;
	bool is_section_found = false;

	uci_foreach_element(&pkg->sections, e) {
		s = uci_to_section(e);

		if (!strcmp(s->type, "policy")) {
			is_section_found = true;
			break;
		}
	}

	if (!is_section_found) {
		s = NULL;

		/* add a new section 'policy' */
		ret = uci_add_section(ctx, pkg, "policy", &s);
		if (ret != UCI_OK)
			return -1;

		uci_save(ctx, pkg);
	}


	/* Add exclude list & btm exclude list in 'policy' section */

	/* steer_exclude */
	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf),
				"%s.%s.%s", pkg->e.name,
				s->e.name, "steer_exclude");

	/* empty current UCI steer_exclude list first */
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK ||
			!(ptr.flags & UCI_LOOKUP_DONE))
		return -1;

	if (ptr.flags & UCI_LOOKUP_COMPLETE) {
		/* steer_exclude list found - remove */
		if (uci_delete(ctx, &ptr) == UCI_OK)
			uci_save(ctx, pkg);
	}

	// FIXME: Use config_update2 instead

	/* add entries to steer_exclude UCI list */
	local_disallowed_sta_nr = data[offset++];
	for (i = 0; i < local_disallowed_sta_nr; i++) {
		memset(addr, 0, sizeof(addr));
		memset(mac, 0, sizeof(mac));
		memcpy(mac, data + offset, 6);
		offset += 6;
		hwaddr_ntoa(mac, addr);

		memset(buf, 0, sizeof(buf));
		snprintf(buf, sizeof(buf),
				"%s.%s.steer_exclude=%s",
				pkg->e.name, s->e.name, addr);

		ret = uci_lookup_ptr(ctx, &ptr, buf, true);
		if (ret != UCI_OK ||
				!(ptr.flags & UCI_LOOKUP_DONE))
			return -1;

		if (uci_add_list(ctx, &ptr) == UCI_OK)
			uci_save(ctx, ptr.p);
	}

	/* steer_exclude_btm */
	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf),
				"%s.%s.%s", pkg->e.name,
				s->e.name, "steer_exclude_btm");

	/* empty current UCI steer_exclude_btm list first */
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK ||
			!(ptr.flags & UCI_LOOKUP_DONE))
		return -1;

	if (ptr.flags & UCI_LOOKUP_COMPLETE) {
		/* steer_exclude_btm list found - remove */
		if (uci_delete(ctx, &ptr) == UCI_OK)
			uci_save(ctx, pkg);
	}

	/* add entries to steer_exclude_btm UCI list */
	btm_disallowed_sta_nr = data[offset++];
	for (i = 0; i < btm_disallowed_sta_nr; i++) {
		memset(addr, 0, sizeof(addr));
		memset(mac, 0, sizeof(mac));
		memcpy(mac, data + offset, 6);
		offset += 6;
		hwaddr_ntoa(mac, addr);

		memset(buf, 0, sizeof(buf));
		snprintf(buf, sizeof(buf),
				"%s.%s.steer_exclude_btm=%s",
				pkg->e.name, s->e.name, addr);

		ret = uci_lookup_ptr(ctx, &ptr, buf, true);
		if (ret != UCI_OK ||
				!(ptr.flags & UCI_LOOKUP_DONE))
			return -1;

		if (uci_add_list(ctx, &ptr) == UCI_OK)
			uci_save(ctx, ptr.p);
	}

	skip_offset = offset;
	num_radio = data[offset++];
	memcpy(radio_id, data + offset, 6);
	if ((num_radio == 1) &&
			!memcmp(radio_id, generic_id, 6)) {
		fill_steering_policy_all(a, p, ctx, pkg, skip_offset);
	} else
		fill_steering_policy_from_tlv(a, p, ctx, pkg, skip_offset);

	return 0;
}

int add_metric_report_policy_config(struct uci_context *ctx,
		struct uci_package *pkg, struct uci_section *s,
		uint8_t rcpi_threshold, uint8_t rcpi_hysteresis,
		uint8_t util_threshold, uint8_t include)
{
	int ret;
	char buf[64] = {0};
	struct uci_ptr ptr;

	/* Add the policy config params */
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.report_rcpi_threshold=%d",
			pkg->e.name, s->e.name,
			rcpi_threshold);
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.rcpi_hysteresis_margin=%d",
			pkg->e.name, s->e.name,
			rcpi_hysteresis);
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.report_util_threshold=%d",
			pkg->e.name, s->e.name,
			util_threshold);
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.include_sta_stats=%d",
			pkg->e.name, s->e.name,
			(!!(include & INCLUDE_STA_STATS) ? 1 : 0));
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.include_sta_metric=%d",
			pkg->e.name, s->e.name,
			(!!(include & INCLUDE_STA_LINK_METRICS) ? 1 : 0));
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

#if (EASYMESH_VERSION > 2)
	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.include_wifi6_sta_status=%d",
			pkg->e.name, s->e.name,
			(!!(include & INCLUDE_STA_STATUS_REPORT) ? 1 : 0));
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);
#endif

	return 0;
}

int fill_metric_report_policy_from_tlv(struct agent *a,
		struct tlv_metric_report_policy *p, struct uci_context *ctx,
		struct uci_package *pkg)
{
	int ret, i;
	int offset = 0;
	int num_radio;
	struct uci_element *e;
	struct uci_ptr ptr;
	char buf[64] = {0};
	uint8_t *data = (uint8_t *)p;

	offset = 1;
	num_radio = data[offset++];

	/* Add radio specific params in ap section */
	for (i = 0; i < num_radio; i++) {
		struct wifi_radio_element *re;
		uint8_t macaddr[6] = {0};
		uint8_t rcpi_threshold;
		uint8_t rcpi_hysteresis;
		uint8_t util_threshold;
		uint8_t include;

		memcpy(macaddr, data + offset, 6);
		offset += 6;
		rcpi_threshold = data[offset++];
		rcpi_hysteresis = data[offset++];
		util_threshold = data[offset++];
		include = data[offset++];

		re = agent_get_radio(a, macaddr);
		if (!re)
			continue;

		uci_foreach_element(&pkg->sections, e) {
			struct uci_section *s = uci_to_section(e);

			if (strcmp(s->type, "radio"))
				continue;

			memset(buf, 0, sizeof(buf));
			snprintf(buf, sizeof(buf) - 1, "%s.%s.device",
					pkg->e.name, s->e.name);
			ret = uci_lookup_ptr(ctx, &ptr, buf, true);
			if (ptr.value && (ret != UCI_OK)) {
				fprintf(stderr, "value not found\n");
				continue;
			}

			if (strncmp(ptr.o->v.string, re->name, 16))
				continue;

			add_metric_report_policy_config(ctx, pkg, s,
					rcpi_threshold, rcpi_hysteresis,
					util_threshold, include);
			break;
		}

	}

	return 0;
}

/* for metric_reporting_policy_tlv:
 * num_radio = 1 && radio_id == ff:ff:ff:ff:ff:ff;
 * config need to apply on all ap section.
 */
int fill_metric_report_policy_all(struct agent *a,
		struct tlv_metric_report_policy *p, struct uci_context *ctx,
		struct uci_package *pkg)
{
	int offset = 0;
	struct uci_element *e;
	uint8_t *data = (uint8_t *)p;
	uint8_t rcpi_threshold;
	uint8_t rcpi_hysteresis;
	uint8_t util_threshold;
	uint8_t include;

	offset = 1 + 1 + 6;
	rcpi_threshold = data[offset++];
	rcpi_hysteresis = data[offset++];
	util_threshold = data[offset++];
	include = data[offset++];

	/* add configuration in each iface section */
	uci_foreach_element(&pkg->sections, e) {
		struct uci_section *s = uci_to_section(e);

		if (strcmp(s->type, "radio"))
			continue;

		add_metric_report_policy_config(ctx, pkg, s,
				rcpi_threshold, rcpi_hysteresis,
				util_threshold, include);
	}

	return 0;
}


int agent_fill_metric_report_policy(struct agent *a,
		struct tlv_metric_report_policy *p, struct uci_context *ctx,
		struct uci_package *pkg)
{
	int ret, offset = 0;
	int num_radio;
	uint8_t radio_id[6] = {0};
	struct uci_element *e;
	struct uci_ptr ptr;
	char buf[64] = {0};
	bool is_section_found = false;
	uint8_t *data = (uint8_t *)p;
	struct uci_section *s;
	uint8_t generic_id[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};

	uci_foreach_element(&pkg->sections, e) {
		s = uci_to_section(e);

		if (!strcmp(s->type, "policy")) {
			is_section_found = true;
			break;
		}
	}

	if (!is_section_found) {
		s = NULL;

		/* add a new section 'policy' */
		ret = uci_add_section(ctx, pkg, "policy", &s);
		if (ret != UCI_OK)
			return -1;

		uci_save(ctx, pkg);
	}

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.report_interval=%d",
			pkg->e.name, s->e.name, data[offset++]);
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	num_radio = data[offset++];
	memcpy(radio_id, data + offset, 6);
	if ((num_radio == 1) &&
			!memcmp(radio_id, generic_id, 6)) {
		fill_metric_report_policy_all(a, p, ctx, pkg);
	} else
		fill_metric_report_policy_from_tlv(a, p, ctx, pkg);

	return 0;
}

int agent_fill_8021q_setting(struct agent *a, uint16_t pvid, uint8_t pcp)
{
	int ret;
	struct uci_element *e;
	struct uci_ptr ptr;
	char buf[64] = {0};
	struct uci_section *s;
	bool is_section_found = false;
	struct uci_context *ctx;
	struct uci_package *pkg;

	ctx = uci_alloc_context();
	if (!ctx)
		return -1;

	if (uci_load(ctx, "mapagent", &pkg)) {
		uci_free_context(ctx);
		return -1;
	}

	uci_foreach_element(&pkg->sections, e) {
		s = uci_to_section(e);

		if (!strcmp(s->type, "policy")) {
			is_section_found = true;
			break;
		}
	}

	if (!is_section_found) {
		s = NULL;

		/* add a new section 'policy' */
		ret = uci_add_section(ctx, pkg, "policy", &s);
		if (ret != UCI_OK)
			return -1;

		uci_save(ctx, pkg);
	}

	dbg("Received Primary VID %u\n", pvid);

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1, "%s.%s.pvid=%d",
			pkg->e.name, s->e.name,
			pvid);
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	a->pvid = pvid;

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf), "%d", pvid);
	set_value_by_string("ieee1905", "ieee1905", "primary_vid", buf, UCI_TYPE_STRING);
	ieee1905_ubus_set_vid(a->ubus_ctx, pvid);

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1, "%s.%s.pcp_default=%d",
			pkg->e.name, s->e.name,
			(pcp & PCP_MASK) >> 5);
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	uci_unload(ctx, pkg);
	uci_free_context(ctx);
	return 0;
}

int agent_fill_8021q_setting_from_tlv(struct agent *a,
		struct tlv_default_8021q_settings *p)
{
	uint16_t pvid = BUF_GET_BE16(p->pvid);
	uint8_t pcp = p->pcp;

	return agent_fill_8021q_setting(a, pvid, pcp);
}

int agent_clear_traffic_sep(struct agent *a)
{
	bool reload = false;
	struct netif_apcfg *acfg = NULL;

	list_for_each_entry(acfg, &a->cfg.aplist, list) {
		int band = 0;
		uint16_t vid;

		dbg("FH/BH AP name %s multi_ap %d\n", acfg->name, acfg->multi_ap);
		vid = acfg->vid;
		if (!is_vid_valid(vid))
			vid = a->ts.primary_vid;

		if (a->cfg.brcm_setup) {
			dbg("/lib/wifi/multiap ts isolate_wds %s %s wds+\n", "del", acfg->name);
			runCmd("/lib/wifi/multiap ts isolate_wds %s %s wds+", "del", acfg->name);
		}
		else {
			struct netif_wds *wds = NULL;

			list_for_each_entry(wds, &a->wdslist, list) {
				dbg("/lib/wifi/multiap ts isolate_wds %s %s %s\n", "del", acfg->name, wds->ifname);
				runCmd("/lib/wifi/multiap ts isolate_wds %s %s %s", "del", acfg->name, wds->ifname);
			}
		}

		if (acfg->band == BAND_2)
			band = 2;
		else if (acfg->band == BAND_5)
			band = 5;
		else if (acfg->band == BAND_6)
			band = 6;
		else
			continue;

		dbg("/lib/wifi/multiap ts isolate_ap %s %d %d %d %s\n", "del", a->ts.primary_vid, acfg->vid, band, acfg->name);
		runCmd("/lib/wifi/multiap ts isolate_ap %s %d %d %d %s", "del", a->ts.primary_vid, acfg->vid, band, acfg->name);
	}

	/* if pvid was previously enabled, disable and wake nl */
	if (agent_is_ts_enabled(a))
		reload = true;

	uci_clear_traffic_sep(&a->cfg);
	ieee1905_ubus_set_vid(a->ubus_ctx, 0);
	if (reload) {
		runCmd("/lib/wifi/multiap ts cleanup"); /* delete ebtable rule(s), enable pktfwd if applicable */
		runCmd("/lib/wifi/multiap ts keep none"); /* cleanup all vids */
		nl_check_vlan(a, true);
	}

	return 0;
}

int agent_fill_traffic_sep_policy(struct agent *a,
		struct tlv_traffic_sep_policy *p)
{
	int ret, i;
	int offset = 0;
	int num_ssid;
	uint8_t *data = (uint8_t *)p;
	struct uci_element *e;
	struct uci_ptr ptr;
	char buf[64];
	struct uci_context *ctx;
	struct uci_package *pkg;

	ctx = uci_alloc_context();
	if (!ctx)
		return -1;

	if (uci_load(ctx, "mapagent", &pkg)) {
		uci_free_context(ctx);
		return -1;
	}

	num_ssid = data[offset++];
	for (i = 0; i < num_ssid; i++) {
		int ssid_len;
		uint16_t vid;
		char ssid[32] = {0};

		ssid_len = data[offset++];
		memcpy(ssid, data + offset, ssid_len);
		offset += ssid_len;
		vid = BUF_GET_BE16(data[offset]);
		offset += 2;

		uci_foreach_element(&pkg->sections, e) {
			char ifname[16] = {0};
			char ap_type[16] = {0};
			struct uci_element *e1;
			struct uci_section *s = uci_to_section(e);
			char section_ssid[32] = {0};
			uint8_t maxlen = 0;

			if (strcmp(s->type, "ap"))
				continue;

			memset(buf, 0, sizeof(buf));
			snprintf(buf, sizeof(buf) - 1, "%s.%s.ssid",
					pkg->e.name, s->e.name);
			ret = uci_lookup_ptr(ctx, &ptr, buf, true);
			if (ret != UCI_OK)
				return -1;


			e1 = ptr.last;
			if (e1->type == UCI_TYPE_OPTION)
				strncpy(section_ssid, ptr.o->v.string, 31);


			maxlen = (strlen(section_ssid) > ssid_len ?
					strlen(section_ssid) : ssid_len);

			if (strncmp(section_ssid, ssid, maxlen))
				continue;

			memset(buf, 0, sizeof(buf));
			snprintf(buf, sizeof(buf) - 1, "%s.%s.vid=%d",
					pkg->e.name, s->e.name,
					vid);
			ret = uci_lookup_ptr(ctx, &ptr, buf, true);
			if (ret != UCI_OK)
				return -1;

			uci_set(ctx, &ptr);
			uci_save(ctx, ptr.p);


			memset(buf, 0, sizeof(buf));
			snprintf(buf, sizeof(buf) - 1, "%s.%s.ifname",
					pkg->e.name, s->e.name);
			ret = uci_lookup_ptr(ctx, &ptr, buf, true);
			if (ret != UCI_OK)
				continue;

			if (ptr.last->type != UCI_TYPE_OPTION)
				continue;

			strncpy(ifname, ptr.o->v.string, sizeof(ifname) - 1);


			snprintf(buf, sizeof(buf) - 1, "%s.%s.type",
					pkg->e.name, s->e.name);
			ret = uci_lookup_ptr(ctx, &ptr, buf, true);
			if (ret != UCI_OK)
				continue;

			if (ptr.last->type != UCI_TYPE_OPTION)
				continue;

			strncpy(ap_type, ptr.o->v.string, sizeof(ap_type) - 1);

			/* if BSS is not FH, set primary vlan id in UCI */
			if (strcmp(ap_type, "fronthaul")) {
				snprintf(buf, sizeof(buf), "%d", a->pvid);
				uci_set_wireless_interface_option("wireless",
								  "wifi-iface",
								  "ifname",
								  ifname,
								  "multi_ap_primary_vlan_id",
								  buf);
			}

			/* if guest isolation is set, set isolate for guest VIDs */

			if (vid == a->cfg.pcfg->pvid)
				continue;

			uci_set_wireless_interface_option("wireless",
							  "wifi-iface",
							  "ifname",
							  ifname,
							  "wps_pushbutton",
							  "0");

			if (a->cfg.guest_isolation) {
				uci_set_wireless_interface_option("wireless",
								  "wifi-iface",
								  "ifname",
								  ifname,
								  "isolate",
								  "1");
			}
		}
	}

	uci_unload(ctx, pkg);
	uci_free_context(ctx);
	return 0;
}

int agent_fill_ch_scan_rep_policy(struct agent *a,
		struct tlv_channel_scan_report_policy *p, struct uci_context *ctx,
		struct uci_package *pkg)
{
	int ret;
	struct uci_element *e;
	struct uci_ptr ptr;
	char buf[64] = {0};
	bool is_section_found = false;
	struct uci_section *s;

	uci_foreach_element(&pkg->sections, e) {
		s = uci_to_section(e);

		if (!strcmp(s->type, "policy")) {
			is_section_found = true;
			break;
		}
	}

	if (!is_section_found) {
		s = NULL;

		/* add a new section 'policy' */
		ret = uci_add_section(ctx, pkg, "policy", &s);
		if (ret != UCI_OK)
			return -1;

		uci_save(ctx, pkg);
	}

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1, "%s.%s.report_scan=%d",
			pkg->e.name, s->e.name,
			!!(p->report & REPORT_CHANNEL_SCANS) ? 1 : 0);
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	return 0;
}

int agent_fill_unsuccess_assoc_policy(struct agent *a,
		struct tlv_unsuccess_assoc_policy *p, struct uci_context *ctx,
		struct uci_package *pkg)
{
	int ret;
	struct uci_element *e;
	struct uci_ptr ptr;
	char buf[64] = {0};
	bool is_section_found = false;
	struct uci_section *s;

	uci_foreach_element(&pkg->sections, e) {
		s = uci_to_section(e);

		if (!strcmp(s->type, "policy")) {
			is_section_found = true;
			break;
		}
	}

	if (!is_section_found) {
		s = NULL;

		/* add a new section 'policy' */
		ret = uci_add_section(ctx, pkg, "policy", &s);
		if (ret != UCI_OK)
			return -1;

		uci_save(ctx, pkg);
	}

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.report_sta_assocfails=%d",
			pkg->e.name, s->e.name,
			!!(p->report & UNSUCCESSFUL_ASSOC_REPORT) ? 1 : 0);
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	memset(buf, 0, sizeof(buf));
	snprintf(buf, sizeof(buf) - 1,
			"%s.%s.report_sta_assocfails_rate=%d",
			pkg->e.name, s->e.name,
			BUF_GET_BE32(p->max_report_rate));
	ret = uci_lookup_ptr(ctx, &ptr, buf, true);
	if (ret != UCI_OK)
		return -1;

	uci_set(ctx, &ptr);
	uci_save(ctx, ptr.p);

	return 0;
}

int agent_fill_backhaul_bss_config(struct agent *a,
		struct tlv_bbss_config *p, struct uci_context *ctx,
		struct uci_package *pkg)
{
	struct netif_ap *ap;
	bool add;

	ap = agent_get_ap(a, p->bssid);
	if (!ap)
		return -1;

	if (!ap->cfg || !(ap->cfg->multi_ap & 1))
		return -1;

	add = (p->config & BBSS_CONFIG_P1_BSTA_DISALLOWED);
	config_update2("mapagent", &a->cfg, "ap", "ifname",
			ap->cfg->name, "disallow_bsta_profile",
			add, "1", 1);

	add = (p->config & BBSS_CONFIG_P2_BSTA_DISALLOWED);
	config_update2("mapagent", &a->cfg, "ap", "ifname",
			ap->cfg->name, "disallow_bsta_profile",
			add, "2", 1);

	return 0;
}

int agent_fill_backhaul_bss_config_all(struct agent *a,
		struct tlv_bbss_config *p, struct uci_context *ctx,
		struct uci_package *pkg)
{
	struct netif_apcfg *apcfg = NULL;

	list_for_each_entry(apcfg, &a->cfg.aplist, list) {
		bool add;

		if (!(apcfg->multi_ap & 1))
			continue;

		add = (p->config & BBSS_CONFIG_P1_BSTA_DISALLOWED);
		config_update2("mapagent", &a->cfg, "ap", "ifname",
				apcfg->name, "disallow_bsta_profile",
				add, "1", 1);

		add = (p->config & BBSS_CONFIG_P2_BSTA_DISALLOWED);
		config_update2("mapagent", &a->cfg, "ap", "ifname",
				apcfg->name, "disallow_bsta_profile",
				add, "2", 1);
	}

	return 0;
}

struct tlv_vendor_specific *agent_gen_vendor_specific_tlv(struct agent *a, uint8_t depth)
{
	return NULL;
}

char *get_timestamp_old(time_t *t, char **tbuf)
{
	char tmpbuf[64] = {0};
	struct tm res;
	char sign;
	long toff, toff_hour, toff_min;
	const time_t now = time(t);

	//if (!tbuf)
	//        return NULL;

	/* E.g. "2019-02-11T06:42:31.23039-08:00" */

	localtime_r(&now, &res);
	tzset();
	toff = timezone;
	sign = toff > 0 ? '-' : '+';
	toff *= -1L;

	toff_hour = toff / 3600;
	toff_min = (toff % 3600) / 60;

	snprintf(tmpbuf, 63, "%04d-%02d-%02dT%02d:%02d:%02d%c%02ld:%02ld",
		 res.tm_year + 1900, res.tm_mon + 1, res.tm_mday,
		 res.tm_hour, res.tm_min, res.tm_sec,
		 sign, toff_hour, toff_min);

	if (!*tbuf) {
		*tbuf = calloc(1, strlen(tmpbuf) + 1);
		if (!*tbuf)
			return NULL;
	}

	snprintf(*tbuf, strlen(tmpbuf) + 1, "%s", tmpbuf);
	return *tbuf;
}

int agent_gen_timestamp_tlv(struct agent *agent, struct cmdu_buff *frm)
{
	int ret;
	struct tlv *t;
	char tsp[TIMESTAMP_TLV_MAX_LEN] = {0};
	struct tlv_timestamp *data;
	/* Allocate the TLV of the cmdu_data */
	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	/* Define the TLV */
	t->type = MAP_TLV_TIMESTAMP;
	data = (struct tlv_timestamp *) t->data;
	time_to_timestr(NULL, tsp);
	data->len = strlen(tsp);
	memcpy(data->timestamp, (uint8_t *)tsp, data->len);
	t->len = sizeof(*data) + data->len;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}
	return 0;
}

/* Updates Scan Result TLV's NumberOfNeighbors */
void scan_result_tlv_update_num_nbr(struct tlv *t, uint32_t num_nbr)
{
	uint8_t status;
	uint8_t ts_len;
	int offset = 0;

	offset += 6; /* RUID */
	offset += 1; /* opclass */
	offset += 1; /* channel */
	status = t->data[offset];
	offset += 1; /* status code */

	/* Nothing to update */
	if (status != CH_SCAN_STATUS_SUCCESS)
		return;

	ts_len = t->data[offset];
	offset += 1; /* timestamp len */
	offset += ts_len; /* timestamp */
	offset += 1; /* utilization */
	offset += 1; /* noise */
	/* num neighbors */
	BUF_PUT_BE16(t->data[offset], num_nbr);
}

/* Reserves reserve_len bytes for scan response TLV and fills in repeatable data */
struct tlv *cmdu_reserve_scan_response_tlv(struct cmdu_buff *cmdu,
		int reserve_len, char *tsp, uint8_t *radio_mac, uint8_t opclass_id,
		struct wifi_scanres_channel_element *ch, uint8_t status, int *offset)
{
	trace("%s ---->\n", __func__);

	struct tlv *t;
	*offset = 0;

	t = cmdu_reserve_tlv(cmdu, reserve_len);
	if (!t)
		return NULL;

	t->type = MAP_TLV_CHANNEL_SCAN_RES;
	memcpy(&t->data[*offset], radio_mac, 6);
	*offset += 6;
	t->data[*offset] = opclass_id;	/* opclass */
	*offset += 1;
	t->data[*offset] = ch->channel;	/* channel */
	*offset += 1;
	t->data[*offset] = status;		/* status code */
	*offset += 1;

	/* Put (non-success) status code only */
	if (status != CH_SCAN_STATUS_SUCCESS)
		return t;

	t->data[*offset] = strlen(ch->tsp) + 1;		/* timestamp len */
	*offset += 1;
	memcpy(&t->data[*offset], ch->tsp, strlen(ch->tsp));	/* timestamp */
	*offset += strlen(ch->tsp);
	t->data[*offset] = '\0';
	*offset += 1;
	t->data[*offset] = ch->utilization;	/* utilization */
	*offset += 1;
	t->data[*offset] = ch->anpi;		/* noise */
	*offset += 1;
	/* num neighbors */
	BUF_PUT_BE16(t->data[*offset], ch->num_neighbors);
	*offset += 2;

	return t;
}

/* Puts Channel Scan Response TLV(s) into CMDU. Splits neighbors evenly
 * between TLVs so that TLV is always less than CH_SCAN_RESP_MAX_DATALEN.
 */
int agent_gen_ch_scan_response_tlv(struct agent *a, struct cmdu_buff *cmdu,
			uint8_t *radio_mac, uint8_t opclass_id,
			struct wifi_scanres_channel_element *ch, uint8_t status)
{
	trace("%s ---->\n", __func__);

	char tsp[32] = {0};
	struct tlv *t;
	uint8_t bssload_elem_pres = CH_SCAN_RESULT_BSSLOAD_PRESENT;
	int i, ret, offset = 0;
	int reserve_len = CH_SCAN_RESP_TLV_MAX_LEN;
	uint32_t num_nbr = 0;

	trace("\t %s:INFO: radio " MACFMT ", opclass %d, channel %d\n", __func__,
		  MAC2STR(radio_mac), opclass_id, ch->channel);

	time_to_timestr(NULL, tsp);
	memcpy(ch->tsp, tsp, strlen(tsp));
	t = cmdu_reserve_scan_response_tlv(cmdu, reserve_len,
				tsp, radio_mac, opclass_id, ch, status, &offset);
	if (!t)
		return -1;

	/* Put (non-success) status code only */
	if (status != CH_SCAN_STATUS_SUCCESS)
		goto put_tlv;

	for (i = 0; i < ch->num_neighbors; i++) {
		char bw_str[16] = {0};
		struct wifi_scanres_neighbor_element *nbr = ch->nbrlist + i;

		snprintf(bw_str, 15, "%u", nbr->bw);

		/* Check if nbr data will fit within TLV limits */
		if ((offset + 18 + strlen(nbr->ssid) + strlen(bw_str)) >= reserve_len) {
			/* Always add scan duration and scan type to TLV */
			BUF_PUT_BE32(t->data[offset], ch->scan_duration);
			offset += 4;
			t->data[offset++] = SCAN_RESULT_SCAN_TYPE;

			/* Update the TLV length */
			t->len = offset;
			/* Update NumberOfNeighbors */
			if (num_nbr != ch->num_neighbors)
				scan_result_tlv_update_num_nbr(t, num_nbr);
			/* Put intermediate TLV into CMDU ... */
			ret = cmdu_put_tlv(cmdu, t);
			if (ret) {
				fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
				return -1;
			}

			/* ... and start over */
			t = cmdu_reserve_scan_response_tlv(cmdu,
						reserve_len,
						tsp,
						radio_mac,
						opclass_id,
						ch,
						status,
						&offset);
			if (!t)
				return -1;
			num_nbr = 0;
		}

		memcpy(&t->data[offset], nbr->bssid, 6);	/* bssid */
		offset += 6;
		t->data[offset++] = strlen(nbr->ssid);		/* ssid len */
		/* ssid */
		memcpy(&t->data[offset], nbr->ssid, strlen(nbr->ssid));
		offset += strlen(nbr->ssid);
		t->data[offset++] = rssi_to_rcpi(nbr->rssi);	/* rssi */

		t->data[offset++] = strlen(bw_str); //+ 1;		/* BW length */
		memcpy(&t->data[offset], bw_str, strlen(bw_str));
		/* NOTE: It is not a real string terminated with EOS null */
		offset += strlen(bw_str);
		//t->data[offset++] = '\0';
		t->data[offset++] = bssload_elem_pres;		/* BSS load element */

		if (bssload_elem_pres & CH_SCAN_RESULT_BSSLOAD_PRESENT) {
			t->data[offset++] = nbr->utilization;			/* channel utilization */
			BUF_PUT_BE16(t->data[offset], nbr->num_stations);	/* station count */
			offset += 2;
		}
		num_nbr++;
	}

	BUF_PUT_BE32(t->data[offset], ch->scan_duration);	/* scan duration */
	offset += 4;
	t->data[offset++] = SCAN_RESULT_SCAN_TYPE;	/* scan type */

put_tlv:
	/* update the tlv length */
	t->len = offset;
	/* Update NumberOfNeighbors */
	if (num_nbr != ch->num_neighbors)
		scan_result_tlv_update_num_nbr(t, num_nbr);
	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_bk_sta_radio_cap_tlv(struct agent *a, struct cmdu_buff *cmdu,
				   struct wifi_radio_element *re)
{
	struct tlv_bsta_radio_cap *p;
	struct tlv *t;
	int ret;

	if (!re->has_bsta)
		return 0;

	/* Allocate the TLV of the cmdu_data */
	t = cmdu_reserve_tlv(cmdu, 256);
	if (!t)
		return -1;

	/* Define the TLV */
	t->type = MAP_TLV_BACKHAUL_STA_RADIO_CAPABILITY;
	t->len = 7;
	p = (struct tlv_bsta_radio_cap *) t->data;
	memcpy(p->radio, re->macaddr, 6);

	p->macaddr_included = 0;
	if (!hwaddr_is_zero(re->bk.macaddr)) {
		memcpy(p->macaddr, re->bk.macaddr, 6);
		p->macaddr_included = BSTA_MACADDRESS_INCLUDED;
		t->len += 6;
	}

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		trace("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_client_assoc_event_tlv(struct agent *agent, struct cmdu_buff *frm,
		uint8_t *mac, uint8_t *bssid, uint8_t assoc_event)
{
	struct tlv *t;
	struct tlv_client_assoc_event *data;
	int ret;

	t = cmdu_reserve_tlv(frm, 128);
	if (!t)
		return -1;

	t->type = MAP_TLV_CLIENT_ASSOCIATION_EVENT;
	t->len = 13;
	data = (struct tlv_client_assoc_event *) t->data;

	memcpy(data->macaddr, mac, 6);
	memcpy(data->bssid, bssid, 6);
	if (assoc_event == 0x01)
		data->event = 1 << 7;
	else
		data->event = 0;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_sta_mac(struct agent *agent,
		struct cmdu_buff *frm, uint8_t *mac)
{
	int ret;
	struct tlv *t;
	struct tlv_sta_mac *data;

	t = cmdu_reserve_tlv(frm, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_STA_MAC_ADDRESS;
	t->len = sizeof(*data);
	data = (struct tlv_sta_mac *) t->data;

	memcpy(data->macaddr, mac, 6);
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_reason_code(struct agent *agent,
		struct cmdu_buff *frm, uint16_t reason_code)
{
	int ret;
	struct tlv *t;
	struct tlv_reason_code *data;

	t = cmdu_reserve_tlv(frm, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_REASON_CODE;
	t->len = sizeof(*data);
	data = (struct tlv_reason_code *) t->data;

	BUF_PUT_BE16(data->code, reason_code);
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_oper_bss_tlv(struct agent *a,
		struct cmdu_buff *frm)
{
	struct wifi_radio_element *re = NULL;
	struct tlv_ap_oper_bss *data;
	struct tlv *t;
	uint8_t *ptr;
	int ret;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_OPERATIONAL_BSS;
	t->len = 1;

	ptr = t->data;
	data = (struct tlv_ap_oper_bss *)t->data;

	data->num_radio = (uint8_t) a->num_radios;
	ptr += 1;

	list_for_each_entry(re, &a->radiolist, list) {
		struct netif_ap *ap = NULL;
		uint8_t *num_bss;

		/* radio mac */
		memcpy(ptr, re->macaddr, 6);
		ptr += 6;
		t->len += 6;

		/* num_bss */
		num_bss = ptr;
		*num_bss = 0;
		ptr += 1;
		t->len += 1;

		list_for_each_entry(ap, &re->aplist, list) {
			struct wifi_bss_element *bss = &ap->bss;

			/* only report BSS in PWR_ON or PWR_SAVE mode */
			if (bss->enabled) {
				int len;

				(*num_bss)++;

				len = strlen(bss->ssid);

				/* iface bssid */
				memcpy(ptr, bss->bssid, 6);
				ptr += 6;
				t->len += 6;

				/* ssid len */
				memcpy(ptr, &len, 1);
				ptr += 1;
				t->len += 1;

				/* ssid */
				memcpy(ptr, bss->ssid, len);
				ptr += len;
				t->len += len;
			}
		}
	}

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_assoc_client_tlv(struct agent *a, struct cmdu_buff *frm)
{
	struct wifi_radio_element *re = NULL;
	struct tlv_assoc_client *data;
	int offset = 0;
	struct tlv *t;
	int ret;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_ASSOCIATED_CLIENTS;
	t->len = 1;

	data = (struct tlv_assoc_client *) t->data;
	offset += 1;
	data->num_bss = 0;

	list_for_each_entry(re, &a->radiolist, list) {
		struct netif_ap *ap;

		list_for_each_entry(ap, &re->aplist, list) {
			struct wifi_bss_element *bss = &ap->bss;

			if (!bss->enabled)
				continue;

			data->num_bss++;
			/* bssid */
			memcpy(&t->data[offset], bss->bssid, 6);
			offset += 6;

#if 1 /* TODO: replace with stalist when ready for use */
			{
				struct netif_ap *ap;
				struct sta *s;
				uint8_t *num_sta_ptr;
				uint16_t num_sta = 0;

				ap = agent_get_ap(a, bss->bssid);
				if (!ap)
					continue;

				num_sta_ptr = &t->data[offset];
				offset += 2;

				list_for_each_entry(s, &ap->stalist, list) {
					uint16_t conntime = 0;

#if (EASYMESH_VERSION >= 6)
					if (s->is_affiliated_sta) {
						//err("%s: "MACFMT" is affiliated STA\n", __func__, MAC2STR(s->macaddr));
						continue;
					}
#endif
					/* macaddr */
					memcpy(&t->data[offset], s->macaddr, 6);
					offset += 6;

					conntime = ((s->connected_ms / 1000) > 0xFFFF)
						? 0xFFFF
						: (s->connected_ms / 1000) & 0xFFFF;

					/* conntime */
					BUF_PUT_BE16(t->data[offset], conntime);
					num_sta++;
					offset += 2;
				}
				BUF_PUT_BE16(*num_sta_ptr, num_sta);
			}
#endif
#if 0
			p->bss[idx + i].clients =
				calloc(bss->num_stations,
				       sizeof(*p->bss[idx + i].clients));
			if (!p->bss[idx + i].clients)
				return NULL;

			p->bss[idx + i].assoc_clients_nr =
				bss->num_stations;

			for (k = 0; k < p->bss[idx + i].assoc_clients_nr; k++) {
				memcpy(p->bss[idx + i].clients[k].client_addr,
				       bss->stalist[k].macaddr,
				       sizeof(p->bss[idx + i].clients[k].client_addr));

				/* Note: cap at 0xFFFF, see above */
				p->bss[idx + i].clients[k].uptime =
					bss->stalist[k].conn_time;
			}
#endif
		}
	}
#if (EASYMESH_VERSION >= 6)
	struct netif_mld *mld = NULL;

	list_for_each_entry(mld, &a->apmldlist, list) {
		uint8_t *num_sta_ptr;
		uint16_t num_sta = 0;
		struct sta_mld *smld = NULL;
		int bssid_offset = 0;
		bool found_bssid = false;
		int i;

		bssid_offset += 1; /* num bss */

		/**
		 * Don't add duplicate BSSIDs, in-case MLD macaddr and
		 * AP macaddr are same.
		 */
		for (i = 0; i < data->num_bss; i++) {
			if (memcmp(&t->data[bssid_offset], mld->macaddr, 6)) {
				/* Macaddr did not match, step past */
				bssid_offset += 6;
				num_sta = BUF_GET_BE16(t->data[bssid_offset]);
				bssid_offset += 2;
				bssid_offset += (num_sta * 8);
				continue;
			}
			found_bssid = true;
			break;
		}

		if (!found_bssid) {
			bssid_offset = offset;
			data->num_bss++;
			/* bssid */
			memcpy(&t->data[bssid_offset], mld->macaddr, 6);
			num_sta = 0;
			/*
			 * New entry is added to the end, increase offset
			 * on bssid and sta counter fields length.
			 */
			offset += 6 + 2;
		}

		bssid_offset += 6; /* step past written bssid */
		num_sta_ptr = &t->data[bssid_offset];
		if (found_bssid)
			num_sta = BUF_GET_BE16(t->data[bssid_offset]);
		bssid_offset += 2;
		list_for_each_entry(smld, &mld->stamldlist, list) {
			uint16_t conntime = 0;

			if (found_bssid && bssid_offset != offset) {
				/* shift data to make room for new STA entry */
				memmove(&t->data[bssid_offset + 8], &t->data[bssid_offset], offset - bssid_offset);
			}

			/* macaddr */
			memcpy(&t->data[bssid_offset], smld->macaddr, 6);
			bssid_offset += 6;
			offset += 6;

			if (smld->num_affiliated_sta > 0) {
				/** conntime is not (yet) available for MLD dev
				 *  use first affiliated STA conntime
				 */
				conntime = ((smld->affiliated_sta[0]->connected_ms / 1000) > 0xFFFF)
					? 0xFFFF
					: (smld->affiliated_sta[0]->connected_ms / 1000) & 0xFFFF;
			}

			/* conntime */
			BUF_PUT_BE16(t->data[bssid_offset], conntime);
			num_sta++;
			bssid_offset += 2;
			offset += 2;
		}
		BUF_PUT_BE16(*num_sta_ptr, num_sta);
	}
#endif

	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tlv_backhaul_steer_resp(struct agent *a, struct cmdu_buff *frm,
		uint8_t *target_bssid, uint8_t *macaddr, uint8_t res)
{
	struct tlv *t;
	struct tlv_backhaul_steer_resp *data;
	int ret;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_BACKHAUL_STEERING_RESPONSE;
	t->len = 13;
	data = (struct tlv_backhaul_steer_resp *) t->data;

	memcpy(data->target_bssid, target_bssid, 6);
	memcpy(data->macaddr, macaddr, 6);
	data->result = res;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_assoc_status_notif(struct agent *a, struct cmdu_buff *frm,
		int num_data, void *data)
{
	int i, ret;
	int offset = 0;
	struct tlv *t;
	struct bss_data {
		uint8_t bssid[6];
		uint8_t status;
	} *bss_data = NULL;

	if (!data)
		return -1;

	bss_data = (struct bss_data *)data;
	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_ASSOCIATION_STATUS_NOTIF;

	t->data[offset++] = (uint8_t) num_data;		/* num bss */
	for (i = 0; i < num_data; i++) {
		memcpy(&t->data[offset], bss_data[i].bssid, 6);	/* bssid */
		offset += 6;
		t->data[offset++] = bss_data[i].status;		/* status */
	}

	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_client_info(struct agent *a, struct cmdu_buff *frm,
		uint8_t *sta, uint8_t *bssid)
{
	int ret;
	struct tlv *t;
	struct tlv_client_info *data;

	t = cmdu_reserve_tlv(frm, 30);
	if (!t)
		return -1;

	t->type = MAP_TLV_CLIENT_INFO;
	t->len = sizeof(*data);
	data = (struct tlv_client_info *)t->data;
	memcpy(data->bssid, bssid, 6);
	memcpy(data->macaddr, sta, 6);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_client_cap_report(struct agent *a, struct cmdu_buff *frm,
		uint8_t result, struct sta *s)
{
	int ret;
	struct tlv *t;
	struct tlv_client_cap_report *data;

	if (result != 0x00) {
		t = cmdu_reserve_tlv(frm, 10);
		if (!t)
			return -1;

		t->type = MAP_TLV_CLIENT_CAPABILITY_REPORT;
		t->len = sizeof(*data);
		data = (struct tlv_client_cap_report *)t->data;

		data->result = result;
	} else {
		/* [re]assoc frame received without the header
		 * so no need to skip bytes
		 */
		uint8_t *body;
		int len;

		len = s->assoc_frame->len;
		body = s->assoc_frame->frame;

		/* reserve addition few bytes (say 10 more bytes)
		 * actual required: len + 3; (1: tlv_type, 2: tlv_len)
		 */
		t = cmdu_reserve_tlv(frm, len + 10);
		if (!t)
			return -1;

		t->type = MAP_TLV_CLIENT_CAPABILITY_REPORT;
		t->len = sizeof(*data);
		data = (struct tlv_client_cap_report *)t->data;
		data->result = result;

		/* copy the raw frame */
		memcpy(data->frame, body, len);

		t->len += len;
	}

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_tlv_higher_layer_data(struct agent *a, struct cmdu_buff *frm,
		uint8_t proto, uint8_t *data, int len)
{
	struct tlv *t;

	t = cmdu_reserve_tlv(frm, len + 1);
	if (!t)
		return -1;

	t->type = MAP_TLV_HIGHER_LAYER_DATA;
	t->len = len + 1;
	t->data[0] = proto;
	memcpy(t->data + 1, data, len);

	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}

int agent_gen_status_code(struct agent *a, struct cmdu_buff *frm,
		int status_code)
{
	int ret;
	struct tlv *t;
	struct tlv_status_code *data;

	t = cmdu_reserve_tlv(frm, 20);
	if (!t)
		return -1;

	t->type = MAP_TLV_STATUS_CODE;
	t->len = sizeof(*data);
	data = (struct tlv_status_code *) t->data;

	BUF_PUT_BE16(data->code, status_code & 0xffff);
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

#if (EASYMESH_VERSION > 2)
int agent_gen_bss_config_report_tlv(struct agent *a, struct cmdu_buff *frm)
{
	struct tlv_bss_configuration_report *data;
	struct wifi_radio_element *re = NULL;
	struct tlv *t;
	int ret;
	uint8_t *ptr;

	t = cmdu_reserve_tlv(frm, 1024);
	if (!t)
		return -1;

	t->type = MAP_TLV_BSS_CONFIGURATION_REPORT;
	t->len = 1;

	ptr = t->data;
	data = (struct tlv_bss_configuration_report *)t->data;

	data->num_radio = (uint8_t) a->num_radios;
	ptr += 1;

	list_for_each_entry(re, &a->radiolist, list) {
		struct netif_ap *ap = NULL;
		uint8_t *num_bss;

		/* radio mac */
		memcpy(ptr, re->macaddr, 6);
		ptr += 6;
		t->len += 6;

		/* num_bss */
		num_bss = ptr;
		*num_bss = 0;
		ptr += 1;
		t->len += 1;
		list_for_each_entry(ap, &re->aplist, list) {
			struct wifi_bss_element *bss = &ap->bss;
			uint8_t report = 0x00;

			/* only report BSS in PWR_ON or PWR_SAVE mode that are in fronthaul*/
			if (bss->enabled) {
				int len;

				(*num_bss)++;

				len = strlen(bss->ssid);

				/* iface bssid */
				memcpy(ptr, bss->bssid, 6);
				ptr += 6;
				t->len += 6;

				/*report bits */
				if (!ap->cfg)
					continue;

				/* Here the in use flag is 0 and not in use flag is 1*
				 * So if it is backhaul the fronthaul flag is 1
				 */
				if (ap->cfg->multi_ap == 0x01 || ap->cfg->multi_ap == 0x03) {
					/*As the R1 or R2 disallow will only happen in backhaul*/
					if (ap->cfg->bsta_disallow == 0x01)
						report = report | 0x20;
					else if (ap->cfg->bsta_disallow == 0x02)
						report = report | 0x10;
					else if (ap->cfg->bsta_disallow == 0x03) {
						report = report | 0x20;
						report = report | 0x10;
					}
				}

				/* Backhaul BSS 'in use' flag */
				if (ap->cfg->multi_ap & 0x01)
					report = report | 0x80;

				/* Fronthaul BSS 'in use' flag */
				if (ap->cfg->multi_ap & 0x02)
					report = report | 0x40;

				/*TODO BSS_CONFIG_MBSSID*/
				/*TOD0 BSS_CONFIG_TX_MBSSID*/

				memcpy(ptr, &report, 1);
				ptr += 1;
				t->len += 1;

				/*reserved octet*/
				ptr += 1;
				t->len += 1;

				/* ssid len */
				memcpy(ptr, &len, 1);
				ptr += 1;
				t->len += 1;

				/* ssid */
				memcpy(ptr, bss->ssid, len);
				ptr += len;
				t->len += len;
			}
		}
	}

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_akm_suite_cap(struct agent *a, struct cmdu_buff *frm)
{
	struct tlv *tlv;
	uint8_t *data_ptr;
	int i;
	int len = 0;

	// todo: populate with real data
	uint8_t num_bh_bss_akm_suite_selectors = 2;
	uint8_t num_fh_bss_akm_suite_selectors = 2;

	tlv = cmdu_reserve_tlv(frm, 100);
	if (!tlv)
		return -1;
	tlv->type = MAP_TLV_AKM_SUITE_CAPS;
	tlv->len = 0;

	data_ptr = tlv->data;

	*data_ptr = num_bh_bss_akm_suite_selectors;
	len++;

	data_ptr += sizeof(num_bh_bss_akm_suite_selectors);

	for (i = 0; i < num_bh_bss_akm_suite_selectors; ++i) {
		struct akm_suite *suite_selector = (struct akm_suite *)data_ptr;

		suite_selector->oui[0] = 0;
		suite_selector->oui[1] = 1;
		suite_selector->oui[2] = 2;
		suite_selector->type = 3;

		data_ptr += sizeof(*suite_selector);
		len += sizeof(*suite_selector);
	}

	*data_ptr = num_fh_bss_akm_suite_selectors;
	len++;

	data_ptr += sizeof(num_fh_bss_akm_suite_selectors);

	for (i = 0; i < num_fh_bss_akm_suite_selectors; ++i) {
		struct akm_suite *suite_selector = (struct akm_suite *)data_ptr;

		suite_selector->oui[0] = 0;
		suite_selector->oui[1] = 1;
		suite_selector->oui[2] = 2;
		suite_selector->type = 3;

		data_ptr += sizeof(*suite_selector);
		len += sizeof(*suite_selector);
	}

	tlv->len = len;

	if (cmdu_put_tlv(frm, tlv))
		return -1;

	return 0;
}

int agent_gen_device_1905_layer_security_cap(struct agent *a,
		struct cmdu_buff *frm)
{
	struct tlv *t;
	struct tlv_1905_security_cap *data;

	t = cmdu_reserve_tlv(frm, 10);
	if (!t)
		return -1;

	t->type = MAP_TLV_1905_SECURITY_CAPS;
	data = (struct tlv_1905_security_cap *)t->data;

	t->len = sizeof(*data);
	/* TODO: need to do the mapping */
	data->protocol = SECURITY_PROTOCOL_DPP;
	data->mic = SECURITY_MIC_HMAC_SHA256;
	data->enc = SECURITY_ENC_AES_SIV;

	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}

int agent_gen_conf_req_object_atrributes(struct agent *a, struct cmdu_buff *frm)
{
	struct tlv *tlv;
	int data_len;

	// todo: populate with real data, a sample JSON DPP conf. req. object:
	const char *data =
		"{\
			\"name\":\"My Device\",\
			\"wi-fi_tech\":\"infra\",\
			\"netRole\":\"ap\"\
		}";

	data_len = strlen(data);

	tlv = cmdu_reserve_tlv(frm, data_len);
	if (!tlv)
		return -1;
	tlv->type = MAP_TLV_BSS_CONFIGURATION_REQUEST;
	tlv->len = data_len;

	memcpy(tlv->data, data, data_len);

	if (cmdu_put_tlv(frm, tlv))
		return -1;

	return 0;
}

int agent_gen_device_inventory(struct agent *a, struct cmdu_buff *frm)
{
	int reserve_len = (65 * 3) + (a->num_radios * 72);
	struct wifi_radio_element *re = NULL;
	int lsn, lsv, lee;
	int offset = 0;
	struct tlv *t;
	uint8_t *data;

	t = cmdu_reserve_tlv(frm, reserve_len);
	if (!t)
		return -1;

	t->type = MAP_TLV_DEVICE_INVENTORY;
	data = (uint8_t *)t->data;

	lsn = strlen(a->device_inventory.serial_number);
	data[offset] = lsn;
	offset++;	/* serial number len */
	memcpy(&data[offset], a->device_inventory.serial_number, lsn);
	offset += lsn;	/* serial number */

	lsv = strlen(a->device_inventory.sw_version);
	data[offset] = lsv;
	offset++;	/* software version len */
	memcpy(&data[offset], a->device_inventory.sw_version, lsv);
	offset += lsv;	/* software version */

	lee = 0;
	data[offset] = lee;
	offset++;	/* execution env len */
	/* TODO : not available currently */
	// data->exenv.ee ??;
	offset += lee;	/* execution env */

	data[offset] = a->num_radios;
	offset++;	/* num radio */
	list_for_each_entry(re, &a->radiolist, list) {
		struct device_inventory_radio *ir =
			(struct device_inventory_radio *)&t->data[offset];

		memcpy(ir->ruid, re->macaddr, 6);
		ir->lcv = strlen(re->vendor);
		memcpy(ir->cv, re->vendor, ir->lcv);
		offset += 6 + 1 + ir->lcv;
	}

	t->len = offset;
	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}

int agent_gen_dpp_chirp_value(struct agent *a, struct cmdu_buff *frm,
			uint8_t *enrollee, uint8_t validity,
			uint16_t hashlen, uint8_t *hash)
{
	struct tlv *t;
	int offset = 0;
	uint8_t flags = 0x00;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_DPP_CHIRP_VALUE;

	if (enrollee)
		flags |= 1 << 7;


	flags |= validity << 6; /* TODO:  0: purge (when?) -- 1: establish */

	t->data[offset++] = flags;

	if (enrollee) {
		memcpy(&t->data[offset], enrollee, 6);
		offset += 6;
	}

	t->data[offset++] = hashlen;

	memcpy(&t->data[offset], hash, hashlen);

	offset += hashlen;
	t->len = offset;
	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}

int agent_gen_assoc_wifi6_sta_status_report(struct agent *a,
		struct cmdu_buff *frm, struct sta *s)
{
	int ret, offset = 0;
	struct tlv *t = NULL;
	struct tlv_assoc_wifi6_sta_status_report *data;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_ASSOCIATED_WIFI6_STA_STATUS;
	data = (struct tlv_assoc_wifi6_sta_status_report *)t->data;
	memcpy(data->macaddr, s->macaddr, 6);
#if (EASYMESH_VERSION >= 6)
	if (s->is_affiliated_sta && s->mld)
		memcpy(data->macaddr, s->mld->macaddr, 6);
#endif
	offset += sizeof(*data);
	/* TODO:
	 * fill the queue size & tid data
	 * need to add the same in struct sta
	 */
	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_dpp_bootstrapping_uri_notif(struct agent *a,
		struct cmdu_buff *frm, uint8_t *radio_id, uint8_t *bssid,
		uint8_t *bksta, char *dpp_uri, int uri_len)
{
	struct tlv *t;
	struct tlv_dpp_uri_bootstrap *data;
	int reserve_len = uri_len + 30;

	t = cmdu_reserve_tlv(frm, reserve_len);
	if (!t)
		return -1;

	t->type = MAP_TLV_DPP_BOOTSTRAP_URI_NOTIFICATION;
	t->len = sizeof(*data) + uri_len;
	data = (struct tlv_dpp_uri_bootstrap *)t->data;

	memcpy(data->ruid, radio_id, 6);
	memcpy(data->bssid, bssid, 6);
	memcpy(data->bsta, bksta, 6);
	memcpy(data->uri, dpp_uri, uri_len);

	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}

int agent_gen_1905_encap_dpp_tlv(struct agent *a, struct cmdu_buff *frm,
				 uint8_t *enrollee, uint8_t frametype,
				 uint16_t framelen, uint8_t *frame)
{
#if USE_LIBDPP
	struct tlv *t;
	int offset = 0;
	uint8_t flags = 0x0;
	bool is_gas = !!FRAME_IS_DPP_GAS_FRAME(&frametype);

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_1905_ENCAP_DPP;

	if (!is_gas)
		flags |= 1 << 5;//ENCAP_DPP_FRAME_INDICATOR;
	if (enrollee)
		flags |= 1 << 7;//ENCAP_DPP_ENROLLEE_MAC_PRESENT

	t->data[offset++] = flags;

	if (enrollee) {
		memcpy(&t->data[offset], enrollee, 6);
		offset += 6;
	}

	/* Frame Type */
	t->data[offset++] = (is_gas ? 255 : frametype);

	/* Encapsulated frame length field */
	BUF_PUT_BE16(t->data[offset], framelen);
	offset += 2;
	/* Encapsulated frame */
	memcpy(&t->data[offset], frame, framelen);
	offset += framelen;

	t->len = offset;
	if (cmdu_put_tlv(frm, t))
		return -1;
#endif
	return 0;
}

int agent_gen_profile2_error_code_tlv(struct agent *a, struct cmdu_buff *frm,
                                      uint8_t reason,
                                      uint8_t bssid[6],
                                      uint32_t spr_id,
                                      uint16_t qmid)
{
	struct tlv *t;
	int offset = 0;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_PROFILE2_ERR_CODE;
	t->data[offset++] = reason;

	switch (reason)
	{
		case TLV_PROFILE2_ERR_CODE_REASON_SPR_NOT_FOUND:
		case TLV_PROFILE2_ERR_CODE_REASON_TOO_MANY_SPR:
			BUF_PUT_BE32(t->data[offset], spr_id);
			offset += 4;
			break;
		case TLV_PROFILE2_ERR_CODE_REASON_TS_COMBINED_BSS_UNSUPPORTED:
		case TLV_PROFILE2_ERR_CODE_REASON_TS_UNSUPPORTED:
			memcpy(&t->data[offset], bssid, 6);
			offset += 6;
			break;
		case TLV_PROFILE2_ERR_CODE_REASON_QM_CONFIGURE_FAIL:
			BUF_PUT_BE16(t->data[offset], qmid);
			offset += 2;
			break;
		default:
			break;
	}

	t->len = offset;
	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}

int agent_gen_dpp_message_tlv(struct agent *a, struct cmdu_buff *frm,
			      uint16_t framelen, uint8_t *frame)
{
	struct tlv *t;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_DPP_MESSAGE;

	memcpy(&t->data[0], frame, framelen);

	t->len = framelen;
	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}

#endif
#if (EASYMESH_VERSION >= 6)
int agent_gen_wifi7_agent_caps(struct agent *a, struct cmdu_buff *frm)
{
	struct wifi_radio_element *re = NULL;
	struct tlv_wifi7_agent_caps *data;
	int offset = 0;
	struct tlv *t;

	/* max linknum are reported per-radio */
	list_for_each_entry(re, &a->radiolist, list) {
		/* 're' will be set to first radio */
		break;
	}

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_WIFI7_AGENT_CAPABILITIES;
	t->len = 0;

	data = (struct tlv_wifi7_agent_caps *) t->data;

	data->max_mldnum = 8;
	offset++;
	data->max_linknum = ((re->wifi7_caps.max_ap_links << 4) & 0xf0);
	data->max_linknum |= (re->wifi7_caps.max_bsta_links & 0x0f);
	offset++;
	data->flag = re->wifi7_caps.ap_ttlm; /* Dont support TID-to-Link Mapping */
	offset++;

	offset += 13; /* reserved */

	data->num_radio = a->num_radios;
	offset++;

	list_for_each_entry(re, &a->radiolist, list) {
		struct wifi7_radio_capabilities *caps = &re->wifi7_caps;
		int cap_iter;

		memcpy(&t->data[offset], re->macaddr, 6);
		offset += 6;
		offset += 24; /* reserved */

		if (caps->ap_str_support)
			t->data[offset] |= WIFI7_AP_MLO_STR_SUPPORTED;
		if (caps->ap_nstr_support)
			t->data[offset] |= WIFI7_AP_MLO_NSTR_SUPPORTED;
		if (caps->ap_emlsr_support)
			t->data[offset] |= WIFI7_AP_MLO_EMLSR_SUPPORTED;
		if (caps->ap_emlmr_support)
			t->data[offset] |= WIFI7_AP_MLO_EMLMR_SUPPORTED;
		offset++;

		if (caps->bsta_str_support)
			t->data[offset] |= WIFI7_BSTA_MLO_STR_SUPPORTED;
		if (caps->bsta_nstr_support)
			t->data[offset] |= WIFI7_BSTA_MLO_NSTR_SUPPORTED;
		if (caps->bsta_emlsr_support)
			t->data[offset] |= WIFI7_BSTA_MLO_EMLSR_SUPPORTED;
		if (caps->bsta_emlmr_support)
			t->data[offset] |= WIFI7_BSTA_MLO_EMLMR_SUPPORTED;
		offset++;


#define NUM_MLO_MODES 6
		for (cap_iter = 0; cap_iter < NUM_MLO_MODES; cap_iter++) {
			struct wifi_radio_element *subr = NULL;
			uint8_t *num = &t->data[offset++];

			*num = 0;
			list_for_each_entry(subr, &a->radiolist, list) {
				struct wifi7_radio_capabilities *caps2 = &subr->wifi7_caps;

				if (cap_iter == 0 && caps->ap_str_support &&
					   caps2->ap_str_support) {
					(*num)++;
					memcpy(&t->data[offset], subr->macaddr, 6);
					offset += 6;
					t->data[offset++] = 0;
				} else if (cap_iter == 1 && caps->ap_emlsr_support &&
					   caps2->ap_str_support) {
					(*num)++;
					memcpy(&t->data[offset], subr->macaddr, 6);
					offset += 6;
					t->data[offset++] = 0;
				} else if (cap_iter == 2 && caps->ap_emlmr_support &&
					   caps2->ap_str_support) {
					(*num)++;
					memcpy(&t->data[offset], subr->macaddr, 6);
					offset += 6;
					t->data[offset++] = 0;
				} else if (cap_iter == 3 && caps->bsta_str_support &&
					   caps2->ap_str_support) {
					(*num)++;
					memcpy(&t->data[offset], subr->macaddr, 6);
					offset += 6;
					t->data[offset++] = 0;
				} else if (cap_iter == 4 && caps->bsta_emlsr_support &&
					   caps2->ap_str_support) {
					(*num)++;
					memcpy(&t->data[offset], subr->macaddr, 6);
					offset += 6;
					t->data[offset++] = 0;
				} else if (cap_iter == 5 && caps->bsta_emlmr_support &&
					   caps2->ap_str_support) {
					(*num)++;
					memcpy(&t->data[offset], subr->macaddr, 6);
					offset += 6;
					t->data[offset++] = 0;
				}
			}
		}
#undef NUM_MLO_MODES
	}

	t->len = offset;

	if (cmdu_put_tlv(frm, t)) {
		dbg("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_ap_mld_config(struct agent *a, struct cmdu_buff *frm)
{
	trace("%s: --->\n", __func__);

	struct tlv_ap_mld_config *data;
	struct netif_mld *mld = NULL;
	int offset = 0;
	struct tlv *t;
	int ret = 0;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_AP_MLD_CONFIG;
	t->len = 0;

	data = (struct tlv_ap_mld_config *) t->data;

	data->num_mlds = a->num_ap_mld;
	offset++;

	list_for_each_entry(mld, &a->apmldlist, list) {
		int ssid_len = strlen(mld->ssid);
		struct netif_ap *ap = NULL;
		int i;

		/* AP MLD MAC Addr Valid */
		t->data[offset++] = AP_MLD_CONF_AP_MLD_ADDR_VALID;

		/* ssidLength */
		t->data[offset++] = ssid_len;

		/* SSID */
		memcpy(&t->data[offset], mld->ssid, ssid_len);
		offset += ssid_len;

		/* AP MLD MAC Addr */
		memcpy(&t->data[offset], mld->macaddr, 6);
		offset += 6;

		/* STR */
		t->data[offset] = 0;
		if (mld->ap_str_enabled)
			t->data[offset] |= AP_MLD_CONFIG_STR;
		/* NSTR */
		if (mld->ap_nstr_enabled)
			t->data[offset] |= AP_MLD_CONFIG_NSTR;
		/* EMLSR */
		if (mld->ap_emlsr_enabled)
			t->data[offset] |= AP_MLD_CONFIG_EMLSR;
		/* EMLMR */
		if (mld->ap_emlmr_enabled)
			t->data[offset] |= AP_MLD_CONFIG_EMLMR;
		offset++;

		offset += 20; /* reserved */

		/* numAffiliatedAPs */
		t->data[offset++] = mld->num_affiliated_ap;
		for (i = 0; i < mld->num_affiliated_ap; i++) {
			struct wifi_radio_element *re = NULL;
			ap = mld->affiliated_ap[i];

			if (!ap || !ap->is_affiliated_ap)
				continue;

			/* Affiliated AP MAC Addr Valid */
			t->data[offset] = AP_MLD_CONF_AFF_AP_ADDR_VALID;
			// TODO: ap->link_id_valid
			//t->data[offset] |= AP_MLD_CONF_LINK_ID_VALID;
			offset++;

			/* RUID */
			re = agent_get_radio_by_band(a, ap->band);
			if (re)
				memcpy(&t->data[offset], re->macaddr, 6);
			offset += 6;

			/* Affiliated AP MAC Addr */
			memcpy(&t->data[offset], ap->bssid, 6);
			offset += 6;

			/* LinkID */
			t->data[offset++] = ap->linkid;

			offset += 18; /* reserved */
		}
	}

	t->len = offset;


	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return ret;
}

int agent_gen_bsta_mld_config(struct agent *a, struct cmdu_buff *frm)
{
	trace("%s: --->\n", __func__);

	struct tlv *t;
	struct tlv_bsta_mld_config *data;
	//struct mld_credential *mld = NULL;
	int offset = 0;
	uint8_t *num_aff_bstas;
	struct bsta_mld *bsta = &a->bstamld;
	uint8_t valid_flag = BSTA_MLD_CONF_AFFILIATED_BSTA_MLD_ADDR_VALID;
	int ret;

	t = cmdu_reserve_tlv(frm, 512);
	if (!t)
		return -1;

	t->type = MAP_TLV_BACKHAUL_STA_MLD_CONFIG;
	t->len = 0;

	data = (struct tlv_bsta_mld_config *)t->data;

	/* bSTA_MLD_MAC_ADDR_VALID & AP_MLD_MAC_Addr_Valid */
	data->flag = 0;
	offset++;

	uint8_t caps_flag = 0;


	data->flag = 0;

	/* bSTA_MLD_MAC_Addr */
	if (!hwaddr_is_zero(bsta->macaddr)) {
		memcpy(&t->data[offset], bsta->macaddr, 6);
		data->flag |= BSTA_MLD_CONF_BSTA_MLD_ADDR_VALID;
	}
	offset += 6;

	/* AP_MLD_MAC_Addr */
	/* "addr of the AP MLD to which the bSTA MLD is associated" */
	if (!hwaddr_is_zero(bsta->bssid)) {
		memcpy(&t->data[offset], bsta->bssid, 6);
		data->flag |= BSTA_MLD_CONF_AP_MLD_ADDR_VALID;
	}
	offset += 6;

	/* STR/NSTR/EMLSR/EMLMR */
	if (bsta->str_enabled)
		caps_flag |= BSTA_MLD_CONFIG_STR;
	if (bsta->nstr_enabled)
		caps_flag |= BSTA_MLD_CONFIG_NSTR;
	if (bsta->emlsr_enabled)
		caps_flag |= BSTA_MLD_CONFIG_EMLSR;
	if (bsta->emlmr_enabled)
		caps_flag |= BSTA_MLD_CONFIG_EMLMR;

	data->flag2 = caps_flag;
	offset++;

	/* reserved */
	offset += 17;

	/* numAffiliatedbSTAs */
	num_aff_bstas = &t->data[offset];
	*num_aff_bstas = bsta->num_affiliated_bsta;
	offset++;

	if (*num_aff_bstas) {
		int i;

		for (i = 0; i < *num_aff_bstas; i++) {
			struct netif_bk *affsta = bsta->affiliated_bsta[i];
			struct wifi_radio_element *re;

			if (!affsta->cfg)
				continue;

			re = agent_get_radio_by_band(a, affsta->cfg->band);
			if (!re)
				continue;

			/* Affiliated_bSTA_MAC_Addr_Valid */
			t->data[offset] = valid_flag;
			offset++;

			/* RUID */
			memcpy(&t->data[offset], re->macaddr, 6);
			offset += 6;

			/* Affilated_bSTA_MAC_Addr */
			memcpy(&t->data[offset], affsta->macaddr, 6);
			offset += 6;

			/* reserved */
			offset += 19;
		}
	}

	t->len = offset;


	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_eht_operations(struct agent *a, struct cmdu_buff *frm)
{
	struct wifi_radio_element *re = NULL;
	uint32_t precalc_len = 1;
	int offset = 0;
	struct tlv *t;
	int ret;

	list_for_each_entry(re, &a->radiolist, list) {
		struct netif_ap *ap = NULL;

		precalc_len += (6 + 1 + 25);
		list_for_each_entry(ap, &re->aplist, list) {
			if (ap->bss.enabled) {
				precalc_len += (6 + 10 + 16);
			}
		}
	}

	t = cmdu_reserve_tlv(frm, precalc_len);
	if (!t)
		return -1;

	t->type = MAP_TLV_EHT_OPERATIONS;
	t->len = 0;

	offset += 32;

	t->data[offset] = a->num_radios;
	offset += sizeof(uint8_t);
	list_for_each_entry(re, &a->radiolist, list) {
		struct netif_ap *ap = NULL;
		uint8_t *num_bss;

		memcpy(&t->data[offset], re->macaddr, 6);
		offset += 6;

		num_bss = &t->data[offset];
		*num_bss = 0;

		offset += sizeof(uint8_t);

		list_for_each_entry(ap, &re->aplist, list) {
			struct wifi_bss_element *bss = &ap->bss;

			if (bss->enabled) {
				(*num_bss)++;

				/* BSSID */
				memcpy(&t->data[offset], bss->bssid, 6);
				offset += 6;

				memcpy(&t->data[offset], bss->eht_oper_caps, sizeof(bss->eht_oper_caps));
				offset += 10;

				offset += 16; /* reserved */
			}
		}

		offset += 25; /* reserved */
	}

	t->len = offset;
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_affiliated_ap_metrics(struct agent *a, struct cmdu_buff *frm,
				    struct wifi_bss_element *bss)
{
	struct tlv_affiliated_ap_metrics *data;
	struct tlv *t;
	int ret;

	if (!wifi_is_affiliated_ap(a, bss->bssid))
		return 0;

	t = cmdu_reserve_tlv(frm, sizeof(*data));
	if (!t)
		return -1;

	t->type = MAP_TLV_AFFILIATED_AP_METRICS;
	t->len = sizeof(*data);
	data = (struct tlv_affiliated_ap_metrics *)t->data;

	memcpy(data->bssid, bss->bssid, 6);

	BUF_PUT_BE32(data->tx_packets, bss->tx_packets);
	BUF_PUT_BE32(data->rx_packets, bss->rx_packets);
	BUF_PUT_BE32(data->tx_bytes_ucast, bss->tx_ucast_bytes);
	BUF_PUT_BE32(data->rx_bytes_ucast, bss->rx_ucast_bytes);
	BUF_PUT_BE32(data->tx_bytes_mcast, bss->tx_mcast_bytes);
	BUF_PUT_BE32(data->rx_bytes_mcast, bss->rx_mcast_bytes);
	BUF_PUT_BE32(data->tx_bytes_bcast, bss->tx_bcast_bytes);
	BUF_PUT_BE32(data->rx_bytes_bcast, bss->rx_bcast_bytes);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}


int agent_gen_affiliated_sta_metrics(struct agent *a, struct cmdu_buff *frm,
				     struct sta *s)
{
	struct tlv_affiliated_sta_metrics *data;
	struct tlv *t;
	int ret;

	t = cmdu_reserve_tlv(frm, sizeof(*data));
	if (!t)
		return -1;

	t->type = MAP_TLV_AFFILIATED_STA_METRICS;
	t->len = sizeof(*data);
	data = (struct tlv_affiliated_sta_metrics *)t->data;

	memcpy(data->macaddr, s->macaddr, 6);

	BUF_PUT_BE32(data->tx_bytes, s->tx_bytes);
	BUF_PUT_BE32(data->rx_bytes, s->rx_bytes);
	BUF_PUT_BE32(data->tx_packets, s->tx_pkts);
	BUF_PUT_BE32(data->rx_packets, s->rx_pkts);
	BUF_PUT_BE32(data->tx_err_packets, s->tx_fail_pkts);

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int agent_gen_associated_sta_mld_config(struct agent *a, struct cmdu_buff *frm,
					struct sta_mld *mld)
{
	struct tlv_associated_sta_mld_config *data;
	struct tlv *t;
	uint8_t caps_flag = 0;
	int offset = 0;
	int ret;
	int i;

	t = cmdu_reserve_tlv(frm, 256);
	if (!t)
		return -1;

	t->type = MAP_TLV_STA_MLD_CONFIG;
	data = (struct tlv_associated_sta_mld_config *)t->data;

	memcpy(data->sta_mld_macaddr, mld->macaddr, 6);
	offset += 6;
	memcpy(data->ap_mld_macaddr, mld->bssid, 6);
	offset += 6;

	/* STR/NSTR/EMLSR/EMLMR */
	if (mld->str_enabled)
		caps_flag |= STA_MLD_CONFIG_STR;
	if (mld->nstr_enabled)
		caps_flag |= STA_MLD_CONFIG_NSTR;
	if (mld->emlsr_enabled)
		caps_flag |= STA_MLD_CONFIG_EMLSR;
	if (mld->emlmr_enabled)
		caps_flag |= STA_MLD_CONFIG_EMLMR;
	data->flag = caps_flag;
	offset += 1;

	offset += 18; /* reserved */

	data->num_affiliated_sta = mld->num_affiliated_sta;
	offset += 1;

	for (i = 0; i < mld->num_affiliated_sta; i++) {
		struct sta *s = mld->affiliated_sta[i];

		memcpy(&t->data[offset], s->bssid, 6);
		offset += 6;
		memcpy(&t->data[offset], s->macaddr, 6);
		offset += 6;
		offset += 19;
	}

	t->len = offset;

	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		err("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}
#endif
