/*
 * cntlr_tlv.c - tlv building function
 *
 * Copyright (C) 2020 IOPSYS Software Solutions AB. All rights reserved.
 *
 * Author: jakob.olsson@iopsys.eu
 *
 */

#include <1905_tlvs.h>
#include <cmdu.h>
#if (EASYMESH_VERSION > 2)
#ifdef USE_LIBDPP
#include <dpputils.h>
#endif
#endif
#include <easy/easy.h>
#include <easymesh.h>
#include <libubox/list.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <i1905_wsc.h>
#include <wifidefs.h>


#include "utils/utils.h"
#include "utils/debug.h"
#include "cntlr.h"
#include "cntlr_tlv.h"
#include "config.h"
#include "acs.h"
#include "wifi_dataelements.h"
#include "wifi_opclass.h"


int cntlr_gen_8021q_settings(struct controller *c, struct cmdu_buff *frm)
{
	struct tlv *t;
	struct tlv_default_8021q_settings *data;
	int ret;

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

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

	data = (struct tlv_default_8021q_settings *) t->data;
	BUF_PUT_BE16(data->pvid, c->cfg.primary_vid);
	data->pcp = (c->cfg.default_pcp << 5) & PCP_MASK;

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

	return 0;
}

int cntlr_gen_traffic_sep_policy(struct controller *c, struct cmdu_buff *frm)
{
	struct tlv *t;
	struct tlv_traffic_sep_policy *ts;
	int ret;
	struct iface_credential *ap = NULL;
	uint8_t *ptr;

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

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

	ts = (struct tlv_traffic_sep_policy *) t->data;
	ts->num_ssid = 0;
	ptr = (uint8_t *) ts + 1;

	list_for_each_entry(ap, &c->cfg.aplist, list) {
		uint8_t len;
		int i;
		bool found = false;

		len = strlen((char *)ap->cred.ssid);

		/**
		 * Don't add duplicate SSIDs.
		 * Having one SSID mapping to multiple VLAN IDs is not supported.
		 */
		for (i = 0; i < ts->num_ssid; i++) {
			struct ssid_info *info;
			uint8_t *p = (uint8_t *) ts + 1;
			int j;

			for (j = 0; j < i; j++) {
				p += *p; /* ssid len */
				p++; /* len */
				p += 2; /* vid */
			}

			info = (struct ssid_info *) p;

			if (len != info->len)
				continue;

			if (memcmp(ap->cred.ssid, info->ssid, len))
				continue;

			found = true;
			break;
		}

		/* skip duplicate */
		if (found)
			continue;

		t->len++; /* len */
		*ptr = len;
		ptr++;

		t->len += len; /* ssid */
		memcpy(ptr, ap->cred.ssid, len);
		ptr += len;

		t->len += 2; /* vid */
		buf_put_be16(ptr, ap->vlanid);
		ptr += 2;

		ts->num_ssid++;
	}

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

	return 0;
}


int cntlr_gen_wsc(struct controller *c, struct cmdu_buff *frm,
		  struct wps_credential *cred,
		  struct iface_credential *ap,
		  uint8_t *msg, uint16_t msglen,
		  uint8_t band, uint16_t m1_auth)
{
	struct tlv *t;
	uint16_t m2_size;
	uint8_t *m2;
	int ret;

#define ATTR_ENABLED (0x4C) /* IOPSYS m2 vendor extension */

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

	t->type = TLV_TYPE_WSC;

	if (ap == NULL || cred == NULL) {
		struct wps_credential teardown_cred = {0};

		teardown_cred.mapie = 1 << 4;
		teardown_cred.band = band;
		info("|%s:%d| setting teardown bit as all ap in radio is disabled\n", __func__, __LINE__);
		ret = wsc_build_m2(msg, msglen, &teardown_cred, NULL, 0, &m2, &m2_size);
		if (ret) {
			err("%s: Error building m2!\n", __func__);
			return ret;
		}
	} else {
		/* if m1 does not support cred auth or cred auth is strictly higher,
		* reject and teardown
		*/
		if ((cred->auth_type & m1_auth) != cred->auth_type) {
			cred->mapie |= 1 << 4;
			info("|%s:%d| setting teardown bit (m1 auth:%04x "\
				"creds auth:%04x)\n", __func__, __LINE__,
				m1_auth, cred->auth_type);
		}

		ret = wsc_build_m2(msg, msglen, cred,
				(struct wsc_vendor_ie *) ap->ven_ies,
				ap->num_ven_ies, &m2, &m2_size);
		cred->mapie &= ~(1 << 4);
		if (ret) {
			err("%s: Error building m2!\n", __func__);
			return ret;
		}
	}

	t->len = m2_size;

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

	free(m2);
	return 0;
}

int cntlr_gen_ap_radio_identifier(struct controller *c,
		struct cmdu_buff *frm, uint8_t *hwaddr)
{
	struct tlv *t;
	struct tlv_ap_radio_identifier *data;
	int ret;

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

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

	data = (struct tlv_ap_radio_identifier *) t->data;
	memcpy(data->radio, hwaddr, 6);

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

	return 0;
}

int cntlr_gen_supp_role(struct controller *c, struct cmdu_buff *frm,
		uint8_t role)
{
	struct tlv *t;
	struct tlv_supported_role *data;
	int ret;

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

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

	data = (struct tlv_supported_role *) t->data;
	data->role = role;

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

	return 0;
}

int cntlr_gen_supp_service(struct controller *c, struct cmdu_buff *frm)
{
	struct tlv *t;
	int ret;
	int num = 0;
	struct tlv_supported_service *t_supp_serv;

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

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

	if (c->state != CNTLR_INIT) {
		t_supp_serv->services[num++] = SUPPORTED_SERVICE_MULTIAP_CONTROLLER;

		if (cntlr_find_node(c, c->almacaddr))
			t_supp_serv->services[num++] = SUPPORTED_SERVICE_MULTIAP_AGENT;
	} else
		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) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_map_profile(struct controller *c, 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) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_ch_scan_rep_policy( struct controller *c,
		struct node_policy *a, struct cmdu_buff *frm)
{
	int ret;
	struct tlv *t;
	struct tlv_channel_scan_report_policy *data;

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

	t->type = MAP_TLV_CHANNEL_SCAN_REPORTING_POLICY;
	t->len = sizeof(*data);
	data = (struct tlv_channel_scan_report_policy *) t->data;

	if (a->report_scan)
		data->report = REPORT_CHANNEL_SCANS;

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

	return 0;
}

int cntlr_gen_al_mac(struct controller *c, struct cmdu_buff *frm,
		uint8_t *hwaddr)
{
	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;

	dbg("hwaddr " MACFMT "\n", MAC2STR(hwaddr));

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

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

	return 0;
}

int cntlr_gen_unsuccess_assoc_policy(struct controller *c,
		struct node_policy *a, struct cmdu_buff *frm)
{
	int ret;
	struct tlv *t;
	struct tlv_unsuccess_assoc_policy *data;

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

	t->type = MAP_TLV_UNSUCCESS_ASSOCIATION_POLICY;
	t->len = sizeof(*data);
	data = (struct tlv_unsuccess_assoc_policy *) t->data;

	if (a->report_sta_assocfails)
		data->report = UNSUCCESSFUL_ASSOC_REPORT;

	BUF_PUT_BE32(data->max_report_rate, a->report_sta_assocfails_rate);

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

	return 0;
}

int cntlr_gen_supported_freq_band(struct controller *c, struct cmdu_buff *frm,
		uint8_t freq_band)
{
	struct tlv *t;
	struct tlv_supported_band *data;
	int ret;

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

	t->type = TLV_TYPE_SUPPORTED_FREQ_BAND;
	t->len = 1;
	data = (struct tlv_supported_band *) t->data;
	data->band = freq_band;

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

	return 0;
}

