From 32c260621b73c5662442dd29bdfddbf5269e831b Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Wed, 5 Jun 2024 22:10:24 +0800
Subject: [PATCH 10/13] iw: add per-link txpower config

Add per-link txpower config & info dump

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 ieee80211.h |  3 +++
 interface.c |  9 +++++++--
 phy.c       | 38 ++++++++++++++++++++++++++------------
 3 files changed, 36 insertions(+), 14 deletions(-)

diff --git a/ieee80211.h b/ieee80211.h
index 3713a4d..c86984d 100644
--- a/ieee80211.h
+++ b/ieee80211.h
@@ -75,4 +75,7 @@ struct ieee80211_vht_cap {
 #define WLAN_CIPHER_SUITE_BIP_GMAC_256  SUITE(0x000FAC, 12)
 #define WLAN_CIPHER_SUITE_BIP_CMAC_256  SUITE(0x000FAC, 13)
 
+/* multi-link device */
+#define IEEE80211_MLD_MAX_NUM_LINKS	15
+
 #endif /* __IEEE80211 */
diff --git a/interface.c b/interface.c
index bb1a1d3..7a690fa 100644
--- a/interface.c
+++ b/interface.c
@@ -406,6 +406,7 @@ static int print_iface_handler(struct nl_msg *msg, void *arg)
 	struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
 	unsigned int *wiphy = arg;
 	const char *indent = "";
+	int32_t txp;
 
 	nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 		  genlmsg_attrlen(gnlh, 0), NULL);
@@ -449,8 +450,7 @@ static int print_iface_handler(struct nl_msg *msg, void *arg)
 	}
 
 	if (tb_msg[NL80211_ATTR_WIPHY_TX_POWER_LEVEL]) {
-		int32_t txp = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_TX_POWER_LEVEL]);
-
+		txp = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_TX_POWER_LEVEL]);
 		printf("%s\ttxpower %d.%.2d dBm\n",
 		       indent, txp / 100, txp % 100);
 	}
@@ -490,6 +490,11 @@ static int print_iface_handler(struct nl_msg *msg, void *arg)
 				printf("\n%s\t   ", indent);
 				print_channel(tb);
 			}
+			if (tb[NL80211_ATTR_WIPHY_TX_POWER_LEVEL]) {
+				txp = nla_get_u32(tb[NL80211_ATTR_WIPHY_TX_POWER_LEVEL]);
+				printf("\n%s\t   ", indent);
+				printf("txpower %d.%.2d dBm", txp / 100, txp % 100);
+			}
 			printf("\n");
 		}
 	}
diff --git a/phy.c b/phy.c
index 584b103..f9a5b0f 100644
--- a/phy.c
+++ b/phy.c
@@ -686,37 +686,51 @@ static int handle_txpower(struct nl80211_state *state,
 			  enum id_input id)
 {
 	enum nl80211_tx_power_setting type;
-	int mbm;
+	unsigned int link_id;
+	int mbm, pos = 0;
+	char *endptr;
 
-	/* get the required args */
-	if (argc != 1 && argc != 2)
+	/* check args number */
+	if (argc < 1 && argc > 4)
 		return 1;
 
-	if (!strcmp(argv[0], "auto"))
+	if (!strcmp(argv[0], "-l")) {
+		link_id = strtol(argv[1], &endptr, 10);
+		if (*endptr)
+			return 1;
+
+		if (link_id >= IEEE80211_MLD_MAX_NUM_LINKS) {
+			printf("link id %d exceeds max number of links\n", link_id);
+			return 2;
+		}
+		NLA_PUT_U8(msg, NL80211_ATTR_MLO_LINK_ID, link_id);
+		pos += 2;
+	}
+
+	if (!strcmp(argv[pos], "auto"))
 		type = NL80211_TX_POWER_AUTOMATIC;
-	else if (!strcmp(argv[0], "fixed"))
+	else if (!strcmp(argv[pos], "fixed"))
 		type = NL80211_TX_POWER_FIXED;
-	else if (!strcmp(argv[0], "limit"))
+	else if (!strcmp(argv[pos], "limit"))
 		type = NL80211_TX_POWER_LIMITED;
 	else {
-		printf("Invalid parameter: %s\n", argv[0]);
+		printf("Invalid parameter: %s\n", argv[pos]);
 		return 2;
 	}
 
 	NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_TX_POWER_SETTING, type);
 
 	if (type != NL80211_TX_POWER_AUTOMATIC) {
-		char *endptr;
-		if (argc != 2) {
+		if (argc < 2 + pos) {
 			printf("Missing TX power level argument.\n");
 			return 2;
 		}
 
-		mbm = strtol(argv[1], &endptr, 10);
+		mbm = strtol(argv[pos + 1], &endptr, 10);
 		if (*endptr)
 			return 2;
 		NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_TX_POWER_LEVEL, mbm);
-	} else if (argc != 1)
+	} else if (argc != 1 + pos)
 		return 1;
 
 	return 0;
@@ -727,7 +741,7 @@ static int handle_txpower(struct nl80211_state *state,
 COMMAND(set, txpower, "<auto|fixed|limit> [<tx power in mBm>]",
 	NL80211_CMD_SET_WIPHY, 0, CIB_PHY, handle_txpower,
 	"Specify transmit power level and setting type.");
-COMMAND(set, txpower, "<auto|fixed|limit> [<tx power in mBm>]",
+COMMAND(set, txpower, "[-l <link_id>] <auto|fixed|limit> [<tx power in mBm>]",
 	NL80211_CMD_SET_WIPHY, 0, CIB_NETDEV, handle_txpower,
 	"Specify transmit power level and setting type.");
 
-- 
2.18.0