From 59416db0ed5d9702cfc94743254f943b9734a0c0 Mon Sep 17 00:00:00 2001
From: "nevadita.chatterjee" <nevadita.chatterjee@iopsys.eu>
Date: Mon, 11 Jan 2021 14:50:19 +0530
Subject: [PATCH] map-controller: Adding Steering sec 11.1 and 11.2 code

---
 src/core/cntlr_map_debug.c |  35 ++++
 src/core/cntlr_ubus.c      | 320 ++++++++++++++++++++++++++++++++++++-
 2 files changed, 353 insertions(+), 2 deletions(-)

diff --git a/src/core/cntlr_map_debug.c b/src/core/cntlr_map_debug.c
index f4fc3a9d..7072b3bd 100644
--- a/src/core/cntlr_map_debug.c
+++ b/src/core/cntlr_map_debug.c
@@ -587,12 +587,47 @@ int debug_beacon_metrics_response(void *cntlr, struct cmdu_cstruct *cmdu)
 int debug_sta_steer_btm_report(void *cntlr, struct cmdu_cstruct *cmdu)
 {
 	trace("%s: --->\n", __func__);
+	trace("parsing steer btm report of |%s:" MACFMT "|\n",
+		cmdu->intf_name, MAC2STR(cmdu->origin));
+
+	int i, j;
+	uint8_t *tlv = NULL;
+
+	for (i = 0; i < cmdu->num_tlvs; i++) {
+		tlv = (uint8_t *) cmdu->tlvs[i];
+		trace("CMDU type: %s\n", map_stringify_tlv_type(*tlv));
+		switch (*tlv) {
+		case MAP_TLV_STEERING_BTM_REPORT:
+			{
+				struct tlv_steer_btm_report *p =
+					(struct tlv_steer_btm_report *)tlv;
+
+				trace("\tbssid: " MACFMT "\n",
+					MAC2STR(p->bssid));
+				trace("\tsta_macaddr: " MACFMT "\n",
+					MAC2STR(p->sta_mac_addr));
+				trace("\tstatus_code: %d\n",
+					p->status_code);
+				if (p->status_code == 0x00)
+					trace("\ttarget_bbsid: " MACFMT "\n",
+						MAC2STR(p->target_bssid));
+				break;
+			}
+		default:
+				fprintf(stdout, "unknown TLV in CMDU:|%s|",
+				map_stringify_cmdu_type(cmdu->message_type));
+				break;
+		}
+		trace("\n");
+	}
 	return 0;
 }
 
 int debug_sta_steer_complete(void *cntlr, struct cmdu_cstruct *cmdu)
 {
 	trace("%s: --->\n", __func__);
+	trace("parsing sta steer complete of |%s:" MACFMT "|\n",
+		cmdu->intf_name, MAC2STR(cmdu->origin));
 	return 0;
 }
 
diff --git a/src/core/cntlr_ubus.c b/src/core/cntlr_ubus.c
index ed10aaa9..b0e57a85 100644
--- a/src/core/cntlr_ubus.c
+++ b/src/core/cntlr_ubus.c
@@ -142,6 +142,35 @@ static const struct blobmsg_policy ap_policy_config_params[__AP_POLICY_CONFIG_MA
 	[AP_POLICY_CONFIG_BSS] = { .name = "bsslist", .type = BLOBMSG_TYPE_ARRAY },
 };
 