int cntlr_gen_backhaul_bss_config(struct controller *c,
		struct node_policy *a, struct cmdu_buff *frm,
		const uint8_t *bssid)
{
	int ret;
	struct tlv *t;
	struct tlv_bbss_config *data;

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

	t->type = MAP_TLV_BACKHAUL_BSS_CONFIG;
	t->len = sizeof(*data);
	data = (struct tlv_bbss_config *) t->data;

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

#if 0
	if (a->disallow_bsta_p1)
#endif
	data->config |= BBSS_CONFIG_P1_BSTA_DISALLOWED;
#if 0
	if (a->disallow_bsta_p2)
		data->config |= BBSS_CONFIG_P2_BSTA_DISALLOWED;
#endif
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_steering_policy(struct controller *c, struct node_policy *a,
		struct cmdu_buff *frm, int num_radio, uint8_t *radiolist)
{
	int ret, i;
	int offset = 0;
	struct tlv *t;
	struct stax *ex = NULL, *btex = NULL;
	uint8_t num_nosteer_index = 0;
	uint8_t num_nobtmsteer_index = 0;
	uint8_t num_nosteer = 0;
	uint8_t num_nobtmsteer = 0;

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

	t->type = MAP_TLV_STEERING_POLICY;

	num_nosteer_index = offset++;
	list_for_each_entry(ex, &a->steer_exlist, list) {
		num_nosteer++;
		memcpy(t->data + offset, ex->macaddr, 6);
		offset += 6;
	}
	t->data[num_nosteer_index] = num_nosteer;

	num_nobtmsteer_index = offset++;
	list_for_each_entry(btex, &a->btmsteer_exlist, list) {
		num_nobtmsteer++;
		memcpy(t->data + offset, btex->macaddr, 6);
		offset += 6;
	}
	t->data[num_nobtmsteer_index] = num_nobtmsteer;

	t->data[offset++] = num_radio;
	for (i = 0; i < num_radio; i++) {
		struct radio_policy *rp;

		rp = cntlr_get_radio_policy(&c->cfg, &radiolist[i*6]);
		if (!rp)
			continue;

		memcpy(t->data + offset, &radiolist[i*6], 6);
		offset += 6;
		t->data[offset++] = rp->policy;
		t->data[offset++] = rp->util_threshold;
		t->data[offset++] = rp->rcpi_threshold;
	}

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

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

	return 0;
}

int cntlr_gen_metric_report_policy(struct controller *c, struct node_policy *a,
		struct cmdu_buff *frm, int num_radio, uint8_t *radiolist)
{
	int ret, i;
	struct tlv *t;
	int offset = 0;
	uint8_t include_flag;

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

	t->type = MAP_TLV_METRIC_REPORTING_POLICY;

	t->data[offset++] = a->report_metric_periodic;
	t->data[offset++] = num_radio;

	for (i = 0; i < num_radio; i++) {
		struct radio_policy *rp;
		include_flag = 0x00;

		rp = cntlr_get_radio_policy(&c->cfg, &radiolist[i*6]);
		if (!rp)
			continue;

		memcpy(t->data + offset, &radiolist[i*6], 6);
		offset += 6;
		t->data[offset++] = rp->report_rcpi_threshold;
		t->data[offset++] = rp->report_rcpi_hysteresis_margin;
		t->data[offset++] = rp->report_util_threshold;

		if (rp->include_sta_stats)
			include_flag |= INCLUDE_STA_STATS;

		if (rp->include_sta_metric)
			include_flag |= INCLUDE_STA_LINK_METRICS;

#if (EASYMESH_VERSION > 2)
		if (rp->include_wifi6_sta_status)
			include_flag |= INCLUDE_STA_STATUS_REPORT;
#endif

		t->data[offset++] = include_flag;
	}

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

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

	return 0;
}

int cntlr_gen_client_info(struct controller *c, 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, 40);
	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) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_backhaul_steer_req(struct controller *c, struct cmdu_buff *frm,
		uint8_t *bkhaul, uint8_t *target_bssid, uint8_t op_class,
		uint8_t channel)
{
	struct tlv *t;
	struct tlv_backhaul_steer_request *data;
	int ret;

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

	t->type = MAP_TLV_BACKHAUL_STEERING_REQUEST;
	t->len = 14;
	data = (struct tlv_backhaul_steer_request *) t->data;
	memcpy(data->target_bssid, target_bssid, 6);
	memcpy(data->macaddr, bkhaul, 6);
	data->target_opclass = op_class;
	data->target_channel = channel;

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

	return 0;
}

static int _cntlr_gen_tlv_steer_request(struct controller *c,
					struct cmdu_buff *frm,
					uint8_t tlv_type,
					uint8_t *bssid, uint32_t steer_op_window,
					uint32_t num_sta, uint8_t stas[][6],
					uint32_t num_target_bssid, uint8_t target_bssid[][6],
					bool btm_abridged, bool btm_disassoc_imminent,
					uint32_t btm_disassoc_timeout, uint8_t mbo_reason,
					bool is_mandate)
{
	struct tlv_steer_request *st;
	struct steer_target_bss *tbss;
	int offset = 0;
	struct tlv *t;
	int ret;
	int i;

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

	t->type = tlv_type;

	st = (struct tlv_steer_request *)t->data;
	memcpy(st->bssid, bssid, 6);

	if (btm_abridged)
		st->mode |= STEER_REQUEST_BTM_ABRIDGED;

	if (btm_disassoc_imminent)
		st->mode |= STEER_REQUEST_BTM_DISASSOC_IMM;

	if (btm_disassoc_timeout > 0)
		BUF_PUT_BE16(st->btm_disassoc_timer, btm_disassoc_timeout);
	else
		BUF_PUT_BE16(st->btm_disassoc_timer, 0x0000);

	if (is_mandate) {
		st->mode |= STEER_REQUEST_MODE;		//MANDATE
		BUF_PUT_BE16(st->op_window, 0x0000);
	} else {
		BUF_PUT_BE16(st->op_window, steer_op_window);
	}

	offset += 11;	/* bssid + mode + op_window + btm_disassoc_timer */

	st->sta.num = (uint8_t)num_sta;
	memcpy(st->sta.sta, stas, num_sta * 6);
	offset++; /* num_sta */
	offset += (num_sta * 6);

	tbss = (struct steer_target_bss *)&t->data[offset];
	tbss->num = 0;
	offset++;	/* sizeof(tbss->num) */
	if (is_mandate) {
		for (i = 0; i < num_target_bssid; i++) {
			struct netif_radio *radio;
			uint8_t opclass = 0;
			uint8_t channel = 0;

			radio = cntlr_find_radio_with_bssid(c, target_bssid[i]);
			if (!radio)
				continue;

			opclass = ctrl_radio_cur_opclass_id(radio->radio_el);
			channel = ctrl_radio_cur_opclass_ctrl_chan(radio->radio_el);

			tbss->num += 1;
			memcpy(&t->data[offset], target_bssid[i], 6);
			offset += 6;
			t->data[offset++] = opclass;
			t->data[offset++] = channel;
			if (tlv_type == MAP_TLV_PROFILE2_STEERING_REQ)
				t->data[offset++] = mbo_reason;
		}
	}

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

	return 0;
}

int cntlr_gen_tlv_steer_request(struct controller *c,
				struct cmdu_buff *frm,
				uint8_t *bssid, uint32_t steer_op_window,
				uint32_t num_sta, uint8_t stas[][6],
				uint32_t num_target_bssid, uint8_t target_bssid[][6],
				bool btm_abridged, bool btm_disassoc_imminent,
				uint32_t btm_disassoc_timeout,
				bool is_mandate)
{
	return _cntlr_gen_tlv_steer_request(c, frm,
					    MAP_TLV_STEERING_REQUEST,
					    bssid, steer_op_window,
					    num_sta, stas,
					    num_target_bssid, target_bssid,
					    btm_abridged, btm_disassoc_imminent,
					    btm_disassoc_timeout, 0,
					    is_mandate);
}

int cntlr_gen_tlv_profile2_steer_request(struct controller *c,
					  struct cmdu_buff *frm,
					  uint8_t *bssid, uint32_t steer_op_window,
					  uint32_t num_sta, uint8_t stas[][6],
					  uint32_t num_target_bssid, uint8_t target_bssid[][6],
					  bool btm_abridged, bool btm_disassoc_imminent,
					  uint32_t btm_disassoc_timeout, uint8_t mbo_reason,
					  bool is_mandate)
{
	return _cntlr_gen_tlv_steer_request(c, frm,
					    MAP_TLV_PROFILE2_STEERING_REQ,
					    bssid, steer_op_window,
					    num_sta, stas,
					    num_target_bssid, target_bssid,
					    btm_abridged, btm_disassoc_imminent,
					    btm_disassoc_timeout, mbo_reason,
					    is_mandate);
}

int cntlr_gen_tlv_assoc_ctrl_request(struct controller *c,
		struct cmdu_buff *frm, uint8_t *bssid,
		uint8_t assoc_cntl_mode, uint16_t assoc_timeout,
		uint8_t sta_nr, uint8_t *stalist)
{
	int i, ret, offset = 0;
	struct tlv *t;
	struct tlv_client_assoc_ctrl_request *data;

	if (!stalist)
		return -1;

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

	t->type = MAP_TLV_CLIENT_ASSOCIATION_CONTROL_REQUEST;
	offset = sizeof(*data);
	data = (struct tlv_client_assoc_ctrl_request *)t->data;

	memcpy(data->bssid, bssid, 6);
	data->control = assoc_cntl_mode;
	BUF_PUT_BE16(data->validity_period, assoc_timeout);
	data->num_sta = sta_nr;
	for (i = 0; i < sta_nr; i++) {
		memcpy(&t->data[offset], &stalist[i * 6], 6);
		offset += 6;
	}

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

	return 0;
}

