Add support for forcing TX MCS in 60 GHz tests

Signed-off-by: Alexei Avshalom Lazar <ailizaro@codeaurora.org>
diff --git a/ap.c b/ap.c
index 1fafbbe..7b48a67 100644
--- a/ap.c
+++ b/ap.c
@@ -6679,6 +6679,21 @@
 }
 
 
+static int ap_set_force_mcs(struct sigma_dut *dut, int force, int mcs)
+{
+	switch (get_driver_type()) {
+#ifdef __linux__
+	case DRIVER_WIL6210:
+		return wil6210_set_force_mcs(dut, force, mcs);
+#endif /* __linux__ */
+	default:
+		sigma_dut_print(dut, DUT_MSG_ERROR,
+				"Unsupported ap_set_force_mcs with the current driver");
+		return -1;
+	}
+}
+
+
 int cmd_ap_config_commit(struct sigma_dut *dut, struct sigma_conn *conn,
 			 struct sigma_cmd *cmd)
 {
@@ -7600,6 +7615,16 @@
 				  "errorCode,Could not set ExtSch");
 			return 0;
 		}
+		if (dut->ap_fixed_rate) {
+			sigma_dut_print(dut, DUT_MSG_DEBUG,
+					"forcing TX MCS index %d",
+					dut->ap_mcs);
+			if (ap_set_force_mcs(dut, 1, dut->ap_mcs)) {
+				send_resp(dut, conn, SIGMA_ERROR,
+					  "errorCode,Could not force MCS");
+				return -2;
+			}
+		}
 	}
 
 	dut->hostapd_running = 1;
@@ -8149,6 +8174,12 @@
 			return SIGMA_DUT_ERROR_CALLER_SEND_STATUS;
 		}
 
+		if (ap_set_force_mcs(dut, 0, 1)) {
+			sigma_dut_print(dut, DUT_MSG_ERROR,
+					"Failed to reset force MCS");
+			return SIGMA_DUT_ERROR_CALLER_SEND_STATUS;
+		}
+
 		if (set_ps(get_main_ifname(), dut, 1)) {
 			sigma_dut_print(dut, DUT_MSG_ERROR,
 					"Failed to enable power save");
diff --git a/sigma_dut.h b/sigma_dut.h
index ef0e43c..0dcf160 100644
--- a/sigma_dut.h
+++ b/sigma_dut.h
@@ -920,6 +920,7 @@
 		    struct sigma_ese_alloc *allocs);
 int sta_extract_60g_ese(struct sigma_dut *dut, struct sigma_cmd *cmd,
 			struct sigma_ese_alloc *allocs, int *allocs_size);
+int wil6210_set_force_mcs(struct sigma_dut *dut, int force, int mcs);
 
 /* p2p.c */
 int p2p_cmd_sta_get_parameter(struct sigma_dut *dut, struct sigma_conn *conn,
diff --git a/sta.c b/sta.c
index 87ec5e4..b0a2a39 100644
--- a/sta.c
+++ b/sta.c
@@ -135,6 +135,20 @@
 	struct wil_wmi_ese_slot slots[WIL_WMI_MAX_ESE_SLOTS];
 } __attribute__((packed));
 
+#define WIL_WMI_UT_FORCE_MCS	6
+struct wil_wmi_force_mcs {
+	/* WIL_WMI_UT_HW_SYSAPI */
+	uint16_t module_id;
+	/* WIL_WMI_UT_FORCE_MCS */
+	uint16_t subtype_id;
+	/* cid (ignored in oob_mode, affects all stations) */
+	uint32_t cid;
+	/* 1 to force MCS, 0 to restore default behavior */
+	uint32_t force_enable;
+	/* MCS index, 0-12 */
+	uint32_t mcs;
+} __attribute__((packed));
+
 #define WIL_WMI_UT_HW_SYSAPI 10
 #define WIL_WMI_UT_FORCE_RSN_IE	0x29
 struct wil_wmi_force_rsn_ie {
@@ -467,6 +481,20 @@
 }
 
 
+int wil6210_set_force_mcs(struct sigma_dut *dut, int force, int mcs)
+{
+	struct wil_wmi_force_mcs cmd = { };
+
+	cmd.module_id = WIL_WMI_UT_HW_SYSAPI;
+	cmd.subtype_id = WIL_WMI_UT_FORCE_MCS;
+	cmd.force_enable = (uint32_t) force;
+	cmd.mcs = (uint32_t) mcs;
+
+	return wil6210_wmi_send(dut, WIL_WMI_UNIT_TEST_CMDID,
+				&cmd, sizeof(cmd));
+}
+
+
 static int wil6210_force_rsn_ie(struct sigma_dut *dut, int state)
 {
 	struct wil_wmi_force_rsn_ie cmd = { };
@@ -5076,6 +5104,21 @@
 }
 
 
+static int sta_set_force_mcs(struct sigma_dut *dut, int force, int mcs)
+{
+	switch (get_driver_type()) {
+#ifdef __linux__
+	case DRIVER_WIL6210:
+		return wil6210_set_force_mcs(dut, force, mcs);
+#endif /* __linux__ */
+	default:
+		sigma_dut_print(dut, DUT_MSG_ERROR,
+				"Unsupported sta_set_force_mcs with the current driver");
+		return -1;
+	}
+}
+
+
 static int sta_60g_force_rsn_ie(struct sigma_dut *dut, int state)
 {
 	switch (get_driver_type()) {
@@ -5140,6 +5183,15 @@
 				"Setting BAckRcvBuf to %s", val);
 	}
 
+	val = get_param(cmd, "MCS_FixedRate");
+	if (val) {
+		if (sta_set_force_mcs(dut, 1, atoi(val))) {
+			sigma_dut_print(dut, DUT_MSG_ERROR,
+					"Failed to force MCS");
+			return SIGMA_DUT_ERROR_CALLER_SEND_STATUS;
+		}
+	}
+
 	return SIGMA_DUT_SUCCESS_CALLER_SEND_STATUS;
 }
 
@@ -6975,6 +7027,12 @@
 					buf);
 			return SIGMA_DUT_ERROR_CALLER_SEND_STATUS;
 		}
+
+		if (sta_set_force_mcs(dut, 0, 1)) {
+			sigma_dut_print(dut, DUT_MSG_ERROR,
+					"Failed to reset force MCS");
+			return SIGMA_DUT_ERROR_CALLER_SEND_STATUS;
+		}
 	}
 
 	wpa_command(intf, "WPS_ER_STOP");
@@ -10706,6 +10764,17 @@
 		return sta_set_60g_ese(dut, count, allocs);
 	}
 
+	val = get_param(cmd, "MCS_FixedRate");
+	if (val) {
+		int sta_mcs = atoi(val);
+
+		sigma_dut_print(dut, DUT_MSG_INFO, "Force STA MCS to %d",
+				sta_mcs);
+		wil6210_set_force_mcs(dut, 1, sta_mcs);
+
+		return SIGMA_DUT_SUCCESS_CALLER_SEND_STATUS;
+	}
+
 	send_resp(dut, conn, SIGMA_ERROR,
 		  "errorCode,Invalid sta_set_rfeature(60G)");
 	return SIGMA_DUT_SUCCESS_STATUS_SENT;