cntlr.c

/*
 * cntlr.c - Multi-AP controller
 *
 * Copyright (C) 2020-2022 IOPSYS Software Solutions AB. All rights reserved.
 *
 * See LICENSE file for source code license information.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif

#include <easy/easy.h>

#include <cmdu.h>
#include <1905_tlvs.h>
#include <i1905_wsc.h>
#include <easymesh.h>
#include <map_module.h>
#include <uci.h>

#include <wifidefs.h>
#include "wifi_dataelements.h"

#include "timer.h"
#include "utils/debug.h"
#include "utils/utils.h"
#include "cntlr.h"
#include "allsta.h"
#include "cntlr_ubus.h"
#include "cntlr_map.h"
#include "cntlr_cmdu.h"
#include "steer_module.h"
#include "cntlr_acs.h"
#include "wifi_opclass.h"

#define map_plugin	"ieee1905.map"


extern bool waitext;

/* find interface by macaddress - no radio argument */
struct netif_iface *find_interface_by_mac_nor(struct controller *c,
		uint8_t *hwaddr)
{
	struct netif_iface *p = NULL;	/* fh anf bk iface */
	struct netif_radio *r = NULL;
	struct node *n = NULL;

	list_for_each_entry(n, &c->nodelist, list) {
		list_for_each_entry(r, &n->radiolist, list) {
			list_for_each_entry(p, &r->iflist, list) {
				if (!memcmp(p->bss->bssid, hwaddr, 6))
					return p;
			}
		}
	}

	return NULL;
}

/* find interface by macaddress */
struct netif_iface *find_interface_by_mac(struct controller *c,
		struct netif_radio *r, uint8_t *hwaddr)
{
	struct netif_iface *p = NULL;

	list_for_each_entry(p, &r->iflist, list) {
		if (!memcmp(p->bss->bssid, hwaddr, 6))
			return p;
	}

	return NULL;
}

/* find radio by ssid */
struct netif_radio *find_radio_by_ssid(struct controller *c,
		struct node *n, char *ssid)
{
	struct netif_radio *r = NULL;

	list_for_each_entry(r, &n->radiolist, list) {

		struct netif_iface *p = NULL;

		list_for_each_entry(p, &r->iflist, list) {
			if (!memcmp(p->bss->ssid, ssid, 33))
				return r;
		}
	}

	return NULL;
}

/* find netif by ssid */
struct netif_iface *find_interface_by_ssid(struct controller *c,
		struct node *n, char *ssid)
{
	struct netif_radio *r = NULL;

	list_for_each_entry(r, &n->radiolist, list) {

		struct netif_iface *p = NULL;

		list_for_each_entry(p, &r->iflist, list) {
			if (!memcmp(p->bss->ssid, ssid, 33))
				return p;
		}
	}

	return NULL;
}

/* find radio by node */
struct netif_radio *find_radio_by_node(struct controller *c, struct node *n,
		uint8_t *radio_mac)
{
	struct netif_radio *p = NULL;

	list_for_each_entry(p, &n->radiolist, list) {
		if (!memcmp(p->radio_el->macaddr, radio_mac, 6))
			return p;
	}

	return NULL;
}

/* find radio by macaddress, search all nodes */
struct netif_radio *find_radio_by_mac(struct controller *c, uint8_t *mac)
{
	struct node *n = NULL;
	struct netif_radio *r = NULL;

	list_for_each_entry(n, &c->nodelist, list) {
		r = find_radio_by_node(c, n, mac);
		if (r)
			return r;
	}

	return NULL;
}

/* finds radio struct from interface macaddr */
struct netif_radio *find_radio_by_bssid(struct controller *c, uint8_t *bssid)
{
	struct node *n = NULL;
	struct netif_radio *r = NULL;
	struct netif_iface *p = NULL;

	list_for_each_entry(n, &c->nodelist, list) {
		list_for_each_entry(r, &n->radiolist, list) {
			list_for_each_entry(p, &r->iflist, list) {
				if (!memcmp(p->bss->bssid, bssid, 6))
					return r;
			}
		}
	}

	return NULL;
}

/* find link by macaddress */
struct netif_link *find_link_by_mac(struct controller *c, uint8_t *upstream, uint8_t *downstream)
{
	struct netif_link *l = NULL;