+enum {
+	STEERING_POLICY_AGENT,
+	STEERING_POLICY_FROM_BSSID,
+	STEERING_POLICY_STA,
+	STEERING_POLICY_TARGET_BSSID,
+	STEERING_POLICY_STEER_TIMEOUT,
+	STEERING_POLICY_BTM_TIMEOUT,
+	STEERING_POLICY_REQUEST_MODE,
+#if profile2
+	STEERING_POLICY_TARGET_BSSID_MULTIBAND,
+	STEERING_POLICY_STA_MULTIBAND,
+#endif
+	__STEERING_POLICY_MAX,
+};
+
+static const struct blobmsg_policy client_steering_policy_params[__STEERING_POLICY_MAX] = {
+	[STEERING_POLICY_AGENT] = { .name = "agent", .type = BLOBMSG_TYPE_STRING },
+	[STEERING_POLICY_FROM_BSSID] = { .name = "from_bssid", .type = BLOBMSG_TYPE_STRING },
+	[STEERING_POLICY_STA] = { .name = "sta", .type = BLOBMSG_TYPE_ARRAY},
+	[STEERING_POLICY_TARGET_BSSID] = { .name = "target_bssid", .type = BLOBMSG_TYPE_ARRAY },
+	[STEERING_POLICY_STEER_TIMEOUT] = { .name = "steer_timeout", .type = BLOBMSG_TYPE_INT32 },
+	[STEERING_POLICY_BTM_TIMEOUT] = { .name = "btm_timeout", .type = BLOBMSG_TYPE_INT32 },
+	[STEERING_POLICY_REQUEST_MODE] = { .name = "steer_req_mode", .type = BLOBMSG_TYPE_BOOL },
+#if profile2
+	[STEERING_POLICY_TARGET_BSSID_MULTIBAND] = { .name = "target_bssid_multiband", .type = BLOBMSG_TYPE_ARRAY },
+	[STEERING_POLICY_STA_MULTIBAND] = { .name = "sta_multiband", .type = BLOBMSG_TYPE_ARRAY },
+#endif
+};
+
 void send_cmdu_cb(struct ubus_request *req,
 				int type, struct blob_attr *msg)
 {
@@ -576,6 +605,292 @@ fail_cmdu:
 	return UBUS_STATUS_UNKNOWN_ERROR;
 }
 