int cntlr_gen_tlv_beacon_metrics_query(struct controller *c,
		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 (channel != 255 || !num_report || !report) {
		/* 17.2.27: If the value of Channel Number field is not set
		 *          to 255, Number of AP Channel Reports is set to 0.
		 */
		dbg("%s: no reports will be included!\n", __func__);

		/* data->num_report */
		*(data_p++) = 0;

		/* decrease by one report already counted for in sizeof (query) */
		t->len -= sizeof(struct ap_channel_report);

	} else {

		/* data->num_report */
		*(data_p++) = num_report;

		/* data->report */
		/* -1: one report always counted for in sizeof (query) */
		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 = sizeof(channel[]) */
			t->len += num_channel;
			/* (len + opclass) + channel[] */
			data_p += 2 + num_channel;
		}
	}

	/* Request elements */
	if (reporting_detail != 1 || !num_element || !element) {
		/* 17.2.27: If the value of Reporting Detail fields is not
		 *          set to 1, Number of element IDs is set to 0.
		 */
		dbg("%s: no element IDs will be included!\n", __func__);
		/* data->num_element */
		*(data_p++) = 0;
	} else {
		/* data->num_element */
		*(data_p++) = num_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 cntlr_gen_1905_link_metric_tlv(struct controller *c,
		struct cmdu_buff *frm)
{
	int ret;
	struct tlv *t;
	struct tlv_linkmetric_query *data;

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

	t->type = TLV_TYPE_LINK_METRIC_QUERY;
	t->len = sizeof(*data);
	data = (struct tlv_linkmetric_query *) t->data;
	data->nbr_type = LINKMETRIC_QUERY_NEIGHBOR_ALL;
	/* data->nbr_mac is not present because of
		default LINKMETRIC_QUERY_NEIGHBOR_ALL */
	data->query_type = LINKMETRIC_QUERY_TYPE_BOTH;

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

	return 0;
}

int cntlr_gen_ap_metric_query(struct controller *c,
		struct cmdu_buff *frm, uint8_t 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) {
		fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_ap_metrics_tlv(struct controller *c,
		struct cmdu_buff *frm, uint8_t *listbss)
{
	int ret, index;
	struct tlv *t;
	struct tlv_ap_metrics *data;

	struct netif_iface *ifc;
	struct wifi_bss_element *bss;

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

	t->type = MAP_TLV_AP_METRICS;
	t->len = sizeof(*data);
	data = (struct tlv_ap_metrics *) t->data;
	ifc = cntlr_find_iface(c, listbss);
	if (!ifc) {
		warn("Incorrect bssid!\n");
		return -1;
	}
	bss = ifc->bss;
	memcpy(data->bssid, bss->bssid, 6);
	data->channel_utilization = bss->ch_util;
	data->num_station = bss->num_stations;
	data->esp_ac = bss->esp_ac;

	/* Mandatory ESP Information field for BE */
	if (!(bss->esp_ac & ESP_AC_BE)) {
		dbg("|%s %d|: BE not set, forcing 1\n", __func__, __LINE__);
		/* Easy Mesh Spec 17.2.22: "This field shall be set to one" */
		data->esp_ac |= ESP_AC_BE;
	}
	memcpy(data->esp_be, bss->est_wmm_be, 3);

	/* Optional ESP Information Fields for BK, VO & VI */
	index = 0;
	if (bss->esp_ac & ESP_AC_BK) {
		memcpy(data->esp + index, bss->est_wmm_bk, 3);
		index += 3;
	}
	if (bss->esp_ac & ESP_AC_VO) {
		memcpy(data->esp + index, bss->est_wmm_vo, 3);
		index += 3;
	}
	if (bss->esp_ac & ESP_AC_VI)
		memcpy(data->esp + index, bss->est_wmm_vi, 3);

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

	return 0;
}

int cntlr_gen_tx_link_metric_tlv(struct controller *c,
		struct cmdu_buff *frm, struct node *n, uint8_t *neighbor_almac)
{
	int ret;
	struct tlv *t;
	struct tlv_tx_linkmetric *data;
	struct tx_link_info *info;

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

	t->type = TLV_TYPE_TRANSMITTER_LINK_METRIC;
	data = (struct tlv_tx_linkmetric *) t->data;
	memcpy(data->aladdr, n->almacaddr, 6);
	memcpy(data->neighbor_aladdr, neighbor_almac, 6);
	t->len = 12;

	info = (struct tx_link_info *)&data->link[0];
	memcpy(info, &n->tx_metrics, sizeof(struct tx_link_info));
	t->len += 29;

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

	return 0;
}

int cntlr_gen_rx_link_metric_tlv(struct controller *c,
		struct cmdu_buff *frm, struct node *n, uint8_t *neighbor_almac)
{
	int ret;
	struct tlv *t;
	struct tlv_rx_linkmetric *data;
	struct rx_link_info *info;

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

	t->type = TLV_TYPE_RECEIVER_LINK_METRIC;
	data = (struct tlv_rx_linkmetric *) t->data;
	memcpy(data->aladdr, n->almacaddr, 6);
	memcpy(data->neighbor_aladdr, neighbor_almac, 6);
	t->len = 12;

	info = (struct rx_link_info *)&data->link[0];
	memcpy(info, &n->rx_metrics, sizeof(struct rx_link_info));
	t->len += 23;

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

	return 0;
}

int cntlr_gen_comb_infra_metrics(struct controller *c,
		struct cmdu_buff *frm, uint8_t *bssid)
{
	struct node *n = NULL;
	int ret = 0;

	/* Add one AP Metrics TLV */
	ret = cntlr_gen_ap_metrics_tlv(c, frm, bssid);
	if (ret) {
		warn("%s |%d|: tlv gen error\n", __func__, __LINE__);
		return -1;
	}

	/* Add the 1905 Link Metric TLVs (downstream nodes only) */
	list_for_each_entry(n, &c->nodelist, list) {
		struct node *neighbor_node;

		if (hwaddr_is_zero(n->upstream_bssid))
			continue;

		neighbor_node = cntlr_find_node_with_bssid(c, n->upstream_bssid);
		if (!neighbor_node)
			continue;

		ret = cntlr_gen_tx_link_metric_tlv(c, frm, n, neighbor_node->almacaddr);
		if (ret) {
			warn("%s |%d|: tlv gen error\n", __func__, __LINE__);
			return -1;
		}
		ret = cntlr_gen_rx_link_metric_tlv(c, frm, n, neighbor_node->almacaddr);
		if (ret) {
			warn("%s |%d|: tlv gen error\n", __func__, __LINE__);
			return -1;
		}
	}

	return 0;
}

int cntlr_gen_sta_mac(struct controller *c,
		struct cmdu_buff *frm, uint8_t *sta)
{
	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, sta, 6);
	ret = cmdu_put_tlv(frm, t);
	if (ret) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_unassociated_sta_link_metrics(struct controller *c,
		struct cmdu_buff *frm, uint8_t opclass,
		uint8_t num_metrics, struct unassoc_sta_metric *metrics)
{
	int ret, i, j, num_sta;
	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++) {
		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) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_searched_role(struct controller *c, struct cmdu_buff *frm,
		uint8_t role)
{
	struct tlv *t;
	struct tlv_searched_role *data;
	int ret;

	t = cmdu_reserve_tlv(frm, 128);
	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) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cnltr_gen_searched_service(struct controller *c, struct cmdu_buff *frm,
		uint8_t service)
{
	struct tlv *t;
	int ret;

	t = cmdu_reserve_tlv(frm, 128);
	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) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_autoconf_freq_band(struct controller *c, struct cmdu_buff *frm,
		uint8_t band)
{
	struct tlv *t;
	struct tlv_autoconfig_band *data;
	int ret;

	t = cmdu_reserve_tlv(frm, 128);
	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) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_tlv_error_code(struct controller *c,
	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)) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		return -1;
	}

	return 0;
}

int cntlr_gen_channel_scan_req(struct controller *c, struct cmdu_buff *frm,
		struct scan_req_data *req_data)
{
	struct tlv *t;
	struct tlv_channel_scan_request *data;
	struct channel_scan_request_radio *radio_data;
	struct channel_scan_request_opclass *opclass_data;
	int num_channel;
	uint8_t *channel_data;
	int ret, offset;

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

