From fc638478a8a2eb45e023557bee5c0aefdf0bba6d Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Fri, 2 Sep 2022 01:03:23 +0800
Subject: [PATCH] Add three wire PTA ctrl hostapd vendor command

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 hostapd/config_file.c             |  4 ++++
 src/ap/ap_config.c                |  1 +
 src/ap/ap_config.h                | 13 ++++++++++++
 src/ap/ap_drv_ops.c               | 11 +++++++++++
 src/ap/ap_drv_ops.h               |  1 +
 src/ap/hostapd.c                  |  2 ++
 src/common/mtk_vendor.h           | 16 +++++++++++++++
 src/drivers/driver.h              |  1 +
 src/drivers/driver_nl80211.c      | 33 +++++++++++++++++++++++++++++++
 src/drivers/driver_nl80211.h      |  1 +
 src/drivers/driver_nl80211_capa.c |  3 +++
 11 files changed, 86 insertions(+)

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index 85d58cd..ee5decb 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -4770,6 +4770,10 @@ static int hostapd_config_fill(struct hostapd_config *conf,
 			return 1;
 		}
 		conf->edcca_compensation = (s8) val;
+	} else if (os_strcmp(buf, "three_wire_enable") == 0) {
+		u8 en = atoi(pos);
+
+		conf->three_wire_enable = en;
 	} else {
 		wpa_printf(MSG_ERROR,
 			   "Line %d: unknown configuration item '%s'",
diff --git a/src/ap/ap_config.c b/src/ap/ap_config.c
index 79e215f..34e4fb1 100644
--- a/src/ap/ap_config.c
+++ b/src/ap/ap_config.c
@@ -297,6 +297,7 @@ struct hostapd_config * hostapd_config_defaults(void)
 
 	conf->edcca_enable = EDCCA_MODE_AUTO;
 	conf->edcca_compensation = EDCCA_DEFAULT_COMPENSATION;
+	conf->three_wire_enable = THREE_WIRE_MODE_DISABLE;
 
 	return conf;
 }
diff --git a/src/ap/ap_config.h b/src/ap/ap_config.h
index 737cc2f..1402891 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -1153,6 +1153,7 @@ struct hostapd_config {
 	unsigned int ch_switch_eht_config;
 	u8 edcca_enable;
 	s8 edcca_compensation;
+	u8 three_wire_enable;
 };
 
 enum edcca_mode {
@@ -1164,6 +1165,18 @@ enum edcca_mode {
 #define EDCCA_MIN_COMPENSATION -126
 #define EDCCA_MAX_COMPENSATION 126
 
+enum three_wire_mode {
+	THREE_WIRE_MODE_DISABLE,
+	THREE_WIRE_MODE_EXT0_ENABLE,
+	THREE_WIRE_MODE_EXT1_ENABLE,
+	THREE_WIRE_MODE_ALL_ENABLE,
+
+	/* keep last */
+	NUM_THREE_WIRE_MODE,
+	THREE_WIRE_MODE_MAX =
+		NUM_THREE_WIRE_MODE - 1
+};
+
 static inline u8 hostapd_get_oper_chwidth(struct hostapd_config *conf)
 {
 #ifdef CONFIG_IEEE80211BE
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index 3de6748..a48c22e 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1035,3 +1035,14 @@ int hostapd_drv_hemu_dump(struct hostapd_data *hapd, u8 *hemu_onoff)
 		return 0;
 	return hapd->driver->hemu_dump(hapd->drv_priv, hemu_onoff);
 }
+
+int hostapd_drv_three_wire_ctrl(struct hostapd_data *hapd)
+{
+	if (!hapd->driver || !hapd->driver->three_wire_ctrl)
+		return 0;
+	if (hapd->iconf->three_wire_enable > THREE_WIRE_MODE_MAX) {
+		wpa_printf(MSG_INFO, "Invalid value for three wire enable\n");
+		return 0;
+	}
+	return hapd->driver->three_wire_ctrl(hapd->drv_priv, hapd->iconf->three_wire_enable);
+}
diff --git a/src/ap/ap_drv_ops.h b/src/ap/ap_drv_ops.h
index 5bcb225..0a8bf0c 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -141,6 +141,7 @@ int hostapd_drv_dpp_listen(struct hostapd_data *hapd, bool enable);
 int hostapd_drv_configure_edcca_threshold(struct hostapd_data *hapd);
 int hostapd_drv_hemu_ctrl(struct hostapd_data *hapd);
 int hostapd_drv_hemu_dump(struct hostapd_data *hapd, u8 *hemu_onoff);
+int hostapd_drv_three_wire_ctrl(struct hostapd_data *hapd);
 
 #include "drivers/driver.h"
 
diff --git a/src/ap/hostapd.c b/src/ap/hostapd.c
index 3b752fc..0c02171 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -2299,6 +2299,8 @@ dfs_offload:
 		goto fail;
 	if (hostapd_drv_hemu_ctrl(hapd) < 0)
 		goto fail;
+	if (hostapd_drv_three_wire_ctrl(hapd) < 0)
+		goto fail;
 
 	wpa_printf(MSG_DEBUG, "%s: Setup of interface done.",
 		   iface->bss[0]->conf->iface);
diff --git a/src/common/mtk_vendor.h b/src/common/mtk_vendor.h
index 5c8f179..99a4d7f 100644
--- a/src/common/mtk_vendor.h
+++ b/src/common/mtk_vendor.h
@@ -13,6 +13,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_HEMU_CTRL = 0xc5,
 	MTK_NL80211_VENDOR_SUBCMD_PHY_CAPA_CTRL= 0xc6,
 	MTK_NL80211_VENDOR_SUBCMD_EDCCA_CTRL = 0xc7,
+	MTK_NL80211_VENDOR_SUBCMD_3WIRE_CTRL = 0xc8
 };
 
 enum mtk_vendor_attr_edcca_ctrl {
@@ -48,6 +49,21 @@ static struct nla_policy edcca_ctrl_policy[NUM_MTK_VENDOR_ATTRS_EDCCA_CTRL] = {
 	[MTK_VENDOR_ATTR_EDCCA_CTRL_COMPENSATE] = { .type = NLA_U8 },
 };
 
+enum mtk_vendor_attr_3wire_ctrl {
+	MTK_VENDOR_ATTR_3WIRE_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_3WIRE_CTRL_MODE,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_3WIRE_CTRL,
+	MTK_VENDOR_ATTR_3WIRE_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_3WIRE_CTRL - 1
+};
+
+static struct nla_policy three_wire_ctrl_policy[NUM_MTK_VENDOR_ATTRS_3WIRE_CTRL] = {
+	[MTK_VENDOR_ATTR_3WIRE_CTRL_MODE] = {.type = NLA_U8 },
+};
+
 enum mtk_vendor_attr_csi_ctrl {
 	MTK_VENDOR_ATTR_CSI_CTRL_UNSPEC,
 
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index 6cc7e9b..0048120 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -4688,6 +4688,7 @@ struct wpa_driver_ops {
 	 */
 	 int (*hemu_ctrl)(void *priv, u8 hemu_onoff);
 	 int (*hemu_dump)(void *priv, u8 *hemu_onoff);
+	 int (*three_wire_ctrl)(void *priv, u8 three_wire_enable);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 0009fb4..5834720 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -12500,6 +12500,38 @@ static int nl80211_configure_edcca_threshold(void *priv,
 	return ret;
 }
 
+static int nl80211_enable_three_wire(void *priv, const u8 three_wire_enable)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	/* Prepare nl80211 cmd */
+	struct nl_msg *msg;
+	struct nlattr *data;
+	int ret;
+
+	if (!drv->mtk_3wire_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: Driver does not support setting three wire control");
+		return 0;
+	}
+
+	if (!(msg = nl80211_drv_msg(drv, 0, NL80211_CMD_VENDOR)) ||
+	    nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_MTK) ||
+	    nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
+			MTK_NL80211_VENDOR_SUBCMD_3WIRE_CTRL) ||
+	    !(data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA)) ||
+	    nla_put_u8(msg, MTK_VENDOR_ATTR_3WIRE_CTRL_MODE, three_wire_enable)) {
+		nlmsg_free(msg);
+		return -ENOBUFS;
+	}
+	nla_nest_end(msg, data);
+	ret = send_and_recv_msgs(drv, msg, NULL, NULL, NULL, NULL);
+	if (ret) {
+		wpa_printf(MSG_ERROR, "Failed to enable three wire. ret=%d (%s) ",
+			   ret, strerror(-ret));
+	}
+	return ret;
+}
 
 const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.name = "nl80211",
@@ -12650,4 +12682,5 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 #endif /* CONFIG_TESTING_OPTIONS */
 /* Need ifdef CONFIG_DRIVER_NL80211_MTK */
 	.configure_edcca_threshold = nl80211_configure_edcca_threshold,
+	.three_wire_ctrl = nl80211_enable_three_wire,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index 62d9696..196d24f 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -182,6 +182,7 @@ struct wpa_driver_nl80211_data {
 	unsigned int brcm_do_acs:1;
 	unsigned int mtk_edcca_vendor_cmd_avail:1;
 	unsigned int mtk_hemu_vendor_cmd_avail:1;
+	unsigned int mtk_3wire_vendor_cmd_avail:1;
 
 	u64 vendor_scan_cookie;
 	u64 remain_on_chan_cookie;
diff --git a/src/drivers/driver_nl80211_capa.c b/src/drivers/driver_nl80211_capa.c
index b5cd2c5..f0397f5 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1053,6 +1053,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 				case MTK_NL80211_VENDOR_SUBCMD_HEMU_CTRL :
 					drv->mtk_hemu_vendor_cmd_avail = 1;
 					break;
+				case MTK_NL80211_VENDOR_SUBCMD_3WIRE_CTRL :
+					drv->mtk_3wire_vendor_cmd_avail = 1;
+					break;
 				}
 			}
 
-- 
2.18.0