[PATCH rdma-next 2/2] IB/mlx5: Implement query_iboe_speed driver API

[Leon Romanovsky <leon@kernel.org>]

From: Mark Zhang <markzhang@nvidia.com>

Implement this API for RoCE, get the link speed by querying PTYS
register.

Signed-off-by: Mark Zhang <markzhang@nvidia.com>
Signed-off-by: Leon Romanovsky <leonro@nvidia.com>
---
 drivers/infiniband/hw/mlx5/main.c            | 41 ++++++++++++++++++++
 drivers/infiniband/ulp/ipoib/ipoib_ethtool.c | 24 ------------
 include/rdma/ib_verbs.h                      | 23 +++++++++++
 3 files changed, 64 insertions(+), 24 deletions(-)

diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index 5e5ed1c8299d..2f108006b7e6 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -3912,6 +3912,46 @@ static int mlx5_ib_stage_raw_eth_non_default_cb(struct mlx5_ib_dev *dev)
 	return 0;
 }
 
+static int mlx5_ib_query_iboe_speed(struct ib_device *ibdev, u32 port_num,
+				    int *speed)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	u32 out[MLX5_ST_SZ_DW(ptys_reg)] = {};
+	u32 eth_prot_oper, mdev_port_num;
+	struct mlx5_core_dev *mdev;
+	u16 active_speed;
+	u8 active_width;
+	bool ext;
+	int err;
+
+	mdev = mlx5_ib_get_native_port_mdev(dev, port_num, &mdev_port_num);
+	if (!mdev)
+		return -EINVAL;
+
+	if (dev->is_rep)
+		mdev_port_num = 1;
+
+	err = mlx5_query_port_ptys(mdev, out, sizeof(out), MLX5_PTYS_EN,
+				   mdev_port_num);
+	if (err)
+		goto out;
+
+	ext = !!MLX5_GET_ETH_PROTO(ptys_reg, out, true, eth_proto_capability);
+	eth_prot_oper = MLX5_GET_ETH_PROTO(ptys_reg, out, ext, eth_proto_oper);
+
+	active_width = IB_WIDTH_4X;
+	active_speed = IB_SPEED_QDR;
+
+	translate_eth_proto_oper(eth_prot_oper, &active_speed,
+				 &active_width, ext);
+	*speed = ib_speed_enum_to_int(active_speed) *
+		ib_width_enum_to_int(active_width);
+
+out:
+	mlx5_ib_put_native_port_mdev(dev, port_num);
+	return err;
+}
+
 static const struct ib_device_ops mlx5_ib_dev_common_roce_ops = {
 	.create_rwq_ind_table = mlx5_ib_create_rwq_ind_table,
 	.create_wq = mlx5_ib_create_wq,
@@ -3919,6 +3959,7 @@ static const struct ib_device_ops mlx5_ib_dev_common_roce_ops = {
 	.destroy_wq = mlx5_ib_destroy_wq,
 	.get_netdev = mlx5_ib_get_netdev,
 	.modify_wq = mlx5_ib_modify_wq,
+	.query_iboe_speed = mlx5_ib_query_iboe_speed,
 
 	INIT_RDMA_OBJ_SIZE(ib_rwq_ind_table, mlx5_ib_rwq_ind_table,
 			   ib_rwq_ind_tbl),
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_ethtool.c b/drivers/infiniband/ulp/ipoib/ipoib_ethtool.c
index 8af99b18d361..8ece31078558 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_ethtool.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_ethtool.c
@@ -155,30 +155,6 @@ static int ipoib_get_sset_count(struct net_device __always_unused *dev,
 	return -EOPNOTSUPP;
 }
 
-/* Return lane speed in unit of 1e6 bit/sec */
-static inline int ib_speed_enum_to_int(int speed)
-{
-	switch (speed) {
-	case IB_SPEED_SDR:
-		return SPEED_2500;
-	case IB_SPEED_DDR:
-		return SPEED_5000;
-	case IB_SPEED_QDR:
-	case IB_SPEED_FDR10:
-		return SPEED_10000;
-	case IB_SPEED_FDR:
-		return SPEED_14000;
-	case IB_SPEED_EDR:
-		return SPEED_25000;
-	case IB_SPEED_HDR:
-		return SPEED_50000;
-	case IB_SPEED_NDR:
-		return SPEED_100000;
-	}
-
-	return SPEED_UNKNOWN;
-}
-
 static int ipoib_get_link_ksettings(struct net_device *netdev,
 				    struct ethtool_link_ksettings *cmd)
 {
diff --git a/include/rdma/ib_verbs.h b/include/rdma/ib_verbs.h
index b143258b847f..4a7a62c7e3e8 100644
--- a/include/rdma/ib_verbs.h
+++ b/include/rdma/ib_verbs.h
@@ -563,6 +563,29 @@ enum ib_port_speed {
 	IB_SPEED_NDR	= 128,
 };
 
+static inline int ib_speed_enum_to_int(int speed)
+{
+	switch (speed) {
+	case IB_SPEED_SDR:
+		return SPEED_2500;
+	case IB_SPEED_DDR:
+		return SPEED_5000;
+	case IB_SPEED_QDR:
+	case IB_SPEED_FDR10:
+		return SPEED_10000;
+	case IB_SPEED_FDR:
+		return SPEED_14000;
+	case IB_SPEED_EDR:
+		return SPEED_25000;
+	case IB_SPEED_HDR:
+		return SPEED_50000;
+	case IB_SPEED_NDR:
+		return SPEED_100000;
+	}
+
+	return SPEED_UNKNOWN;
+}
+
 enum ib_stat_flag {
 	IB_STAT_FLAG_OPTIONAL = 1 << 0,
 };
-- 
2.39.2