	/* Define the TLV */
	t->type = MAP_TLV_CHANNEL_SCAN_REQ;
	data = (struct tlv_channel_scan_request *) t->data;
	if (req_data->is_fresh_scan)
		data->mode |= SCAN_REQUEST_FRESH_SCAN;

	data->num_radio = req_data->num_radio;
	offset = sizeof(*data);

	if (data->num_radio > SCAN_REQ_MAX_NUM_RADIO)
		return -1;

	for (int i = 0; i < data->num_radio; i++) {
		radio_data = (struct channel_scan_request_radio *)&t->data[offset];
		memcpy(radio_data->radio, req_data->radios[i].radio_mac, 6); /* radio id */

		if (req_data->is_fresh_scan)
			radio_data->num_opclass = req_data->radios[i].num_opclass;
		else
			/* If a Multi-AP Controller sends a Channel Scan Request
			 * to a Multi-AP Agent with the Perform Fresh Scan bit set
			 * to zero, it shall set the Number of Operating Classes
			 * field for each radio listed to zero.
			 */
			radio_data->num_opclass = 0;

		if (radio_data->num_opclass > SCAN_REQ_MAX_NUM_OPCLASS)
			return -1;
		offset += sizeof(*radio_data);

		for (int j = 0; j < radio_data->num_opclass; j++) {
			opclass_data = (struct channel_scan_request_opclass *) &t->data[offset];
			opclass_data->classid = req_data->radios[i].opclasses[j].classid;
			num_channel = req_data->radios[i].opclasses[j].num_channel;
			if (num_channel > SCAN_REQ_MAX_NUM_CHAN)
				return -1;
			opclass_data->num_channel = num_channel;
			offset += sizeof(*opclass_data);

			if (num_channel) {
				channel_data = (uint8_t *) &t->data[offset];
				memcpy(channel_data, req_data->radios[i].opclasses[j].channels, num_channel);
				offset += num_channel;
			}
		}
	}
	/* Update the TLV length */
	t->len = offset;

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

	return 0;
}

struct cmdu_buff *cntlr_gen_channel_sel_request(struct controller *c,
						uint8_t *agent,
						uint8_t *radio_id,
						struct wifi_radio_opclass *opclass)
{
	struct wifi_radio_opclass_channel *channel;
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass empty = {0};
	int opclass_num_offset, channel_num_offset;
	int opclass_num, channel_num;
	struct netif_radio *radio;
	struct radio_policy *rp;
	struct cmdu_buff *cmdu;
	int ch_idx, i, j, k;
	int ret, offset = 0;
	uint16_t mid = 0;
	struct tlv *t;

	/* Find radio and supported opclasses */
	radio = cntlr_find_radio(c, radio_id);
	if (!radio || !radio->radio_el)
		return NULL;

	cmdu = cmdu_alloc_simple(CMDU_CHANNEL_SELECTION_REQ, &mid);
	if (!cmdu)
		return NULL;

	memcpy(cmdu->origin, agent, 6);

	t = cmdu_reserve_tlv(cmdu, 1024);
	if (!t) {
		cmdu_free(cmdu);
		return NULL;
	}

	t->type = MAP_TLV_CHANNEL_PREFERENCE;

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

	if (!opclass) {
		memcpy(&empty, &radio->radio_el->supp_opclass, sizeof(empty));
		wifi_opclass_set_preferences(&empty, 0x0);
		opclass = &empty;
	}

	rp = cntlr_get_radio_policy(&c->cfg, radio_id);
	if (rp)
		wifi_apply_disallow_opclass(opclass, &rp->dis_opclass);

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

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

		if (wifi_opclass_id_same_preference(opclass, entry->id, &preference)) {
			if (((preference & CHANNEL_PREF_MASK) >> 4) == 15)
				continue;

			t->data[offset++] = entry->id;
			t->data[offset++] = 0;					/* k */
			t->data[offset++] = preference;
			opclass_num++;
			continue;
		}

		for (ch_idx = 0; ch_idx < entry->num_channel; ch_idx++) {
			channel = &entry->channel[ch_idx];
			bool added = false;

			/* check if this preference was already handled */
			for (j = 0; j < ch_idx; j++) {
				if (entry->channel[j].preference == channel->preference) {
					added = true;
					break;
				}
			}

			if (added || ((channel->preference & CHANNEL_PREF_MASK) >> 4) == 15)
				continue;

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

			/* reserve space for channel num offset, fill later */
			channel_num_offset = offset;
			t->data[offset++] = 0;						/* k */

			/* add the first (self) channel directly */
			t->data[offset++] = channel->channel;
			channel_num = 1;

			/* collect remaining channels with the same preference */
			for (k = ch_idx + 1; k < entry->num_channel; k++) {
				if (entry->channel[k].preference != channel->preference)
					continue;

				t->data[offset++] = entry->channel[k].channel;
				channel_num++;
			}

			t->data[offset++] = channel->preference;
			t->data[channel_num_offset] = channel_num;	/* k */

			opclass_num++;
		}
	}

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

	ret = cmdu_put_tlv(cmdu, t);
	if (ret) {
		warn("%s: error: cmdu_put_tlv()\n", __func__);
		cmdu_free(cmdu);
		return NULL;
	}

	return cmdu;
}

int cntlr_gen_txpower_limit(struct controller *c, struct cmdu_buff *frm,
		uint8_t *radio_id, uint8_t txpower_limit)
{
	int ret;
	struct tlv *t;
	struct tlv_txpower_limit *data;

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

	t->type = MAP_TLV_TRANSMIT_POWER_LIMIT;
	t->len = sizeof(*data);
	data = (struct tlv_txpower_limit *)t->data;

	memcpy(data->radio, radio_id, 6);
	data->limit = txpower_limit;

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

	return 0;
}

int cntlr_gen_cac_tlv(struct controller *c, struct cmdu_buff *frm,
		      uint8_t tlv_type, int num_data, struct cac_data *cac_data)
{
	int i, ret, offset = 0;
	struct tlv *t;

	if (!cac_data)
		return -1;

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

	t->type = tlv_type;
	t->data[offset++] = num_data;
	for (i = 0; i < num_data; i++) {
		uint8_t mode = 0x00;
		uint8_t cac_method, cac_action;

		memcpy(&t->data[offset], cac_data[i].radio, 6);
		offset+= 6;
		t->data[offset++] = cac_data[i].opclass;
		t->data[offset++] = cac_data[i].channel;
		cac_method = cac_data[i].cac_method;
		cac_action = cac_data[i].cac_action;

		if (tlv_type == MAP_TLV_CAC_REQ) {
			mode = (cac_method << 5) & CAC_REQUEST_METHOD;
			mode |= (cac_action << 3) & CAC_REQUEST_COMPLETE_ACTION;
			t->data[offset++] = mode;
		}
	}

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

	return 0;
}

int cntlr_gen_tlv_higher_layer_data(struct controller *c, 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;
	if (data)
		memcpy(t->data + 1, data, len);

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

	return 0;
}

#if (EASYMESH_VERSION >= 3)
/* WiFi EasyMesh Specification 17.2.70 - Service Prioritization Rule TLV */
int cntlr_gen_spr_tlv(struct controller *c,
                      struct cmdu_buff *frm,
                      uint32_t rule_id,
                      bool add,
                      uint8_t precedence,
                      uint8_t output,
                      bool always_match)
{
	trace("%s: --->\n", __func__);

	struct tlv *t;
	struct tlv_spr *tlv_data;
	const uint16_t max_tlv_len = sizeof(struct tlv_spr);

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

	t->type = MAP_TLV_SERVICE_PRIORITIZATION_RULE;
	t->len = sizeof(struct tlv_spr);

	tlv_data = (struct tlv_spr *)t->data;
	BUF_PUT_BE32(tlv_data->rule_id, rule_id);
	tlv_data->rule_flags = add ? SPR_FLAG_ADD_REMOVE : 0;
	tlv_data->precedence = precedence;
	tlv_data->output = output;
	tlv_data->match_flags = always_match ? SPR_FLAG_ALWAYS_MATCH : 0;

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

	return 0;
}