	list_for_each_entry(l, &c->linklist, list) {
		if (!memcmp(l->upstream->bss->bssid, upstream, 6)
				&& !memcmp(l->downstream->bss->bssid, downstream, 6))
			return l;
	}

	return NULL;
}

/* find node by macaddress */
struct node *cntlr_find_node(struct controller *c, uint8_t *almac)
{
	struct node *n = NULL;

	list_for_each_entry(n, &c->nodelist, list) {
		if (!memcmp(n->alid, almac, 6))
			return n;
	}

	return NULL;
}

/* find sta by macaddress */
struct sta *cntlr_find_sta(struct controller *c, uint8_t *mac)
{
	struct sta *s = NULL;

	list_for_each_entry(s, &c->stalist, list) {
		if (!memcmp(s->de_sta->macaddr, mac, 6))
			return s;
	}

	return NULL;
}

struct bcnreq *cntlr_find_bcnreq(struct controller *c, uint8_t *sta, uint8_t *alid)
{
	struct bcnreq *br = NULL;

	dbg("%s: --->\n", __func__);

	list_for_each_entry(br, &c->bcnreqlist, list) {
		if (!memcmp(br->sta_mac, sta, 6) && !memcmp(br->agent_mac, alid, 6))
			return br;
	}

	return NULL;
}

#if 0
/* find node by macaddress */
struct netif_iface *cntlr_get_fbss_by_mac(struct controller *c, struct node *n,
		uint8_t *mac)
{
	struct netif_iface *p;

	list_for_each_entry(p, &n->iflist, list) {
		if (!memcmp(p->bss->bssid, mac, 6))
			return p;
	}

	return NULL;
}
#endif
/* find fbss based on macaddr */
struct netif_iface *cntlr_iterate_fbss(struct controller *c, uint8_t *mac)
{
	struct node *n = NULL;
	struct netif_radio *r = NULL;
	struct netif_iface *p = NULL;

	list_for_each_entry(n, &c->nodelist, list) {
		list_for_each_entry(r, &n->radiolist, list) {
			list_for_each_entry(p, &r->iflist, list) {
				if (!memcmp(p->bss->bssid, mac, 6))
					return p;
			}
		}
	}

	return NULL;
}

/* find node based on bssid */
struct node *cntlr_find_node_by_iface(struct controller *c, uint8_t *bssid)
{
	struct node *n = NULL;

	list_for_each_entry(n, &c->nodelist, list) {
		struct netif_radio *r;

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

			list_for_each_entry(p, &r->iflist, list) {
				if (!memcmp(p->bss->bssid, bssid, 6))
					return n;
			}
		}
	}

	return NULL;
}

#if 0
/* find node by ip address */
static struct node *find_node_by_ip(struct controller *c, const char *ip)
{
	struct node *p;
	struct in_addr ipn;

	if (ip && strlen(ip) && !inet_aton(ip, &ipn)) {
		warn("Invalid ipaddr: %s\n", ip);
		return NULL;
	}

	list_for_each_entry(p, &c->nodelist, list) {
		if (!memcmp(&p->ipaddr, &ipn, sizeof(struct in_addr)))
			return p;
	}

	return NULL;
}
#endif

struct node_policy *agent_find_policy(struct controller *c, uint8_t *agent)
{
	struct node_policy *a = NULL;

	list_for_each_entry(a, &c->cfg.nodelist, list) {
		if (!memcmp(agent, a->agent_id, 6))
			return a;
	}

	return NULL;
}

struct radio_policy *agent_find_radio_policy(struct controller *c, uint8_t *radio_mac)
{
	struct node_policy *a = NULL;

	list_for_each_entry(a, &c->cfg.nodelist, list) {
		struct radio_policy *r = NULL;

		list_for_each_entry(r, &a->radiolist, list) {
			if (!memcmp(radio_mac, r->macaddr, 6))
				return r;
		}
	}


	return NULL;
}

#if 0
/* find node by any of its fh-bssid */
struct node *get_node_by_bssid(struct controller *c, unsigned char *bssid)
{
	struct node *n;
	struct netif_iface *p;

	list_for_each_entry(n, &c->nodelist, list) {
		list_for_each_entry(p, &n->iflist, list) {
			if (memcmp(bssid, p->bss->bssid, 6))
				continue;

			return n;
		}
	}

	return NULL;
}
#endif

