From ecfbe0e44f98de44dc7a1e3d8566cf9eb9987458 Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Thu, 13 Jun 2024 16:48:00 +0800
Subject: [PATCH 12/13] iw: add per-radio antenna config

Add per-radio antenna config & info dump
Currently, there is no radio index or supported band bitmap in wiphy data struct
& NL80211 ATTRS.
Therefore, we just use NL80211_BANDS_XX to specify the radio we desire
to config.

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 info.c | 49 +++++++++++++++++++++++++++++++++++++++++--------
 phy.c  | 50 ++++++++++++++++++++++++++++++++++++++++----------
 2 files changed, 81 insertions(+), 18 deletions(-)

diff --git a/info.c b/info.c
index c5e863f..ddfef67 100644
--- a/info.c
+++ b/info.c
@@ -335,6 +335,14 @@ static int print_phy_handler(struct nl_msg *msg, void *arg)
 	static int last_band = -1;
 	static bool band_had_freq = false;
 	bool print_name = true;
+	static const char * const bands[] = {
+		[NL80211_BAND_2GHZ] = "2G",
+		[NL80211_BAND_5GHZ] = "5G",
+		[NL80211_BAND_60GHZ] = "60G",
+		[NL80211_BAND_6GHZ] = "6G",
+		[NL80211_BAND_S1GHZ] = "S1G",
+		[NL80211_BAND_LC] = "LC",
+	};
 
 	nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 		  genlmsg_attrlen(gnlh, 0), NULL);
@@ -540,16 +548,41 @@ next:
 	}
 
 	if (tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_TX] &&
-	    tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_RX])
-		printf("\tAvailable Antennas: TX %#x RX %#x\n",
-		       nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_TX]),
-		       nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_RX]));
+	    tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_RX]) {
+		int i, tx_num, rx_num;
+		__u32 *avail_ants_tx, *avail_ants_rx;
+
+		tx_num = nla_len(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_TX]) / sizeof(__u32);
+		rx_num = nla_len(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_RX]) / sizeof(__u32);
+		if (tx_num == rx_num && tx_num == NUM_NL80211_BANDS) {
+			avail_ants_tx = nla_data(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_TX]);
+			avail_ants_rx = nla_data(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_AVAIL_RX]);
+			printf("\tAvailable Antennas:\n");
+			for (i = 0; i < tx_num; i++)
+				if (avail_ants_tx[i] && avail_ants_rx[i])
+					printf("\t\t%s band: TX %#x RX %#x\n",
+					       bands[i], avail_ants_tx[i],
+					       avail_ants_rx[i]);
+		}
+	}
 
 	if (tb_msg[NL80211_ATTR_WIPHY_ANTENNA_TX] &&
-	    tb_msg[NL80211_ATTR_WIPHY_ANTENNA_RX])
-		printf("\tConfigured Antennas: TX %#x RX %#x\n",
-		       nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_TX]),
-		       nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_RX]));
+	    tb_msg[NL80211_ATTR_WIPHY_ANTENNA_RX]) {
+		int i, tx_num, rx_num;
+		__u32 *ants_tx, *ants_rx;
+
+		tx_num = nla_len(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_TX]) / sizeof(__u32);
+		rx_num = nla_len(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_RX]) / sizeof(__u32);
+		if (tx_num == rx_num && tx_num == NUM_NL80211_BANDS) {
+			ants_tx = nla_data(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_TX]);
+			ants_rx = nla_data(tb_msg[NL80211_ATTR_WIPHY_ANTENNA_RX]);
+			printf("\tConfigured Antennas:\n");
+			for (i = 0; i < tx_num; i++)
+				if (ants_tx[i] && ants_rx[i])
+					printf("\t\t%s band: TX %#x RX %#x\n",
+					       bands[i], ants_tx[i], ants_rx[i]);
+		}
+	}
 
 	if (tb_msg[NL80211_ATTR_SUPPORTED_IFTYPES])
 		print_iftype_list("\tSupported interface modes", "\t\t",
diff --git a/phy.c b/phy.c
index f9a5b0f..594fb25 100644
--- a/phy.c
+++ b/phy.c
@@ -752,34 +752,64 @@ static int handle_antenna(struct nl80211_state *state,
 {
 	char *end;
 	uint32_t tx_ant = 0, rx_ant = 0;
+	uint32_t tx_ants[NUM_NL80211_BANDS], rx_ants[NUM_NL80211_BANDS];
+	int i = 0, bands = 0xffffffff;
+
+	if (!strncmp(argv[0], "-b", 2)) {
+		bands = 0;
+		for (i = 1; i < argc; i++) {
+			if (!strncasecmp("2ghz", argv[i], 4))
+				bands |= BIT(NL80211_BAND_2GHZ);
+			else if (!strncasecmp("5ghz", argv[i], 4))
+				bands |= BIT(NL80211_BAND_5GHZ);
+			else if (!strncasecmp("6ghz", argv[i], 4))
+				bands |= BIT(NL80211_BAND_6GHZ);
+			else
+				break;
+		}
+
+		if (i == 1) {
+			printf("Missing configured bands argument.\n");
+			return 2;
+		}
+	}
 
-	if (argc == 1 && strcmp(argv[0], "all") == 0) {
+	if (argc == 1 + i && strncmp(argv[i], "all", 3) == 0) {
 		tx_ant = 0xffffffff;
 		rx_ant = 0xffffffff;
-	} else if (argc == 1) {
-		tx_ant = rx_ant = strtoul(argv[0], &end, 0);
+	} else if (argc == 1 + i) {
+		tx_ant = rx_ant = strtoul(argv[i], &end, 0);
 		if (*end)
 			return 1;
-	}
-	else if (argc == 2) {
-		tx_ant = strtoul(argv[0], &end, 0);
+	} else if (argc == 2 + i) {
+		tx_ant = strtoul(argv[i], &end, 0);
 		if (*end)
 			return 1;
-		rx_ant = strtoul(argv[1], &end, 0);
+		rx_ant = strtoul(argv[i + 1], &end, 0);
 		if (*end)
 			return 1;
 	} else
 		return 1;
 
-	NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_ANTENNA_TX, tx_ant);
-	NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_ANTENNA_RX, rx_ant);
+	memset(tx_ants, 0, sizeof(tx_ants));
+	memset(rx_ants, 0, sizeof(rx_ants));
+	for (i = 0; i < NUM_NL80211_BANDS; i++) {
+		if (!(bands & BIT(i)))
+			continue;
+
+		tx_ants[i] = tx_ant;
+		rx_ants[i] = rx_ant;
+	}
+
+	NLA_PUT(msg, NL80211_ATTR_WIPHY_ANTENNA_TX, sizeof(tx_ants), tx_ants);
+	NLA_PUT(msg, NL80211_ATTR_WIPHY_ANTENNA_RX, sizeof(rx_ants), rx_ants);
 
 	return 0;
 
  nla_put_failure:
 	return -ENOBUFS;
 }
-COMMAND(set, antenna, "<bitmap> | all | <tx bitmap> <rx bitmap>",
+COMMAND(set, antenna, "[-b [2GHz] [5GHz] [6GHz]] <bitmap> | all | <tx bitmap> <rx bitmap>",
 	NL80211_CMD_SET_WIPHY, 0, CIB_PHY, handle_antenna,
 	"Set a bitmap of allowed antennas to use for TX and RX.\n"
 	"The driver may reject antenna configurations it cannot support.");
-- 
2.18.0