/*
 * cntlr_cmdu.c - cmdu building function
 *
 * Copyright (C) 2020 IOPSYS Software Solutions AB. All rights reserved.
 *
 * Author: jakob.olsson@iopsys.eu
 *
 */
#include "cntlr_cmdu.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <time.h>
#include <libubox/list.h>
#include <i1905_wsc.h>
#include <cmdu.h>
#include <1905_tlvs.h>
#include <easymesh.h>

#include "config.h"
#include "utils/debug.h"
#include "cntlr.h"
#include "cntlr_map.h"
#include "cntlr_tlv.h"
#include "steer.h"
#include "sta.h"
#include "wifi_dataelements.h"


struct cmdu_buff *cntlr_gen_ap_autoconfig_renew(struct controller *c,
		uint8_t *dst)
{
	struct cmdu_buff *resp;
	int ret;
	uint16_t mid = 0;

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

	ret = cntlr_gen_supp_role(c, resp, IEEE80211_ROLE_REGISTRAR);
	if (ret)
		goto out;

	ret = cntlr_gen_al_mac(c, resp, c->almacaddr);
	if (ret)
		goto out;

	/* Hard-code dummy 5GHz, ignored by agent according to spec */
	ret = cntlr_gen_supported_freq_band(c, resp, IEEE80211_FREQUENCY_BAND_5_GHZ);
	if (ret)
		goto out;

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

struct cmdu_buff *cntlr_gen_ap_capability_query(struct controller *c,
		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 *cntlr_gen_client_caps_query(struct controller *c,
		uint8_t *origin, uint8_t *sta, uint8_t *bssid)
{
	uint16_t mid = 0;
	int ret;
	struct cmdu_buff *req;

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

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

	ret = cntlr_gen_client_info(c, req, sta, bssid);
	if (ret)
		goto error;

	cmdu_put_eom(req);

	return req;
error:
	cmdu_free(req);

	return NULL;
}

struct bcnreq *cntlr_add_bcnreq(struct controller *c, uint8_t *sta_mac,
				uint8_t *agent_mac, int num_chreport)
{
	struct bcnreq *br;

	dbg("%s: adding STA " MACFMT " to list of active bcn requests\n",
		    __func__, MAC2STR(sta_mac));

	br = cntlr_find_bcnreq(c, sta_mac, agent_mac);
	if (br) {
		time(&br->req_time);
		return br;
	}

	br = calloc(1, sizeof(struct bcnreq));
	if (!br)
		return NULL;

	memcpy(br->sta_mac, sta_mac, 6);
	memcpy(br->agent_mac, agent_mac, 6);
	br->num_channel = num_chreport;
	list_add(&br->list, &c->bcnreqlist);

	return br;
}

void cntlr_del_bcnreq(struct controller *c, uint8_t *sta_mac, uint8_t *agent_mac)
{
	trace("%s: --->\n", __func__);

	struct bcnreq *br;

	br = cntlr_find_bcnreq(c, sta_mac, agent_mac);
	if (br) {
		dbg("%s Remove Beacon-Metrics request for STA " MACFMT "\n",
		    __func__, MAC2STR(br->sta_mac));
		list_del(&br->list);
		free(br);
	}
}

struct cmdu_buff *cntlr_gen_beacon_metrics_query(struct controller *c,
		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)
{
	trace("%s:--->\n", __func__);

	struct cmdu_buff *resp = NULL;
	int ret = 0;
	uint16_t mid = 0;

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

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

	/* Beacon metrics query TLV */
	ret = cntlr_gen_tlv_beacon_metrics_query(c, resp,
			sta_addr, opclass, channel, bssid,
			reporting_detail, ssid, num_report,
			report, num_element, element);

	if (ret)
		goto fail;

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

	cmdu_put_eom(resp);


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

struct cmdu_buff *cntlr_gen_backhaul_steer_request(struct controller *c,
		uint8_t *origin, uint8_t *bkhaul, uint8_t *target_bssid,
		uint8_t op_class, uint8_t channel)
{
	int ret;
	struct cmdu_buff *resp;

	resp = cmdu_alloc_frame(1500);
	if (!resp) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	cmdu_set_type(resp, CMDU_BACKHAUL_STEER_REQUEST);

	ret = cntlr_gen_backhaul_steer_req(c, resp, bkhaul, target_bssid, op_class,
						channel);
	if (ret)
		return NULL;

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

int cntlr_send_backhaul_steer_request(struct controller *c, uint8_t *agent,
				      uint8_t *bsta, uint8_t *tbssid,
				      uint8_t opclass, uint8_t channel)
{
	trace("%s:--->\n", __func__);

	struct cmdu_buff *cmdu = NULL;

	cmdu = cntlr_gen_backhaul_steer_request(c, agent, bsta, tbssid,
						opclass, channel);
	if (!cmdu)
		return -1;

	cntlr_notify_backhaul_steer_req_evt(c, agent, bsta, tbssid, opclass, channel);

	send_cmdu(c, cmdu);
	cmdu_free(cmdu);
	return 0;
}

struct cmdu_buff *cntlr_gen_1905_link_metric_query(struct controller *c,
		uint8_t *origin)
{
	int ret;
	uint16_t mid = 0;
	struct cmdu_buff *req;

	req = cmdu_alloc_simple(CMDU_TYPE_LINK_METRIC_QUERY, &mid);
	if (!req) {
		fprintf(stderr, "%s: -ENOMEM\n", __func__);
		return NULL;
	}

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

	/* 1905 Link Metric Query TLV */
	ret = cntlr_gen_1905_link_metric_tlv(c, req);
	if (ret)
		goto out;

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

struct cmdu_buff *cntlr_gen_ap_metrics_query(struct controller *c,
		uint8_t *origin, int num_bss, uint8_t *bsslist,
		int num_radio, uint8_t *radiolist)
{
	int i, ret;
	uint16_t mid = 0;
	struct cmdu_buff *req;

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

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

	/* AP Metric Query TLV */
	ret = cntlr_gen_ap_metric_query(c, req, num_bss, bsslist);
	if (ret)
		goto out;

// #ifdef PROFILE2
	/* AP Radio Identifier TLV */
	for (i = 0; i < num_radio; i++) {
		uint8_t radio_mac[6] = {0};

		memcpy(radio_mac, &radiolist[i * 6], 6);
		ret = cntlr_gen_ap_radio_identifier(c, req, radio_mac);
		if (ret)
			goto out;
	}
// #endif
	cmdu_put_eom(req);
	return req;
out:
	cmdu_free(req);
	return NULL;
}

struct cmdu_buff *cntlr_gen_policy_config_req(struct controller *c,
		uint8_t *agent_id, struct node_policy *found,
		int num_radio, uint8_t *radiolist,
		int num_bss, uint8_t *bsslist)
{
	int i, ret;
	uint16_t mid = 0;
	struct cmdu_buff *frm;

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

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

	/* Steering Policy TLV */
	ret = cntlr_gen_steering_policy(c, found, frm,
			num_radio, radiolist);
	if (ret)
		goto out;

	/* Metric Reporting Policy TLV */
	ret = cntlr_gen_metric_report_policy(c, found, frm,
			num_radio, radiolist);
	if (ret)
		goto out;

// #ifdef PROFILE2
	if (c->cfg.enable_ts) {
		/* Default 802.1Q setting TLV */
		ret = cntlr_gen_8021q_settings(c, frm);
		if (ret)
			goto out;

		/* Traffic Seperation Policy TLV */
		ret = cntlr_gen_traffic_sep_policy(c, frm);
		if (ret)
			goto out;
	}

#if (EASYMESH_VERSION >= 4)
	/* QoS management policy TLV */
	if (c->cfg.qos.enabled) {
		ret = cntlr_gen_qos_management_policy_tlv(c, frm);
		if (ret)
			goto out;
	}
#endif

	/* Channel Scan Reporting Policy TLV */
	ret = cntlr_gen_ch_scan_rep_policy(c, found, frm);
	if (ret)
		goto out;

	/* Unsuccessful Association Policy TLV */
	ret = cntlr_gen_unsuccess_assoc_policy(c, found, frm);
	if (ret)
		goto out;

	/* Backhaul BSS Configuration TLV */
	for (i = 0; i < num_bss; i++) {
		uint8_t bssid[6] = {0};

		memcpy(bssid, &bsslist[i*6], 6);
		ret = cntlr_gen_backhaul_bss_config(c, found, frm, bssid);
		if (ret)
			goto out;
	}
// #endif
	cmdu_put_eom(frm);

	return frm;
out:
	cmdu_free(frm);

	return NULL;
}

struct cmdu_buff *cntlr_gen_sta_metric_query(struct controller *c,
		uint8_t *origin, uint8_t *sta)
{
	int ret;
	uint16_t mid = 0;
	struct cmdu_buff *frm;

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

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

	/* STA MAC Address type TLV */
	ret = cntlr_gen_sta_mac(c, frm, sta);
	if (ret)
		goto out;

	cmdu_put_eom(frm);
	return frm;

out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *cntlr_gen_unassoc_sta_metric_query(struct controller *c,
		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;
	struct node *n;

	/* A Multi-AP Controller 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.
	 */
	n = cntlr_find_node(c, origin);
	if (!n)
		return NULL;

	cntlr_dbg(LOG_STA, "%s %d ap_cap = 0x%02x\n", __func__, __LINE__, n->ap_cap);
	if (!(n->ap_cap & (UNASSOC_STA_REPORTING_ONCHAN | UNASSOC_STA_REPORTING_OFFCHAN))) {
		cntlr_dbg(LOG_STA, "%s: Unassoc STA metric not supported by " MACFMT "\n",
			  __func__, MAC2STR(origin));
		return NULL;
	}

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

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

	/* Unassociated STA link metrics query TLV */
	ret = cntlr_gen_unassociated_sta_link_metrics(c, frm,
				opclass, num_metrics, metrics);
	if (ret)
		goto out;

	cmdu_put_eom(frm);
	return frm;

out:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *cntlr_gen_bk_caps_query(struct controller *c,
		uint8_t *origin)
{
	uint16_t mid = 0;
	struct cmdu_buff *resp;

	/* Allocate the cmdu_data structure */
	resp = cmdu_alloc_frame(3000);
	if (!resp) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}
	cmdu_set_type(resp, CMDU_BACKHAUL_STA_CAPABILITY_QUERY);
	cmdu_set_mid(resp, mid);

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

struct cmdu_buff *cntlr_gen_ap_autoconfig_search(struct controller *c,
		uint8_t profile, uint8_t band)
{
	struct cmdu_buff *frm = NULL;
	int ret = 0;
	uint16_t mid = 0;
	uint8_t origin[6] = {0x01, 0x80, 0xc2, 0x00, 0x00, 0x13};

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

	CMDU_SET_RELAY_MCAST(frm->cdata);

	ret = cntlr_gen_al_mac(c, frm, c->almacaddr);
	if (ret)
		goto out;

	ret = cntlr_gen_searched_role(c, frm, IEEE80211_ROLE_REGISTRAR);
	if (ret)
		goto out;

	ret = cntlr_gen_autoconf_freq_band(c, frm, band);
	if (ret)
		goto out;

	ret = cntlr_gen_supp_service(c, frm);
	if (ret)
		goto out;

	ret = cnltr_gen_searched_service(c, frm,
			SEARCHED_SERVICE_MULTIAP_CONTROLLER);
	if (ret)
		goto out;

	ret = cntlr_gen_map_profile(c, frm, c->cfg.map_profile);
	if (ret)
		goto out;

#if (EASYMESH_VERSION > 2)
#if 0
	if (c->cfg.map_profile > MULTIAP_PROFILE_2) {
		ret = cntlr_gen_chirp_value_tlv(c, frm);
		if (ret)
			goto out;
	}
#endif
#endif

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

struct cmdu_buff *cntlr_gen_ap_autoconfig_response(struct controller *c,
#if (EASYMESH_VERSION > 2)
#ifdef USE_LIBDPP
		bool hash_validity, uint8_t *hash, uint16_t hashlen,
#endif
#endif
		uint8_t *dest, uint8_t band, uint16_t mid)
{
	struct cmdu_buff *resp;
	int ret;

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

	ret = cntlr_gen_supp_role(c, resp, IEEE80211_ROLE_REGISTRAR);
	if (ret)
		goto out;

	ret = cntlr_gen_supported_freq_band(c, resp, band);
	if (ret)
		goto out;

	ret = cntlr_gen_supp_service(c, resp);
	if (ret)
		goto out;

	ret = cntlr_gen_map_profile(c, resp, c->cfg.map_profile);
	if (ret)
		goto out;

#if (EASYMESH_VERSION > 2)
	if (c->cfg.map_profile > MULTIAP_PROFILE_2) {
		ret = cntlr_gen_device_1905_layer_security_cap(c, resp);
		if (ret)
			goto out;

#ifdef USE_LIBDPP
		if (hash) {
			ret = cntlr_gen_chirp_value_tlv(c, resp, NULL,
							hash_validity, hashlen,
							hash);
			if (ret)
				goto out;
		}
#endif /* USE_LIBDPP */

#if (EASYMESH_VERSION > 5)
		ret = cntlr_gen_cntlr_capability(c, resp, CONTROLLER_EARLY_AP_CAP_SUPPORTED);
#elif (EASYMESH_VERSION >= 4)
		ret = cntlr_gen_cntlr_capability(c, resp, 0x00);
#endif
		if (ret)
			goto out;
	}
#endif

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

struct cmdu_buff *cntlr_gen_ap_autoconfig_wsc(struct controller *c,
		struct node *n, struct cmdu_buff *rx_cmdu, uint8_t *radio_id,
		struct tlv *wsc, uint16_t mid)
{
	struct iface_credential *ap = NULL;
	struct cmdu_buff *resp;
	uint16_t msglen;
	uint8_t *msg;
	uint16_t e_auth = 0;
	uint8_t e_band = 0;
	uint16_t attrlen = 0;
	int ret;
	struct netif_radio *r;

	resp = cmdu_alloc_frame(6400);
	if (!resp) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	cmdu_set_type(resp, CMDU_TYPE_AP_AUTOCONFIGURATION_WSC);
	cmdu_set_mid(resp, mid);

	msglen = tlv_length(wsc);
	msg = wsc->data;

	ret = wsc_msg_get_attr(msg, msglen, ATTR_RF_BANDS, &e_band, &attrlen);
	if (ret) {
		warn("Error getting band from wsc msg\n");
		goto out;
	}

	ret = wsc_msg_get_attr(msg, msglen, ATTR_AUTH_TYPE_FLAGS,
			       (uint8_t *) &e_auth, &attrlen);
	if (ret) {
		warn("Error getting auth from wsc msg\n");
		goto out;
	}

	e_auth = buf_get_be16((uint8_t *) &e_auth);

	if (c->cfg.enable_ts) {
		ret = cntlr_gen_8021q_settings(c, resp);
		if (ret)
			goto out;

		ret = cntlr_gen_traffic_sep_policy(c, resp);
		if (ret)
			goto out;
	}

	ret = cntlr_gen_ap_radio_identifier(c, resp, radio_id);
	if (ret)
		goto out;

	/* only add radio sections for discovered agents */
	if (cntlr_get_node_policy(&c->cfg, rx_cmdu->origin)) {
		cntlr_config_add_node_radio(&c->cfg, rx_cmdu->origin, radio_id, e_band);
		cntlr_resync_config(c, true);
	}

	r = cntlr_node_add_radio(c, n, radio_id);
	if (!r)
		goto out;

	r->radio_el->band = e_band;

#if (EASYMESH_VERSION >= 6)
	if (cntlr_radio_support_wifi7(&r->radio_el->wifi7caps_ap) && c->cfg.num_mlds)
		cntlr_gen_ap_mld_config(c, resp, n, &r->radio_el->wifi7caps_ap);

	if (cntlr_radio_support_wifi7(&r->radio_el->wifi7caps_bsta) && c->cfg.num_mlds)
		cntlr_gen_bsta_mld_config(c, resp, n, &r->radio_el->wifi7caps_bsta);

	cntlr_gen_bss_advanced_config_tlv(c, resp, n);
	if (e_auth & WPS_AUTH_0x0200)
		cntlr_gen_rsn_parameters_config_tlv(c, resp, n);
#endif

	ret = 1;
	list_for_each_entry(ap, &c->cfg.aplist, list) {
		struct wps_credential cred = {0};

		if (ap->band != e_band)
			continue;

		if (!(e_auth & WPS_AUTH_0x0200) &&
		    ap->sec == BIT(WIFI_SECURITY_OWE))
			continue;

		memcpy(&cred, &ap->cred, sizeof(cred));

#if (EASYMESH_VERSION >= 6)
		/* if agent supports MLD then send MLD credentials instead of
		 * the iface credentials
		 */
		if (ap->mld_id) {
			if ((ap->multi_ap == 0x02 && cntlr_radio_support_wifi7(&r->radio_el->wifi7caps_ap)) ||
			    (ap->multi_ap == 0x01 && cntlr_radio_support_wifi7(&r->radio_el->wifi7caps_bsta))) {
				struct mld_credential *mld;

				mld = cntlr_config_get_mld_by_id(&c->cfg, ap->mld_id);
				if (mld) {
					memcpy(&cred, &mld->cred, sizeof(cred));
					/* mld configs are not per-band
					 * so must be set during usage
					 */
					cred.band = e_band;
					if (mld->cred.auth_type == 0) {
						cred.auth_type = ap->cred.auth_type;
						cred.enc_type = ap->cred.enc_type;
					}
				}
			}
		}
#endif
		/* populate global WSC metadata into credential */
		strncpy(cred.manufacturer, c->cfg.wscdev.manufacturer, 64);
		strncpy(cred.model_name, c->cfg.wscdev.model_name, 32);
		strncpy(cred.device_name, c->cfg.wscdev.device_name, 32);
		memcpy(cred.model_number, c->cfg.wscdev.model_number, 32);
		memcpy(cred.serial_number, c->cfg.wscdev.serial_number, 32);
		memcpy(cred.device_type, c->cfg.wscdev.device_type, 8);

		/* Will return non-zero if band did not match OR on failure */
		ret = cntlr_gen_wsc(c, resp, &cred, ap, msg, msglen, e_band, e_auth);
	}

	/* This means that none of the ap in the radio is enabled so set a tear down bit for this */
	if (ret == 1)
		cntlr_gen_wsc(c, resp, NULL, NULL, msg, msglen, e_band, e_auth);

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

#if (EASYMESH_VERSION >= 6)
struct cmdu_buff *cntlr_gen_bsta_mld_configuration_request(struct controller *c,
							   uint8_t *agent_mac,
							   struct wifi_wifi7_capabilities *caps)
{
	struct cmdu_buff *resp;
	struct node *n = NULL;
	int ret;

	n = cntlr_find_node(c, agent_mac);
	if (!n)
		return NULL;

	if (!cntlr_radio_support_wifi7(caps))
		return NULL;

	resp = cmdu_alloc_frame(6400);
	if (!resp) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	cmdu_set_type(resp, CMDU_BSTA_MLD_CONFIG_REQUEST);

	/* Agent BSTA MLD Configuration TLV */
	ret = cntlr_gen_bsta_mld_config(c, resp, n, caps);
	if (ret)
		goto out;

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


struct cmdu_buff *cntlr_gen_ap_mld_configuration_request(struct controller *c,
							 uint8_t *agent_mac,
							 struct wifi_wifi7_capabilities *caps)
{
	struct cmdu_buff *resp;
	struct node *n = NULL;
	int ret;

	n = cntlr_find_node(c, agent_mac);
	if (!n)
		return NULL;

	if (!cntlr_radio_support_wifi7(caps))
		return NULL;

	resp = cmdu_alloc_frame(6400);
	if (!resp) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	cmdu_set_type(resp, CMDU_AP_MLD_CONFIG_REQUEST);

	/* Agent AP MLD Configuration TLV */
	ret = cntlr_gen_ap_mld_config(c, resp, n, caps);
	if (ret)
		goto out;

	/* EHT Operations TLV */
	ret = cntlr_gen_eht_operations(c, resp, n);
	if (ret)
		goto out;

	ret = cntlr_gen_bss_advanced_config_tlv(c, resp, n);
	if (ret)
		goto out;

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


#endif

struct cmdu_buff *cntlr_gen_topology_query(struct controller *c,
		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) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}

	ret = cntlr_gen_map_profile(c, resp, c->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 *cntlr_gen_cmdu_1905_ack(struct controller *c,
		struct cmdu_buff *rx_cmdu,
		struct sta_error_response *sta_resp, uint32_t sta_count)
{
	trace("%s:--->\n", __func__);

	struct cmdu_buff *resp = NULL;
	uint16_t mid = cmdu_get_mid(rx_cmdu);
	int j;

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

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

	/* Error Code TLV 17.2.36 */
	for (j = 0; j < sta_count; j++) {
		cntlr_gen_tlv_error_code(c, resp, sta_resp[j].sta_mac,
				sta_resp[j].response);
	}

	cmdu_put_eom(resp);
	return resp;
}

struct cmdu_buff *cntlr_gen_channel_scan_request(struct controller *c,
		uint8_t *agent_mac, struct scan_req_data *req_data)
{
	int ret;
	uint16_t mid = 0;
	struct cmdu_buff *resp;

	/* Allocate the cmdu_data structure */
	resp = cmdu_alloc_frame(3000);
	if (!resp) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}
	cmdu_set_type(resp, CMDU_CHANNEL_SCAN_REQUEST);
	cmdu_set_mid(resp, mid);
	ret = cntlr_gen_channel_scan_req(c, resp, req_data);
	if (ret)
		goto error;

	memcpy(resp->origin, agent_mac, 6);
	cmdu_put_eom(resp);

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

struct cmdu_buff *cntlr_gen_channel_preference_query(
		struct controller *c, uint8_t *agent)
{
	uint16_t mid = 0;
	struct cmdu_buff *req;

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

	memcpy(req->origin, agent, 6);
	cmdu_put_eom(req);

	return req;
}

int cntlr_send_channel_preference_query(struct controller *c, uint8_t *agent)
{
	struct cmdu_buff *cmdu;

	cmdu = cntlr_gen_channel_preference_query(c, agent);
	if (!cmdu)
		return -1;
	send_cmdu(c, cmdu);
	cmdu_free(cmdu);

	return 0;
}

uint16_t cntlr_send_channel_selection(struct controller *c, uint8_t *agent, uint8_t *radio,
				      struct wifi_radio_opclass *opclass)
{
	struct cmdu_buff *cmdu;
	uint16_t mid;
#if (EASYMESH_VERSION >= 6)
	struct node *n;
	int ret;
#endif

	cmdu = cntlr_gen_channel_sel_request(c, agent, radio, opclass);
	if (!cmdu)
		return -1;

#if (EASYMESH_VERSION >= 6)
	n = cntlr_find_node(c, agent);
	if (n) {
		ret = cntlr_gen_eht_operations(c, cmdu, n);
		if (ret) {
			cmdu_free(cmdu);
			return 0xffff;
		}
	}
#endif

	cmdu_put_eom(cmdu);
	mid = send_cmdu(c, cmdu);
	cmdu_free(cmdu);

	return mid;
}

struct cmdu_buff* cntlr_gen_cac_req(struct controller *c, uint8_t *agent,
		int num_data, struct cac_data *data)
{
	uint16_t mid = 0;
	int ret;
	struct cmdu_buff *req;

	if (!agent || !data)
		return NULL;

	req = cmdu_alloc_simple(CMDU_CAC_REQUEST, &mid);
	if (!req)
		return NULL;

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

	ret = cntlr_gen_cac_tlv(c, req, MAP_TLV_CAC_REQ,
			num_data, data);
	if (ret) {
		cmdu_free(req);
		return NULL;
	}

	return req;
}

struct cmdu_buff* cntlr_gen_cac_term(struct controller *c, uint8_t *agent,
		int num_data, struct cac_data *data)
{
	uint16_t mid = 0;
	int ret;
	struct cmdu_buff *req;

	if (!agent || !data)
		return NULL;

	req = cmdu_alloc_simple(CMDU_CAC_TERMINATION, &mid);
	if (!req)
		return NULL;

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

	ret = cntlr_gen_cac_tlv(c, req, MAP_TLV_CAC_TERMINATION,
			num_data, data);
	if (ret) {
		cmdu_free(req);
		return NULL;
	}

	return req;

}

int cntlr_send_channel_scan_request(struct controller *c, uint8_t *agent_mac,
			struct scan_req_data *data)
{
	struct cmdu_buff *cmdu_data = NULL;

	cmdu_data = cntlr_gen_channel_scan_request(c, agent_mac, data);

	if (!cmdu_data)
		return -1;

	send_cmdu(c, cmdu_data);
	cmdu_free(cmdu_data);

	return 0;
}

uint16_t cntlr_send_cac_req(struct controller *c, uint8_t *agent,
			    int num_data, struct cac_data *data)
{
	struct cmdu_buff *cmdu;
	uint16_t mid = 0xffff;

	cmdu = cntlr_gen_cac_req(c, agent, num_data, data);
	if (!cmdu)
		return mid;

	mid = send_cmdu(c, cmdu);
	cmdu_free(cmdu);

	return mid;
}

int cntlr_send_cac_term(struct controller *c, uint8_t *agent,
		        int num_data, struct cac_data *data)
{
	struct cmdu_buff *cmdu;

	cmdu = cntlr_gen_cac_term(c, agent, num_data, data);
	if (!cmdu)
		return -1;

	send_cmdu(c, cmdu);
	cmdu_free(cmdu);

	return 0;
}

struct cmdu_buff *cntlr_gen_client_assoc_ctrl_request(struct controller *c,
		uint8_t *agent_mac, uint8_t *bssid,
		uint8_t assoc_cntl_mode, uint16_t assoc_timeout,
		uint8_t sta_nr, uint8_t *stalist)
{
	struct cmdu_buff *frm;
	uint16_t mid = 0;
	int ret = 0;

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

	/* Client ASSOC CONTROL TLV REQUEST 17.2.31 */
	ret = cntlr_gen_tlv_assoc_ctrl_request(c, frm, bssid,
			assoc_cntl_mode, assoc_timeout, sta_nr, stalist);

	if (ret)
		goto error;

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

error:
	cmdu_free(frm);
	return NULL;
}

int cntlr_send_client_assoc_ctrl_request(struct controller *c,
					 uint8_t *agent_mac, uint8_t *bssid,
					 uint8_t ctrl_mode,
					 uint16_t validity_period,
					 uint8_t sta_nr, uint8_t *stalist,
					 uint16_t *mid)
{
	struct cmdu_buff *cmdu_data = NULL;

	cmdu_data = cntlr_gen_client_assoc_ctrl_request(c, agent_mac,
							bssid,
							ctrl_mode,
							validity_period,
							sta_nr, stalist);

	if (!cmdu_data)
		return -1;

	*mid = cmdu_get_mid(cmdu_data);

	send_cmdu(c, cmdu_data);
	cmdu_free(cmdu_data);

	return 0;
}

struct cmdu_buff *cntlr_gen_higher_layer_data(struct controller *c, uint8_t *addr,
					      uint8_t proto, uint8_t *data,
					      int len)
{
	struct cmdu_buff *frm;
	int ret;

	frm = cmdu_alloc_frame(len + 128);
	if (!frm)
		return NULL;

	cmdu_set_type(frm, CMDU_HIGHER_LAYER_DATA);
	memcpy(frm->origin, addr, 6);

	ret = cntlr_gen_tlv_higher_layer_data(c, frm, proto, data, len);
	if (ret)
		goto error;

	cmdu_put_eom(frm);
	return frm;

error:
	cmdu_free(frm);
	return NULL;
}

struct cmdu_buff *cntlr_gen_client_steer_request(struct controller *c,
						 uint8_t *origin,
						 uint8_t *bssid,
						 uint32_t steer_timeout,
						 uint32_t sta_nr, uint8_t stas[][6],
						 uint32_t bssid_nr, 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 cmdu_buff *frm;
	struct node *n;
	uint16_t mid = 0;
	int ret;

	n = cntlr_find_node(c, origin);
	if (!n) {
		cntlr_warn(LOG_STEER, "%s: Could not find node " MACFMT "\n",
			  __func__, MAC2STR(origin));
		return NULL;
	}

	frm = cmdu_alloc_simple(CMDU_CLIENT_STEERING_REQUEST, &mid);
	if (!frm) {
		cntlr_warn(LOG_STEER,
			   "%s: err = -ENOMEM, Failed to generate cmdu for steering sta!\n",
			   __func__);
		return NULL;
	}

	if (n->map_profile >= MULTIAP_PROFILE_2) {
		/* Profile-2: Separate STAs into agile and non-agile groups */
		uint8_t agile_stas[sta_nr][6];
		uint8_t non_agile_stas[sta_nr][6];
		uint32_t agile_count = 0;
		uint32_t non_agile_count = 0;
		int i;

		for (i = 0; i < sta_nr; i++) {
			struct sta *s = cntlr_find_sta(c->sta_table, stas[i]);

			cntlr_dbg(LOG_STEER, "%s: sta[%u] = " MACFMT "\n",
				  __func__, i, MAC2STR(stas[i]));

			if (s && sta_is_agile_multiband(s))
				memcpy(agile_stas[agile_count++], stas[i], 6);
			else
				memcpy(non_agile_stas[non_agile_count++], stas[i], 6);
		}

		if (non_agile_count > 0) {
			cntlr_dbg(LOG_STEER, "%s: Generating Steering Request TLV for %u non-agile STA(s)\n",
				  __func__, non_agile_count);
			ret = cntlr_gen_tlv_steer_request(c, frm,
							  bssid,
							  steer_timeout,
							  non_agile_count, non_agile_stas,
							  bssid_nr, target_bssid,
							  btm_abridged, btm_disassoc_imminent,
							  btm_disassoc_timeout,
							  is_mandate);
			if (ret)
				goto error;
		}

		if (agile_count > 0) {
			cntlr_dbg(LOG_STEER, "%s: Generating Profile-2 TLV for %u agile STA(s)\n",
				  __func__, agile_count);
			ret = cntlr_gen_tlv_profile2_steer_request(c, frm,
								   bssid,
								   steer_timeout,
								   agile_count, agile_stas,
								   bssid_nr, target_bssid,
								   btm_abridged, btm_disassoc_imminent,
								   btm_disassoc_timeout, mbo_reason,
								   is_mandate);
			if (ret)
				goto error;
		}
	} else {
		cntlr_dbg(LOG_STEER,
			  "%s: Agent doesn't support Profile-2, using Steering Request TLV for all %u STA(s)\n",
			  __func__, sta_nr);
		ret = cntlr_gen_tlv_steer_request(c, frm,
						  bssid,
						  steer_timeout,
						  sta_nr, stas,
						  bssid_nr, target_bssid,
						  btm_abridged, btm_disassoc_imminent,
						  btm_disassoc_timeout,
						  is_mandate);
		if (ret)
			goto error;
	}

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

error:
	cmdu_free(frm);
	return NULL;
}

int cntlr_send_client_steer_request(struct controller *c,
				    uint8_t *agent_mac, uint8_t *bssid,
				    uint32_t steer_timeout,
				    uint32_t sta_nr, uint8_t stas[][6],
				    uint32_t bssid_nr, uint8_t target_bssid[][6],
				    uint32_t request_mode,
				    uint32_t reason)
{
	trace("%s:--->\n", __func__);

	struct cmdu_buff *cmdu_data = NULL;
	bool btm_disassoc_imminent = true;
	uint8_t mbo_reason = (int)reason;
	uint32_t btm_disassoc_timeout;
	bool btm_abridged = false;
	bool is_mandate = true;

	btm_disassoc_timeout = (steer_timeout * 1000 * 1000) / 1024; /* in TUs */;
	cmdu_data = cntlr_gen_client_steer_request(c, agent_mac, bssid,
						   steer_timeout, sta_nr, stas,
						   bssid_nr, target_bssid,
						   btm_abridged,
						   btm_disassoc_imminent,
						   btm_disassoc_timeout,
						   mbo_reason, is_mandate);
	if (!cmdu_data)
		return -1;

	cntlr_notify_client_steer_req_evt(c, bssid, sta_nr, stas, bssid_nr, target_bssid);

	send_cmdu(c, cmdu_data);
	cmdu_free(cmdu_data);

	return 0;
}

struct cmdu_buff *cntlr_gen_comb_infra_metrics_query(struct controller *c, uint8_t *origin, uint8_t *bssid)
{
	struct cmdu_buff *frm;
	uint16_t mid = 0;
	int ret;

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

	ret = cntlr_gen_comb_infra_metrics(c, frm, bssid);
	if (ret)
		goto error;

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

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

#if (EASYMESH_VERSION > 2)
#ifdef USE_LIBDPP
struct cmdu_buff *cntlr_gen_direct_encap_dpp(struct controller *c,
					      uint8_t *dst,
					      uint8_t *frame,
					      uint16_t framelen)
{
	struct cmdu_buff *frm;
	uint16_t mid = 0;

	frm = cmdu_alloc_frame(5000);
	if (!frm) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}
	cmdu_set_type(frm, CMDU_DIRECT_ENCAP_DPP);
	cmdu_set_mid(frm, mid);

	/* One DPP Message TLV */
	if (cntlr_gen_dpp_message_tlv(c, frm, framelen, frame)) {
		warn("%s: cntlr_gen_dpp_message_tlv failed.\n", __func__);
		goto out;
	}

	cmdu_put_eom(frm);
	return frm;

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

struct cmdu_buff *cntlr_gen_proxied_encap_dpp(struct controller *c,
					      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_frame(5000);
	if (!frm) {
		err("%s: -ENOMEM\n", __func__);
		return NULL;
	}
	cmdu_set_type(frm, CMDU_PROXIED_ENCAP_DPP);
	cmdu_set_mid(frm, mid);


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

	/* Zero or One Chirp Value TLV */
	if (hashlen && hash) {
		if (cntlr_gen_chirp_value_tlv(c, frm, enrollee, 1 /* TODO: hash_validity */, hashlen, hash)) {
			warn("%s: cntlr_gen_chirp_value_tlv failed.\n", __func__);
			goto out;
		}
	}

	cmdu_put_eom(frm);
	return frm;

out:
	warn("%s |%d|: cmdu gen error\n", __func__, __LINE__);
	cmdu_free(frm);
	return NULL;
}
#endif /* USE_LIBDPP */

struct cmdu_buff *cntlr_gen_bss_configuration_response(struct controller *c, struct cmdu_buff *request_cmdu)
{
	struct cmdu_buff *resp_cmdu;
	uint16_t mid = 0;

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

	/* One or more BSS Configuration Response TLV */
	if (cntlr_gen_bss_config_response_tlv(c, resp_cmdu)) {
		warn("%s: cntlr_gen_bss_config_response_tlv failed.\n", __func__);
		goto out;
	}

	if (c->cfg.enable_ts) {
		/* Zero or one Default 802.1Q Settings TLV */
		if (cntlr_gen_8021q_settings(c, resp_cmdu)) {
			warn("%s: cntlr_gen_8021q_settings failed.\n", __func__);
			goto out;
		}

		/* Zero or one Traffic Separation Policy TLV */
		if (cntlr_gen_traffic_sep_policy(c, resp_cmdu)) {
			warn("%s: cntlr_gen_traffic_sep_policy failed.\n", __func__);
			goto out;
		}
	}

	cmdu_put_eom(resp_cmdu);
	memcpy(resp_cmdu->origin, request_cmdu->origin, 6);
	return resp_cmdu;

out:
	cmdu_free(resp_cmdu);
	return NULL;
}

#ifdef USE_LIBDPP
struct cmdu_buff *cntlr_gen_dpp_cce_indication(struct controller *c,
		uint8_t *agent, bool cce_advertise)
{
	int ret;
	uint16_t mid = 0;
	struct cmdu_buff *frm;

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

	ret = cntlr_gen_dpp_cce_indication_tlv(c, frm, cce_advertise);
	if (ret)
		goto error;

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

	return frm;

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

struct cmdu_buff *cntlr_gen_dpp_bootstrapping_uri_notification(
		struct controller *c, uint8_t *radio, uint8_t *bssid,
		uint8_t *bsta, int uri_len, char *dpp_uri)
{
	int ret;
	uint16_t mid = 0;
	struct cmdu_buff *frm = NULL;

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

	ret = cntlr_gen_dpp_bootstrapping_uri_notif(c, frm, radio,
			bssid, bsta, uri_len, dpp_uri);
	if (ret)
		goto out;

	cmdu_put_eom(frm);

	return frm;
out:
	cmdu_free(frm);
	return NULL;
}
#endif /* USE_LIBDPP */

struct cmdu_buff *cntlr_gen_agent_list(struct controller *c)
{
	int ret;
	uint16_t mid = 0;
	struct cmdu_buff *cmdu;

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

	/* 1905 AgentList TLV */
	ret = cntlr_gen_agent_list_tlv(c, cmdu, 0x00);
	if (ret)
		goto out;

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

int send_agent_list_to_all_nodes(struct controller *c)
{
	trace("%s: --->\n", __func__);

	struct cmdu_buff *cmdu;
	struct node *node = NULL;
	int ret;

	cmdu = cntlr_gen_agent_list(c);
	if (!cmdu) {
		warn("%s: cntlr_gen_agent_list failed.\n", __func__);
		return -1;
	}

	ret = 0;
	list_for_each_entry(node, &c->nodelist, list) {
		memcpy(cmdu->origin, node->almacaddr, 6);
		if (send_cmdu(c, cmdu) == 0xffff)
			ret = -1;
	}

	cmdu_free(cmdu);

	return ret;
}

#endif /* EASYMESH_VERSION > 2 */