struct node *cntlr_add_node(struct controller *c, uint8_t *almac)
{
	struct node *n;
	char mac_str[18] = {0};
	int ret;

	if (!hwaddr_ntoa(almac, mac_str))
		return NULL;

	n = cntlr_find_node(c, almac);
	if (!n) {
		n = cntlr_alloc_node(c, almac);
		if (!n) {
			err("|%s:%d| failed to allocate node "MACFMT"\n",
			    __func__, __LINE__, MAC2STR(almac));
			return NULL;
		}
	} else {
		return n;
	}

	ret = cntlr_config_add_node(&c->cfg, mac_str);
	if (!ret) {
		dbg("|%s:%d| resync config\n", __func__, __LINE__);
		cntlr_resync_config(c, true);
	}

#ifdef CONTROLLER_SYNC_DYNAMIC_CNTLR_CONFIG
	if (!hwaddr_equal(c->almac, almac))
		cntlr_sync_dyn_controller_config(c, almac);
#endif

	return n;
}

static void cntlr_log_nodes(struct controller *c)
{
	struct node *n;
	int i = 0;

	list_for_each_entry(n, &c->nodelist, list) {
		cntlr_dbg("  %d | agent = %p,  hwaddr = '"MACFMT"')\n",
				i++, n, MAC2STR(n->alid));
	}
}


#if 0
static int forall_node_update_neighbors(struct controller *c)
{
	return 0;
}
#endif

void cntlr_update_sta_steer_counters(struct controller *c,
				     uint8_t *sta_mac,
				     uint8_t *src_bssid,
				     uint8_t *dst_bssid,
				     uint32_t mode,
				     enum steer_trigger trigger)
{
	trace("%s:--->\n", __func__);

	struct wifi_apsta_steer_history *a;
	struct sta *s = cntlr_find_sta(c, sta_mac);
	struct wifi_multiap_sta *mapsta;

	if (!s) {
		dbg("|%s:%d| Unrecognized STA "MACFMT", skip!\n",
		    __func__, __LINE__, MAC2STR(sta_mac));
		return;
	}

	mapsta = &s->de_sta->mapsta;
	if (mapsta->num_steer_hist >= MAX_STEER_HISTORY) {
		int i;

		for (i = 0; i < MAX_STEER_HISTORY - 1; i++) {
			memcpy(&mapsta->steer_history[i], &mapsta->steer_history[i+1],
					sizeof(struct wifi_apsta_steer_history));
		}
		a = &mapsta->steer_history[MAX_STEER_HISTORY - 1];
	} else {
		a = &mapsta->steer_history[mapsta->num_steer_hist];
	}

	/* Update SteeringHistory */
	timestamp_update(&a->time);

	if (src_bssid)
		memcpy(a->src_bssid, src_bssid, 6);

	if (dst_bssid)
		memcpy(a->dst_bssid, dst_bssid, 6);

	a->trigger = trigger;
	switch (mode) {
	case STEER_MODE_ASSOC_CTL:
		a->method = STEER_METHOD_ASSOC_CTL;
		break;
	case STEER_MODE_BTM_REQ:
		a->method = STEER_METHOD_BTM_REQ;
		/* Update SteeringSummaryStats - per STA & per Network */
		s->de_sta->mapsta.stats.btm_attempt_cnt++;
		c->dlem.network.steer_summary.btm_attempt_cnt++;
		break;
	case STEER_MODE_OPPORTUNITY:
		a->method = STEER_METHOD_ASYNC_BTM;
		/*TODO: add counter for opportunity (incl blacklis count) */
		break;
	default:
		a->method = STEER_METHOD_UNKNOWN;
		break;
	}
	a->duration = 0;

	/* Record tsp for most recent steer attempt */
	timestamp_update(&s->de_sta->mapsta.stats.last_attempt_tsp);
	mapsta->num_steer_hist += 1;
}

#if 0
static int invoke_disconnect_sta(struct node *n, struct netif_iface *p,
		uint8_t *sta_mac)
{
	/* TODO implement */
	return 0;
}
#endif

static int invoke_disconnect_sta_by_bssid(struct controller *c, unsigned char *bssid,
						uint8_t *sta_mac)
{
	/* TODO implement */

	/* The intention of this function is to force disconnect of the STA with given MAC
	 * address by means other than BTM steering. I.e. find out the way to inform agent
	 * on the necessity to deatuthenticate / disassociate the STA connected to one of
	 * its BSSes immediately. This is because association control only disallows new
	 * STAs from connecting to given BSS, but - according to spec - is not meant to
	 * cause disconnection of already connected STA from that BSS (error scenario).
	 */

	return 0;
}