/* WiFi EasyMesh Specification 17.2.71 - DSCP Mapping Table TLV */
int cntlr_gen_dscp_mapping_table_tlv(struct controller *c,
                                     struct cmdu_buff *frm,
                                     uint8_t dscp_pcp[64])
{
	trace("%s: --->\n", __func__);

	struct tlv *t;
	struct tlv_dscp_pcp *tlv_data;
	const uint16_t max_tlv_len = sizeof(struct tlv_dscp_pcp);

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

	t->type = MAP_TLV_DSCP_MAPPING_TABLE;
	t->len = sizeof(*tlv_data);

	tlv_data = (struct tlv_dscp_pcp *)t->data;
	memcpy(tlv_data->dscp_pcp, dscp_pcp, sizeof(tlv_data->dscp_pcp));

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

	return 0;
}

#if (EASYMESH_VERSION >= 4)
/* WiFi EasyMesh Specification 17.2.92 - QoS Management Policy TLV */
static int _cntlr_gen_qos_management_policy_tlv(struct controller *c,
                                                struct cmdu_buff *frm,
                                                uint8_t num_mscs_disallowed,
                                                uint8_t mscs_disallowed[][6],
                                                uint8_t num_scs_disallowed,
                                                uint8_t scs_disallowed[][6])
{
	trace("%s: --->\n", __func__);

	struct tlv *t;
	struct sta_macaddr *lst;
	const uint16_t max_tlv_len =
		sizeof(uint8_t) +
		sizeof(uint8_t) * 6 * num_mscs_disallowed +
		sizeof(uint8_t) +
		sizeof(uint8_t) * 6 * num_scs_disallowed +
		sizeof(uint8_t) * 20 /* reserved */;
	size_t i;

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

	t->type = MAP_TLV_QOS_MANAGEMENT_POLICY;
	t->len = max_tlv_len;

	lst = (struct sta_macaddr *)t->data;
	lst->num = num_mscs_disallowed;
	for (i = 0; i < num_mscs_disallowed; ++i) {
		memcpy(lst->macaddr[i], mscs_disallowed[i], sizeof(uint8_t) * 6);
	}

	lst = (struct sta_macaddr *)(t->data +
	    sizeof(uint8_t) /* skip numMSCS */ +
	    sizeof(uint8_t) * 6 *
	    num_mscs_disallowed /* skip MSCS disallowed */);
	lst->num = num_scs_disallowed;
	for (i = 0; i < num_scs_disallowed; ++i) {
		memcpy(lst->macaddr[i], scs_disallowed[i], sizeof(uint8_t) * 6);
	}
	memcpy(lst->macaddr, scs_disallowed,
		   sizeof(uint8_t) * 6 * num_scs_disallowed);

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

	return 0;
}

int cntlr_gen_qos_management_policy_tlv(struct controller *c,
                                        struct cmdu_buff *frm)
{
	struct qos_mgmt_policy_elem *elem = NULL;
	size_t mscs_i = 0, scs_i = 0;
	int ret = -1;

	/* Fulfill MSCS disallowed/SCS disallowed MAC lists */
	uint8_t mscs_data[c->cfg.qos.policy.mscs_num][6];
	uint8_t scs_data[c->cfg.qos.policy.scs_num][6];

	list_for_each_entry(elem, &c->cfg.qos.policy.mscs_disallowed, list) {
		memcpy(mscs_data[mscs_i], elem->mac, sizeof(elem->mac));
		mscs_i++;
	}

	list_for_each_entry(elem, &c->cfg.qos.policy.scs_disallowed, list) {
		memcpy(scs_data[scs_i], elem->mac, sizeof(elem->mac));
		scs_i++;
	}

	ret = _cntlr_gen_qos_management_policy_tlv(c, frm,
				c->cfg.qos.policy.mscs_num, mscs_data,
				c->cfg.qos.policy.scs_num, scs_data);


	return ret;
}

/* WiFi EasyMesh Specification 17.2.93 - QoS Management Descriptor TLV */
int cntlr_gen_qos_management_desc_tlv(struct controller *c,
                                      struct cmdu_buff *frm,
                                      uint16_t qmid,
                                      uint8_t  bssid[6],
                                      uint8_t  sta_mac[6],
                                      uint8_t  desc[],
                                      uint32_t desc_size)
{
	trace("%s: --->\n", __func__);

	struct tlv *t;
	struct tlv_qos_mgmt_desc *tlv_data;
	const uint16_t max_tlv_len = sizeof(struct tlv_qos_mgmt_desc) + desc_size;

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

	t->type = MAP_TLV_QOS_MANAGEMENT_DESCRIPTOR;
	t->len = max_tlv_len;

	tlv_data = (struct tlv_qos_mgmt_desc *)t->data;
	BUF_PUT_BE32(tlv_data->qmid, qmid);
	memcpy(tlv_data->bssid, bssid, sizeof(tlv_data->bssid));
	memcpy(tlv_data->sta, sta_mac, sizeof(tlv_data->sta));
	memcpy(tlv_data->desc, desc, desc_size);

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

	return 0;
}

uint32_t tclas_elem_frame_len(struct tclas_frame_classifier *cls,
                              size_t *err_count)
{
	switch (cls->type)
	{
		case TCLAS_FRAME_CLASSIFIER_TYPE_IP:
		{
			struct tclas_type4 *type4 =
				(struct tclas_type4 *)&cls->params4;

			switch (type4->ip_version)
			{
				case 4:
				{
					return sizeof(cls->type) +
						   sizeof(type4->classifier_mask) +
						   sizeof(type4->ip_version) +
						   sizeof(type4->ipv4);
				}
				case 6:
				{
					return sizeof(cls->type) +
						   sizeof(type4->classifier_mask) +
						   sizeof(type4->ip_version) +
						   sizeof(type4->ipv6);
				}
				default:
				{
					(*err_count)++;
					return 0;
				}
			}
		}
		default:
		{
			(*err_count)++;
			return 0;
		}
	}
}

uint32_t tclas_elem_len(struct tclas_elem *cls, size_t *err_count)
{
	return sizeof(cls->user_priority) +
	       tclas_elem_frame_len(&cls->classifier, err_count);
}

int cntlr_gen_qos_tclas_frame_classifier_elem(
    struct tclas_frame_classifier *cls,
    struct tclas_frame_classifier *in)
{
	cls->type = in->type;

	switch (in->type)
	{
		case TCLAS_FRAME_CLASSIFIER_TYPE_IP:
		{
			struct tclas_type4 *in_type4 =
				(struct tclas_type4 *)
					&in->params4;
			struct tclas_type4 *out_type4 =
				(struct tclas_type4 *)
					&cls->params4;

			out_type4->classifier_mask = in_type4->classifier_mask;
			out_type4->ip_version = in_type4->ip_version;
			switch (in_type4->ip_version)
			{
				case 4:
				{
					memcpy(&out_type4->ipv4,
						   &in_type4->ipv4,
						   sizeof(struct tclas_type4_ipv4));
					BUF_PUT_BE32(out_type4->ipv4.src_ip,
					             in_type4->ipv4.src_ip);
					BUF_PUT_BE32(out_type4->ipv4.dst_ip, in_type4->ipv4.dst_ip);
					BUF_PUT_BE16(out_type4->ipv4.src_port,
					             in_type4->ipv4.src_port);
					BUF_PUT_BE16(out_type4->ipv4.dst_port,
					             in_type4->ipv4.dst_port);
					return 0;
				}
				case 6:
				{
					memcpy(&out_type4->ipv6,
						   &in_type4->ipv6,
						   sizeof(struct tclas_type4_ipv6));
					BUF_PUT_BE16(out_type4->ipv6.src_port,
					             in_type4->ipv6.src_port);
					BUF_PUT_BE16(out_type4->ipv6.dst_port,
					             in_type4->ipv6.dst_port);
					return 0;
				}
				default:
				{
					break;
				}
			}
			break;
		}
		default:
		{
			break;
		}
	}

	return -1;
}

int cntlr_gen_qos_tclas_elem(uint8_t *data,
                             struct tclas_elem *in_elem,
                             uint32_t elem_len)
{
	struct ie *ieee_elem = (struct ie *)data;
	struct tclas_elem *out_elem = (struct tclas_elem *)
		ieee_elem->data;

	ieee_elem->eid = IE_TCLAS;
	ieee_elem->len = elem_len;

	out_elem->user_priority = in_elem->user_priority;

	return cntlr_gen_qos_tclas_frame_classifier_elem(&out_elem->classifier,
	                                                 &in_elem->classifier);
}

/*
 * Part 11: Wireless LAN Medium Access Control (MAC) and Physical Layer (PHY)
 * Specifications 9.4.2.121 SCS Descriptor element
 */
