From 9beed070aaf957cfb6ff5a96b2ddb05cd797cb4c Mon Sep 17 00:00:00 2001
From: Maxim Menshikov <maxim.menshikov@iopsys.eu>
Date: Wed, 20 Mar 2024 16:09:17 +0000
Subject: [PATCH] Add controller EHT TLVs

---
 src/cntlr_cmdu.c |  8 ++++
 src/cntlr_tlv.c  | 96 ++++++++++++++++++++++++++++++++++++++++++++++++
 src/cntlr_tlv.h  |  5 +++
 3 files changed, 109 insertions(+)

diff --git a/src/cntlr_cmdu.c b/src/cntlr_cmdu.c
index baa8a9af..4daa204c 100644
--- a/src/cntlr_cmdu.c
+++ b/src/cntlr_cmdu.c
@@ -665,6 +665,14 @@ struct cmdu_buff *cntlr_gen_ap_autoconfig_wsc(struct controller *c,
 	if (ret == 1)
 		cntlr_gen_wsc(c, resp, NULL, msg, msglen, e_band, e_auth);
 
+#if (EASYMESH_VERSION >= 6)
+	if (e_band == BAND_6) {
+		ret = cntlr_gen_eht_operations(c, b, resp);
+		if (ret)
+			goto out;
+	}
+#endif
+
 	hwaddr_ntoa(radio_id, macstr);
 	hwaddr_ntoa(rx_cmdu->origin, alidstr);
 	if (e_band == BAND_2)
diff --git a/src/cntlr_tlv.c b/src/cntlr_tlv.c
index 23609f3f..54102326 100644
--- a/src/cntlr_tlv.c
+++ b/src/cntlr_tlv.c
@@ -2050,3 +2050,99 @@ int cntlr_gen_agent_list_tlv(struct controller *c, struct cmdu_buff *frm, uint8_
 	return 0;
 }
 #endif /* EASYMESH_VERSION > 2 */
+
+#if (EASYMESH_VERSION > 5)
+
+int cntlr_gen_eht_operations(struct controller *c, struct node *n,
+	struct cmdu_buff *frm)
+{
+	struct tlv *t;
+	int offset = 0;
+	uint32_t precalc_len = 0;
+	struct netif_iface *p = NULL;
+	struct netif_radio *r = NULL;
+
+	if (!n)
+		return -1;
+
+	list_for_each_entry(r, &n->radiolist, list) {
+		precalc_len += (6 + 1 + 25) * sizeof(uint8_t);
+		list_for_each_entry(p, &r->iflist, list) {
+			precalc_len += (6 + 1 + 4 + 1 + 1 + 1 + 2 + 16) * sizeof(uint8_t);
+		}
+	}
+
+	t = cmdu_reserve_tlv(frm, precalc_len);
+	if (!t)
+		return -1;
+
+	t->type = MAP_TLV_EHT_OPERATIONS;
+	t->len = 0;
+
+	t->data[offset] = a->num_radios;
+	offset += sizeof(uint8_t);
+
+	list_for_each_entry(r, &n->radiolist, list) {
+		uint8_t *num_bss;
+
+		/* radio mac */
+		memcpy(&t->data[offset], r->radio_el->macaddr, 6);
+		offset += 6 * sizeof(uint8_t);
+
+		num_bss = &t->data[offset];
+		*num_bss = 0;
+
+		offset += sizeof(uint8_t);
+
+		list_for_each_entry(p, &r->iflist, list) {
+			(*num_bss)++;
+
+			/* BSSID */
+			memcpy(&t->data[offset], p->bss->bssid, 6);
+			offset += 6 * sizeof(uint8_t);
+
+			/* flag1 */
+			t->data[offset] = 0;
+			offset += sizeof(uint8_t);
+
+			/* mcs_nss */
+			memset(&t->data[offset], 0, 4 * sizeof(uint8_t));
+			offset += 4 * sizeof(uint8_t);
+
+			/* control */
+			t->data[offset] = 0;
+			offset += sizeof(uint8_t);
+
+			/* ccfs0 */
+			t->data[offset] = 0;
+			offset += sizeof(uint8_t);
+
+			/* ccfs1 */
+			t->data[offset] = 0;
+			offset += sizeof(uint8_t);
+
+			/* disabled_subchannel[0, 1] */
+			t->data[offset] = 0;
+			t->data[offset + 1] = 0;
+			offset += 2 * sizeof(uint8_t);
+
+			/* reserved for bss */
+			memset(&t->data[offset], 0, 16 * sizeof(uint8_t));
+			offset += 16 * sizeof(uint8_t);
+		}
+
+		/* reserved for radio */
+		memset(&t->data[offset], 0, 25 * sizeof(uint8_t));
+		offset += 25 * sizeof(uint8_t);
+	}
+
+	t->len = offset;
+	ret = cmdu_put_tlv(frm, t);
+	if (ret) {
+		err("%s: error: cmdu_put_tlv()\n", __func__);
+		return -1;
+	}
+
+	return 0;
+}
+#endif
diff --git a/src/cntlr_tlv.h b/src/cntlr_tlv.h
index f0d42edc..e72c175b 100644
--- a/src/cntlr_tlv.h
+++ b/src/cntlr_tlv.h
@@ -185,4 +185,9 @@ struct cmdu_buff * cntrl_gen_channel_sel_request(struct controller *c,
 						uint8_t *agent,
 						uint8_t *radio_id,
 						struct wifi_radio_opclass *opclass);
+#if (EASYMESH_VERSION >= 6)
+int cntlr_gen_eht_operations(struct controller *c, struct node *n,
+	struct cmdu_buff *frm);
+#endif
+
 #endif /* CNTLR_TLV_GEN_H */
-- 
GitLab