static void cntlr_btm_req_timer_cb(atimer_t *t)
{
	trace("%s:--->\n", __func__);

	struct sta *s = container_of(t, struct sta, btm_req_timer);
	struct node *n = s->fh->agent;
	struct controller *c = n->cntlr;

	if (s->de_sta->mapsta.pending_btm_resp_num > 0) {
		s->de_sta->mapsta.stats.failed_steer_attempts +=
				s->de_sta->mapsta.pending_btm_resp_num;
		s->de_sta->mapsta.stats.btm_failure_cnt +=
				s->de_sta->mapsta.pending_btm_resp_num;
		c->dlem.network.steer_summary.btm_failure_cnt +=
				s->de_sta->mapsta.pending_btm_resp_num;
		s->de_sta->mapsta.pending_btm_resp_num = 0;
	}
}

static int cntlr_steer_sta(struct controller *c, struct sta *s,
			   struct wifi_sta_meas_report *to, uint32_t mode,
			   uint32_t reason)
{
	int ret = 0;
	uint16_t mid;

	trace("%s:--->\n", __func__);

	if (!to || hwaddr_is_zero(to->bssid)) {
		dbg("%s: steer verdict = OK, but target AP = NULL!\n", __func__);
		return 0;
	}

	if (!memcmp(to->bssid, s->bssid, 6)) {
		s->de_sta->mapsta.stats.no_candidate_cnt++;
		c->dlem.network.steer_summary.no_candidate_cnt++;
		dbg("%s: " MACFMT " connected to best AP! No steer needed.\n",
		    __func__, MAC2STR(s->de_sta->macaddr));
		return 0;
	}

	dbg("%s: Try to steer " MACFMT " from " MACFMT " to " MACFMT "\n",
	     __func__, MAC2STR(s->de_sta->macaddr), MAC2STR(s->bssid), MAC2STR(to->bssid));

	UNUSED(reason);

	switch (mode) {
	case STEER_MODE_ASSOC_CTL:
		/* Issue client assoc control */
		invoke_disconnect_sta_by_bssid(c, s->bssid, s->de_sta->macaddr);
		ret = cntlr_send_client_assoc_ctrl_request(c, s->fh->agent->alid,
					s->bssid, 0, 10, /* block bssid for 10 sec */
					1, s->de_sta->macaddr, &mid);
		if (ret) {
			warn("%s: Failed to send cmdu for assoc control!\n", __func__);
			//s->de_sta->mapsta.stats.failed_steer_attempts++;
			return ret;
		}
		/* Keep mid & check assoc control succesful in ACK msg */
		s->latest_assoc_cntrl_mid = mid;
		dbg("%s: cmdu->cdata->hdr.mid %u\n", __func__, mid);
		break;
	case STEER_MODE_BTM_REQ:
	case STEER_MODE_OPPORTUNITY:
		ret = cntlr_send_client_steer_request(c, s->fh->agent->alid,
					s->bssid, 0,
					1, (uint8_t (*)[6])s->de_sta->macaddr,
					1, (uint8_t (*)[6])to->bssid,
					mode);
		if (ret) {
			warn("%s: Failed to send cmdu for steering sta!\n", __func__);
			return ret;
		}

		/* Expect btm-resp from STA forwarded to cntlr */
		s->de_sta->mapsta.pending_btm_resp_num++;
		timer_set(&s->btm_req_timer, BTM_RESP_EXP_TIMEOUT * 1000);

		break;
	case STEER_MODE_UNDEFINED:
	default:
		dbg("%s: steer mode is undefined\n", __func__);
		return 0;
	}

	cntlr_update_sta_steer_counters(c, s->de_sta->macaddr, s->bssid, to->bssid,
				mode, STEER_TRIGGER_LINK_QUALITY);

	return 0;
}

/* returns steer_control_config for current steer_control */
struct steer_control_config *get_steer_control_config(struct controller *c)
{
	struct steer_control_config *e = NULL;
	struct steer_control *sc;

	if (!c)
		return NULL;

	sc = cntlr_get_steer_control(c);