uint8_t *cntlr_gen_qos_scs_desc(struct controller *c,
                                uint8_t request_type,
                                struct scs_desc_usr *in,
                                uint32_t *out_len)
{
	uint8_t  desc_len;
	uint8_t *desc;
	struct ie *elem;
	struct scs_desc *scs_desc;
	size_t err_count = 0;

	desc_len = sizeof(scs_desc->scsid) + sizeof(scs_desc->request_type);

	switch (request_type)
	{
		case SCS_DESC_REQ_TYPE_ADD:
		case SCS_DESC_REQ_TYPE_CHANGE:
		{
			size_t i;

			desc_len += sizeof(uint8_t) * 3;/* intra_access_category_priority */

			for (i = 0; i < in->tclas_elems_count; ++i) {
				desc_len += tclas_elem_len(&in->tclas_elems[i], &err_count);
			}

			if (in->processing_present) {
				desc_len += sizeof(struct ie);
				desc_len += sizeof(uint8_t);
			}
			break;
		}
		case SCS_DESC_REQ_TYPE_REMOVE:
		default:
		{
			break;
		}
	}

	if (err_count != 0)
		return NULL;

	desc = calloc(sizeof(struct ie) + desc_len, sizeof(uint8_t));
	if (desc == NULL)
		return NULL;

	elem = (struct ie *)desc;

	elem->eid = IE_SCS_DESC;
	elem->len = desc_len;

	scs_desc = (struct scs_desc *)elem->data;
	scs_desc->scsid = in->scsid;
	scs_desc->request_type = request_type;

	switch (request_type)
	{
		case SCS_DESC_REQ_TYPE_ADD:
		case SCS_DESC_REQ_TYPE_CHANGE:
		{
			struct ie *proc_elem;
			uint8_t *tmp = (uint8_t *)scs_desc->add_change.hdr.tclas_elems;
			size_t   i;
			uint32_t prio;

			BUF_PUT_BE32(
				prio,
				in->intra_access_category_priority);
			scs_desc->add_change.hdr.intra_access_category_priority = prio;
			for (i = 0; i < in->tclas_elems_count; ++i) {
				uint32_t tmp_len = tclas_elem_len(&in->tclas_elems[i],
				                                  &err_count);

				cntlr_gen_qos_tclas_elem(tmp, &in->tclas_elems[i], tmp_len);
				tmp += tmp_len;
			}

			if (in->processing_present) {
				/* After all elements */
				proc_elem = (struct ie *)tmp;
				proc_elem->eid = IE_TCLAS_PROCESSING;
				proc_elem->len = sizeof(uint8_t);
				proc_elem->data[0] = in->processing;
			}
			break;
		}
		case SCS_DESC_REQ_TYPE_REMOVE:
		default:
		{
			break;
		}
	}

	*out_len = sizeof(struct ie) + desc_len;

	return desc;
}

/*
 * Part 11: Wireless LAN Medium Access Control (MAC) and Physical Layer (PHY)
 * Specifications 9.4.2.243 SCS Descriptor element
 */
uint8_t *cntlr_gen_qos_mscs_desc(struct controller *c,
                                 uint8_t request_type,
                                 struct mscs_desc_usr *in,
                                 uint32_t *out_len)
{
	uint8_t  desc_len;
	uint8_t *desc;
	struct ext_ie *elem;
	struct mscs_desc *mscs_desc;
	size_t err_count = 0;

	desc_len = sizeof(mscs_desc->request_type) +
	           sizeof(mscs_desc->up_bitmap) +
	           sizeof(mscs_desc->up_limit) +
	           sizeof(mscs_desc->stream_timeout);

	switch (request_type)
	{
		case SCS_DESC_REQ_TYPE_ADD:
		case SCS_DESC_REQ_TYPE_CHANGE:
		{
			size_t i;

			for (i = 0; i < in->tclas_elems_count; ++i) {
				desc_len += tclas_elem_len(&in->tclas_elems[i], &err_count);
			}

			break;
		}
		case SCS_DESC_REQ_TYPE_REMOVE:
		default:
		{
			break;
		}
	}

	if (err_count != 0)
		return NULL;

	desc = calloc(sizeof(struct ext_ie) + desc_len,
				  sizeof(uint8_t));
	if (desc == NULL)
		return NULL;

	elem = (struct ext_ie *)desc;

	elem->eid = IE_EXT;
	elem->eid_ext = IE_EXT_MSCS_DESC;
	elem->len = desc_len;

	mscs_desc = (struct mscs_desc *)elem->data;
	mscs_desc->request_type = request_type;
	mscs_desc->up_bitmap = in->up_bitmap;
	mscs_desc->up_limit = in->up_limit;
	BUF_PUT_BE32(mscs_desc->stream_timeout, in->stream_timeout);

	switch (request_type)
	{
		case SCS_DESC_REQ_TYPE_ADD:
		case SCS_DESC_REQ_TYPE_CHANGE:
		{
			uint8_t *tmp = (uint8_t *)mscs_desc->tclas_elems;
			size_t   i;

			for (i = 0; i < in->tclas_elems_count; ++i) {
				uint32_t tmp_len = tclas_elem_len(&in->tclas_elems[i],
				                                  &err_count);

				cntlr_gen_qos_tclas_elem(tmp, &in->tclas_elems[i], tmp_len);
				tmp += tmp_len;
			}

			break;
		}
		case SCS_DESC_REQ_TYPE_REMOVE:
		default:
		{
			break;
		}
	}

	*out_len = sizeof(struct ext_ie) + desc_len;

	return desc;
}

/*
 * WiFi QoS Management Specification Version 2.0
 * 5.4 QoS Management attribute
 */
uint32_t cntlr_gen_qos_mgmt_attr_desc_len(struct controller *c,
                                          struct qos_mgmt_attr_usr *in,
                                          size_t *err_count)
{
	uint32_t desc_len;

	desc_len = sizeof(struct ie);

	switch (in->attribute_id)
	{
		case QOS_MGMT_ATTR_ID_PORT_RANGE:
		{
			desc_len += sizeof(in->port_range);
			break;
		}
		case QOS_MGMT_ATTR_ID_DSCP_POLICY:
		{
			desc_len += sizeof(in->dscp_policy);
			break;
		}
		case QOS_MGMT_ATTR_ID_TCLAS:
		{
			desc_len += tclas_elem_frame_len(&in->tclas.classifier, err_count);
			break;
		}
		case QOS_MGMT_ATTR_ID_DOMAIN_NAME:
		{
			desc_len += in->domain_name.domain_name_len;
			break;
		}
		default:
		{
			(*err_count)++;
			break;
		}
	}

	return desc_len;
}

/*
 * WiFi QoS Management Specification Version 2.0
 * 5.3 QoS Management element
 */
uint8_t *cntlr_gen_qos_mgmt_elem_desc(struct controller *c,
                                      uint8_t request_type,
                                      struct qos_mgmt_attr_usr *in,
                                      uint32_t *out_len)
{
	uint8_t *data;
	uint32_t desc_len;
	uint32_t inner_len;
	uint8_t oui[3] = WFA_OUI;
	struct qos_mgmt_ie *ve;
	struct qos_mgmt_attr_usr *attr;
	size_t err_count = 0;

	inner_len = cntlr_gen_qos_mgmt_attr_desc_len(c, in, &err_count);
	if (err_count != 0) {
		return NULL;
	}

	desc_len = sizeof(struct qos_mgmt_ie) +
			   inner_len;

	data = calloc(desc_len, sizeof(uint8_t));
	if (data == NULL)
		return NULL;

	ve = (struct qos_mgmt_ie *)data;
	ve->hdr.eid = IE_VEND_SPEC;
	ve->hdr.len = inner_len;
	memcpy(&ve->oui, oui, sizeof(oui));
	ve->oui_type = WFA_QOS_MGMT_OUI_TYPE;

	attr = (struct qos_mgmt_attr_usr *)ve->data;

	switch (in->attribute_id)
	{
		case QOS_MGMT_ATTR_ID_PORT_RANGE:
		{
			BUF_PUT_BE16(attr->port_range.start_port,
			             in->port_range.start_port);
			BUF_PUT_BE16(attr->port_range.end_port, in->port_range.end_port);
			break;
		}
		case QOS_MGMT_ATTR_ID_DSCP_POLICY:
		{
			attr->dscp_policy.dscp_policy_id = in->dscp_policy.dscp_policy_id;
			attr->dscp_policy.request_type = in->dscp_policy.request_type;
			attr->dscp_policy.dscp = in->dscp_policy.dscp;
			break;
		}
		case QOS_MGMT_ATTR_ID_TCLAS:
		{
			cntlr_gen_qos_tclas_frame_classifier_elem(
				(struct tclas_frame_classifier *)&attr->tclas,
				&in->tclas.classifier);
			break;
		}
		case QOS_MGMT_ATTR_ID_DOMAIN_NAME:
		{
			memcpy(&attr->domain_name,
				   in->domain_name.domain_name,
				   in->domain_name.domain_name_len);
			break;
		}
		default:
		{
			break;
		}
	}

	return data;
}
#endif /* EASYMESH_VERSION >= 4 */
#endif /* EASYMESH_VERSION >= 3 */

