[PATCH net-next 3/5] net: hns3: add ethtool -p support for phy device

[Peng Li <lipeng321@huawei.com>]

From: Jian Shen <shenjian15@huawei.com>

Add led location support for phy device. The led will keep blinking
with frequency 2HZ when locating.

Signed-off-by: Jian Shen <shenjian15@huawei.com>
Signed-off-by: Peng Li <lipeng321@huawei.com>
---
 drivers/net/ethernet/hisilicon/hns3/hnae3.h        |  2 +
 drivers/net/ethernet/hisilicon/hns3/hns3_ethtool.c | 12 +++
 .../ethernet/hisilicon/hns3/hns3pf/hclge_main.c    | 88 ++++++++++++++++++++++
 .../ethernet/hisilicon/hns3/hns3pf/hclge_main.h    | 12 +++
 4 files changed, 114 insertions(+)

diff --git a/drivers/net/ethernet/hisilicon/hns3/hnae3.h b/drivers/net/ethernet/hisilicon/hns3/hnae3.h
index d104ce5..fd06bc7 100644
--- a/drivers/net/ethernet/hisilicon/hns3/hnae3.h
+++ b/drivers/net/ethernet/hisilicon/hns3/hnae3.h
@@ -405,6 +405,8 @@ struct hnae3_ae_ops {
 	int (*set_channels)(struct hnae3_handle *handle, u32 new_tqps_num);
 	void (*get_flowctrl_adv)(struct hnae3_handle *handle,
 				 u32 *flowctrl_adv);
+	int (*set_led_id)(struct hnae3_handle *handle,
+			  enum ethtool_phys_id_state status);
 };
 
 struct hnae3_dcb_ops {
diff --git a/drivers/net/ethernet/hisilicon/hns3/hns3_ethtool.c b/drivers/net/ethernet/hisilicon/hns3/hns3_ethtool.c
index 1c8b293..7410205 100644
--- a/drivers/net/ethernet/hisilicon/hns3/hns3_ethtool.c
+++ b/drivers/net/ethernet/hisilicon/hns3/hns3_ethtool.c
@@ -1084,6 +1084,17 @@ static void hns3_get_regs(struct net_device *netdev,
 	h->ae_algo->ops->get_regs(h, &cmd->version, data);
 }
 
+static int hns3_set_phys_id(struct net_device *netdev,
+			    enum ethtool_phys_id_state state)
+{
+	struct hnae3_handle *h = hns3_get_handle(netdev);
+
+	if (!h->ae_algo || !h->ae_algo->ops || !h->ae_algo->ops->set_led_id)
+		return -EOPNOTSUPP;
+
+	return h->ae_algo->ops->set_led_id(h, state);
+}
+
 static const struct ethtool_ops hns3vf_ethtool_ops = {
 	.get_drvinfo = hns3_get_drvinfo,
 	.get_ringparam = hns3_get_ringparam,
@@ -1126,6 +1137,7 @@ static const struct ethtool_ops hns3_ethtool_ops = {
 	.set_coalesce = hns3_set_coalesce,
 	.get_regs_len = hns3_get_regs_len,
 	.get_regs = hns3_get_regs,
+	.set_phys_id = hns3_set_phys_id,
 };
 
 void hns3_ethtool_set_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/hisilicon/hns3/hns3pf/hclge_main.c b/drivers/net/ethernet/hisilicon/hns3/hns3pf/hclge_main.c
index 6e64bed..73caf06 100644
--- a/drivers/net/ethernet/hisilicon/hns3/hns3pf/hclge_main.c
+++ b/drivers/net/ethernet/hisilicon/hns3/hns3pf/hclge_main.c
@@ -5819,6 +5819,93 @@ static void hclge_get_regs(struct hnae3_handle *handle, u32 *version,
 			"Get 64 bit register failed, ret = %d.\n", ret);
 }
 
+static int hclge_set_led_status_phy(struct phy_device *phydev, int value)
+{
+	int ret, cur_page;
+
+	mutex_lock(&phydev->lock);
+
+	ret = phy_read(phydev, HCLGE_PHY_PAGE_REG);
+	if (ret < 0)
+		goto out;
+	else
+		cur_page = ret;
+
+	ret = phy_write(phydev, HCLGE_PHY_PAGE_REG, HCLGE_PHY_PAGE_LED);
+	if (ret)
+		goto out;
+
+	ret = phy_write(phydev, HCLGE_LED_FC_REG, value);
+	if (ret)
+		goto out;
+
+	ret = phy_write(phydev, HCLGE_PHY_PAGE_REG, cur_page);
+
+out:
+	mutex_unlock(&phydev->lock);
+	return ret;
+}
+
+static int hclge_get_led_status_phy(struct phy_device *phydev, int *value)
+{
+	int ret, cur_page;
+
+	mutex_lock(&phydev->lock);
+
+	ret = phy_read(phydev, HCLGE_PHY_PAGE_REG);
+	if (ret < 0)
+		goto out;
+	else
+		cur_page = ret;
+
+	ret = phy_write(phydev, HCLGE_PHY_PAGE_REG, HCLGE_PHY_PAGE_LED);
+	if (ret)
+		goto out;
+
+	*value = phy_read(phydev, HCLGE_LED_FC_REG);
+
+	ret = phy_write(phydev, HCLGE_PHY_PAGE_REG, cur_page);
+
+out:
+	mutex_unlock(&phydev->lock);
+	return ret;
+}
+
+static int hclge_set_led_id(struct hnae3_handle *handle,
+			    enum ethtool_phys_id_state status)
+{
+#define BLINK_FREQUENCY		2
+	struct hclge_vport *vport = hclge_get_vport(handle);
+	struct hclge_dev *hdev = vport->back;
+	struct phy_device *phydev = hdev->hw.mac.phydev;
+	int ret = 0;
+
+	if (!phydev)
+		return -EOPNOTSUPP;
+
+	switch (status) {
+	case ETHTOOL_ID_ACTIVE:
+		ret = hclge_get_led_status_phy(phydev, &hdev->phy_led_val);
+		if (ret)
+			return ret;
+		return BLINK_FREQUENCY;
+	case ETHTOOL_ID_ON:
+		ret = hclge_set_led_status_phy(phydev, HCLGE_LED_FORCE_ON);
+		break;
+	case ETHTOOL_ID_OFF:
+		ret = hclge_set_led_status_phy(phydev, HCLGE_LED_FORCE_OFF);
+		break;
+	case ETHTOOL_ID_INACTIVE:
+		ret = hclge_set_led_status_phy(phydev, hdev->phy_led_val);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
 static const struct hnae3_ae_ops hclge_ops = {
 	.init_ae_dev = hclge_init_ae_dev,
 	.uninit_ae_dev = hclge_uninit_ae_dev,
@@ -5872,6 +5959,7 @@ static const struct hnae3_ae_ops hclge_ops = {
 	.get_flowctrl_adv = hclge_get_flowctrl_adv,
 	.get_regs_len = hclge_get_regs_len,
 	.get_regs = hclge_get_regs,
+	.set_led_id = hclge_set_led_id,
 };
 
 static struct hnae3_ae_algo ae_algo = {
diff --git a/drivers/net/ethernet/hisilicon/hns3/hns3pf/hclge_main.h b/drivers/net/ethernet/hisilicon/hns3/hns3pf/hclge_main.h
index eeb6c8d..9969d3c 100644
--- a/drivers/net/ethernet/hisilicon/hns3/hns3pf/hclge_main.h
+++ b/drivers/net/ethernet/hisilicon/hns3/hns3pf/hclge_main.h
@@ -63,6 +63,7 @@
 
 #define HCLGE_PHY_PAGE_MDIX		0
 #define HCLGE_PHY_PAGE_COPPER		0
+#define HCLGE_PHY_PAGE_LED		3
 
 /* Page Selection Reg. */
 #define HCLGE_PHY_PAGE_REG		22
@@ -73,6 +74,15 @@
 /* Copper Specific Status Register */
 #define HCLGE_PHY_CSS_REG		17
 
+/* LED Function Control Register */
+#define HCLGE_LED_FC_REG		16
+
+/* LED Polarity Control Register */
+#define HCLGE_LED_PC_REG		17
+
+#define HCLGE_LED_FORCE_ON		9
+#define HCLGE_LED_FORCE_OFF		8
+
 #define HCLGE_PHY_MDIX_CTRL_S		(5)
 #define HCLGE_PHY_MDIX_CTRL_M		GENMASK(6, 5)
 
@@ -550,6 +560,8 @@ struct hclge_dev {
 	bool accept_mta_mc; /* Whether accept mta filter multicast */
 
 	struct hclge_vlan_type_cfg vlan_type_cfg;
+
+	int phy_led_val;
 };
 
 /* VPort level vlan tag configuration for TX direction */
-- 
2.9.3