	if (!sc)
		/* steer plugin is not loaded yet */
		return NULL;

	list_for_each_entry(e, &c->cfg.scclist, list) {
		if (!strncmp(e->name, sc->name, 63))
			return e;
	}

	return NULL;
}

static void cntlr_configure_steer(struct controller *c, struct sta *s,
			struct steer_control_config *e)
{
	struct netif_radio *r;
	struct radio_policy *rp = NULL;
	struct steer_config scfg = {};

	r = find_radio_by_bssid(c, s->bssid);
	if (!r)
		return;

	if (!e)
		return;

	/* Ensure band is set in interface data struct */
	s->fh->band = wifi_opclass_get_band(r->radio_el->cur_opclass.opclass[0].id);

	rp = agent_find_radio_policy(c, r->radio_el->macaddr);
	if (!rp)
		return;

	/* RCPI threshold */
	if (rp->rcpi_threshold > 0)
		scfg.rcpi_threshold = rp->rcpi_threshold; /* band dependent */
	else {
		switch (rp->band) {
			case BAND_5:
				scfg.rcpi_threshold = CONFIG_DEFAULT_RCPI_TH_5G;
				break;
			case BAND_6:
				scfg.rcpi_threshold = CONFIG_DEFAULT_RCPI_TH_6G;
				break;
			case BAND_DUAL:
			case BAND_2:
			default:
				scfg.rcpi_threshold = CONFIG_DEFAULT_RCPI_TH_2G;
		}
	}

	scfg.rcpi_hysteresis = 5; /* TODO: unused */

	/* diffsnr */
	if (e->diffsnr > 0)
		scfg.rcpi_diffsnr = e->diffsnr;
	else
		scfg.rcpi_diffsnr = 8; /* default diffsnr */

	/* bandsteer */
	scfg.bandsteer = e->bandsteer;

	/* maximum number of btm tries before assoc control */
	/* TODO: use c->cfg */
	scfg.max_btm_attempt = DEFAULT_MAX_BTM_ATTEMPT;

	cntlr_configure_steer_module(c, &scfg);
}

static void cntlr_try_steer_sta(struct controller *c, struct sta *s)
{
	trace("%s:--->\n", __func__);
	struct steer_sta candidate = {
		.sta = s->de_sta,
		.nbrlist = NULL,
		.meas_reportlist = &s->de_sta->meas_reportlist,
		.best = NULL,
		.band = s->fh->band,
	};
	int ret;

	memcpy(candidate.bssid, s->bssid, 6);

	/* check if sta should be steered */
	ret = cntlr_maybe_steer_sta(c, &candidate);
	if (ret) {
		dbg("cntlr_maybe_steer_sta() ret = %d\n", ret);
		return;
	}

	switch (candidate.verdict) {
	case STEER_VERDICT_OK:
		if (!timestamp_expired(&s->de_sta->mapsta.stats.last_attempt_tsp,
					STEER_ATTEMPT_MIN_ITV)) {
			dbg("%s: last steer attempt < %us ago; skip steering\n",
			    __func__, STEER_ATTEMPT_MIN_ITV / 1000);
			return;
		}
		if (!timestamp_expired(&s->de_sta->mapsta.stats.last_steer_tsp,
					STEER_SUCCESS_MIN_ITV)) {
			dbg("%s: last successful steer < %us ago; skip steering\n",
			    __func__, STEER_SUCCESS_MIN_ITV / 1000);
			return;
		}
		cntlr_steer_sta(c, s, candidate.best, candidate.mode, candidate.reason);
		break;
	case STEER_VERDICT_NOK:
		return;
	case STEER_VERDICT_MAYBE:
		/* TODO: check next steer-control ? */
		break;
	case STEER_VERDICT_EXCLUDE:
		/* STA excluded from subsequent steer attempts */
		dbg("%s: sticky STA excluded from steering, elapsed %us of %us\n", __func__,
		    timestamp_elapsed_sec(&s->de_sta->mapsta.stats.last_attempt_tsp),
		    STEER_ATTEMPT_STICKY_ITV / 1000);
		if (timestamp_expired(&s->de_sta->mapsta.stats.last_attempt_tsp,
					STEER_ATTEMPT_STICKY_ITV))
			/* time up, allow steering again */
			s->de_sta->mapsta.stats.failed_steer_attempts = 0;

		/* TODO: consider update of BTM steering disallowed STA list in agent */
		break;
	default:
		break;
	}

}