#if (EASYMESH_VERSION > 2)
int cntlr_gen_bss_config_response_tlv(struct controller *c, struct cmdu_buff *cmdu)
{
	struct tlv *tlv;
	int data_len;

	// todo:
	/* One or more JSON encoded DPP Configuration Object attributes */
	const char *data =
	"{\
		\"wi-fi_tech\": \"infra\",\
		\"discovery\": {\
			\"ssid\": \"mywifi\"\
		},\
		\"cred\": {\
			\"akm\": \"dpp\",\
			\"signedConnector\": \"eyJ0eXAiOiJkcHBDb24iLCJraWQiOiJrTWNlZ0RCUG1OWlZha0FzQlpPek9vQ3N2UWprcl9uRUFwOXVGLUVEbVZFIiwi\",\
			\"csign\": {\
				\"kty\": \"EC\",\
				\"crv\": \"P-256\",\
				\"x\": \"MKBCTNIcKUSDii11ySs3526iDZ8AiTo7Tu6KPAqv7D4\",\
				\"y\": \"4Etl6SRW2YiLUrN5vfvVHuhp7x8PxltmWWlbbM4IFyM\",\
				\"kid\": \"kMcegDBPmNZVakAsBZOzOoCsvQjkr_nEAp9uF-EDmVE\"\
			},\
			\"ppKey\": {\
				\"kty\": \"EC\",\
				\"crv\": \"P-256\",\
				\"x\": \"XX_ZuJR9nMDSb54C_okhGiJ7OjCZOlWOU9m8zAxgUrU\",\
				\"y\": \"Fekm5hyGii80amM_REV5sTOG3-sl1H6MDpZ8TSKnb7c\"\
			}\
		}\
	}";

	data_len = strlen(data);

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

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

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

	return 0;
}

#ifdef USE_LIBDPP
int cntlr_gen_dpp_cce_indication_tlv(struct controller *c,
		struct cmdu_buff *frm, bool cce_advertise)
{
	struct tlv *t;
	struct tlv_dpp_cce *data;

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

	t->type = MAP_TLV_DPP_CCE_INDICATION;
	t->len = sizeof(*data);
	data = (struct tlv_dpp_cce *)t->data;

	data->enable = cce_advertise;
	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}

int cntlr_gen_dpp_message_tlv(struct controller *c, struct cmdu_buff *frm,
				uint16_t framelen, uint8_t *frame)
{
	struct tlv *t;

	t = cmdu_reserve_tlv(frm, 4700);
	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;
}

int cntlr_gen_1905_encap_dpp_tlv(struct controller *c, struct cmdu_buff *frm,
				 uint8_t *enrollee, uint8_t frametype,
				 uint16_t framelen, uint8_t *frame)
{
	struct tlv *t;
	int offset = 0;
	uint8_t flags = 0x0;
	bool is_gas = !!FRAME_IS_DPP_GAS_FRAME(&frametype);

	t = cmdu_reserve_tlv(frm, 4700);
	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;

	return 0;
}

int cntlr_gen_chirp_value_tlv(struct controller *c, struct cmdu_buff *frm,
			      uint8_t *enrollee, bool hash_validity,
			      uint16_t hashlen, uint8_t *hash)
{
	struct tlv *t;
	int offset = 0;
	uint8_t flags = 0x00;

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

	t->type = MAP_TLV_DPP_CHIRP_VALUE;

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

	flags |= hash_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;
}
#endif /* USE_LIBDPP */

