From 00a1908f92e31d6d85edc877c98387a9fdb737af 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 07/15] hostapd: mtk: 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              |  8 ++++++++
 src/drivers/driver_nl80211.c      | 33 +++++++++++++++++++++++++++++++
 src/drivers/driver_nl80211.h      |  1 +
 src/drivers/driver_nl80211_capa.c |  3 +++
 11 files changed, 93 insertions(+)

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index cd27d0890..df9c1e7a7 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -4796,6 +4796,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 0e1f192cf..9249a6bb8 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 0616c30bb..0c370e75b 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -1158,6 +1158,19 @@ struct hostapd_config {
 	u8 edcca_enable;
 	s8 edcca_compensation;
 	int *edcca_threshold;
+	u8 three_wire_enable;
+};
+
+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
 };
 
 enum edcca_mode {
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index 4598737a3..a1d83e4ee 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1053,3 +1053,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 bca39c502..5ba62978c 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -144,6 +144,7 @@ int hostapd_drv_configure_edcca_threshold(struct hostapd_data *hapd,
 int hostapd_drv_get_edcca(struct hostapd_data *hapd, const u8 mode, u8 *value);
 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 44a378aa9..c972b15ea 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -2304,6 +2304,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 69a46dfbb..ee5a4f436 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 {
@@ -55,6 +56,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 4cd7505db..9ca19af81 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -4693,6 +4693,14 @@ struct wpa_driver_ops {
 	 */
 	 int (*hemu_ctrl)(void *priv, u8 hemu_onoff);
 	 int (*hemu_dump)(void *priv, u8 *hemu_onoff);
+
+	/**
+	 * three_wire_ctrl - set three_wire_ctrl mode
+	 * @priv: Private driver interface data
+	 * @three_wire_enable: three_wire_ctrl mode
+	 *
+	 */
+	 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 f8b3f40c2..92882cb73 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -12637,6 +12637,38 @@ static int nl80211_get_edcca(void *priv, const u8 mode, u8 *value)
 	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",
@@ -12789,4 +12821,5 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.configure_edcca_enable = nl80211_configure_edcca_enable,
 	.configure_edcca_threshold = nl80211_configure_edcca_threshold,
 	.get_edcca = nl80211_get_edcca,
+	.three_wire_ctrl = nl80211_enable_three_wire,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index 57f02497e..9fe7811b9 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -183,6 +183,7 @@ struct wpa_driver_nl80211_data {
 	unsigned int uses_6ghz: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 cc146d9fc..04bc54e77 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1059,6 +1059,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.25.1