+static int cntlr_client_steering(struct ubus_context *ctx, struct ubus_object *obj,
+		struct ubus_request_data *req, const char *method,
+		struct blob_attr *msg)
+{
+	struct blob_attr *tb[__STEERING_POLICY_MAX];
+	struct controller *c = container_of(obj, struct controller, obj);
+	char agent[18] = {0};
+	char bss_id[18] = {0};
+	char sta_id[30][18] = {0};
+	char sta_multi_id[30][18] = {0};
+	char target_bbsid[30][18] = {0};
+	char target_bbsid_multi[30][18] = {0};
+	char to_id[18] = {0};
+	struct cmdu_cstruct *cmdu_data;
+	uint32_t timeout = 0;
+	struct tlv_steer_Req *p;
+	struct tlv_profile2_steer_req *p1;
+	uint32_t sta_nr = 0, bssid_nr = 0, sta_multi_nr = 0, bssid_multi_nr = 0;
+	uint32_t request_mode = 0, request_mode_present = -1, tlv_index = 0;
+	uint32_t sta_present = -1, sta_multi_present = -1, bssid_present = -1;
+	uint32_t target_bssid_present = -1, target_bssid_multi_present = -1;
+	struct blob_attr *cur;
+	int rem, l = 0, j = 0, count = 0;
+	uint32_t steer_timeout = 0, btm_timeout = 0;
+
+	cmdu_data = (struct cmdu_cstruct *)calloc(1,
+		sizeof(struct cmdu_cstruct));
+
+	if (!cmdu_data) {
+		fprintf(stderr, "failed to malloc cmdu\n");
+		return UBUS_STATUS_UNKNOWN_ERROR;
+	}
+
+	blobmsg_parse(client_steering_policy_params, __STEERING_POLICY_MAX, tb,
+		blob_data(msg), blob_len(msg));
+
+	if (tb[STEERING_POLICY_AGENT]) {
+		strncpy(agent, blobmsg_data(tb[STEERING_POLICY_AGENT]),
+			sizeof(agent) - 1);
+		if (!hwaddr_aton(agent, cmdu_data->origin))
+			goto fail_cmdu;
+	}
+	// TODO: ff:ff:ff:ff:ff:ff = send to all agents
+
+	if (tb[STEERING_POLICY_FROM_BSSID]) {
+		strncpy(bss_id, blobmsg_data(tb[STEERING_POLICY_FROM_BSSID]),
+			sizeof(bss_id) - 1);
+		bssid_present = 1;
+	}
+
+	if (tb[STEERING_POLICY_STA]) {
+		sta_present = 1;
+		sta_nr = blobmsg_check_array(tb[STEERING_POLICY_STA],
+			BLOBMSG_TYPE_STRING);
+		l = 0;
+		blobmsg_for_each_attr(cur, tb[STEERING_POLICY_STA], rem)
+			strncpy(sta_id[l++], blobmsg_get_string(cur), 18);
+	}
+
+	if (tb[STEERING_POLICY_TARGET_BSSID]) {
+		target_bssid_present = 1;
+		bssid_nr = blobmsg_check_array(tb[STEERING_POLICY_TARGET_BSSID],
+				BLOBMSG_TYPE_STRING);
+		l = 0;
+		blobmsg_for_each_attr(cur, tb[STEERING_POLICY_TARGET_BSSID], rem)
+			strncpy(target_bbsid[l++], blobmsg_get_string(cur), 18);
+	}
+
+	if (tb[STEERING_POLICY_STEER_TIMEOUT])
+		steer_timeout = (int) blobmsg_get_u32(
+				tb[STEERING_POLICY_STEER_TIMEOUT]);
+
+	if (tb[STEERING_POLICY_REQUEST_MODE]) {
+		request_mode = blobmsg_get_bool(tb[STEERING_POLICY_REQUEST_MODE]);
+		request_mode_present = 1;
+	}
+
+	if (tb[STEERING_POLICY_BTM_TIMEOUT])
+		btm_timeout = (int) blobmsg_get_u32(tb[STEERING_POLICY_BTM_TIMEOUT]);
+
+#if profile2
+
+	if (tb[STEERING_POLICY_TARGET_BSSID_MULTIBAND]) {
+		target_bssid_multi_present = 1;
+		bssid_multi_nr = blobmsg_check_array(
+					tb[STEERING_POLICY_TARGET_BSSID_MULTIBAND],
+					BLOBMSG_TYPE_INT32);
+		l = 0;
+		blobmsg_for_each_attr(cur,
+			tb[STEERING_POLICY_TARGET_BSSID_MULTIBAND], rem)
+			strncpy(target_bbsid_multi[l++],
+				blobmsg_get_string(cur), 18);
+	}
+
+	if (tb[STEERING_POLICY_STA_MULTIBAND]) {
+		sta_multi_present = 1;
+		sta_multi_nr = blobmsg_check_array(tb[STEERING_POLICY_STA_MULTIBAND],
+				BLOBMSG_TYPE_INT32);
+		l = 0;
+		blobmsg_for_each_attr(cur, tb[STEERING_POLICY_STA_MULTIBAND], rem)
+			strncpy(sta__multi_id[l++], blobmsg_get_string(cur), 18);
+	}
+
+#endif
+
+	cmdu_data->message_type = CMDU_CLIENT_STEERING_REQUEST;
+
+	if (bssid_present == 1 && request_mode_present == 1) {
+		trace("values  are requestmode %d timeout %d sta_cnt %d
+			bssid_nr %d\n", request_mode, timeout, sta_nr,
+			bssid_nr);
+
+		p = calloc(1, sizeof(struct tlv_steer_Req));
+		if (!p) {
+			fprintf(stderr, "failed to malloc cmdu\n");
+			goto fail_cmdu;
+		}
+
+		p->tlv_type = MAP_TLV_STEERING_REQUEST;
+		hwaddr_aton(bss_id, p->bssid);
+
+		//Here we need to know if the this is for
+		//STA mandate or a STA opportunity
+
+		if (request_mode == 1) {
+			p->request_mode = 0x01;
+			p->steer_opp_window = 0x00;
+		} else if (request_mode == 0) {
+			p->request_mode = 0x00;
+			p->steer_opp_window = steer_timeout;
+			p->target_bssid_list_cnt = 0x00;
+			p->steering_req_target_bssids = NULL;
+		} else {
+			fprintf(stderr, "Unknown request mode\n");
+			goto fail_p;
+		}
+
+		//Here we need to fill the BTM bits
+		p->btm_disassoc_imminent = 0x00; //TODO default value
+		p->btm_abridged = 0x00; //TODO default value
+		p->reserved = 0x00;
+		p->btm_disassoc_timer = 0x00; //TODO default value
+
+		p->sta_list_cnt = (uint8_t) sta_nr;
+
+		if (p->sta_list_cnt > 0) {
+			p->steering_req_macs = calloc(p->sta_list_cnt,
+						sizeof(*p->steering_req_macs));
+			if (!p->steering_req_macs) {
+				fprintf(stderr, "|%s:%d| out of memory!\n",
+					__func__, __LINE__);
+				p->sta_list_cnt = 0;
+				goto fail_p;
+			}
+		}
+
+		for (j = 0; j < p->sta_list_cnt; j++)
+			hwaddr_aton(sta_id[j], p->steering_req_macs[j].addr);
+
+		p->target_bssid_list_cnt = (uint8_t) bssid_nr;
+
+		if (p->target_bssid_list_cnt > 0) {
+			p->steering_req_target_bssids = calloc(
+							p->target_bssid_list_cnt,
+							sizeof(*p->steering_req_target_bssids));
+
+			if (!p->steering_req_target_bssids) {
+				fprintf(stderr, "|%s:%d| out of memory!\n",
+					__func__, __LINE__);
+				p->target_bssid_list_cnt = 0;
+				goto fail_p;
+			}
+		}
+
+		for (j = 0; j < p->target_bssid_list_cnt; j++) {
+			hwaddr_aton(target_bbsid[j], p->steering_req_target_bssids[j].bssid);
+			p->steering_req_target_bssids[j].op_class = 0x00;
+			p->steering_req_target_bssids[j].channel = 0x00;
+		}
+		count++;
+	}
+
+#if profile2
+	//Here we need to add tlv for 17.2.57
+	if (bssid_present == 1 && request_mode_present != 1 &&
+		(target_bssid_multi_present != -1 ||
+				sta_multi_present != -1)) {
+
+		p1 = calloc(1, sizeof(struct tlv_profile2_steer_req));
+		if (!p1) {
+			fprintf(stderr, "failed to malloc cmdu\n");
+			goto fail_cmdu;
+		}
+
+		p1->tlv_type = MAP_TLV_PROFILE2_STEERING_REQ;
+
+		hwaddr_aton(bss_id, p1->bssid);
+
+		//Here we need to know if the this is for
+		//STA mandate or a STA opportunity
+		if (request_mode == 1) {
+			p1->req_mode = 0x01;
+			p1->steering_opt_win = 0x00;
+		} else if (request_mode == 0) {
+			p1->req_mode = 0x00;
+			p1->steering_opt_win = timeout;
+			p1->target_bssid_count = 0x00;
+			p1->target_data = NULL;
+		} else {
+			fprintf(stderr, "Unknown request mode\n");
+			goto fail_p;
+		}
+
+		//Here we need to fill the BTM bits
+		p1->btm_imminent_bit = 0x00; //TODO default value
+		p1->btm_abridged_bit = 0x00; //TODO default value
+		p1->btm_dis_timer = 0x00; //TODO default value
+		p1->sta_list_count = (uint8_t) sta_multi_nr;
+
+		if (p1->sta_list_count > 0) {
+			p1->sta_mac = calloc(p1->sta_list_count,
+					sizeof(*p1->sta_mac));
+			if (!p1->sta_mac) {
+				fprintf(stderr, "|%s:%d| out of memory!\n",
+						__func__, __LINE__);
+				p1->sta_list_count = 0;
+				goto fail_p;
+			}
+		}
+
+		for (j = 0; j < p1->sta_list_count; j++)
+			hwaddr_aton(sta_multi_id[j], p1->sta_mac[j].mac);
+
+		//Here we also need the bssid mapped to the STAs
+		if (p1->target_bssid_count > 0) {
+			p1->target_data = calloc(p1->target_bssid_count,
+			sizeof(*p1->target_data));
+
+			if (!p1->target_data) {
+				fprintf(stderr, "|%s:%d| out of memory!\n",
+						__func__, __LINE__);
+				p1->target_bssid_count = 0;
+				goto fail_p;
+			}
+		}
+
+		for (j = 0; j < p1->target_bssid_count; j++) {
+			hwaddr_aton(target_bbsid_multi[j], p1->target_data[j].bssid);
+			p1->target_data[j].bss_op_class = 0x00;
+			//TODO filled as a default value now
+			p1->target_data[j].bss_ch = 0x00;
+			//TODO filled as a default value now
+		}
+		count++;
+	}
+#endif
+	cmdu_data->num_tlvs = count;
+
+#ifndef profile2
+	//Here we need to check that in profile 1 atleast 1 tlv
+	//should be there to send the CMDU
+	if (cmdu_data->num_tlvs == 0) {
+		fprintf(stderr, "|%s:%d| No TLvs\n", __func__, __LINE__);
+		return UBUS_STATUS_UNKNOWN_ERROR;
+	}
+#endif
+	cmdu_data->tlvs = (uint8_t **)calloc(cmdu_data->num_tlvs,
+				sizeof(uint8_t *));
+	if (cmdu_data->tlvs && p != NULL)
+		cmdu_data->tlvs[tlv_index++] = (uint8_t *)p;
+	if (cmdu_data->tlvs && p1 != NULL)
+		cmdu_data->tlvs[tlv_index++] = (uint8_t *)p1;
+	send_cmdu(c, cmdu_data);
+	map_free_cmdu(cmdu_data);
+	return 0;
+fail_p:
+	if (p != NULL)
+		map_free_tlv_cstruct((uint8_t *) p);
+	if (p1 != NULL)
+		map_free_tlv_cstruct((uint8_t *) p1);
+fail_cmdu:
+	map_free_cmdu(cmdu_data);
+	return UBUS_STATUS_UNKNOWN_ERROR;
+}
+
+
 static int cntlr_sta_caps(struct ubus_context *ctx, struct ubus_object *obj,
 			struct ubus_request_data *req, const char *method,
 			struct blob_attr *msg)
