/*
 * agent_cmdu.c - cmdu 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_cmdu.h"

#include <1905_tlvs.h>
#include <cmdu.h>
#include <wifidefs.h>
#ifdef USE_LIBDPP
#include <dpp_api.h>
#endif
#include <easy/utils.h>
#include <easymesh.h>
#include <errno.h>
#include <libubox/list.h>
#include <map_module.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "agent.h"
#include "agent_map.h"
#include "agent_tlv.h"
#include "config.h"
#if (EASYMESH_VERSION >= 6)
#include "mld.h"
#endif
#include "utils/1905_ubus.h"
#include "utils/debug.h"
#include "utils/utils.h"
#include "wifi.h"
#include "wifi_opclass.h"
#include "wifi_scanresults.h"


struct cmdu_buff *agent_gen_ap_autoconfig_search(struct agent *a,
		uint8_t ieee1905band, uint8_t profile)
{
	uint8_t origin[6] = {0x01, 0x80, 0xc2, 0x00, 0x00, 0x13};
	struct cmdu_buff *frm = NULL;
	uint16_t mid = 0;
	int ret = 0;

	frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH, &mid);
	if (!frm) {
		agnt_dbg(LOG_APCFG, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	CMDU_SET_RELAY_MCAST(frm->cdata);

	ret = agent_gen_al_mac(a, frm, a->almac);
	if (ret)
		goto out;

	ret = agent_gen_searched_role(a, frm, IEEE80211_ROLE_REGISTRAR);
	if (ret)
		goto out;

	ret = agent_gen_autoconf_freq_band(a, frm, ieee1905band);
	if (ret)
		goto out;

	ret = agent_gen_supported_service(a, frm);
	if (ret)
		goto out;

	ret = agent_gen_searched_service(a, frm, SEARCHED_SERVICE_MULTIAP_CONTROLLER);
	if (ret)
		goto out;

	ret = agent_gen_map_profile(a, frm, a->cfg.map_profile);
	if (ret)
		goto out;

#if (EASYMESH_VERSION > 2)
#ifdef USE_LIBDPP
	if (a->cfg.map_profile > 2 /* && !configured (?) */) {
		uint8_t *hash;
		uint16_t len = 0;

		hash = dpp_get_bootstrap_pubkey_hash(a->own_peer->own_bi, &len);
		if (hash) {
			ret = agent_gen_dpp_chirp_value(a, frm, a->almac, 1,
							len, hash);
			if (ret)
				goto out;
		}
	}
#endif
#endif

	memcpy(frm->origin, origin, 6);
	cmdu_put_eom(frm);

	agnt_dbg(LOG_APCFG, "%s: band = 0x%x\n", __func__, ieee1905band);
	return frm;
out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_ap_autoconfig_wsc(struct agent *a, uint8_t *origin,
		struct wifi_radio_element *radio)
{
	agnt_trace(LOG_APCFG, "agent: %s: --->\n", __func__);
	struct cmdu_buff *frm = NULL;
	uint16_t mid = 0;
	int ret = 0;


	frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_WSC, &mid);
	if (!frm) {
		agnt_dbg(LOG_APCFG, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, origin, 6);

	ret = agent_gen_wsc(a, frm, radio);
	if (ret)
		goto out;

	ret = agent_gen_ap_radio_adv_cap(a, frm, radio);
	if (ret)
		goto out;

	ret = agent_gen_ap_radio_basic_cap(a, frm, radio);
	if (ret)
		goto out;

	ret = agent_gen_profile2_ap_cap(a, frm);
	if (ret)
		goto out;

	agnt_trace(LOG_APCFG, "build ap autoconfig wsc!\n");
	cmdu_put_eom(frm);

	return frm;
out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_higher_layer_response(struct agent *a,
		struct cmdu_buff *rx_cmdu)
{
	struct cmdu_buff *resp;

	/* query i1905d base CMDU */
	resp = ieee1905_ubus_buildcmdu(a->ubus_ctx, a->i1905obj_id,
				       CMDU_TYPE_HIGHER_LAYER_RESPONSE,
				       rx_cmdu->dev_ifname, 0, NULL);
	if (!resp) {
		dbg("No response from stack when generating 0x%04x\n",
				CMDU_TYPE_HIGHER_LAYER_RESPONSE);
		return NULL;
	}

	cmdu_set_mid(resp, cmdu_get_mid(rx_cmdu));
	memcpy(resp->origin, rx_cmdu->origin, 6);
	cmdu_put_eom(resp);
	return resp;
}

struct cmdu_buff *agent_gen_ap_metrics_response(struct agent *a,
		struct cmdu_buff *rx_cmdu, struct node *n)
{
	struct cmdu_buff *frm;
	int ret;
	int i = 0;
	uint16_t mid = 0;
	struct tlv *tv[AP_METRICS_QUERY_NUM_OF_TLV_TYPES][TLV_MAXNUM] = {0};

	/* Parse AP Metric Query TLV */
	ret = map_cmdu_validate_parse(rx_cmdu, tv, ARRAY_SIZE(tv), n->map_profile);
	if (!ret) {
		dbg("%s: map_cmdu_validate_parse failed, err = (%d) '%s'\n", __func__,
		    map_error, map_strerror(map_error));
		return NULL;
	}
	if (!tv[AP_METRICS_QUERY_AP_METRIC_QUERY_IDX][0])
		return NULL;

	/* Allocate the cmdu */
	frm = cmdu_alloc_frame(AGENT_MAX_CMDU);
	if (!frm) {
		trace("%s: -ENOMEM\n", __func__);
		return NULL;
	}
	/* Define the cmdu */
	mid = cmdu_get_mid(rx_cmdu);
	cmdu_set_type(frm, CMDU_AP_METRICS_RESPONSE);
	cmdu_set_mid(frm, mid);

	memcpy(frm->origin, rx_cmdu->origin, 6);

	/* AP Radio Identifier TLV */
	while (i < TLV_MAXNUM && tv[AP_METRICS_QUERY_AP_RADIO_IDENTIFIER_IDX][i]) {
		struct tlv_ap_radio_identifier *p = (struct tlv_ap_radio_identifier *)
			tv[AP_METRICS_QUERY_AP_RADIO_IDENTIFIER_IDX][i++]->data;
		struct wifi_radio_element *re;

		re = agent_get_radio(a, p->radio);
		if (re) {
			/* Radio Metrics TLV */
			ret = agent_gen_radio_metrics(a, frm, re);
			if (ret)
				goto out;
		}
	}

	/* AP Metric Query TLV */
	if (tv[AP_METRICS_QUERY_AP_METRIC_QUERY_IDX][0]) {
		struct wifi_radio_element *re;
		struct wifi_bss_element *bss;
		struct tlv_ap_metric_query *query =
			(struct tlv_ap_metric_query *) (tv[AP_METRICS_QUERY_AP_METRIC_QUERY_IDX][0]->data);
		uint8_t *p = (uint8_t *) (tv[AP_METRICS_QUERY_AP_METRIC_QUERY_IDX][0]->data);
		int offset = 0;
		int j;

		if (query->num_bss <= 0) {
			dbg("%s: skip AP metrics response, num_bss = %d\n",
			    __func__, query->num_bss);
			goto out;
		}

		offset += 1; /* num_bss */
		for (j = 0; j < query->num_bss; j++) {
			struct agent_config_radio *rcfg;
			uint8_t bssid[6] = {0};
			struct netif_ap *ap;
			struct sta *s = NULL;

			memcpy(bssid, &p[offset], 6);
			offset += 6;

			re = agent_get_radio_with_ap(a, bssid);
			if (!re)
				continue;

			ap = agent_get_ap(a, bssid);
			if (ap == NULL || !ap->cfg)
				continue;
			bss = &ap->bss;

			/* AP Metrics TLV */
			ret = agent_gen_ap_metrics(a, frm, re, bss);
			if (ret)
				goto out;

			/* AP Extended Metrics TLV */
			ret = agent_gen_ap_ext_metrics(a, frm, bss);
			if (ret)
				goto out;


#if (EASYMESH_VERSION >= 6)
			ret = agent_gen_affiliated_ap_metrics(a, frm, bss);
			if (ret)
				goto out;
#endif

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

			rcfg = get_agent_config_radio(&a->cfg, ap->cfg->device);
			if (!rcfg)
				return NULL;

			list_for_each_entry(s, &ap->stalist, list) {
#if (EASYMESH_VERSION >= 6)
				/* handle affiliated STAs from stamldlist */
				if (s->is_affiliated_sta)
					continue;
#endif

				if (rcfg->include_sta_stats) {
					/* Associated STA Traffic Stats TLV */
					ret = agent_gen_assoc_sta_traffic_stats(a, frm, s->macaddr, s);
					if (ret)
						goto out;
				}

				if (rcfg->include_sta_metric) {
					/* Associated STA Link Metrics TLV */
					ret = agent_gen_assoc_sta_link_metrics(a, frm, s, bss->bssid);
					if (ret)
						goto out;
// #ifdef PROFILE2

					/* Associated STA Extended Link Metrics TLV */
					ret = agent_gen_assoc_sta_ext_link_metric(a, frm, s, bss->bssid);
					if (ret)
						goto out;
// #endif
				}

#if (EASYMESH_VERSION > 2)
				if (rcfg->include_wifi6_sta_status &&
				    a->cfg.map_profile > 2) {
					/* Associated Wi-Fi 6 STA Status Report TLV */
					ret = agent_gen_assoc_wifi6_sta_status_report(a, frm, s);
					if (ret)
						goto out;
				}
#endif
			}

#if (EASYMESH_VERSION >= 6)
			struct netif_mld *mld = NULL;

			list_for_each_entry(mld, &a->apmldlist, list) {
				struct sta_mld *smld = NULL;

				list_for_each_entry(smld, &mld->stamldlist, list) {

					for (i = 0; i < smld->num_affiliated_sta; i++) {
						s = agent_get_sta(a, smld->affiliated_sta[i]);
						if (!s)
							continue;

						if (memcmp(s->bssid, bssid, 6))
							continue;

						if (rcfg->include_sta_stats) {
							ret = agent_gen_affiliated_sta_metrics(a, frm, s);
							if (ret)
								goto out;
						}

						if (rcfg->include_sta_metric) {
							/* Associated STA Link Metrics TLV */
							ret = agent_gen_assoc_sta_link_metrics(a, frm, s, bss->bssid);
							if (ret)
								goto out;

							/* Associated STA Extended Link Metrics TLV */
							ret = agent_gen_assoc_sta_ext_link_metric(a, frm, s, bss->bssid);
							if (ret)
								goto out;
						}

						/* TODO: traffic stats are per-mld, today use first client statistics */
						ret = agent_gen_assoc_sta_traffic_stats(a, frm, smld->macaddr, s);
						if (ret)
							goto out;

						if (rcfg->include_wifi6_sta_status &&
						    a->cfg.map_profile > 2) {
							/* TLV is sent only for SLO and MLD clients */
							ret = agent_gen_assoc_wifi6_sta_status_report(a, frm, s);
							if (ret)
								goto out;
						}
					}

				}


			}
#endif
		}
	}

	cmdu_put_eom(frm);
	return frm;
out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_assoc_sta_metric_responsex(struct agent *a,
		uint8_t *origin, struct sta *s, struct netif_ap *ap)
{
	uint16_t mid = 0;
	struct cmdu_buff *frm;
	int ret;

	frm = cmdu_alloc_simple(CMDU_ASSOC_STA_LINK_METRICS_RESPONSE, &mid);
	if (!frm) {
		agnt_dbg(LOG_STA, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	if (s) {
		/* Associated STA Link Metrics TLV */
		ret = agent_gen_assoc_sta_link_metrics(a, frm, s, ap->bssid);
		if (ret)
			goto error;

// #ifdef PROFILE2
		/* Associated STA Extended Link Metrics TLV */
		ret = agent_gen_assoc_sta_ext_link_metric(a, frm, s, ap->bssid);
		if (ret)
			goto error;
// #endif
	} else {
		uint8_t reason_code = 0x00;
		struct tlv *t;
		struct tlv_assoc_sta_link_metrics *data;

		/* Associated STA Link Metrics TLV */
		t = cmdu_reserve_tlv(frm, 20);
		if (!t)
			goto error;

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

		data = (struct tlv_assoc_sta_link_metrics *) t->data;
		/* Reported BSS for STA */
		data->num_bss = 0;
		ret = cmdu_put_tlv(frm, t);
		if (ret) {
			agnt_dbg(LOG_STA, "%s: error: cmdu_put_tlv()\n", __func__);
			goto error;
		}

		/* Error Code TLV */
		reason_code = 0x02;	/* STA not associated with any BSS */
		ret = agent_gen_tlv_error_code(a, frm, data->macaddr,
				reason_code);
		if (ret)
			goto error;
	}

	cmdu_put_eom(frm);
	memcpy(frm->origin, origin, 6);
	return frm;
error:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_assoc_sta_metric_response(
		struct agent *a, struct cmdu_buff *rx_cmdu, struct node *n)
{
	struct tlv *tv[ASSOC_STA_LINK_METRICS_QUERY_NUM_OF_TLV_TYPES][TLV_MAXNUM] = {0};
	struct tlv_sta_mac *query;
	struct cmdu_buff *frm;
	uint16_t mid = 0;
	struct tlv *tv0;
	struct sta *s;
	int ret;

	ret = map_cmdu_validate_parse(rx_cmdu, tv, ARRAY_SIZE(tv), n->map_profile);
	if (!ret) {
		agnt_dbg(LOG_STA, "%s: map_cmdu_validate_parse failed, err = (%d) '%s'\n", __func__,
		    map_error, map_strerror(map_error));
		return NULL;
	}

	if (!tv[ASSOC_STA_LINK_METRICS_QUERY_STA_MAC][0])
		return NULL;

	mid = cmdu_get_mid(rx_cmdu);

	frm = cmdu_alloc_simple(CMDU_ASSOC_STA_LINK_METRICS_RESPONSE, &mid);
	if (!frm) {
		agnt_dbg(LOG_STA, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, rx_cmdu->origin, 6);

	tv0 = tv[ASSOC_STA_LINK_METRICS_QUERY_STA_MAC][0];
	query = (struct tlv_sta_mac *) tv0->data;

	s = agent_get_sta(a, query->macaddr);
	if (s) {
		/* Associated STA Link Metrics TLV */
		ret = agent_gen_assoc_sta_link_metrics(a, frm, s, s->bssid);
		if (ret)
			goto error;

		/* Associated STA Extended Link Metrics TLV */
		ret = agent_gen_assoc_sta_ext_link_metric(a, frm, s, s->bssid);
		if (ret)
			goto error;
	} else {
		uint8_t reason_code = 0x00;
		struct tlv *t;
		struct tlv_assoc_sta_link_metrics *data;

		/* Associated STA Link Metrics TLV */
		t = cmdu_reserve_tlv(frm, 20);
		if (!t)
			goto error;

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

		data = (struct tlv_assoc_sta_link_metrics *) t->data;
		memcpy(data->macaddr, query->macaddr, 6);
		/* Reported BSS for STA */
		data->num_bss = 0;
		ret = cmdu_put_tlv(frm, t);
		if (ret) {
			agnt_dbg(LOG_STA, "%s: error: cmdu_put_tlv()\n", __func__);
			goto error;
		}

		/* Error Code TLV */
		reason_code = 0x02;	/* STA not associated with any BSS */
		ret = agent_gen_tlv_error_code(a, frm,
				query->macaddr, reason_code);
		if (ret)
			goto error;
	}

	cmdu_put_eom(frm);

	return frm;
error:
	cmdu_free(frm);

	return NULL;
}

struct cmdu_buff *agent_gen_beacon_metrics_query(struct agent *a,
			uint8_t *agent_mac, 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, uint8_t *element)
{
	struct cmdu_buff *frm = NULL;
	int ret = 0;
	uint16_t mid = 0;

	agnt_trace(LOG_STEER, "agent: %s: --->\n", __func__);

	if (!agent_mac || !sta_addr || !bssid)
		return NULL;

	frm = cmdu_alloc_simple(CMDU_BEACON_METRICS_QUERY, &mid);
	if (!frm) {
		agnt_dbg(LOG_STEER, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	/* Beacon metrics query TLV */
	ret = agent_gen_tlv_beacon_metrics_query(a, frm,
			sta_addr, opclass, channel, bssid,
			reporting_detail, ssid, num_report,
			report, num_element, element);

	if (ret)
		goto fail;

	/* destination agent */
	memcpy(frm->origin, agent_mac, 6);

	cmdu_put_eom(frm);

	return frm;
fail:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_cmdu_beacon_metrics_resp(struct agent *a,
			uint8_t *sta_addr, uint8_t report_elems_nr,
			uint8_t *report_elem, uint16_t elem_len)
{
	struct cmdu_buff *frm = NULL;
	uint8_t *tlv = NULL;
	uint16_t mid = 0;
	uint8_t origin[6] = { 0 };
	uint32_t t_len = 0;

	/* Since no excess allocation (1500 octets in cmdu_alloc_simple)
	 * and cmdu_put_tlv is not used - one must specify the exact size.
	 * I.e. must manually add +3: +1 for type and +2 for length.
	 */
	t_len = sizeof(struct tlv_beacon_metrics_resp) + elem_len + 3;

	tlv = (uint8_t *) calloc(1, t_len);

	if (!tlv)
		return NULL;

	if (agent_gen_tlv_beacon_metrics_resp(
					a, tlv, sta_addr,
					report_elems_nr,
					report_elem,
					elem_len))
		goto out;

	frm = cmdu_alloc_custom(CMDU_BEACON_METRICS_RESPONSE,
				&mid, NULL, origin, tlv, t_len);

	if (!frm) {
		dbg("%s: -ENOMEM\n", __func__);
		goto out;
	}

	/* destination controller */
	memcpy(frm->origin, a->cntlr_almac, 6);

	cmdu_put_eom(frm);
out:
	if (tlv)
		free(tlv);
	return frm;
}

struct cmdu_buff *agent_gen_unassoc_sta_metric_query(struct agent *a,
		uint8_t *origin, uint8_t opclass,
		uint8_t num_metrics, struct unassoc_sta_metric *metrics)
{
	int ret;
	uint16_t mid = 0;
	struct cmdu_buff *frm;

	/* TODO: A Multi-AP Agent shall not send an Unassociated
	 * STA Link Metrics Query message to a Multi-AP Agent that
	 * does not indicate support for Unassociated STA Link
	 * Metrics in the AP Capability TLV.
	 */

	frm = cmdu_alloc_simple(CMDU_UNASSOC_STA_LINK_METRIC_QUERY, &mid);
	if (!frm) {
		agnt_dbg(LOG_STA, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, origin, 6);

	/* Unassociated STA link metrics query TLV */
	ret = agent_gen_tlv_unassoc_sta_lm_query(a, frm,
			opclass, num_metrics, metrics);

	if (ret)
		goto out;

	cmdu_put_eom(frm);
	return frm;

out:
	return NULL;
}

struct cmdu_buff *agent_gen_tunneled_msg(struct agent *a, uint8_t protocol,
		uint8_t *sta, int frame_len, uint8_t *frame_body)
{
	int ret;
	struct cmdu_buff *cmdu;
	uint16_t mid = 0;

	if (!sta && !frame_body)
		return NULL;

	cmdu = cmdu_alloc_simple(CMDU_TUNNELED, &mid);
	if (!cmdu) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(cmdu->origin, a->cntlr_almac, 6);

	/* Source Info TLV */
	ret = agent_gen_source_info(a, cmdu, sta);
	if (ret)
		goto error;

	/* Tunneled message type TLV */
	ret = agent_gen_tunnel_msg_type(a, cmdu, protocol);
	if (ret)
		goto error;

	/* Tunneled TLV */
	ret = agent_gen_tunneled(a, cmdu, frame_len, frame_body);
	if (ret)
		goto error;

	cmdu_put_eom(cmdu);

	return cmdu;

error:
	cmdu_free(cmdu);

	return NULL;
}


struct cmdu_buff *agent_gen_vendor_specific_cmdu(struct agent *a,
		uint8_t *origin, uint8_t depth)
{
	return NULL;
//	struct tlv_vendor_specific *p;
//	struct cmdu_cstruct *cmdu;
//	int tlv_index = 0;
//
//	cmdu = calloc(1, sizeof(struct cmdu_cstruct));
//	if (!cmdu)
//		return NULL;
//
//	cmdu->message_type = CMDU_TYPE_VENDOR_SPECIFIC;
//	memcpy(cmdu->origin, origin, 6);
//	strncpy(cmdu->intf_name, a->cfg.al_bridge, IFNAMESIZE - 1);
//
//	p = agent_gen_vendor_specific_tlv(a, depth);
//	if (!p)
//		goto error;
//
//	cmdu->num_tlvs++;
//	cmdu->tlvs = (uint8_t **)calloc(cmdu->num_tlvs,
//			sizeof(uint8_t *));
//	cmdu->tlvs[tlv_index++] = (uint8_t *)p;
//	return cmdu;
//error:
//	map_free_cmdu(cmdu);
//	return NULL;
}

static int agent_gen_ch_scan_response_all(struct agent *a, struct cmdu_buff *cmdu,
		struct wifi_radio_element *re, uint8_t status)
{
	agnt_trace(LOG_SCAN, "%s --->\n", __func__);

	struct wifi_scanres_element *sl;
	int i, ret;

	if (!a || !cmdu || !re)
		return -1;

	/* The re->scanlist has been created & filled with entries for all
	 * supported opclasses and channels upon start. Now the list gets filled
	 * in with opclasses/channels for which we have actual results stored in
	 * scan cache (re->scanresults).
	 */
	scan_cache_to_scanlist(a, re, false);

	sl = re->scanlist;
	if (WARN_ON(!sl))
		return -1;

	for (i = 0; i < sl->num_opclass; i++) {
		struct wifi_scanres_opclass_element *op;
		int j;

		op = sl->opclass_scanlist + i;

		if (op->bandwidth != 20) /* caps */
			continue;

		for (j = 0; j < op->num_channels; j++) {
			struct wifi_scanres_channel_element *ch;

			ch = op->channel_scanlist + j;

			if (!wifi_opclass_id_channel_supported(&re->opclass,
							       op->opclass,
							       ch->channel)) {
				/* skip channel(s) not supported */
				continue;
			}

			ret = agent_gen_ch_scan_response_tlv(a, cmdu,
						re->macaddr, op->opclass,
						ch, status);
			if (ret)
				return ret;

			agnt_dbg(LOG_SCAN, "%s: Added Channel Scan Result TLV.\n",
				__func__);
		}
	}

	return 0;
}

static int agent_gen_ch_scan_response_opc(struct agent *a,
		struct cmdu_buff *cmdu, struct wifi_radio_element *re,
		struct wifi_scan_request_opclass *req_opc, uint8_t status)
{
	int ci, i, j, ret;
	struct wifi_scanres_element *sl;
	struct wifi_scanres_opclass_element *op;
	struct wifi_scanres_channel_element *ch;
	uint8_t channels[128];
	int num_chan;
	int num_tlv = 0;

	if (!a || !cmdu || !re || !req_opc)
		return -1;

	agnt_trace(LOG_SCAN, "%s: response_opc id %d channels %d\n",
	      __func__, req_opc->classid, req_opc->num_channel);

	/* No Channels specified in the request */
	if (req_opc->num_channel == 0) {
		/* Report all supported channels in Operating Class */
		num_chan = ARRAY_SIZE(channels);
		ret = wifi_opclass_get_supported_channels(&re->opclass,
					req_opc->classid,
					channels,
					&num_chan);
		if (ret)
			return -1;
	}
	/* Number and list of Channels specified in the request */
	else {
		/* Report for channels from the request */
		num_chan = (req_opc->num_channel < ARRAY_SIZE(channels) ?
					req_opc->num_channel : ARRAY_SIZE(channels));
		memcpy(channels, req_opc->channel, num_chan);
	}

	for (ci = 0; ci < num_chan ; ci++) {

		agnt_dbg(LOG_SCAN, "%s: Add Channel Scan Result TLV, op: %u, ch: %u\n",
		    __func__, req_opc->classid, channels[ci]);

		/* Scan failed: add Scan Status only */
		if (status != CH_SCAN_STATUS_SUCCESS) {
			struct wifi_scanres_channel_element ch_elem;

			ch_elem.channel = channels[ci];
			ret = agent_gen_ch_scan_response_tlv(a, cmdu, re->macaddr,
					req_opc->classid, &ch_elem, status);
			if (ret)
				return ret;

			agnt_dbg(LOG_SCAN, "%s: Added Channel Scan Result TLV.\n",
			    __func__);
			num_tlv++;

			continue;
		}

		/* Scan succesful: add full Channel Scan Result TLV */
		if (!re->scanlist) {
			agnt_dbg(LOG_SCAN, "%s: missing scanlist\n", __func__);
			return -1;
		}
		sl = re->scanlist;

		for (i = 0; i < sl->num_opclass; i++) {
			op = sl->opclass_scanlist + i;

			for (j = 0; j < op->num_channels; j++) {
				int k;

				ch = op->channel_scanlist + j;
				agnt_trace(LOG_SCAN, "%s: scan opclass %d channel %d num_neighbors %d\n",
				      __func__, op->opclass, ch->channel, ch->num_neighbors);
				for (k = 0; k < ch->num_neighbors; k++) {
					struct wifi_scanres_neighbor_element *nbr = ch->nbrlist + k;
					agnt_trace(LOG_SCAN, "\t %s: neigh " MACFMT " ssid %s\n", __func__, MAC2STR(nbr->bssid), nbr->ssid);
				}
			}

			if (req_opc->classid != op->opclass) {
				agnt_trace(LOG_SCAN, "%s: skip opclass %d\n", __func__, op->opclass);
				continue;
			}

			for (j = 0; j < op->num_channels; j++) {
				uint8_t st = status;

				ch = op->channel_scanlist + j;

				if (ch->channel != channels[ci])
					continue;

				if (!wifi_opclass_id_channel_supported(&re->opclass,
								       op->opclass,
								       ch->channel)) {
					/* override status for not supported channels */
					st = CH_SCAN_STATUS_SCAN_NOT_SUPPORTED;
				}

				ret = agent_gen_ch_scan_response_tlv(a, cmdu,
						re->macaddr,
						op->opclass, ch, st);
				if (ret)
					return ret;

				agnt_dbg(LOG_SCAN, "%s: Added Channel Scan Result TLV.\n",
				    __func__);
				num_tlv++;
			}
		}
	}

	if (!num_tlv) {
		agnt_dbg(LOG_SCAN, "%s: No Scan Result TLV added.\n",
		    __func__);
		return -1;
	}

	return 0;
}

struct cmdu_buff *agent_gen_independent_ch_scan_response(struct agent *a,
		struct wifi_radio_element *re)
{
	struct cmdu_buff *cmdu_data;
	int ret;

	agnt_trace(LOG_SCAN, "%s --->\n", __func__);

	cmdu_data = cmdu_alloc_frame(CH_SCAN_RESP_MAX_BYTES);
	if (!cmdu_data) {
		agnt_dbg(LOG_SCAN, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	cmdu_set_type(cmdu_data, CMDU_CHANNEL_SCAN_REPORT);
	memcpy(cmdu_data->origin, a->cntlr_almac, 6);

	ret = agent_gen_timestamp_tlv(a, cmdu_data);
	if (ret)
		goto error; /* err */

	agnt_dbg(LOG_SCAN, "%s: added MAP_TLV_TIMESTAMP\n", __func__);

	ret = agent_gen_ch_scan_response_all(a, cmdu_data, re,
			CH_SCAN_STATUS_SUCCESS);

	if (ret)
		goto error; /* err */

	cmdu_put_eom(cmdu_data);
	return cmdu_data;

error:
	agnt_dbg(LOG_SCAN, "%s: failed to build cmdu!\n", __func__);
	cmdu_free(cmdu_data);

	return NULL;
}

struct cmdu_buff *agent_gen_ch_scan_response_radio(struct agent *a,
		struct wifi_radio_element *re,
		struct wifi_scan_request_radio *req, uint8_t status)
{
	agnt_trace(LOG_SCAN, "%s --->\n", __func__);

	int i, ret;
	struct cmdu_buff *cmdu_data; /* The response cmdu */

	if (WARN_ON(!req))
		return NULL;

	if (WARN_ON(!re))
		return NULL;

	/* Allocate the cmdu */
	cmdu_data = cmdu_alloc_frame(CH_SCAN_RESP_MAX_BYTES);
	if (!cmdu_data) {
		agnt_trace(LOG_SCAN, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	/* Define the cmdu */
	cmdu_set_type(cmdu_data, CMDU_CHANNEL_SCAN_REPORT);
	memcpy(cmdu_data->origin, a->cntlr_almac, 6);

	/* Define the TLVs */
	/* Timestamp TLV */
	ret = agent_gen_timestamp_tlv(a, cmdu_data);
	if (ret)
		goto free_cmdu; /* err */

	agnt_trace(LOG_SCAN, "%s: Added Timestamp TLV\n", __func__);

	/* Opclass not provided or fresh_scan == 0 */
	if (req->num_opclass == 0) {
		//	|| !(req->mode & SCAN_REQUEST_FRESH_SCAN) // redundant
		/* Include all stored results for this radio */
		ret = agent_gen_ch_scan_response_all(a, cmdu_data, re, status);
		/* return: */
		if (ret)
			goto free_cmdu; /* err */
		goto put_tlv; /* OK */
	}

	/* One or more opclasses were listed in the request */
	for (i = 0; i < req->num_opclass; i++) {
		/* Include results for opclass given in request */
		ret = agent_gen_ch_scan_response_opc(a, cmdu_data, re,
						&req->opclass[i],
						status);
		if (ret)
			goto free_cmdu; /* err */
	}

put_tlv:
	cmdu_put_eom(cmdu_data);
	return cmdu_data;
free_cmdu:
	agnt_dbg(LOG_SCAN, "%s: failed to build cmdu!\n", __func__);
	cmdu_free(cmdu_data);
	return NULL;
}

struct cmdu_buff *agent_gen_ap_caps_query(struct agent *a, uint8_t *origin)
{
	uint16_t mid = 0;
	struct cmdu_buff *resp;

	/* Allocate the cmdu_data structure */
	resp = cmdu_alloc_simple(CMDU_AP_CAPABILITY_QUERY, &mid);
	if (!resp) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(resp->origin, origin, 6);
	cmdu_put_eom(resp);
	return resp;
}

struct cmdu_buff *agent_gen_ap_caps_response(struct agent *a,
		struct cmdu_buff *rec_cmdu)
{
	struct wifi_radio_element *re = NULL;
	struct cmdu_buff *resp;
	uint16_t mid = 0;
	int ret;

	mid = cmdu_get_mid(rec_cmdu);
	//resp = cmdu_alloc_simple(CMDU_AP_CAPABILITY_REPORT, &mid);
	resp = cmdu_alloc_frame(2500);
	if (!resp) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	cmdu_set_type(resp, CMDU_AP_CAPABILITY_REPORT);
	cmdu_set_mid(resp, mid);
	memcpy(resp->origin, rec_cmdu->origin, 6);

	/* AP Capability TLV */
	ret = agent_gen_ap_caps(a, resp);
	if (ret)
		goto error;

	list_for_each_entry(re, &a->radiolist, list) {
		/* AP Radio Basic Capabilities TLV */
		ret = agent_gen_ap_radio_basic_cap(a, resp, re);
		if (ret)
			goto error;

		/* AP HT Capabilities TLV */
		agent_gen_ap_ht_caps(a, resp, re);

		/* AP HE Capabilities TLV */
		agent_gen_ap_he_caps(a, resp, re);

		/* AP VHT Capabilities TLV */
		agent_gen_ap_vht_caps(a, resp, re);

	}

	if (a->cfg.map_profile >= 2) {
		/* Channel Scan Capabilities TLV */
		ret = agent_gen_ch_scan_cap(a, resp);
		if (ret)
			goto error;

		/* CAC Capabilities TLV */
		ret = agent_gen_cac_cap(a, resp);
		if (ret)
			goto error;

		/* Profile-2 AP Capability TLV */
		ret = agent_gen_profile2_ap_cap(a, resp);
		if (ret)
			goto error;

		/* Metric Collection Interval TLV */
		ret = agent_gen_metric_collection_interval(a, resp);
		if (ret)
			goto error;
	}

#if (EASYMESH_VERSION > 2)
	if (a->cfg.map_profile >= 3) {
		/* AP Wi-Fi 6 Capabilities TLV */
		re = NULL;
		list_for_each_entry(re, &a->radiolist, list) {
			agent_gen_ap_wifi6_caps(a, resp, re);
		}

		/* Device 1905 Layer Security Capability TLV */
		ret = agent_gen_device_1905_layer_security_cap(a, resp);
		if (ret)
			goto error;

		/*  Device Inventory TLV */
		ret = agent_gen_device_inventory(a, resp);
		if (ret)
			goto error;
	}
#endif

#if (EASYMESH_VERSION >= 6)
	/* AKM Suite Capabilities TLV */
	if (agent_gen_akm_suite_cap(a, resp)) {
		warn("%s: agent_gen_akm_suite_cap failed.\n", __func__);
		goto error;
	}

	/* Wi-Fi 7 Agent Capabilites TLV */
	if (agent_gen_wifi7_agent_caps(a, resp)) {
		warn("%s: agent_gen_wifi7_agent_caps failed.\n", __func__);
		goto error;
	}

	/* EHT Operations TLV */
	if (agent_gen_eht_operations(a, resp)) {
		warn("%s: agent_gen_eht_operations failed.\n", __func__);
		goto error;
	}
#endif

	cmdu_put_eom(resp);

	return resp;

error:
	cmdu_free(resp);

	return NULL;
}

struct cmdu_buff *agent_gen_bk_caps_response(struct agent *a,
		struct cmdu_buff *cmdu)
{
	struct wifi_radio_element *re = NULL;
	struct cmdu_buff *cmdu_data;
	uint16_t mid;
	int ret;

	/* Allocate and define the cmdu */
	mid = cmdu_get_mid(cmdu);
	cmdu_data = cmdu_alloc_simple(CMDU_BACKHAUL_STA_CAPABILITY_REPORT, &mid);
	if (!cmdu_data) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

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

	/* Define the TLVs */
	list_for_each_entry(re, &a->radiolist, list) {
		/* Backhaul STA radio capabilities TLV */
		ret = agent_gen_bk_sta_radio_cap_tlv(a, cmdu_data, re);
		if (ret)
			goto error;
		trace("%s: Added MAP_TLV_BACKHAUL_STA_RADIO_CAPABILITY\n", __func__);
	}

	cmdu_put_eom(cmdu_data);
	return cmdu_data;
error:
	cmdu_free(cmdu_data);
	return NULL;
}

struct cmdu_buff *agent_gen_topology_notification(struct agent *agent,
		struct sta *s, struct netif_ap *ap, uint8_t assoc_event)
{

	struct cmdu_buff *frm = NULL;
	uint8_t *macaddr = s->macaddr;
	uint8_t *bssid = ap->bssid;
	int ret = 0;
	uint16_t mid = 0;
	uint8_t origin[6] = {0x01, 0x80, 0xc2, 0x00, 0x00, 0x13};

#if (EASYMESH_VERSION >= 6)
	if (s->is_affiliated_sta) {
		macaddr = s->mld_macaddr;
		bssid = ap->mld_macaddr;
	}
#endif

	frm = cmdu_alloc_simple(CMDU_TYPE_TOPOLOGY_NOTIFICATION, &mid);
	if (!frm) {
		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	CMDU_SET_RELAY_MCAST(frm->cdata);

	ret = agent_gen_al_mac(agent, frm, agent->almac);
	if (ret)
		goto out;

	/* Client Association Event TLV */
	ret = agent_gen_client_assoc_event_tlv(agent, frm, macaddr, bssid,
			assoc_event);
	if (ret)
		goto out;

	memcpy(frm->origin, origin, 6);
	cmdu_put_eom(frm);
	return frm;
out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_client_disassoc(struct agent *a, struct sta *s,
					    uint16_t reason_code)
{
	uint16_t mid = 0;
	int ret;
	struct cmdu_buff *cmdu;
	uint8_t *macaddr = s->macaddr;

	cmdu = cmdu_alloc_simple(CMDU_CLIENT_DISASSOCIATION_STATS, &mid);
	if (!cmdu) {
		agnt_dbg(LOG_STA, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(cmdu->origin, a->cntlr_almac, 6);

#if (EASYMESH_VERSION >= 6)
	if (s->is_affiliated_sta) {
		struct sta_mld *smld;

		macaddr = s->mld_macaddr;
		ret = agent_gen_affiliated_sta_metrics(a, cmdu, s);
		if (ret)
			goto error;

		/* TODO: overwrite s as STA traffic are currently only available on first link */
		smld = agent_get_stamld(a, s->mld_macaddr);
		if (!smld)
			goto error;

		s = agent_get_sta(a, smld->affiliated_sta[0]);
		if (!s)
			goto error;
	}

#endif

	/* STA MAC Address TLV */
	ret = agent_gen_sta_mac(a, cmdu, macaddr);
	if (ret)
		goto error;

	/* Reason Code TLV */
	ret = agent_gen_reason_code(a, cmdu, reason_code);
	if (ret)
		goto error;

	/* Authorized STA Traffic Stats TLV */

	ret = agent_gen_assoc_sta_traffic_stats(a, cmdu, macaddr, s);
	if (ret)
		goto error;

	cmdu_put_eom(cmdu);

	return cmdu;
error:
	cmdu_free(cmdu);

	return NULL;
}

struct cmdu_buff *agent_gen_topology_query(struct agent *a, uint8_t *origin)
{
	struct cmdu_buff *resp;
	uint16_t mid = 0;
	int ret = 0;

	resp = cmdu_alloc_simple(CMDU_TYPE_TOPOLOGY_QUERY, &mid);
	if (!resp) {
		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	ret = agent_gen_map_profile(a, resp, a->cfg.map_profile);
	if (!ret)
		goto error;

	memcpy(resp->origin, origin, 6);
	cmdu_put_eom(resp);
	return resp;
error:
	cmdu_free(resp);
	return NULL;
}

struct cmdu_buff *agent_gen_topology_response(struct agent *a, uint8_t *origin,
						uint16_t mid, char *ifname)
{
	struct cmdu_buff *resp = NULL, *ext = NULL;
	int ret;

	/* query i1905d base CMDU */
	resp = ieee1905_ubus_buildcmdu(a->ubus_ctx, a->i1905obj_id,
				       CMDU_TYPE_TOPOLOGY_RESPONSE, ifname, 0, NULL);
	if (!resp) {
		dbg("%s: No response from stack when generating 0x%04x\n",
				__func__, CMDU_TYPE_TOPOLOGY_RESPONSE);
		return NULL;
	}

	ext = cmdu_realloc(resp, 10000);
	if (!ext)
		goto error;

	resp = ext;

	/* Supported Service */
	ret = agent_gen_supported_service(a, resp);
	if (ret)
		goto error;

	/* AP Operational BSS */
	ret = agent_gen_ap_oper_bss_tlv(a, resp);
	if (ret)
		goto error;

	/* Associated Clients */
	ret = agent_gen_assoc_client_tlv(a, resp);
	if (ret)
		goto error;

#if (EASYMESH_VERSION > 2)
	if (a->cfg.map_profile > 2) {
#if (EASYMESH_VERSION >= 6)
		struct wifi_radio_element *re = NULL;
		struct netif_mld *mld = NULL;
#endif
		/* BSS Configuration Report TLV*/
		ret = agent_gen_bss_config_report_tlv(a, resp);
		if (ret)
			goto error;

#if (EASYMESH_VERSION >= 6)
		/* Backhaul STA Radio Capabilities TLV */
		list_for_each_entry(re, &a->radiolist, list) {
			ret = agent_gen_bk_sta_radio_cap_tlv(a, resp, re);
			if (ret)
				goto error;
		}

		/* Agent AP MLD Configuration TLV */
		if (a->num_ap_mld > 0)
			if (agent_gen_ap_mld_config(a, resp))
				goto error;

		/* Backhaul STA MLD Configuration TLV */
		if (a->bstamld)
			agent_gen_bsta_mld_config(a, resp);

		/* Associated STA MLD Configuration Report TLV */
		list_for_each_entry(mld, &a->apmldlist, list) {
			struct sta_mld *s = NULL;

			list_for_each_entry(s, &mld->stamldlist, list)
				agent_gen_associated_sta_mld_config(a, resp, s);
		}

		/* TODO: TID-to-Link Mapping Policy TLV */
#endif
	}
#endif


	/* Multi-AP profile */
	ret = agent_gen_map_profile(a, resp, a->cfg.map_profile);
	if (ret)
		goto error;

	cmdu_set_mid(resp, mid);
	memcpy(resp->origin, origin, 6);
	cmdu_put_eom(resp);
	return resp;
error:
	cmdu_free(resp);
	return NULL;
}

struct cmdu_buff *agent_gen_link_metric_response(struct agent *a,
		uint8_t *origin, uint16_t mid,
		uint8_t *neighbor, uint8_t query_type)
{
	struct cmdu_buff *resp = NULL;
	char *argv[2] = {NULL, NULL};
	char argv_buf[2][32];
	int argc = 0;

	/* Build argv to pass query parameters to ieee1905 stack */
	snprintf(argv_buf[0], sizeof(argv_buf[0]), "%u", query_type);
	argv[argc++] = argv_buf[0];

	if (!hwaddr_is_zero(neighbor)) {
		hwaddr_ntoa(neighbor, argv_buf[1]);
		argv[argc++] = argv_buf[1];
	}

	/* Query i1905d base CMDU with parameters */
	resp = ieee1905_ubus_buildcmdu(a->ubus_ctx, a->i1905obj_id,
				       CMDU_TYPE_LINK_METRIC_RESPONSE, NULL,
				       argc, (const char **)argv);
	if (!resp) {
		dbg("No response from stack when generating 0x%04x\n",
		    CMDU_TYPE_LINK_METRIC_RESPONSE);
		return NULL;
	}

	if (resp->datalen == 0) {
		dbg("%s: IEEE 1905 stack returned empty Link Metrics Response (datalen=0)\n",
		    __func__);
		cmdu_free(resp);
		return NULL;
	}

	dbg("%s: Successfully built Link Metric Response (datalen=%u)\n",
	    __func__, resp->datalen);

	cmdu_set_mid(resp, mid);
	memcpy(resp->origin, origin, 6);
	cmdu_put_eom(resp);
	return resp;
}

struct cmdu_buff *agent_gen_cmdu_1905_ack(
		struct agent *a, uint8_t *origin, uint16_t mid,
		struct sta_error_response *sta_resp, uint32_t sta_count)
{
	trace("agent: %s: --->\n", __func__);
	struct cmdu_buff *frm = NULL;
	int j;

	frm = cmdu_alloc_simple(CMDU_1905_ACK, &mid);
	if (!frm) {
		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, origin, 6);

	/* Error Code TLV 17.2.36 */
	for (j = 0; j < sta_count; j++) {
		if (agent_gen_tlv_error_code(a, frm,
				sta_resp[j].sta_mac, sta_resp[j].response))
			continue;

	}

	cmdu_put_eom(frm);
	return frm;
}

struct cmdu_buff *agent_gen_cmdu_backhaul_steer_resp(struct agent *a,
		uint8_t *target_bssid, uint8_t *macaddr, uint8_t res,
		uint16_t mid)
{
	agnt_trace(LOG_BSTEER, "agent: %s: --->\n", __func__);
	struct cmdu_buff *frm = NULL;
	int ret = 0;


	frm = cmdu_alloc_simple(CMDU_BACKHAUL_STEER_RESPONSE, &mid);
	if (!frm) {
		agnt_dbg(LOG_BSTEER, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	ret = agent_gen_tlv_backhaul_steer_resp(a, frm, target_bssid, macaddr,
						(res ? 0x01 : 0x00));
	if (ret)
		goto out;

	if (res) {
		ret = agent_gen_tlv_error_code(a, frm,
				NULL, res);
		if (ret)
			goto out;
	}


	cmdu_put_eom(frm);

	return frm;
out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_channel_preference_report(struct agent *a,
						      uint16_t mid,
						      uint8_t *origin,
						      uint8_t *radioid)
{
	struct wifi_radio_element *re = NULL;
	struct cmdu_buff *frm, *ext;
	int ret;

	frm = cmdu_alloc_simple(CMDU_CHANNEL_PREFERENCE_REPORT, &mid);
	if (!frm) {
		agnt_dbg(LOG_CHANNEL, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	if (a->num_radios) {
		ext = cmdu_realloc(frm, a->num_radios * 2048);
		if (!ext) {
			agnt_dbg(LOG_CHANNEL, "%s: realloc -ENOMEM\n", __func__);
			goto error;
		}

		frm = ext;
	}

	if (origin)
		memcpy(frm->origin, origin, 6);
	else
		memcpy(frm->origin, a->cntlr_almac, 6);

	list_for_each_entry(re, &a->radiolist, list) {
		if (radioid && memcmp(re->macaddr, radioid, 6))
			continue;

		/* Channel Preference TLV */
		ret = agent_gen_channel_pref(a, frm, re);
		if (ret)
			goto error;
	}

#if 0
	/*
	 * Disable while not required, before better understand of this.
	 * Seems today we fill it wrong.
	 */
	for (i = 0; i < a->num_radios; i++) {
		/* Radio Operation Restriction TLV */
		ret = agent_gen_radio_oper_restrict(a, frm, i);
		if (ret)
			goto error;
	}
#endif

//#ifdef PROFILE2
	/* CAC Completion Report TLV */
	ret = agent_gen_cac_complete_report(a, frm, radioid);
	if (ret)
		goto error;

	/* CAC Status Report TLV */
	ret = agent_gen_cac_status_report(a, frm, radioid);
	if (ret)
		goto error;
//#endif

	cmdu_put_eom(frm);
	return frm;

error:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_association_status_notify(struct agent *a,
		int num_data, void *data)
{
	uint16_t mid = 0;
	int ret;
	struct cmdu_buff *frm;

	frm = cmdu_alloc_simple(CMDU_ASSOCIATION_STATUS_NOTIFICATION, &mid);
	if (!frm) {
		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	ret = agent_gen_assoc_status_notif(a, frm, num_data, data);
	if (ret)
		goto error;

	cmdu_put_eom(frm);

	return frm;
error:
	cmdu_free(frm);

	return NULL;
}

struct cmdu_buff *agent_gen_oper_channel_response(struct agent *a,
		struct wifi_radio_element *radio,
		uint32_t channel, uint32_t bandwidth, bool all)
{
	struct wifi_radio_element *re = NULL;
	struct cmdu_buff *cmdu;
	uint16_t mid = 0;
	int ret;

	cmdu = cmdu_alloc_simple(CMDU_OPERATING_CHANNEL_REPORT, &mid);
	if (!cmdu) {
		agnt_dbg(LOG_CHANNEL, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

#ifdef OPER_CHAN_CHANGE_RELAY_MCAST
	if (a->cfg.chan_ch_relay_mcast)
		CMDU_SET_RELAY_MCAST(cmdu->cdata);
#endif

	if (all == 1) {
		/* Operating Channel Report TLV 17.2.17 */
		list_for_each_entry(re, &a->radiolist, list) {
			uint32_t chan = re->current_channel;
			uint32_t op_class = re->current_opclass;
			uint32_t bw = re->current_bandwidth;

			if (!strlen(re->name))
				continue;

			ret = agent_gen_oper_channel_report(a, cmdu, re, chan, bw, op_class);
			if (ret)
				goto error;
		}
	} else {
		ret = agent_gen_oper_channel_report(a, cmdu, radio, channel, bandwidth, 0);
		if (ret)
			goto error;
	}

#ifdef OPER_CHAN_CHANGE_RELAY_MCAST
	if (a->cfg.chan_ch_relay_mcast) {
		uint8_t origin[6] = {0x01, 0x80, 0xc2, 0x00, 0x00, 0x13};

		/* destination: relayed multicast */
		memcpy(cmdu->origin, origin, 6);
	} else
#endif
		/* destination: controller */
		memcpy(cmdu->origin, a->cntlr_almac, 6);

	cmdu_put_eom(cmdu);
	return cmdu;

error:
	cmdu_free(cmdu);
	return NULL;
}

struct cmdu_buff *agent_gen_higher_layer_data(struct agent *a, uint8_t *addr,
		uint8_t proto, uint8_t *data, int len)
{
	struct cmdu_buff *frm;
	int ret;

	/* HLD payload with 1 byte proto + tlv header + eom */
	frm = cmdu_alloc_frame(len + 1 + 2*TLV_HLEN);
	if (!frm)
		return NULL;
	cmdu_set_type(frm, CMDU_HIGHER_LAYER_DATA);
	//cmdu_set_mid(frm, 0); /* dynamicly assigned */

	/* Agent send to orgin */
	memcpy(frm->origin, addr, 6);

	ret = agent_gen_tlv_higher_layer_data(a, frm, proto, data, len);
	if (ret)
		goto error;

	cmdu_put_eom(frm);
	return frm;

error:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_sta_caps_response(struct agent *a,
		struct cmdu_buff *rx_cmdu, struct node *n)
{
	uint8_t result_code = 0x00;
	uint8_t reason_code = 0x00;
	uint16_t mid = 0;
	int ret;
	struct cmdu_buff *resp;
	struct sta *s = NULL;
	struct tlv_client_info *cl_info = NULL;
	struct tlv *tv[CLIENT_CAPABILITY_QUERY_NUM_OF_TLV_TYPES][TLV_MAXNUM] = {0};

	/* parse received CMDU */
	ret = map_cmdu_validate_parse(rx_cmdu, tv, ARRAY_SIZE(tv), n->map_profile);
	if (!ret) {
		agnt_dbg(LOG_STA, "%s: map_cmdu_validate_parse failed, err = (%d) '%s'\n", __func__,
		    map_error, map_strerror(map_error));
		return NULL;
	}

	if (!tv[CLIENT_CAPABILITY_QUERY_CLIENT_INFO_IDX][0])
		return NULL;

	mid = cmdu_get_mid(rx_cmdu);
	resp = cmdu_alloc_simple(CMDU_CLIENT_CAPABILITY_REPORT, &mid);
	if (!resp) {
		agnt_dbg(LOG_STA, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(resp->origin, rx_cmdu->origin, 6);

	cl_info = (struct tlv_client_info *)tv[CLIENT_CAPABILITY_QUERY_CLIENT_INFO_IDX][0]->data;

	/* Client Info TLV */
	ret = agent_gen_client_info(a, resp, cl_info->macaddr, cl_info->bssid);
	if (ret)
		goto error;

	s = agent_get_sta(a, cl_info->macaddr);
#if (EASYMESH_VERSION >= 6)
	if (!s) {
		struct sta_mld *stamld;

		stamld = agent_get_stamld(a, cl_info->macaddr);
		if (stamld && stamld->num_affiliated_sta > 0)
			s = agent_get_sta(a, stamld->affiliated_sta[0]);
	}
#endif

	if (s && (s->assoc_frame)) {
		/* sta is associated */
		agnt_trace(LOG_STA, "%s: sta associated\n", __func__);

		/* Client Capability Report TLV */
		ret = agent_gen_client_cap_report(a, resp,
				result_code, s);
		if (ret)
			goto error;

	} else {
		result_code = 0x01;

		if (s) {
			/* assoc frame is not available */
			agnt_trace(LOG_STA, "%s: no assoc frame for sta " MACFMT "\n",
					__func__, MAC2STR(cl_info->macaddr));
			reason_code = 0x03;
		} else {
			/* sta is not associated with any BSS*/
			agnt_trace(LOG_STA, "%s: sta not associated\n",
					__func__);
			reason_code = 0x02;
		}

		/* Client Capability Report TLV */
		ret = agent_gen_client_cap_report(a, resp,
				result_code, NULL);
		if (ret)
			goto error;

		/* Error Code TLV */
		ret = agent_gen_tlv_error_code(a, resp,
				cl_info->macaddr, reason_code);
		if (ret)
			goto error;
	}

	cmdu_put_eom(resp);

	return resp;
error:
	cmdu_free(resp);

	return NULL;
}

struct cmdu_buff *agent_gen_topology_discovery(struct agent *a, uint8_t *almac,
					       uint8_t *macaddr)
{
	struct cmdu_buff *frm = NULL;
	uint16_t mid = 0;
	uint8_t origin[6] = {0x01, 0x80, 0xc2, 0x00, 0x00, 0x13};
	int ret = 0;

	frm = cmdu_alloc_simple(CMDU_TYPE_TOPOLOGY_DISCOVERY, &mid);
	if (!frm) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	ret = agent_gen_al_mac(a, frm, almac);
	if (ret)
		goto out;

	ret = agent_gen_mac(a, frm, macaddr);
	if (ret)
		goto out;

	memcpy(frm->origin, origin, 6);
	cmdu_put_eom(frm);

	return frm;
out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_failed_connection(struct agent *a,
		uint8_t *sta, int status_code, int reason_code)
{
	uint16_t mid = 0;
	int ret;
	struct cmdu_buff *frm = NULL;

	frm = cmdu_alloc_simple(CMDU_FAILED_CONNECTION, &mid);
	if (!frm) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, a->cntlr_almac, 6);

	ret = agent_gen_sta_mac(a, frm, sta);
	if (ret)
		goto out;

	ret = agent_gen_status_code(a, frm, status_code);
	if (ret)
		goto out;

	if (status_code == 0) {
		ret = agent_gen_reason_code(a, frm, reason_code);
		if (ret)
			goto out;
	}

	cmdu_put_eom(frm);

	return frm;

out:
	cmdu_free(frm);
	return NULL;
}

#if (EASYMESH_VERSION > 2)
struct cmdu_buff *agent_gen_bss_configuration_request(struct agent *a)
{
	struct wifi_radio_element *re = NULL;
	struct cmdu_buff *req_cmdu;
	uint16_t mid = 0;

	req_cmdu = cmdu_alloc_simple(CMDU_BSS_CONFIG_REQUEST, &mid);
	if (!req_cmdu) {
		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	/*  One Multi-AP Profile TLV */
	if (agent_gen_map_profile(a, req_cmdu, a->cfg.map_profile)) {
		dbg("%s: agent_gen_map_profile failed.\n", __func__);
		goto out;
	}

	/* One SupportedService TLV */
	if (agent_gen_supported_service(a, req_cmdu)) {
		dbg("%s: agent_gen_supported_service failed.\n", __func__);
		goto out;
	}

	/*  One AKM Suite Capabilities TLV */
	if (agent_gen_akm_suite_cap(a, req_cmdu)) {
		dbg("%s: agent_gen_akm_suite_cap failed.\n", __func__);
		goto out;
	}

	/* One or more AP Radio Basic Capabilities TLV */
	list_for_each_entry(re, &a->radiolist, list) {
		if (agent_gen_ap_radio_basic_cap(a, req_cmdu, re)) {
			dbg("%s: agent_gen_ap_radio_basic_cap failed.\n", __func__);
			goto out;
		}

		/* One or more AP Radio Advanced Capabilities TLV */
		if (agent_gen_ap_radio_adv_cap(a, req_cmdu, re)) {
			dbg("%s: agent_gen_ap_radio_adv_cap failed.\n", __func__);
			goto out;
		}

		/* Zero or more Backhaul STA Radio Capabilities TLV */
		if (agent_gen_bk_sta_radio_cap_tlv(a, req_cmdu, re)) {
			dbg("%s: agent_gen_bk_sta_radio_cap_tlv failed.\n", __func__);
			goto out;
		}
	}

	/* One Profile-2 AP Capability TLV */
	if (agent_gen_profile2_ap_cap(a, req_cmdu)) {
		dbg("%s: agent_gen_profile2_ap_cap failed.\n", __func__);
		goto out;
	}


	/* One or more JSON encoded DPP Configuration Request Object attributes */
	if (agent_gen_conf_req_object_atrributes(a, req_cmdu)) {
		dbg("%s: agent_gen_conf_req_object_atrributes failed.\n", __func__);
		goto out;
	}

	if (cmdu_put_eom(req_cmdu)) {
		dbg("%s: cmdu_put_eom failed.\n", __func__);
		goto out;
	}

	return req_cmdu;

out:
	cmdu_free(req_cmdu);
	return NULL;
}

struct cmdu_buff *agent_gen_bss_configuration_result(struct agent *a)
{
	struct cmdu_buff *result_cmdu;
	uint16_t mid = 0;

	result_cmdu = cmdu_alloc_simple(CMDU_BSS_CONFIG_RESULT, &mid);
	if (!result_cmdu) {
		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	/* One BSS Configuration Report TLV */
	if (agent_gen_bss_config_report_tlv(a, result_cmdu)) {
		dbg("%s: agent_gen_bss_config_report_tlv failed.\n", __func__);
		goto error;
	}

#if (EASYMESH_VERSION >= 6)
	/* Agent AP MLD Configuration TLV */
	if (a->num_ap_mld > 0)
		if (agent_gen_ap_mld_config(a, result_cmdu))
			goto error;

	/* Backhaul STA MLD Configuration TLV */
	if (a->bstamld) {
		if (agent_gen_bsta_mld_config(a, result_cmdu))
			goto error;
	}
#endif

	return result_cmdu;
error:
	cmdu_free(result_cmdu);
	return NULL;
}

struct cmdu_buff *agent_gen_dpp_bootstrapping_uri_notification(
		struct agent *a, uint8_t *radio_id, uint8_t *bssid,
		uint8_t *bksta, char *dpp_uri, int uri_len)
{
	int ret;
	uint16_t mid = 0;
	struct cmdu_buff *frm = NULL;

	frm = cmdu_alloc_simple(CMDU_DPP_BOOTSTRAPING_URI, &mid);
	if (!frm) {
		agnt_dbg(LOG_DPP, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, a->cntlr_almac, 6);
	ret = agent_gen_dpp_bootstrapping_uri_notif(a, frm, radio_id,
			bssid, bksta, dpp_uri, uri_len);
	if (ret)
		goto out;

	cmdu_put_eom(frm);

	return frm;
out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_proxied_encap_dpp(struct agent *a,
					      uint8_t *enrollee,
					      uint8_t frametype,
					      uint8_t *frame,
					      uint16_t framelen,
					      uint8_t *hash,
					      uint16_t hashlen)
{
	struct cmdu_buff *frm;
	uint16_t mid = 0;

	frm = cmdu_alloc_simple(CMDU_PROXIED_ENCAP_DPP, &mid);
	if (!frm) {
		agnt_dbg(LOG_DPP, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, a->cntlr_almac, 6);

	/* One 1905 Encap DPP TLV */
	if (agent_gen_1905_encap_dpp_tlv(a, frm, enrollee,
					 frametype, framelen, frame)) {
		agnt_dbg(LOG_DPP, "%s: agent_gen_1905_encap_dpp_tlv failed.\n", __func__);
		goto out;
	}

	/* Zero or One Chirp Value TLV */
	if (hashlen && hash) {
		if (agent_gen_dpp_chirp_value(a, frm, enrollee, 1, hashlen, hash)) {
			agnt_dbg(LOG_DPP, "%s: agent_gen_chirp_value_tlv failed.\n", __func__);
			goto out;
		}
	}

	cmdu_put_eom(frm);
	return frm;

out:
	agnt_trace(LOG_DPP, "%s: cmdu gen error\n", __func__);
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_profile2_error_code_cmdu(struct agent *a,
                                                     uint8_t *origin,
                                                     uint8_t reason,
                                                     uint8_t bssid[6],
                                                     uint32_t spr_id,
                                                     uint16_t qmid)
{
	struct cmdu_buff *frm;
	uint16_t mid = 0;
	int ret;

	frm = cmdu_alloc_simple(CMDU_ERROR_RESPONSE, &mid);
	if (!frm) {
		trace("%s: cmdu alloc error\n", __func__);

		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	ret = agent_gen_profile2_error_code_tlv(a, frm,
	                                        reason,
	                                        bssid,
	                                        spr_id,
	                                        qmid);
	if (ret)
		goto error;

	memcpy(frm->origin, origin, 6);
	cmdu_put_eom(frm);
	return frm;
error:
	trace("%s: cmdu gen error\n", __func__);
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_direct_encap_dpp(struct agent *a,
					      uint8_t *dst,
					      uint8_t *frame,
					      uint16_t framelen)
{
	struct cmdu_buff *frm;
	uint16_t mid = 0;

	frm = cmdu_alloc_simple(CMDU_DIRECT_ENCAP_DPP, &mid);
	if (!frm) {
		agnt_dbg(LOG_DPP, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, dst, 6);

	/* One DPP Message TLV */
	if (agent_gen_dpp_message_tlv(a, frm, framelen, frame)) {
		agnt_dbg(LOG_DPP, "%s: agent_gen_dpp_message_tlv failed.\n", __func__);
		goto out;
	}

	cmdu_put_eom(frm);
	return frm;

out:
	agnt_trace(LOG_DPP, "%s: cmdu gen error\n", __func__);
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_chirp_notification(struct agent *a,
					      uint8_t *enrollee,
					      uint8_t *hash,
					      uint16_t hashlen)
{
	struct cmdu_buff *frm;
	uint16_t mid = 0;

	frm = cmdu_alloc_simple(CMDU_CHIRP_NOTIFICATION, &mid);
	if (!frm) {
		agnt_dbg(LOG_DPP, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, a->cntlr_almac, 6);

	/* One DPP Chirp Value TLV */
	if (agent_gen_dpp_chirp_value(a, frm, enrollee, 1, hashlen, hash)) {
		agnt_dbg(LOG_DPP, "%s: agent_gen_chirp_value_tlv failed.\n", __func__);
		goto out;
	}

	cmdu_put_eom(frm);
	return frm;

out:
	agnt_trace(LOG_DPP, "%s: cmdu gen error\n", __func__);
	cmdu_free(frm);
	return NULL;
}

#endif // EASYMESH_VERSION > 2

#if (EASYMESH_VERSION >= 6)
struct cmdu_buff *agent_gen_early_ap_cap_report(struct agent *a, uint8_t *origin,
						uint16_t mid)
{
	struct wifi_radio_element *re = NULL;
	struct cmdu_buff *frm;

	frm = cmdu_alloc_simple(CMDU_EARLY_AP_CAPABILITY_REPORT, &mid);
	if (!frm) {
		dbg("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(frm->origin, origin, 6);
	cmdu_set_mid(frm, mid);

	if (agent_gen_ap_caps(a, frm)) {
		err("%s: agent_gen_ap_caps failed.\n", __func__);
		goto out;
	}

	list_for_each_entry(re, &a->radiolist, list) {
		if (agent_gen_ap_radio_basic_cap(a, frm, re)) {
			err("%s: agent_gen_ap_radio_basic_cap failed.\n", __func__);
			goto out;
		}

		/* AP HT Capabilities TLV */
		if (agent_gen_ap_ht_caps(a, frm, re))
			dbg("%s: agent_gen_ap_ht_caps failed.\n", __func__);

		/* AP VHT Capabilities TLV */
		if (agent_gen_ap_vht_caps(a, frm, re))
			dbg("%s: agent_gen_ap_vht_caps failed.\n", __func__);

		/* AP HE Capabilities TLV */
		if (agent_gen_ap_he_caps(a, frm, re))
			dbg("%s: agent_gen_ap_he_caps failed.\n", __func__);

		if (agent_gen_ap_wifi6_caps(a, frm, re))
			dbg("%s: agent_gen_ap_wifi6_caps failed.\n", __func__);

		if (agent_gen_ap_radio_adv_cap(a, frm, re))
			dbg("%s: agent_gen_ap_radio_adv_cap failed.\n", __func__);

	}

	if (agent_gen_akm_suite_cap(a, frm)) {
		err("%s: agent_gen_akm_suite_cap failed.\n", __func__);
		goto out;
	}

	if (agent_gen_wifi7_agent_caps(a, frm))
		err("%s: agent_gen_wifi7_agent_cap failed.\n", __func__);

	if (agent_gen_profile2_ap_cap(a, frm)) {
		err("%s: agent_gen_profile2_ap_cap failed.\n", __func__);
		goto out;
	}

	cmdu_put_eom(frm);
	return frm;

out:
	trace("%s: cmdu gen error\n", __func__);
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *agent_gen_ap_mld_configuration_response(struct agent *a,
						uint8_t *origin, uint16_t mid)
{
	struct cmdu_buff *resp;

	resp = cmdu_alloc_simple(CMDU_AP_MLD_CONFIG_RESPONSE, &mid);
	if (!resp) {
		agnt_dbg(LOG_MLO, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(resp->origin, origin, 6);
	cmdu_set_mid(resp, mid);

	if (agent_gen_bsta_mld_config(a, resp)) {
		agnt_trace(LOG_MLO, "%s: cmdu gen error\n", __func__);
		cmdu_free(resp);
		return NULL;
	}

	cmdu_put_eom(resp);
	return resp;
}

struct cmdu_buff *agent_gen_bsta_mld_configuration_response(struct agent *a,
						uint8_t *origin, uint16_t mid)
{
	struct cmdu_buff *resp;

	resp = cmdu_alloc_simple(CMDU_BSTA_MLD_CONFIG_RESPONSE, &mid);
	if (!resp) {
		agnt_dbg(LOG_MLO, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

	memcpy(resp->origin, origin, 6);
	cmdu_set_mid(resp, mid);

	if (agent_gen_ap_mld_config(a, resp))
		goto out;

	if (agent_gen_eht_operations(a, resp))
		goto out;

	cmdu_put_eom(resp);
	return resp;
out:
	agnt_trace(LOG_MLO, "%s: cmdu gen error\n", __func__);
	cmdu_free(resp);
	return NULL;
}
#endif // EASYMESH_VERSION >= 6