static void cntlr_bcn_metrics_parse(atimer_t *t)
{
	trace("%s:--->\n", __func__);

	struct sta *s = container_of(t, struct sta, bcn_metrics_timer);
	struct node *n = s->fh->agent;
	struct controller *c = n->cntlr;
	struct netif_iface *bss = NULL;
	struct wifi_sta_meas_report *b = NULL, *tmp;
	struct steer_control_config *scc;

	dbg("%s: STA " MACFMT" connected to " MACFMT " in Node " MACFMT"\n",
	    __func__, MAC2STR(s->de_sta->macaddr), MAC2STR(s->bssid), MAC2STR(n->alid));

	list_for_each_entry_safe(b, tmp, &s->de_sta->meas_reportlist, list) {
		dbg("bcn-report from " MACFMT "\n", MAC2STR(b->bssid));

		/* Skip entry not in our network */
		bss = cntlr_iterate_fbss(c, b->bssid);
		if (!bss) {
			list_del(&b->list);
			free(b);
			s->de_sta->num_meas_reports--;
			dbg("Delete alien entry "MACFMT"\n", MAC2STR(b->bssid));
		}
	}

	scc = get_steer_control_config(c);
	if (!scc)
		return;

	if (scc->enable_sta_steer) {
		/* configure individually for each STA */
		cntlr_configure_steer(c, s, scc);
		cntlr_try_steer_sta(c, s);
	}

	dbg("%s exiting\n", __func__);
}

/* TODO: deprecate after assoc control steering added */
static void cntlr_init_sta_steer_counters(struct sta *s)
{
	if (!s || !s->de_sta)
		return;

	s->de_sta->mapsta.stats = (struct wifi_steer_summary){0};

	/* TODO: implement stats marked as NO_DATA */
	s->de_sta->mapsta.stats.blacklist_attempt_cnt = STEER_STATS_NO_DATA;
	s->de_sta->mapsta.stats.blacklist_success_cnt = STEER_STATS_NO_DATA;
	s->de_sta->mapsta.stats.blacklist_failure_cnt = STEER_STATS_NO_DATA;
}

struct wifi_sta_element *cntlr_wifi_alloc_sta(struct controller *c,
		uint8_t *macaddr)
{
	struct wifi_sta_element *wse = NULL;

	wse = calloc(1, sizeof(struct wifi_sta_element));
	if (!wse)
		return NULL;

	INIT_LIST_HEAD(&wse->meas_reportlist);
	wse->num_meas_reports = 0;
	memcpy(wse->macaddr, macaddr, 6);
	wse->mapsta.pending_btm_resp_num = 0;

	return wse;
}

struct sta *cntlr_add_sta(struct controller *c, uint8_t *macaddr)
{
	struct sta *s;

	s = cntlr_find_sta(c, macaddr);
	if (s)
		return s;

	s = calloc(1, sizeof(struct sta));
	if (!s)
		return NULL;

	INIT_LIST_HEAD(&s->unassoclist);
	list_add(&s->list, &c->stalist);
	timer_init(&s->bcn_metrics_timer, cntlr_bcn_metrics_parse);
	timer_init(&s->btm_req_timer, cntlr_btm_req_timer_cb);

	s->de_sta = cntlr_wifi_alloc_sta(c, macaddr);
	if (!s->de_sta) {
		free(s);
		return NULL;
	}

	cntlr_init_sta_steer_counters(s);

	return s;
}