@@ -1147,7 +1462,7 @@ int cntlr_publish_object(struct controller *c, const char *objname)
 	struct ubus_object *obj;
 	struct ubus_object_type *obj_type;
 	struct ubus_method *obj_methods;
-	struct ubus_method m[7] = {
+	struct ubus_method m[8] = {
 		UBUS_METHOD_NOARG("status", cntlr_status),
 		UBUS_METHOD("ap_caps", cntlr_ap_caps, ap_caps_policy_params),
 		UBUS_METHOD("sta_caps", cntlr_sta_caps, sta_caps_policy_params),
@@ -1155,7 +1470,8 @@ int cntlr_publish_object(struct controller *c, const char *objname)
 				channel_pref_policy_params),
 		UBUS_METHOD("bk_steer", cntlr_bk_steer, bk_steer_policy_params),
 		UBUS_METHOD("agent_policy", cntlr_ap_policy_config, ap_policy_config_params),
-		UBUS_METHOD("channel", cntlr_channel_select, channel_select_policy_params)
+		UBUS_METHOD("channel", cntlr_channel_select, channel_select_policy_params),
+		UBUS_METHOD("client_steering", cntlr_client_steering, client_steering_policy_params)
 		/*
 		UBUS_METHOD("teardown_ap", cntlr_teardown_ap,
 				config_policy_params),
-- 
GitLab