int cntlr_gen_device_1905_layer_security_cap(struct controller *c,
		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 cntlr_gen_dpp_bootstrapping_uri_notif(struct controller *c,
		struct cmdu_buff *frm, uint8_t *radio, uint8_t *bssid,
		uint8_t *bsta, int uri_len, char *dpp_uri)
{
	struct tlv *t;
	struct tlv_dpp_uri_bootstrap *data;
	int reserve_len = uri_len + 18;

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

	t->type = MAP_TLV_DPP_BOOTSTRAP_URI_NOTIFICATION;
	t->len = reserve_len;
	data = (struct tlv_dpp_uri_bootstrap *)t->data;

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

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

	return 0;
}

#if (EASYMESH_VERSION >= 4)
int cntlr_gen_cntlr_capability(struct controller *c, struct cmdu_buff *frm, uint8_t caps)
{
	struct tlv *t;
	struct tlv_controller_cap *data;

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

	t->type = MAP_TLV_CONTROLLER_CAPS;
	data = (struct tlv_controller_cap *)t->data;

	data->flag = caps;

	t->len = sizeof(*data);
	if (cmdu_put_tlv(frm, t))
		return -1;

	return 0;
}
#endif

int cntlr_gen_agent_list_tlv(struct controller *c, struct cmdu_buff *frm, uint8_t security)
{
	trace("%s: --->\n", __func__);

	int ret;
	struct tlv *t;
	struct tlv_agent_list *tlv_data;
	struct node *n = NULL;
	int i;
	const uint16_t max_tlv_len = 512;

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

	tlv_data = (struct tlv_agent_list *)t->data;
	tlv_data->num_agent = c->num_nodes;

	dbg("num_agent = %d\n", tlv_data->num_agent);

	t->type = MAP_TLV_AGENT_LIST;
	/* t->len = 1 + num_agents * (6 + 1 + 1) bytes */
	t->len = sizeof(tlv_data->num_agent) +
		 tlv_data->num_agent * sizeof(tlv_data->agent[0]);

	if (t->len > max_tlv_len)
		return -1;

	i = 0;
	list_for_each_entry(n, &c->nodelist, list) {
		dbg("\tagent[%d]:\n", i);

		/* node aladdr */
		memcpy(tlv_data->agent[i].aladdr, n->almacaddr, 6);
		dbg("\t\tagent_id: " MACFMT "\n", MAC2STR(tlv_data->agent[i].aladdr));

		/* map profile */
		tlv_data->agent[i].profile = n->map_profile;
		dbg("\t\tprofile: %d\n", tlv_data->agent[i].profile);

		/* TODO: Here we need to fill the security */
		tlv_data->agent[i].security = security;
		dbg("\t\tsecurity: %d\n", tlv_data->agent[i].security);

		++i;
	}

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

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

#if (EASYMESH_VERSION >= 6)
int cntlr_gen_ap_mld_config(struct controller *c, struct cmdu_buff *frm,
			    struct node *n, struct wifi_wifi7_capabilities *caps)
{
	trace("%s: --->\n", __func__);

	int ret;
	struct tlv *t;
	struct tlv_ap_mld_config *tlv_data;
	struct mld_credential *mld = NULL;
	int len = 0;

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

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

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

	tlv_data->num_mlds = c->cfg.num_mlds;
	len++;

	list_for_each_entry(mld, &c->cfg.mldlist, list) {
		uint8_t ssidlen;
		struct iface_credential *ap = NULL;
		uint8_t *affiliated_aps;
		uint8_t caps_flag = 0;

		ssidlen = strlen((char *)mld->cred.ssid);

		t->data[len++] = 0; /* AP_MLD_MAC_Addr_Valid */
		t->data[len++] = ssidlen;
		memcpy(&t->data[len], (char *)mld->cred.ssid, ssidlen);
		len += ssidlen;
		len += 6; /* skip AP_MLD_MAC_Addr */

		if (caps->str)
			caps_flag |= AP_MLD_CONFIG_STR;
		if (caps->nstr)
			caps_flag |= AP_MLD_CONFIG_NSTR;
		if (caps->emlsr)
			caps_flag |= AP_MLD_CONFIG_EMLSR;
		if (caps->emlmr)
			caps_flag |= AP_MLD_CONFIG_EMLMR;
		t->data[len++] = caps_flag;

		len += 20; /* reserved */

		affiliated_aps = &t->data[len++];
		*affiliated_aps = 0;
		list_for_each_entry(ap, &c->cfg.aplist, list) {
			struct netif_radio *r2;

			if (ap->mld_id != mld->id)
				continue;

			r2 = cntlr_find_radio_in_node_by_band(c, n, ap->band);
			if (!r2) {
				dbg("%s: radio for band:%d not known\n",
				     __func__, ap->band);
				continue;
			}
			t->data[len++] = 0; /* Affiliated_AP_MAC_Addr_Valid */
			memcpy(&t->data[len], r2->radio_el->macaddr, 6);

			len += 6;
			/* skip Affiliated_AP_MAC_Addr */
			len += 6;
			t->data[len++] = mld->id; /* link ID */
			len += 18; /* reserved */
			(*affiliated_aps)++;
		}
	}

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

	return 0;
}

int cntlr_gen_bsta_mld_config(struct controller *c, struct cmdu_buff *frm,
			      struct node *n,
			      struct wifi_wifi7_capabilities *caps)
{
	trace("%s: --->\n", __func__);

	struct tlv_bsta_mld_config *tlv_data;
	struct mld_credential *mld = NULL;
	uint8_t *affiliated_bstas;
	uint8_t caps_flag = 0;
	struct tlv *t;
	int len = 0;
	int ret;

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

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

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

	tlv_data->flag = 0; /* bSTA/AP_MLD_MAC_Addr_Valid */
	len++;

	len += 6; /* bSTA_MLD_MAC_Addr_Valid */
	len += 6; /* AP_MLD_MAC_Addr_Valid */

	tlv_data->flag2 = 0;
	len++; /* MLD supported Operation Mode */

	len += 17; /* reserved */
	affiliated_bstas = &t->data[len++];
	*affiliated_bstas = 0;

	list_for_each_entry(mld, &c->cfg.mldlist, list) {
		struct iface_credential *ap = NULL;

		if (!(mld->multi_ap & 0x01))
			continue;

		list_for_each_entry(ap, &c->cfg.aplist, list) {
			struct netif_radio *r2;

			if (ap->mld_id != mld->id)
				continue;

			r2 = cntlr_find_radio_in_node_by_band(c, n, ap->band);
			if (!r2) {
				dbg("%s: radio for band:%d not known\n",
				     __func__, ap->band);
				continue;
			}

			t->data[len++] = 0; /* Affiliated_bSTA_MAC_Addr_Valid */

			memcpy(&t->data[len], r2->radio_el->macaddr, 6);
			len += 6;

			len += 6; /* skip Affilated_bSTA_MAC_Addr */
			len += 19; /* reserved */
			(*affiliated_bstas)++;
		}
	}

	if ((*affiliated_bstas) > 0) {
		if (caps->str)
			caps_flag |= BSTA_MLD_CONFIG_STR;
		if (caps->nstr)
			caps_flag |= BSTA_MLD_CONFIG_NSTR;
		if (caps->emlsr)
			caps_flag |= BSTA_MLD_CONFIG_EMLSR;
		if (caps->emlmr)
			caps_flag |= BSTA_MLD_CONFIG_EMLMR;
		tlv_data->flag2 = caps_flag; /* TODO: base on pair rather than radio */
	} else {
		/* no bSTA MLD(s) to configure */
		return -1;
	}

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

	return 0;
}

int cntlr_gen_eht_operations(struct controller *c, struct cmdu_buff *frm,
			     struct node *n)
{
	struct netif_radio *r = NULL;
	uint32_t precalc_len = 1;
	int num_radio = 0;
	int offset = 0;
	struct tlv *t;
	int ret = 0;

	list_for_each_entry(r, &n->radiolist, list) {
		struct netif_iface *ap = NULL;

		precalc_len += (6 + 1 + 25);
		num_radio++;
		list_for_each_entry(ap, &r->iflist, list)
			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; /* reserved */

	t->data[offset++] = num_radio; /* num radio */

	list_for_each_entry(r, &n->radiolist, list) {
		struct radio_policy *rp = NULL;
		struct netif_iface *ap = NULL;
		uint8_t *num_bss;

		rp = cntlr_get_radio_policy(&c->cfg, r->radio_el->macaddr);
		if (!rp)
			return -1;

		memcpy(&t->data[offset], r->radio_el->macaddr, 6);
		offset += 6; /* ruid */

		num_bss = &t->data[offset++]; /* num bss */
		*num_bss = 0;

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

			(*num_bss)++;

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

			t->data[offset++] = DISABLED_SUBCHANNEL_VALID; /* flag */
			offset += 4 + 1 + 1 + 1; /* mcs nss + ctrl + ccfs0 + ccfs1 */

			memcpy(&t->data[offset], &rp->punct_bitmap, 2);
			offset += 2; /* disabled subchannel bitmap */

			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 ret;
}

int cntlr_gen_bss_advanced_config_tlv(struct controller *c, struct cmdu_buff *frm,
				       struct node *n)
{
	struct iface_credential *ap = NULL;
	struct netif_radio *r = NULL;
	uint8_t *ptr, *num_radio_ptr;
	struct tlv *t;
	int ret;

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

	t->type = MAP_TLV_BSS_ADVANCED_CONFIG;
	ptr = t->data;

	num_radio_ptr = ptr++;
	*num_radio_ptr = 0;

	list_for_each_entry(r, &n->radiolist, list) {
		uint8_t *num_bss_ptr;
		uint8_t idx = 0;

		memcpy(ptr, r->radio_el->macaddr, 6);
		ptr += 6;

		num_bss_ptr = ptr++;
		*num_bss_ptr = 0;

		list_for_each_entry(ap, &c->cfg.aplist, list) {
			struct bss_adv_config_bss *entry;

			if (ap->band != r->radio_el->band)
				continue;

			entry = (struct bss_adv_config_bss *)ptr;
			memset(entry, 0, sizeof(*entry));
			entry->bss_index = idx++;
			entry->flags = ap->hidden ? 0 : BSS_ADV_CONFIG_SSID_ADVERTISEMENT;
			(*num_bss_ptr)++;
			ptr += sizeof(*entry);
		}

		(*num_radio_ptr)++;
	}

	t->len = ptr - t->data;

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

	return 0;
}

int cntlr_gen_rsn_parameters_config_tlv(struct controller *c,
					 struct cmdu_buff *frm,
					 struct node *n)
{
	const uint8_t owe_rsn_ie[] = {
				0x30, 0x1A,
				0x01, 0x00,
				0x00, 0x0F, 0xAC, 0x04,	/* group cipher */
				0x01, 0x00,
				0x00, 0x0F, 0xAC, 0x04,	/* pairwise cipher */
				0x01, 0x00,
				0x00, 0x0F, 0xAC, 0x12,	/* akm */
				0xE8, 0x00,
				0x00, 0x00,
				0x00, 0x0F, 0xAC, 0x06	/* group mgmt cipher */
	};
	struct iface_credential *ap = NULL;
	struct netif_radio *r = NULL;
	uint8_t *num_radio_ptr;
	bool has_owe = false;
	struct tlv *t;
	uint8_t *ptr;
	int ret;

	list_for_each_entry(ap, &c->cfg.aplist, list) {
		if (ap->sec == BIT(WIFI_SECURITY_OWE)) {
			has_owe = true;
			break;
		}
	}

	if (!has_owe)
		return 0;

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

	t->type = MAP_TLV_RSN_PARAMETERS_CONFIG;
	ptr = t->data;

	num_radio_ptr = ptr++;
	*num_radio_ptr = 0;

	list_for_each_entry(r, &n->radiolist, list) {
		uint8_t *num_bss_ptr;
		uint8_t idx = 0;
		bool radio_has_owe = false;

		list_for_each_entry(ap, &c->cfg.aplist, list) {
			if (ap->band == r->radio_el->band &&
			    ap->sec == BIT(WIFI_SECURITY_OWE)) {
				radio_has_owe = true;
				break;
			}
		}

		if (!radio_has_owe)
			continue;

		memcpy(ptr, r->radio_el->macaddr, 6);
		ptr += 6;

		num_bss_ptr = ptr++;
		*num_bss_ptr = 0;

		list_for_each_entry(ap, &c->cfg.aplist, list) {
			if (ap->band != r->radio_el->band)
				continue;

			if (ap->sec != BIT(WIFI_SECURITY_OWE)) {
				idx++;
				continue;
			}

			/* bssid not known. Set zero */
			memset(ptr, 0, 6);
			ptr += 6;

			*ptr++ = idx++;

			BUF_PUT_BE16(*(uint16_t *)ptr, sizeof(owe_rsn_ie));
			ptr += 2;

			memcpy(ptr, owe_rsn_ie, sizeof(owe_rsn_ie));
			ptr += sizeof(owe_rsn_ie);

			/* rsvd1[16] */
			memset(ptr, 0, 16);
			ptr += 16;

			(*num_bss_ptr)++;
		}

		(*num_radio_ptr)++;
	}

	t->len = ptr - t->data;

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

	return 0;
}
#endif