static void forall_node_get_sta_metrics(struct controller *c)
{
	struct sta *s = NULL;

	list_for_each_entry(s, &c->stalist, list) {
		struct cmdu_buff *cmdu;

		cmdu = cntlr_gen_sta_metric_query(c, s->fh->agent->alid, s->de_sta->macaddr);
		if (!cmdu)
			continue;

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

struct wifi_bss_element *cntlr_wifi_bss(struct controller *c,
					      uint8_t *hwaddr)
{
	struct wifi_bss_element *bss = NULL;

	bss = calloc(1, sizeof(struct wifi_bss_element));
	if (!bss)
		return NULL;

	//INIT_LIST_HEAD(&bss->stalist);
	memcpy(bss->bssid, hwaddr, 6);

	return bss;
}

struct netif_iface *cntlr_radio_add_interface(struct controller *c,
					      struct netif_radio *r,
					      uint8_t *hwaddr)
{
	struct netif_iface *n;

	n = find_interface_by_mac(c, r, hwaddr);
	if (n) {
		n->bss->enabled = true;
		return n;
	}

	n = calloc(1, sizeof(*n));
	if (!n)
		return NULL;

	n->bss = cntlr_wifi_bss(c, hwaddr);
	if (!n->bss) {
		free(n);
		return NULL;
	}

	n->band = wifi_opclass_get_band(r->radio_el->cur_opclass.opclass[0].id);
	n->bss->is_fbss = true;
	n->bss->is_bbss = false;
	n->bss->enabled = true;
	list_add(&n->list, &r->iflist);
	n->agent = r->agent;

	return n;
}

static struct wifi_radio_element *cntlr_create_wifi_radio(struct controller *c)
{
	struct wifi_radio_element *radio_el = NULL;

	radio_el = calloc(1, sizeof(struct wifi_radio_element));
	if (!radio_el)
		return NULL;

	INIT_LIST_HEAD(&radio_el->scanlist);

	return radio_el;
}

struct netif_radio *cntlr_node_add_radio(struct controller *c, struct node *n,
		uint8_t *radio)
{
	struct netif_radio *r;

	r = find_radio_by_node(c, n, radio);
	// trace("-------------------> %s : raadio added "MACFMT"\n", MAC2STR(r->hwaddr));
	if (r)
		return r;

	r = calloc(1, sizeof(*r));
	if (!r)
		return NULL;

	INIT_LIST_HEAD(&r->iflist);
	list_add(&r->list, &n->radiolist);
	r->agent = n;
	r->radio_el = cntlr_create_wifi_radio(c);
	if (!r->radio_el) {
		free(r);
		return NULL;
	}
	memcpy(r->radio_el->macaddr, radio, 6);

	return r;
}

uint8_t cntlr_get_classid_ht20(struct wifi_radio_element *radio, uint8_t channel)
{
	return wifi_opclass_get_id(&radio->pref_opclass, channel, 20);
}

void cntlr_radio_pref_opclass_reset(struct wifi_radio_element *radio)
{
	/*
	 * Build initial preferred opclasses from supported opclasses
	 * we receive in basic radio capabilities.
	 */
	memcpy(&radio->pref_opclass, &radio->supp_opclass, sizeof(radio->pref_opclass));
	wifi_opclass_set_preferences(&radio->pref_opclass, 15 << 4);
}

int cntlr_radio_pref_opclass_add(struct wifi_radio_element *radio, uint8_t classid,
				 uint8_t channel, uint8_t preference)
{
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass_channel chan = {};
	struct wifi_radio_opclass *opclass;

	opclass = &radio->pref_opclass;

	entry = wifi_opclass_find_entry(opclass, classid);
	if (!entry)
		return -1;

	entry->id = classid;
	entry->bandwidth = wifi_opclass_get_bw(classid);

	chan.channel = channel;
	chan.preference = preference;

	timestamp_update(&opclass->entry_time);
	return wifi_opclass_add_channel(entry, &chan);
}

void cntlr_radio_pref_opclass_dump(struct wifi_radio_element *radio)
{
	wifi_opclass_dump(&radio->pref_opclass);
}

void cntlr_radio_cur_opclass_reset(struct wifi_radio_element *radio)
{
	wifi_opclass_reset(&radio->cur_opclass);
}

int cntlr_radio_cur_opclass_add(struct wifi_radio_element *radio, uint8_t classid,
				uint8_t channel, uint8_t txpower)
{
	struct wifi_radio_opclass_entry *entry;
	struct wifi_radio_opclass_channel chan = {};

	entry = wifi_opclass_find_entry(&radio->cur_opclass, classid);
	if (!entry)
		entry = wifi_opclass_new_entry(&radio->cur_opclass);
	if (!entry)
		return -1;

	entry->id = classid;
	entry->bandwidth = wifi_opclass_get_bw(classid);
	entry->max_txpower = txpower;

	chan.channel = channel;
	chan.preference = 15 << 4;

	timestamp_update(&radio->cur_opclass.entry_time);
	return wifi_opclass_add_channel(entry, &chan);
}

void cntlr_radio_pref_opclass_set_pref(struct wifi_radio_element *radio, uint8_t id, uint8_t preference)
{