Adding upstream version 6.6.15.upstream/6.6.15

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

9 files changed, 3294 insertions, 0 deletions

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/debugfs.c b/drivers/net/ethernet/mellanox/mlx5/core/lag/debugfs.c
new file mode 100644
index 0000000000..f4b777d4e1
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/debugfs.c
@@ -0,0 +1,182 @@
+// SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
+/* Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES. All rights reserved. */
+
+#include "lag.h"
+
+static char *get_str_mode_type(struct mlx5_lag *ldev)
+{
+	switch (ldev->mode) {
+	case MLX5_LAG_MODE_ROCE: return "roce";
+	case MLX5_LAG_MODE_SRIOV: return "switchdev";
+	case MLX5_LAG_MODE_MULTIPATH: return "multipath";
+	case MLX5_LAG_MODE_MPESW: return "multiport_eswitch";
+	default: return "invalid";
+	}
+
+	return NULL;
+}
+
+static int type_show(struct seq_file *file, void *priv)
+{
+	struct mlx5_core_dev *dev = file->private;
+	struct mlx5_lag *ldev;
+	char *mode = NULL;
+
+	ldev = mlx5_lag_dev(dev);
+	mutex_lock(&ldev->lock);
+	if (__mlx5_lag_is_active(ldev))
+		mode = get_str_mode_type(ldev);
+	mutex_unlock(&ldev->lock);
+	if (!mode)
+		return -EINVAL;
+	seq_printf(file, "%s\n", mode);
+
+	return 0;
+}
+
+static int port_sel_mode_show(struct seq_file *file, void *priv)
+{
+	struct mlx5_core_dev *dev = file->private;
+	struct mlx5_lag *ldev;
+	int ret = 0;
+	char *mode;
+
+	ldev = mlx5_lag_dev(dev);
+	mutex_lock(&ldev->lock);
+	if (__mlx5_lag_is_active(ldev))
+		mode = mlx5_get_str_port_sel_mode(ldev->mode, ldev->mode_flags);
+	else
+		ret = -EINVAL;
+	mutex_unlock(&ldev->lock);
+	if (ret)
+		return ret;
+
+	seq_printf(file, "%s\n", mode);
+	return 0;
+}
+
+static int state_show(struct seq_file *file, void *priv)
+{
+	struct mlx5_core_dev *dev = file->private;
+	struct mlx5_lag *ldev;
+	bool active;
+
+	ldev = mlx5_lag_dev(dev);
+	mutex_lock(&ldev->lock);
+	active = __mlx5_lag_is_active(ldev);
+	mutex_unlock(&ldev->lock);
+	seq_printf(file, "%s\n", active ? "active" : "disabled");
+	return 0;
+}
+
+static int flags_show(struct seq_file *file, void *priv)
+{
+	struct mlx5_core_dev *dev = file->private;
+	bool fdb_sel_mode_native;
+	struct mlx5_lag *ldev;
+	bool shared_fdb;
+	bool lag_active;
+
+	ldev = mlx5_lag_dev(dev);
+	mutex_lock(&ldev->lock);
+	lag_active = __mlx5_lag_is_active(ldev);
+	if (!lag_active)
+		goto unlock;
+
+	shared_fdb = test_bit(MLX5_LAG_MODE_FLAG_SHARED_FDB, &ldev->mode_flags);
+	fdb_sel_mode_native = test_bit(MLX5_LAG_MODE_FLAG_FDB_SEL_MODE_NATIVE,
+				       &ldev->mode_flags);
+
+unlock:
+	mutex_unlock(&ldev->lock);
+	if (!lag_active)
+		return -EINVAL;
+
+	seq_printf(file, "%s:%s\n", "shared_fdb", shared_fdb ? "on" : "off");
+	seq_printf(file, "%s:%s\n", "fdb_selection_mode",
+		   fdb_sel_mode_native ? "native" : "affinity");
+	return 0;
+}
+
+static int mapping_show(struct seq_file *file, void *priv)
+{
+	struct mlx5_core_dev *dev = file->private;
+	u8 ports[MLX5_MAX_PORTS] = {};
+	struct mlx5_lag *ldev;
+	bool hash = false;
+	bool lag_active;
+	int num_ports;
+	int i;
+
+	ldev = mlx5_lag_dev(dev);
+	mutex_lock(&ldev->lock);
+	lag_active = __mlx5_lag_is_active(ldev);
+	if (lag_active) {
+		if (test_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, &ldev->mode_flags)) {
+			mlx5_infer_tx_enabled(&ldev->tracker, ldev->ports, ports,
+					      &num_ports);
+			hash = true;
+		} else {
+			for (i = 0; i < ldev->ports; i++)
+				ports[i] = ldev->v2p_map[i];
+			num_ports = ldev->ports;
+		}
+	}
+	mutex_unlock(&ldev->lock);
+	if (!lag_active)
+		return -EINVAL;
+
+	for (i = 0; i < num_ports; i++) {
+		if (hash)
+			seq_printf(file, "%d\n", ports[i] + 1);
+		else
+			seq_printf(file, "%d:%d\n", i + 1, ports[i]);
+	}
+
+	return 0;
+}
+
+static int members_show(struct seq_file *file, void *priv)
+{
+	struct mlx5_core_dev *dev = file->private;
+	struct mlx5_lag *ldev;
+	int i;
+
+	ldev = mlx5_lag_dev(dev);
+	mutex_lock(&ldev->lock);
+	for (i = 0; i < ldev->ports; i++) {
+		if (!ldev->pf[i].dev)
+			continue;
+		seq_printf(file, "%s\n", dev_name(ldev->pf[i].dev->device));
+	}
+	mutex_unlock(&ldev->lock);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(type);
+DEFINE_SHOW_ATTRIBUTE(port_sel_mode);
+DEFINE_SHOW_ATTRIBUTE(state);
+DEFINE_SHOW_ATTRIBUTE(flags);
+DEFINE_SHOW_ATTRIBUTE(mapping);
+DEFINE_SHOW_ATTRIBUTE(members);
+
+void mlx5_ldev_add_debugfs(struct mlx5_core_dev *dev)
+{
+	struct dentry *dbg;
+
+	dbg = debugfs_create_dir("lag", mlx5_debugfs_get_dev_root(dev));
+	dev->priv.dbg.lag_debugfs = dbg;
+
+	debugfs_create_file("type", 0444, dbg, dev, &type_fops);
+	debugfs_create_file("port_sel_mode", 0444, dbg, dev, &port_sel_mode_fops);
+	debugfs_create_file("state", 0444, dbg, dev, &state_fops);
+	debugfs_create_file("flags", 0444, dbg, dev, &flags_fops);
+	debugfs_create_file("mapping", 0444, dbg, dev, &mapping_fops);
+	debugfs_create_file("members", 0444, dbg, dev, &members_fops);
+}
+
+void mlx5_ldev_remove_debugfs(struct dentry *dbg)
+{
+	debugfs_remove_recursive(dbg);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/lag.c b/drivers/net/ethernet/mellanox/mlx5/core/lag/lag.c
new file mode 100644
index 0000000000..af3fac090b
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/lag.c
@@ -0,0 +1,1620 @@
+/*
+ * Copyright (c) 2016, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/netdevice.h>
+#include <net/bonding.h>
+#include <linux/mlx5/driver.h>
+#include <linux/mlx5/eswitch.h>
+#include <linux/mlx5/vport.h>
+#include "lib/devcom.h"
+#include "mlx5_core.h"
+#include "eswitch.h"
+#include "esw/acl/ofld.h"
+#include "lag.h"
+#include "mp.h"
+#include "mpesw.h"
+
+enum {
+	MLX5_LAG_EGRESS_PORT_1 = 1,
+	MLX5_LAG_EGRESS_PORT_2,
+};
+
+/* General purpose, use for short periods of time.
+ * Beware of lock dependencies (preferably, no locks should be acquired
+ * under it).
+ */
+static DEFINE_SPINLOCK(lag_lock);
+
+static int get_port_sel_mode(enum mlx5_lag_mode mode, unsigned long flags)
+{
+	if (test_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, &flags))
+		return MLX5_LAG_PORT_SELECT_MODE_PORT_SELECT_FT;
+
+	if (mode == MLX5_LAG_MODE_MPESW)
+		return MLX5_LAG_PORT_SELECT_MODE_PORT_SELECT_MPESW;
+
+	return MLX5_LAG_PORT_SELECT_MODE_QUEUE_AFFINITY;
+}
+
+static u8 lag_active_port_bits(struct mlx5_lag *ldev)
+{
+	u8 enabled_ports[MLX5_MAX_PORTS] = {};
+	u8 active_port = 0;
+	int num_enabled;
+	int idx;
+
+	mlx5_infer_tx_enabled(&ldev->tracker, ldev->ports, enabled_ports,
+			      &num_enabled);
+	for (idx = 0; idx < num_enabled; idx++)
+		active_port |= BIT_MASK(enabled_ports[idx]);
+
+	return active_port;
+}
+
+static int mlx5_cmd_create_lag(struct mlx5_core_dev *dev, u8 *ports, int mode,
+			       unsigned long flags)
+{
+	bool fdb_sel_mode = test_bit(MLX5_LAG_MODE_FLAG_FDB_SEL_MODE_NATIVE,
+				     &flags);
+	int port_sel_mode = get_port_sel_mode(mode, flags);
+	u32 in[MLX5_ST_SZ_DW(create_lag_in)] = {};
+	void *lag_ctx;
+
+	lag_ctx = MLX5_ADDR_OF(create_lag_in, in, ctx);
+	MLX5_SET(create_lag_in, in, opcode, MLX5_CMD_OP_CREATE_LAG);
+	MLX5_SET(lagc, lag_ctx, fdb_selection_mode, fdb_sel_mode);
+
+	switch (port_sel_mode) {
+	case MLX5_LAG_PORT_SELECT_MODE_QUEUE_AFFINITY:
+		MLX5_SET(lagc, lag_ctx, tx_remap_affinity_1, ports[0]);
+		MLX5_SET(lagc, lag_ctx, tx_remap_affinity_2, ports[1]);
+		break;
+	case MLX5_LAG_PORT_SELECT_MODE_PORT_SELECT_FT:
+		if (!MLX5_CAP_PORT_SELECTION(dev, port_select_flow_table_bypass))
+			break;
+
+		MLX5_SET(lagc, lag_ctx, active_port,
+			 lag_active_port_bits(mlx5_lag_dev(dev)));
+		break;
+	default:
+		break;
+	}
+	MLX5_SET(lagc, lag_ctx, port_select_mode, port_sel_mode);
+
+	return mlx5_cmd_exec_in(dev, create_lag, in);
+}
+
+static int mlx5_cmd_modify_lag(struct mlx5_core_dev *dev, u8 num_ports,
+			       u8 *ports)
+{
+	u32 in[MLX5_ST_SZ_DW(modify_lag_in)] = {};
+	void *lag_ctx = MLX5_ADDR_OF(modify_lag_in, in, ctx);
+
+	MLX5_SET(modify_lag_in, in, opcode, MLX5_CMD_OP_MODIFY_LAG);
+	MLX5_SET(modify_lag_in, in, field_select, 0x1);
+
+	MLX5_SET(lagc, lag_ctx, tx_remap_affinity_1, ports[0]);
+	MLX5_SET(lagc, lag_ctx, tx_remap_affinity_2, ports[1]);
+
+	return mlx5_cmd_exec_in(dev, modify_lag, in);
+}
+
+int mlx5_cmd_create_vport_lag(struct mlx5_core_dev *dev)
+{
+	u32 in[MLX5_ST_SZ_DW(create_vport_lag_in)] = {};
+
+	MLX5_SET(create_vport_lag_in, in, opcode, MLX5_CMD_OP_CREATE_VPORT_LAG);
+
+	return mlx5_cmd_exec_in(dev, create_vport_lag, in);
+}
+EXPORT_SYMBOL(mlx5_cmd_create_vport_lag);
+
+int mlx5_cmd_destroy_vport_lag(struct mlx5_core_dev *dev)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_vport_lag_in)] = {};
+
+	MLX5_SET(destroy_vport_lag_in, in, opcode, MLX5_CMD_OP_DESTROY_VPORT_LAG);
+
+	return mlx5_cmd_exec_in(dev, destroy_vport_lag, in);
+}
+EXPORT_SYMBOL(mlx5_cmd_destroy_vport_lag);
+
+static void mlx5_infer_tx_disabled(struct lag_tracker *tracker, u8 num_ports,
+				   u8 *ports, int *num_disabled)
+{
+	int i;
+
+	*num_disabled = 0;
+	for (i = 0; i < num_ports; i++) {
+		if (!tracker->netdev_state[i].tx_enabled ||
+		    !tracker->netdev_state[i].link_up)
+			ports[(*num_disabled)++] = i;
+	}
+}
+
+void mlx5_infer_tx_enabled(struct lag_tracker *tracker, u8 num_ports,
+			   u8 *ports, int *num_enabled)
+{
+	int i;
+
+	*num_enabled = 0;
+	for (i = 0; i < num_ports; i++) {
+		if (tracker->netdev_state[i].tx_enabled &&
+		    tracker->netdev_state[i].link_up)
+			ports[(*num_enabled)++] = i;
+	}
+
+	if (*num_enabled == 0)
+		mlx5_infer_tx_disabled(tracker, num_ports, ports, num_enabled);
+}
+
+static void mlx5_lag_print_mapping(struct mlx5_core_dev *dev,
+				   struct mlx5_lag *ldev,
+				   struct lag_tracker *tracker,
+				   unsigned long flags)
+{
+	char buf[MLX5_MAX_PORTS * 10 + 1] = {};
+	u8 enabled_ports[MLX5_MAX_PORTS] = {};
+	int written = 0;
+	int num_enabled;
+	int idx;
+	int err;
+	int i;
+	int j;
+
+	if (test_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, &flags)) {
+		mlx5_infer_tx_enabled(tracker, ldev->ports, enabled_ports,
+				      &num_enabled);
+		for (i = 0; i < num_enabled; i++) {
+			err = scnprintf(buf + written, 4, "%d, ", enabled_ports[i] + 1);
+			if (err != 3)
+				return;
+			written += err;
+		}
+		buf[written - 2] = 0;
+		mlx5_core_info(dev, "lag map active ports: %s\n", buf);
+	} else {
+		for (i = 0; i < ldev->ports; i++) {
+			for (j  = 0; j < ldev->buckets; j++) {
+				idx = i * ldev->buckets + j;
+				err = scnprintf(buf + written, 10,
+						" port %d:%d", i + 1, ldev->v2p_map[idx]);
+				if (err != 9)
+					return;
+				written += err;
+			}
+		}
+		mlx5_core_info(dev, "lag map:%s\n", buf);
+	}
+}
+
+static int mlx5_lag_netdev_event(struct notifier_block *this,
+				 unsigned long event, void *ptr);
+static void mlx5_do_bond_work(struct work_struct *work);
+
+static void mlx5_ldev_free(struct kref *ref)
+{
+	struct mlx5_lag *ldev = container_of(ref, struct mlx5_lag, ref);
+
+	if (ldev->nb.notifier_call)
+		unregister_netdevice_notifier_net(&init_net, &ldev->nb);
+	mlx5_lag_mp_cleanup(ldev);
+	cancel_delayed_work_sync(&ldev->bond_work);
+	destroy_workqueue(ldev->wq);
+	mutex_destroy(&ldev->lock);
+	kfree(ldev);
+}
+
+static void mlx5_ldev_put(struct mlx5_lag *ldev)
+{
+	kref_put(&ldev->ref, mlx5_ldev_free);
+}
+
+static void mlx5_ldev_get(struct mlx5_lag *ldev)
+{
+	kref_get(&ldev->ref);
+}
+
+static struct mlx5_lag *mlx5_lag_dev_alloc(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	int err;
+
+	ldev = kzalloc(sizeof(*ldev), GFP_KERNEL);
+	if (!ldev)
+		return NULL;
+
+	ldev->wq = create_singlethread_workqueue("mlx5_lag");
+	if (!ldev->wq) {
+		kfree(ldev);
+		return NULL;
+	}
+
+	kref_init(&ldev->ref);
+	mutex_init(&ldev->lock);
+	INIT_DELAYED_WORK(&ldev->bond_work, mlx5_do_bond_work);
+
+	ldev->nb.notifier_call = mlx5_lag_netdev_event;
+	if (register_netdevice_notifier_net(&init_net, &ldev->nb)) {
+		ldev->nb.notifier_call = NULL;
+		mlx5_core_err(dev, "Failed to register LAG netdev notifier\n");
+	}
+	ldev->mode = MLX5_LAG_MODE_NONE;
+
+	err = mlx5_lag_mp_init(ldev);
+	if (err)
+		mlx5_core_err(dev, "Failed to init multipath lag err=%d\n",
+			      err);
+
+	ldev->ports = MLX5_CAP_GEN(dev, num_lag_ports);
+	ldev->buckets = 1;
+
+	return ldev;
+}
+
+int mlx5_lag_dev_get_netdev_idx(struct mlx5_lag *ldev,
+				struct net_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < ldev->ports; i++)
+		if (ldev->pf[i].netdev == ndev)
+			return i;
+
+	return -ENOENT;
+}
+
+static bool __mlx5_lag_is_roce(struct mlx5_lag *ldev)
+{
+	return ldev->mode == MLX5_LAG_MODE_ROCE;
+}
+
+static bool __mlx5_lag_is_sriov(struct mlx5_lag *ldev)
+{
+	return ldev->mode == MLX5_LAG_MODE_SRIOV;
+}
+
+/* Create a mapping between steering slots and active ports.
+ * As we have ldev->buckets slots per port first assume the native
+ * mapping should be used.
+ * If there are ports that are disabled fill the relevant slots
+ * with mapping that points to active ports.
+ */
+static void mlx5_infer_tx_affinity_mapping(struct lag_tracker *tracker,
+					   u8 num_ports,
+					   u8 buckets,
+					   u8 *ports)
+{
+	int disabled[MLX5_MAX_PORTS] = {};
+	int enabled[MLX5_MAX_PORTS] = {};
+	int disabled_ports_num = 0;
+	int enabled_ports_num = 0;
+	int idx;
+	u32 rand;
+	int i;
+	int j;
+
+	for (i = 0; i < num_ports; i++) {
+		if (tracker->netdev_state[i].tx_enabled &&
+		    tracker->netdev_state[i].link_up)
+			enabled[enabled_ports_num++] = i;
+		else
+			disabled[disabled_ports_num++] = i;
+	}
+
+	/* Use native mapping by default where each port's buckets
+	 * point the native port: 1 1 1 .. 1 2 2 2 ... 2 3 3 3 ... 3 etc
+	 */
+	for (i = 0; i < num_ports; i++)
+		for (j = 0; j < buckets; j++) {
+			idx = i * buckets + j;
+			ports[idx] = MLX5_LAG_EGRESS_PORT_1 + i;
+		}
+
+	/* If all ports are disabled/enabled keep native mapping */
+	if (enabled_ports_num == num_ports ||
+	    disabled_ports_num == num_ports)
+		return;
+
+	/* Go over the disabled ports and for each assign a random active port */
+	for (i = 0; i < disabled_ports_num; i++) {
+		for (j = 0; j < buckets; j++) {
+			get_random_bytes(&rand, 4);
+			ports[disabled[i] * buckets + j] = enabled[rand % enabled_ports_num] + 1;
+		}
+	}
+}
+
+static bool mlx5_lag_has_drop_rule(struct mlx5_lag *ldev)
+{
+	int i;
+
+	for (i = 0; i < ldev->ports; i++)
+		if (ldev->pf[i].has_drop)
+			return true;
+	return false;
+}
+
+static void mlx5_lag_drop_rule_cleanup(struct mlx5_lag *ldev)
+{
+	int i;
+
+	for (i = 0; i < ldev->ports; i++) {
+		if (!ldev->pf[i].has_drop)
+			continue;
+
+		mlx5_esw_acl_ingress_vport_drop_rule_destroy(ldev->pf[i].dev->priv.eswitch,
+							     MLX5_VPORT_UPLINK);
+		ldev->pf[i].has_drop = false;
+	}
+}
+
+static void mlx5_lag_drop_rule_setup(struct mlx5_lag *ldev,
+				     struct lag_tracker *tracker)
+{
+	u8 disabled_ports[MLX5_MAX_PORTS] = {};
+	struct mlx5_core_dev *dev;
+	int disabled_index;
+	int num_disabled;
+	int err;
+	int i;
+
+	/* First delete the current drop rule so there won't be any dropped
+	 * packets
+	 */
+	mlx5_lag_drop_rule_cleanup(ldev);
+
+	if (!ldev->tracker.has_inactive)
+		return;
+
+	mlx5_infer_tx_disabled(tracker, ldev->ports, disabled_ports, &num_disabled);
+
+	for (i = 0; i < num_disabled; i++) {
+		disabled_index = disabled_ports[i];
+		dev = ldev->pf[disabled_index].dev;
+		err = mlx5_esw_acl_ingress_vport_drop_rule_create(dev->priv.eswitch,
+								  MLX5_VPORT_UPLINK);
+		if (!err)
+			ldev->pf[disabled_index].has_drop = true;
+		else
+			mlx5_core_err(dev,
+				      "Failed to create lag drop rule, error: %d", err);
+	}
+}
+
+static int mlx5_cmd_modify_active_port(struct mlx5_core_dev *dev, u8 ports)
+{
+	u32 in[MLX5_ST_SZ_DW(modify_lag_in)] = {};
+	void *lag_ctx;
+
+	lag_ctx = MLX5_ADDR_OF(modify_lag_in, in, ctx);
+
+	MLX5_SET(modify_lag_in, in, opcode, MLX5_CMD_OP_MODIFY_LAG);
+	MLX5_SET(modify_lag_in, in, field_select, 0x2);
+
+	MLX5_SET(lagc, lag_ctx, active_port, ports);
+
+	return mlx5_cmd_exec_in(dev, modify_lag, in);
+}
+
+static int _mlx5_modify_lag(struct mlx5_lag *ldev, u8 *ports)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	u8 active_ports;
+	int ret;
+
+	if (test_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, &ldev->mode_flags)) {
+		ret = mlx5_lag_port_sel_modify(ldev, ports);
+		if (ret ||
+		    !MLX5_CAP_PORT_SELECTION(dev0, port_select_flow_table_bypass))
+			return ret;
+
+		active_ports = lag_active_port_bits(ldev);
+
+		return mlx5_cmd_modify_active_port(dev0, active_ports);
+	}
+	return mlx5_cmd_modify_lag(dev0, ldev->ports, ports);
+}
+
+void mlx5_modify_lag(struct mlx5_lag *ldev,
+		     struct lag_tracker *tracker)
+{
+	u8 ports[MLX5_MAX_PORTS * MLX5_LAG_MAX_HASH_BUCKETS] = {};
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	int idx;
+	int err;
+	int i;
+	int j;
+
+	mlx5_infer_tx_affinity_mapping(tracker, ldev->ports, ldev->buckets, ports);
+
+	for (i = 0; i < ldev->ports; i++) {
+		for (j = 0; j < ldev->buckets; j++) {
+			idx = i * ldev->buckets + j;
+			if (ports[idx] == ldev->v2p_map[idx])
+				continue;
+			err = _mlx5_modify_lag(ldev, ports);
+			if (err) {
+				mlx5_core_err(dev0,
+					      "Failed to modify LAG (%d)\n",
+					      err);
+				return;
+			}
+			memcpy(ldev->v2p_map, ports, sizeof(ports));
+
+			mlx5_lag_print_mapping(dev0, ldev, tracker,
+					       ldev->mode_flags);
+			break;
+		}
+	}
+
+	if (tracker->tx_type == NETDEV_LAG_TX_TYPE_ACTIVEBACKUP &&
+	    !(ldev->mode == MLX5_LAG_MODE_ROCE))
+		mlx5_lag_drop_rule_setup(ldev, tracker);
+}
+
+static int mlx5_lag_set_port_sel_mode_roce(struct mlx5_lag *ldev,
+					   unsigned long *flags)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+
+	if (!MLX5_CAP_PORT_SELECTION(dev0, port_select_flow_table)) {
+		if (ldev->ports > 2)
+			return -EINVAL;
+		return 0;
+	}
+
+	if (ldev->ports > 2)
+		ldev->buckets = MLX5_LAG_MAX_HASH_BUCKETS;
+
+	set_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, flags);
+
+	return 0;
+}
+
+static void mlx5_lag_set_port_sel_mode_offloads(struct mlx5_lag *ldev,
+						struct lag_tracker *tracker,
+						enum mlx5_lag_mode mode,
+						unsigned long *flags)
+{
+	struct lag_func *dev0 = &ldev->pf[MLX5_LAG_P1];
+
+	if (mode == MLX5_LAG_MODE_MPESW)
+		return;
+
+	if (MLX5_CAP_PORT_SELECTION(dev0->dev, port_select_flow_table) &&
+	    tracker->tx_type == NETDEV_LAG_TX_TYPE_HASH) {
+		if (ldev->ports > 2)
+			ldev->buckets = MLX5_LAG_MAX_HASH_BUCKETS;
+		set_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, flags);
+	}
+}
+
+static int mlx5_lag_set_flags(struct mlx5_lag *ldev, enum mlx5_lag_mode mode,
+			      struct lag_tracker *tracker, bool shared_fdb,
+			      unsigned long *flags)
+{
+	bool roce_lag = mode == MLX5_LAG_MODE_ROCE;
+
+	*flags = 0;
+	if (shared_fdb) {
+		set_bit(MLX5_LAG_MODE_FLAG_SHARED_FDB, flags);
+		set_bit(MLX5_LAG_MODE_FLAG_FDB_SEL_MODE_NATIVE, flags);
+	}
+
+	if (mode == MLX5_LAG_MODE_MPESW)
+		set_bit(MLX5_LAG_MODE_FLAG_FDB_SEL_MODE_NATIVE, flags);
+
+	if (roce_lag)
+		return mlx5_lag_set_port_sel_mode_roce(ldev, flags);
+
+	mlx5_lag_set_port_sel_mode_offloads(ldev, tracker, mode, flags);
+	return 0;
+}
+
+char *mlx5_get_str_port_sel_mode(enum mlx5_lag_mode mode, unsigned long flags)
+{
+	int port_sel_mode = get_port_sel_mode(mode, flags);
+
+	switch (port_sel_mode) {
+	case MLX5_LAG_PORT_SELECT_MODE_QUEUE_AFFINITY: return "queue_affinity";
+	case MLX5_LAG_PORT_SELECT_MODE_PORT_SELECT_FT: return "hash";
+	case MLX5_LAG_PORT_SELECT_MODE_PORT_SELECT_MPESW: return "mpesw";
+	default: return "invalid";
+	}
+}
+
+static int mlx5_lag_create_single_fdb(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_eswitch *master_esw = dev0->priv.eswitch;
+	int err;
+	int i;
+
+	for (i = MLX5_LAG_P1 + 1; i < ldev->ports; i++) {
+		struct mlx5_eswitch *slave_esw = ldev->pf[i].dev->priv.eswitch;
+
+		err = mlx5_eswitch_offloads_single_fdb_add_one(master_esw,
+							       slave_esw, ldev->ports);
+		if (err)
+			goto err;
+	}
+	return 0;
+err:
+	for (; i > MLX5_LAG_P1; i--)
+		mlx5_eswitch_offloads_single_fdb_del_one(master_esw,
+							 ldev->pf[i].dev->priv.eswitch);
+	return err;
+}
+
+static int mlx5_create_lag(struct mlx5_lag *ldev,
+			   struct lag_tracker *tracker,
+			   enum mlx5_lag_mode mode,
+			   unsigned long flags)
+{
+	bool shared_fdb = test_bit(MLX5_LAG_MODE_FLAG_SHARED_FDB, &flags);
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	u32 in[MLX5_ST_SZ_DW(destroy_lag_in)] = {};
+	int err;
+
+	if (tracker)
+		mlx5_lag_print_mapping(dev0, ldev, tracker, flags);
+	mlx5_core_info(dev0, "shared_fdb:%d mode:%s\n",
+		       shared_fdb, mlx5_get_str_port_sel_mode(mode, flags));
+
+	err = mlx5_cmd_create_lag(dev0, ldev->v2p_map, mode, flags);
+	if (err) {
+		mlx5_core_err(dev0,
+			      "Failed to create LAG (%d)\n",
+			      err);
+		return err;
+	}
+
+	if (shared_fdb) {
+		err = mlx5_lag_create_single_fdb(ldev);
+		if (err)
+			mlx5_core_err(dev0, "Can't enable single FDB mode\n");
+		else
+			mlx5_core_info(dev0, "Operation mode is single FDB\n");
+	}
+
+	if (err) {
+		MLX5_SET(destroy_lag_in, in, opcode, MLX5_CMD_OP_DESTROY_LAG);
+		if (mlx5_cmd_exec_in(dev0, destroy_lag, in))
+			mlx5_core_err(dev0,
+				      "Failed to deactivate RoCE LAG; driver restart required\n");
+	}
+
+	return err;
+}
+
+int mlx5_activate_lag(struct mlx5_lag *ldev,
+		      struct lag_tracker *tracker,
+		      enum mlx5_lag_mode mode,
+		      bool shared_fdb)
+{
+	bool roce_lag = mode == MLX5_LAG_MODE_ROCE;
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	unsigned long flags = 0;
+	int err;
+
+	err = mlx5_lag_set_flags(ldev, mode, tracker, shared_fdb, &flags);
+	if (err)
+		return err;
+
+	if (mode != MLX5_LAG_MODE_MPESW) {
+		mlx5_infer_tx_affinity_mapping(tracker, ldev->ports, ldev->buckets, ldev->v2p_map);
+		if (test_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, &flags)) {
+			err = mlx5_lag_port_sel_create(ldev, tracker->hash_type,
+						       ldev->v2p_map);
+			if (err) {
+				mlx5_core_err(dev0,
+					      "Failed to create LAG port selection(%d)\n",
+					      err);
+				return err;
+			}
+		}
+	}
+
+	err = mlx5_create_lag(ldev, tracker, mode, flags);
+	if (err) {
+		if (test_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, &flags))
+			mlx5_lag_port_sel_destroy(ldev);
+		if (roce_lag)
+			mlx5_core_err(dev0,
+				      "Failed to activate RoCE LAG\n");
+		else
+			mlx5_core_err(dev0,
+				      "Failed to activate VF LAG\n"
+				      "Make sure all VFs are unbound prior to VF LAG activation or deactivation\n");
+		return err;
+	}
+
+	if (tracker && tracker->tx_type == NETDEV_LAG_TX_TYPE_ACTIVEBACKUP &&
+	    !roce_lag)
+		mlx5_lag_drop_rule_setup(ldev, tracker);
+
+	ldev->mode = mode;
+	ldev->mode_flags = flags;
+	return 0;
+}
+
+int mlx5_deactivate_lag(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_eswitch *master_esw = dev0->priv.eswitch;
+	u32 in[MLX5_ST_SZ_DW(destroy_lag_in)] = {};
+	bool roce_lag = __mlx5_lag_is_roce(ldev);
+	unsigned long flags = ldev->mode_flags;
+	int err;
+	int i;
+
+	ldev->mode = MLX5_LAG_MODE_NONE;
+	ldev->mode_flags = 0;
+	mlx5_lag_mp_reset(ldev);
+
+	if (test_bit(MLX5_LAG_MODE_FLAG_SHARED_FDB, &flags)) {
+		for (i = MLX5_LAG_P1 + 1; i < ldev->ports; i++)
+			mlx5_eswitch_offloads_single_fdb_del_one(master_esw,
+								 ldev->pf[i].dev->priv.eswitch);
+		clear_bit(MLX5_LAG_MODE_FLAG_SHARED_FDB, &flags);
+	}
+
+	MLX5_SET(destroy_lag_in, in, opcode, MLX5_CMD_OP_DESTROY_LAG);
+	err = mlx5_cmd_exec_in(dev0, destroy_lag, in);
+	if (err) {
+		if (roce_lag) {
+			mlx5_core_err(dev0,
+				      "Failed to deactivate RoCE LAG; driver restart required\n");
+		} else {
+			mlx5_core_err(dev0,
+				      "Failed to deactivate VF LAG; driver restart required\n"
+				      "Make sure all VFs are unbound prior to VF LAG activation or deactivation\n");
+		}
+		return err;
+	}
+
+	if (test_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, &flags))
+		mlx5_lag_port_sel_destroy(ldev);
+	if (mlx5_lag_has_drop_rule(ldev))
+		mlx5_lag_drop_rule_cleanup(ldev);
+
+	return 0;
+}
+
+#define MLX5_LAG_OFFLOADS_SUPPORTED_PORTS 4
+bool mlx5_lag_check_prereq(struct mlx5_lag *ldev)
+{
+#ifdef CONFIG_MLX5_ESWITCH
+	struct mlx5_core_dev *dev;
+	u8 mode;
+#endif
+	int i;
+
+	for (i = 0; i < ldev->ports; i++)
+		if (!ldev->pf[i].dev)
+			return false;
+
+#ifdef CONFIG_MLX5_ESWITCH
+	for (i = 0; i < ldev->ports; i++) {
+		dev = ldev->pf[i].dev;
+		if (mlx5_eswitch_num_vfs(dev->priv.eswitch) && !is_mdev_switchdev_mode(dev))
+			return false;
+	}
+
+	dev = ldev->pf[MLX5_LAG_P1].dev;
+	mode = mlx5_eswitch_mode(dev);
+	for (i = 0; i < ldev->ports; i++)
+		if (mlx5_eswitch_mode(ldev->pf[i].dev) != mode)
+			return false;
+
+	if (mode == MLX5_ESWITCH_OFFLOADS && ldev->ports > MLX5_LAG_OFFLOADS_SUPPORTED_PORTS)
+		return false;
+#else
+	for (i = 0; i < ldev->ports; i++)
+		if (mlx5_sriov_is_enabled(ldev->pf[i].dev))
+			return false;
+#endif
+	return true;
+}
+
+void mlx5_lag_add_devices(struct mlx5_lag *ldev)
+{
+	int i;
+
+	for (i = 0; i < ldev->ports; i++) {
+		if (!ldev->pf[i].dev)
+			continue;
+
+		if (ldev->pf[i].dev->priv.flags &
+		    MLX5_PRIV_FLAGS_DISABLE_ALL_ADEV)
+			continue;
+
+		ldev->pf[i].dev->priv.flags &= ~MLX5_PRIV_FLAGS_DISABLE_IB_ADEV;
+		mlx5_rescan_drivers_locked(ldev->pf[i].dev);
+	}
+}
+
+void mlx5_lag_remove_devices(struct mlx5_lag *ldev)
+{
+	int i;
+
+	for (i = 0; i < ldev->ports; i++) {
+		if (!ldev->pf[i].dev)
+			continue;
+
+		if (ldev->pf[i].dev->priv.flags &
+		    MLX5_PRIV_FLAGS_DISABLE_ALL_ADEV)
+			continue;
+
+		ldev->pf[i].dev->priv.flags |= MLX5_PRIV_FLAGS_DISABLE_IB_ADEV;
+		mlx5_rescan_drivers_locked(ldev->pf[i].dev);
+	}
+}
+
+void mlx5_disable_lag(struct mlx5_lag *ldev)
+{
+	bool shared_fdb = test_bit(MLX5_LAG_MODE_FLAG_SHARED_FDB, &ldev->mode_flags);
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	bool roce_lag;
+	int err;
+	int i;
+
+	roce_lag = __mlx5_lag_is_roce(ldev);
+
+	if (shared_fdb) {
+		mlx5_lag_remove_devices(ldev);
+	} else if (roce_lag) {
+		if (!(dev0->priv.flags & MLX5_PRIV_FLAGS_DISABLE_ALL_ADEV)) {
+			dev0->priv.flags |= MLX5_PRIV_FLAGS_DISABLE_IB_ADEV;
+			mlx5_rescan_drivers_locked(dev0);
+		}
+		for (i = 1; i < ldev->ports; i++)
+			mlx5_nic_vport_disable_roce(ldev->pf[i].dev);
+	}
+
+	err = mlx5_deactivate_lag(ldev);
+	if (err)
+		return;
+
+	if (shared_fdb || roce_lag)
+		mlx5_lag_add_devices(ldev);
+
+	if (shared_fdb)
+		for (i = 0; i < ldev->ports; i++)
+			if (!(ldev->pf[i].dev->priv.flags & MLX5_PRIV_FLAGS_DISABLE_ALL_ADEV))
+				mlx5_eswitch_reload_reps(ldev->pf[i].dev->priv.eswitch);
+}
+
+static bool mlx5_shared_fdb_supported(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev;
+	int i;
+
+	for (i = MLX5_LAG_P1 + 1; i < ldev->ports; i++) {
+		dev = ldev->pf[i].dev;
+		if (is_mdev_switchdev_mode(dev) &&
+		    mlx5_eswitch_vport_match_metadata_enabled(dev->priv.eswitch) &&
+		    MLX5_CAP_GEN(dev, lag_native_fdb_selection) &&
+		    MLX5_CAP_ESW(dev, root_ft_on_other_esw) &&
+		    mlx5_eswitch_get_npeers(dev->priv.eswitch) ==
+		    MLX5_CAP_GEN(dev, num_lag_ports) - 1)
+			continue;
+		return false;
+	}
+
+	dev = ldev->pf[MLX5_LAG_P1].dev;
+	if (is_mdev_switchdev_mode(dev) &&
+	    mlx5_eswitch_vport_match_metadata_enabled(dev->priv.eswitch) &&
+	    mlx5_esw_offloads_devcom_is_ready(dev->priv.eswitch) &&
+	    MLX5_CAP_ESW(dev, esw_shared_ingress_acl) &&
+	    mlx5_eswitch_get_npeers(dev->priv.eswitch) == MLX5_CAP_GEN(dev, num_lag_ports) - 1)
+		return true;
+
+	return false;
+}
+
+static bool mlx5_lag_is_roce_lag(struct mlx5_lag *ldev)
+{
+	bool roce_lag = true;
+	int i;
+
+	for (i = 0; i < ldev->ports; i++)
+		roce_lag = roce_lag && !mlx5_sriov_is_enabled(ldev->pf[i].dev);
+
+#ifdef CONFIG_MLX5_ESWITCH
+	for (i = 0; i < ldev->ports; i++)
+		roce_lag = roce_lag && is_mdev_legacy_mode(ldev->pf[i].dev);
+#endif
+
+	return roce_lag;
+}
+
+static bool mlx5_lag_should_modify_lag(struct mlx5_lag *ldev, bool do_bond)
+{
+	return do_bond && __mlx5_lag_is_active(ldev) &&
+	       ldev->mode != MLX5_LAG_MODE_MPESW;
+}
+
+static bool mlx5_lag_should_disable_lag(struct mlx5_lag *ldev, bool do_bond)
+{
+	return !do_bond && __mlx5_lag_is_active(ldev) &&
+	       ldev->mode != MLX5_LAG_MODE_MPESW;
+}
+
+static void mlx5_do_bond(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	struct lag_tracker tracker = { };
+	bool do_bond, roce_lag;
+	int err;
+	int i;
+
+	if (!mlx5_lag_is_ready(ldev)) {
+		do_bond = false;
+	} else {
+		/* VF LAG is in multipath mode, ignore bond change requests */
+		if (mlx5_lag_is_multipath(dev0))
+			return;
+
+		tracker = ldev->tracker;
+
+		do_bond = tracker.is_bonded && mlx5_lag_check_prereq(ldev);
+	}
+
+	if (do_bond && !__mlx5_lag_is_active(ldev)) {
+		bool shared_fdb = mlx5_shared_fdb_supported(ldev);
+
+		roce_lag = mlx5_lag_is_roce_lag(ldev);
+
+		if (shared_fdb || roce_lag)
+			mlx5_lag_remove_devices(ldev);
+
+		err = mlx5_activate_lag(ldev, &tracker,
+					roce_lag ? MLX5_LAG_MODE_ROCE :
+						   MLX5_LAG_MODE_SRIOV,
+					shared_fdb);
+		if (err) {
+			if (shared_fdb || roce_lag)
+				mlx5_lag_add_devices(ldev);
+
+			return;
+		} else if (roce_lag) {
+			dev0->priv.flags &= ~MLX5_PRIV_FLAGS_DISABLE_IB_ADEV;
+			mlx5_rescan_drivers_locked(dev0);
+			for (i = 1; i < ldev->ports; i++)
+				mlx5_nic_vport_enable_roce(ldev->pf[i].dev);
+		} else if (shared_fdb) {
+			int i;
+
+			dev0->priv.flags &= ~MLX5_PRIV_FLAGS_DISABLE_IB_ADEV;
+			mlx5_rescan_drivers_locked(dev0);
+
+			for (i = 0; i < ldev->ports; i++) {
+				err = mlx5_eswitch_reload_reps(ldev->pf[i].dev->priv.eswitch);
+				if (err)
+					break;
+			}
+
+			if (err) {
+				dev0->priv.flags |= MLX5_PRIV_FLAGS_DISABLE_IB_ADEV;
+				mlx5_rescan_drivers_locked(dev0);
+				mlx5_deactivate_lag(ldev);
+				mlx5_lag_add_devices(ldev);
+				for (i = 0; i < ldev->ports; i++)
+					mlx5_eswitch_reload_reps(ldev->pf[i].dev->priv.eswitch);
+				mlx5_core_err(dev0, "Failed to enable lag\n");
+				return;
+			}
+		}
+	} else if (mlx5_lag_should_modify_lag(ldev, do_bond)) {
+		mlx5_modify_lag(ldev, &tracker);
+	} else if (mlx5_lag_should_disable_lag(ldev, do_bond)) {
+		mlx5_disable_lag(ldev);
+	}
+}
+
+static void mlx5_queue_bond_work(struct mlx5_lag *ldev, unsigned long delay)
+{
+	queue_delayed_work(ldev->wq, &ldev->bond_work, delay);
+}
+
+static void mlx5_do_bond_work(struct work_struct *work)
+{
+	struct delayed_work *delayed_work = to_delayed_work(work);
+	struct mlx5_lag *ldev = container_of(delayed_work, struct mlx5_lag,
+					     bond_work);
+	int status;
+
+	status = mlx5_dev_list_trylock();
+	if (!status) {
+		mlx5_queue_bond_work(ldev, HZ);
+		return;
+	}
+
+	mutex_lock(&ldev->lock);
+	if (ldev->mode_changes_in_progress) {
+		mutex_unlock(&ldev->lock);
+		mlx5_dev_list_unlock();
+		mlx5_queue_bond_work(ldev, HZ);
+		return;
+	}
+
+	mlx5_do_bond(ldev);
+	mutex_unlock(&ldev->lock);
+	mlx5_dev_list_unlock();
+}
+
+static int mlx5_handle_changeupper_event(struct mlx5_lag *ldev,
+					 struct lag_tracker *tracker,
+					 struct netdev_notifier_changeupper_info *info)
+{
+	struct net_device *upper = info->upper_dev, *ndev_tmp;
+	struct netdev_lag_upper_info *lag_upper_info = NULL;
+	bool is_bonded, is_in_lag, mode_supported;
+	bool has_inactive = 0;
+	struct slave *slave;
+	u8 bond_status = 0;
+	int num_slaves = 0;
+	int changed = 0;
+	int idx;
+
+	if (!netif_is_lag_master(upper))
+		return 0;
+
+	if (info->linking)
+		lag_upper_info = info->upper_info;
+
+	/* The event may still be of interest if the slave does not belong to
+	 * us, but is enslaved to a master which has one or more of our netdevs
+	 * as slaves (e.g., if a new slave is added to a master that bonds two
+	 * of our netdevs, we should unbond).
+	 */
+	rcu_read_lock();
+	for_each_netdev_in_bond_rcu(upper, ndev_tmp) {
+		idx = mlx5_lag_dev_get_netdev_idx(ldev, ndev_tmp);
+		if (idx >= 0) {
+			slave = bond_slave_get_rcu(ndev_tmp);
+			if (slave)
+				has_inactive |= bond_is_slave_inactive(slave);
+			bond_status |= (1 << idx);
+		}
+
+		num_slaves++;
+	}
+	rcu_read_unlock();
+
+	/* None of this lagdev's netdevs are slaves of this master. */
+	if (!(bond_status & GENMASK(ldev->ports - 1, 0)))
+		return 0;
+
+	if (lag_upper_info) {
+		tracker->tx_type = lag_upper_info->tx_type;
+		tracker->hash_type = lag_upper_info->hash_type;
+	}
+
+	tracker->has_inactive = has_inactive;
+	/* Determine bonding status:
+	 * A device is considered bonded if both its physical ports are slaves
+	 * of the same lag master, and only them.
+	 */
+	is_in_lag = num_slaves == ldev->ports &&
+		bond_status == GENMASK(ldev->ports - 1, 0);
+
+	/* Lag mode must be activebackup or hash. */
+	mode_supported = tracker->tx_type == NETDEV_LAG_TX_TYPE_ACTIVEBACKUP ||
+			 tracker->tx_type == NETDEV_LAG_TX_TYPE_HASH;
+
+	is_bonded = is_in_lag && mode_supported;
+	if (tracker->is_bonded != is_bonded) {
+		tracker->is_bonded = is_bonded;
+		changed = 1;
+	}
+
+	if (!is_in_lag)
+		return changed;
+
+	if (!mlx5_lag_is_ready(ldev))
+		NL_SET_ERR_MSG_MOD(info->info.extack,
+				   "Can't activate LAG offload, PF is configured with more than 64 VFs");
+	else if (!mode_supported)
+		NL_SET_ERR_MSG_MOD(info->info.extack,
+				   "Can't activate LAG offload, TX type isn't supported");
+
+	return changed;
+}
+
+static int mlx5_handle_changelowerstate_event(struct mlx5_lag *ldev,
+					      struct lag_tracker *tracker,
+					      struct net_device *ndev,
+					      struct netdev_notifier_changelowerstate_info *info)
+{
+	struct netdev_lag_lower_state_info *lag_lower_info;
+	int idx;
+
+	if (!netif_is_lag_port(ndev))
+		return 0;
+
+	idx = mlx5_lag_dev_get_netdev_idx(ldev, ndev);
+	if (idx < 0)
+		return 0;
+
+	/* This information is used to determine virtual to physical
+	 * port mapping.
+	 */
+	lag_lower_info = info->lower_state_info;
+	if (!lag_lower_info)
+		return 0;
+
+	tracker->netdev_state[idx] = *lag_lower_info;
+
+	return 1;
+}
+
+static int mlx5_handle_changeinfodata_event(struct mlx5_lag *ldev,
+					    struct lag_tracker *tracker,
+					    struct net_device *ndev)
+{
+	struct net_device *ndev_tmp;
+	struct slave *slave;
+	bool has_inactive = 0;
+	int idx;
+
+	if (!netif_is_lag_master(ndev))
+		return 0;
+
+	rcu_read_lock();
+	for_each_netdev_in_bond_rcu(ndev, ndev_tmp) {
+		idx = mlx5_lag_dev_get_netdev_idx(ldev, ndev_tmp);
+		if (idx < 0)
+			continue;
+
+		slave = bond_slave_get_rcu(ndev_tmp);
+		if (slave)
+			has_inactive |= bond_is_slave_inactive(slave);
+	}
+	rcu_read_unlock();
+
+	if (tracker->has_inactive == has_inactive)
+		return 0;
+
+	tracker->has_inactive = has_inactive;
+
+	return 1;
+}
+
+/* this handler is always registered to netdev events */
+static int mlx5_lag_netdev_event(struct notifier_block *this,
+				 unsigned long event, void *ptr)
+{
+	struct net_device *ndev = netdev_notifier_info_to_dev(ptr);
+	struct lag_tracker tracker;
+	struct mlx5_lag *ldev;
+	int changed = 0;
+
+	if (event != NETDEV_CHANGEUPPER &&
+	    event != NETDEV_CHANGELOWERSTATE &&
+	    event != NETDEV_CHANGEINFODATA)
+		return NOTIFY_DONE;
+
+	ldev    = container_of(this, struct mlx5_lag, nb);
+
+	tracker = ldev->tracker;
+
+	switch (event) {
+	case NETDEV_CHANGEUPPER:
+		changed = mlx5_handle_changeupper_event(ldev, &tracker, ptr);
+		break;
+	case NETDEV_CHANGELOWERSTATE:
+		changed = mlx5_handle_changelowerstate_event(ldev, &tracker,
+							     ndev, ptr);
+		break;
+	case NETDEV_CHANGEINFODATA:
+		changed = mlx5_handle_changeinfodata_event(ldev, &tracker, ndev);
+		break;
+	}
+
+	ldev->tracker = tracker;
+
+	if (changed)
+		mlx5_queue_bond_work(ldev, 0);
+
+	return NOTIFY_DONE;
+}
+
+static void mlx5_ldev_add_netdev(struct mlx5_lag *ldev,
+				 struct mlx5_core_dev *dev,
+				 struct net_device *netdev)
+{
+	unsigned int fn = mlx5_get_dev_index(dev);
+	unsigned long flags;
+
+	if (fn >= ldev->ports)
+		return;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev->pf[fn].netdev = netdev;
+	ldev->tracker.netdev_state[fn].link_up = 0;
+	ldev->tracker.netdev_state[fn].tx_enabled = 0;
+	spin_unlock_irqrestore(&lag_lock, flags);
+}
+
+static void mlx5_ldev_remove_netdev(struct mlx5_lag *ldev,
+				    struct net_device *netdev)
+{
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	for (i = 0; i < ldev->ports; i++) {
+		if (ldev->pf[i].netdev == netdev) {
+			ldev->pf[i].netdev = NULL;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&lag_lock, flags);
+}
+
+static void mlx5_ldev_add_mdev(struct mlx5_lag *ldev,
+			       struct mlx5_core_dev *dev)
+{
+	unsigned int fn = mlx5_get_dev_index(dev);
+
+	if (fn >= ldev->ports)
+		return;
+
+	ldev->pf[fn].dev = dev;
+	dev->priv.lag = ldev;
+}
+
+static void mlx5_ldev_remove_mdev(struct mlx5_lag *ldev,
+				  struct mlx5_core_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < ldev->ports; i++)
+		if (ldev->pf[i].dev == dev)
+			break;
+
+	if (i == ldev->ports)
+		return;
+
+	ldev->pf[i].dev = NULL;
+	dev->priv.lag = NULL;
+}
+
+/* Must be called with intf_mutex held */
+static int __mlx5_lag_dev_add_mdev(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev = NULL;
+	struct mlx5_core_dev *tmp_dev;
+
+	tmp_dev = mlx5_get_next_phys_dev_lag(dev);
+	if (tmp_dev)
+		ldev = mlx5_lag_dev(tmp_dev);
+
+	if (!ldev) {
+		ldev = mlx5_lag_dev_alloc(dev);
+		if (!ldev) {
+			mlx5_core_err(dev, "Failed to alloc lag dev\n");
+			return 0;
+		}
+		mlx5_ldev_add_mdev(ldev, dev);
+		return 0;
+	}
+
+	mutex_lock(&ldev->lock);
+	if (ldev->mode_changes_in_progress) {
+		mutex_unlock(&ldev->lock);
+		return -EAGAIN;
+	}
+	mlx5_ldev_get(ldev);
+	mlx5_ldev_add_mdev(ldev, dev);
+	mutex_unlock(&ldev->lock);
+
+	return 0;
+}
+
+void mlx5_lag_remove_mdev(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+
+	ldev = mlx5_lag_dev(dev);
+	if (!ldev)
+		return;
+
+	/* mdev is being removed, might as well remove debugfs
+	 * as early as possible.
+	 */
+	mlx5_ldev_remove_debugfs(dev->priv.dbg.lag_debugfs);
+recheck:
+	mutex_lock(&ldev->lock);
+	if (ldev->mode_changes_in_progress) {
+		mutex_unlock(&ldev->lock);
+		msleep(100);
+		goto recheck;
+	}
+	mlx5_ldev_remove_mdev(ldev, dev);
+	mutex_unlock(&ldev->lock);
+	mlx5_ldev_put(ldev);
+}
+
+void mlx5_lag_add_mdev(struct mlx5_core_dev *dev)
+{
+	int err;
+
+	if (!mlx5_lag_is_supported(dev))
+		return;
+
+recheck:
+	mlx5_dev_list_lock();
+	err = __mlx5_lag_dev_add_mdev(dev);
+	mlx5_dev_list_unlock();
+
+	if (err) {
+		msleep(100);
+		goto recheck;
+	}
+	mlx5_ldev_add_debugfs(dev);
+}
+
+void mlx5_lag_remove_netdev(struct mlx5_core_dev *dev,
+			    struct net_device *netdev)
+{
+	struct mlx5_lag *ldev;
+	bool lag_is_active;
+
+	ldev = mlx5_lag_dev(dev);
+	if (!ldev)
+		return;
+
+	mutex_lock(&ldev->lock);
+	mlx5_ldev_remove_netdev(ldev, netdev);
+	clear_bit(MLX5_LAG_FLAG_NDEVS_READY, &ldev->state_flags);
+
+	lag_is_active = __mlx5_lag_is_active(ldev);
+	mutex_unlock(&ldev->lock);
+
+	if (lag_is_active)
+		mlx5_queue_bond_work(ldev, 0);
+}
+
+void mlx5_lag_add_netdev(struct mlx5_core_dev *dev,
+			 struct net_device *netdev)
+{
+	struct mlx5_lag *ldev;
+	int i;
+
+	ldev = mlx5_lag_dev(dev);
+	if (!ldev)
+		return;
+
+	mutex_lock(&ldev->lock);
+	mlx5_ldev_add_netdev(ldev, dev, netdev);
+
+	for (i = 0; i < ldev->ports; i++)
+		if (!ldev->pf[i].netdev)
+			break;
+
+	if (i >= ldev->ports)
+		set_bit(MLX5_LAG_FLAG_NDEVS_READY, &ldev->state_flags);
+	mutex_unlock(&ldev->lock);
+	mlx5_queue_bond_work(ldev, 0);
+}
+
+bool mlx5_lag_is_roce(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	bool res;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	res  = ldev && __mlx5_lag_is_roce(ldev);
+	spin_unlock_irqrestore(&lag_lock, flags);
+
+	return res;
+}
+EXPORT_SYMBOL(mlx5_lag_is_roce);
+
+bool mlx5_lag_is_active(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	bool res;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	res  = ldev && __mlx5_lag_is_active(ldev);
+	spin_unlock_irqrestore(&lag_lock, flags);
+
+	return res;
+}
+EXPORT_SYMBOL(mlx5_lag_is_active);
+
+bool mlx5_lag_mode_is_hash(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	bool res = 0;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	if (ldev)
+		res = test_bit(MLX5_LAG_MODE_FLAG_HASH_BASED, &ldev->mode_flags);
+	spin_unlock_irqrestore(&lag_lock, flags);
+
+	return res;
+}
+EXPORT_SYMBOL(mlx5_lag_mode_is_hash);
+
+bool mlx5_lag_is_master(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	bool res;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	res = ldev && __mlx5_lag_is_active(ldev) &&
+		dev == ldev->pf[MLX5_LAG_P1].dev;
+	spin_unlock_irqrestore(&lag_lock, flags);
+
+	return res;
+}
+EXPORT_SYMBOL(mlx5_lag_is_master);
+
+bool mlx5_lag_is_sriov(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	bool res;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	res  = ldev && __mlx5_lag_is_sriov(ldev);
+	spin_unlock_irqrestore(&lag_lock, flags);
+
+	return res;
+}
+EXPORT_SYMBOL(mlx5_lag_is_sriov);
+
+bool mlx5_lag_is_shared_fdb(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	bool res;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	res = ldev && test_bit(MLX5_LAG_MODE_FLAG_SHARED_FDB, &ldev->mode_flags);
+	spin_unlock_irqrestore(&lag_lock, flags);
+
+	return res;
+}
+EXPORT_SYMBOL(mlx5_lag_is_shared_fdb);
+
+void mlx5_lag_disable_change(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+
+	ldev = mlx5_lag_dev(dev);
+	if (!ldev)
+		return;
+
+	mlx5_dev_list_lock();
+	mutex_lock(&ldev->lock);
+
+	ldev->mode_changes_in_progress++;
+	if (__mlx5_lag_is_active(ldev))
+		mlx5_disable_lag(ldev);
+
+	mutex_unlock(&ldev->lock);
+	mlx5_dev_list_unlock();
+}
+
+void mlx5_lag_enable_change(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+
+	ldev = mlx5_lag_dev(dev);
+	if (!ldev)
+		return;
+
+	mutex_lock(&ldev->lock);
+	ldev->mode_changes_in_progress--;
+	mutex_unlock(&ldev->lock);
+	mlx5_queue_bond_work(ldev, 0);
+}
+
+struct net_device *mlx5_lag_get_roce_netdev(struct mlx5_core_dev *dev)
+{
+	struct net_device *ndev = NULL;
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+
+	if (!(ldev && __mlx5_lag_is_roce(ldev)))
+		goto unlock;
+
+	if (ldev->tracker.tx_type == NETDEV_LAG_TX_TYPE_ACTIVEBACKUP) {
+		for (i = 0; i < ldev->ports; i++)
+			if (ldev->tracker.netdev_state[i].tx_enabled)
+				ndev = ldev->pf[i].netdev;
+		if (!ndev)
+			ndev = ldev->pf[ldev->ports - 1].netdev;
+	} else {
+		ndev = ldev->pf[MLX5_LAG_P1].netdev;
+	}
+	if (ndev)
+		dev_hold(ndev);
+
+unlock:
+	spin_unlock_irqrestore(&lag_lock, flags);
+
+	return ndev;
+}
+EXPORT_SYMBOL(mlx5_lag_get_roce_netdev);
+
+u8 mlx5_lag_get_slave_port(struct mlx5_core_dev *dev,
+			   struct net_device *slave)
+{
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	u8 port = 0;
+	int i;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	if (!(ldev && __mlx5_lag_is_roce(ldev)))
+		goto unlock;
+
+	for (i = 0; i < ldev->ports; i++) {
+		if (ldev->pf[MLX5_LAG_P1].netdev == slave) {
+			port = i;
+			break;
+		}
+	}
+
+	port = ldev->v2p_map[port * ldev->buckets];
+
+unlock:
+	spin_unlock_irqrestore(&lag_lock, flags);
+	return port;
+}
+EXPORT_SYMBOL(mlx5_lag_get_slave_port);
+
+u8 mlx5_lag_get_num_ports(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev;
+
+	ldev = mlx5_lag_dev(dev);
+	if (!ldev)
+		return 0;
+
+	return ldev->ports;
+}
+EXPORT_SYMBOL(mlx5_lag_get_num_ports);
+
+struct mlx5_core_dev *mlx5_lag_get_next_peer_mdev(struct mlx5_core_dev *dev, int *i)
+{
+	struct mlx5_core_dev *peer_dev = NULL;
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	int idx;
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	if (!ldev)
+		goto unlock;
+
+	if (*i == ldev->ports)
+		goto unlock;
+	for (idx = *i; idx < ldev->ports; idx++)
+		if (ldev->pf[idx].dev != dev)
+			break;
+
+	if (idx == ldev->ports) {
+		*i = idx;
+		goto unlock;
+	}
+	*i = idx + 1;
+
+	peer_dev = ldev->pf[idx].dev;
+
+unlock:
+	spin_unlock_irqrestore(&lag_lock, flags);
+	return peer_dev;
+}
+EXPORT_SYMBOL(mlx5_lag_get_next_peer_mdev);
+
+int mlx5_lag_query_cong_counters(struct mlx5_core_dev *dev,
+				 u64 *values,
+				 int num_counters,
+				 size_t *offsets)
+{
+	int outlen = MLX5_ST_SZ_BYTES(query_cong_statistics_out);
+	struct mlx5_core_dev **mdev;
+	struct mlx5_lag *ldev;
+	unsigned long flags;
+	int num_ports;
+	int ret, i, j;
+	void *out;
+
+	out = kvzalloc(outlen, GFP_KERNEL);
+	if (!out)
+		return -ENOMEM;
+
+	mdev = kvzalloc(sizeof(mdev[0]) * MLX5_MAX_PORTS, GFP_KERNEL);
+	if (!mdev) {
+		ret = -ENOMEM;
+		goto free_out;
+	}
+
+	memset(values, 0, sizeof(*values) * num_counters);
+
+	spin_lock_irqsave(&lag_lock, flags);
+	ldev = mlx5_lag_dev(dev);
+	if (ldev && __mlx5_lag_is_active(ldev)) {
+		num_ports = ldev->ports;
+		for (i = 0; i < ldev->ports; i++)
+			mdev[i] = ldev->pf[i].dev;
+	} else {
+		num_ports = 1;
+		mdev[MLX5_LAG_P1] = dev;
+	}
+	spin_unlock_irqrestore(&lag_lock, flags);
+
+	for (i = 0; i < num_ports; ++i) {
+		u32 in[MLX5_ST_SZ_DW(query_cong_statistics_in)] = {};
+
+		MLX5_SET(query_cong_statistics_in, in, opcode,
+			 MLX5_CMD_OP_QUERY_CONG_STATISTICS);
+		ret = mlx5_cmd_exec_inout(mdev[i], query_cong_statistics, in,
+					  out);
+		if (ret)
+			goto free_mdev;
+
+		for (j = 0; j < num_counters; ++j)
+			values[j] += be64_to_cpup((__be64 *)(out + offsets[j]));
+	}
+
+free_mdev:
+	kvfree(mdev);
+free_out:
+	kvfree(out);
+	return ret;
+}
+EXPORT_SYMBOL(mlx5_lag_query_cong_counters);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/lag.h b/drivers/net/ethernet/mellanox/mlx5/core/lag/lag.h
new file mode 100644
index 0000000000..481e92f39f
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/lag.h
@@ -0,0 +1,126 @@
+/* SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB */
+/* Copyright (c) 2019 Mellanox Technologies. */
+
+#ifndef __MLX5_LAG_H__
+#define __MLX5_LAG_H__
+
+#include <linux/debugfs.h>
+
+#define MLX5_LAG_MAX_HASH_BUCKETS 16
+#include "mlx5_core.h"
+#include "mp.h"
+#include "port_sel.h"
+#include "mpesw.h"
+
+enum {
+	MLX5_LAG_P1,
+	MLX5_LAG_P2,
+};
+
+enum {
+	MLX5_LAG_FLAG_NDEVS_READY,
+};
+
+enum {
+	MLX5_LAG_MODE_FLAG_HASH_BASED,
+	MLX5_LAG_MODE_FLAG_SHARED_FDB,
+	MLX5_LAG_MODE_FLAG_FDB_SEL_MODE_NATIVE,
+};
+
+enum mlx5_lag_mode {
+	MLX5_LAG_MODE_NONE,
+	MLX5_LAG_MODE_ROCE,
+	MLX5_LAG_MODE_SRIOV,
+	MLX5_LAG_MODE_MULTIPATH,
+	MLX5_LAG_MODE_MPESW,
+};
+
+struct lag_func {
+	struct mlx5_core_dev *dev;
+	struct net_device    *netdev;
+	bool has_drop;
+};
+
+/* Used for collection of netdev event info. */
+struct lag_tracker {
+	enum   netdev_lag_tx_type           tx_type;
+	struct netdev_lag_lower_state_info  netdev_state[MLX5_MAX_PORTS];
+	unsigned int is_bonded:1;
+	unsigned int has_inactive:1;
+	enum netdev_lag_hash hash_type;
+};
+
+/* LAG data of a ConnectX card.
+ * It serves both its phys functions.
+ */
+struct mlx5_lag {
+	enum mlx5_lag_mode        mode;
+	unsigned long		  mode_flags;
+	unsigned long		  state_flags;
+	u8			  ports;
+	u8			  buckets;
+	int			  mode_changes_in_progress;
+	u8			  v2p_map[MLX5_MAX_PORTS * MLX5_LAG_MAX_HASH_BUCKETS];
+	struct kref               ref;
+	struct lag_func           pf[MLX5_MAX_PORTS];
+	struct lag_tracker        tracker;
+	struct workqueue_struct   *wq;
+	struct delayed_work       bond_work;
+	struct notifier_block     nb;
+	struct lag_mp             lag_mp;
+	struct mlx5_lag_port_sel  port_sel;
+	/* Protect lag fields/state changes */
+	struct mutex		  lock;
+	struct lag_mpesw	  lag_mpesw;
+};
+
+static inline struct mlx5_lag *
+mlx5_lag_dev(struct mlx5_core_dev *dev)
+{
+	return dev->priv.lag;
+}
+
+static inline bool
+__mlx5_lag_is_active(struct mlx5_lag *ldev)
+{
+	return ldev->mode != MLX5_LAG_MODE_NONE;
+}
+
+static inline bool
+mlx5_lag_is_ready(struct mlx5_lag *ldev)
+{
+	return test_bit(MLX5_LAG_FLAG_NDEVS_READY, &ldev->state_flags);
+}
+
+bool mlx5_lag_check_prereq(struct mlx5_lag *ldev);
+void mlx5_modify_lag(struct mlx5_lag *ldev,
+		     struct lag_tracker *tracker);
+int mlx5_activate_lag(struct mlx5_lag *ldev,
+		      struct lag_tracker *tracker,
+		      enum mlx5_lag_mode mode,
+		      bool shared_fdb);
+int mlx5_lag_dev_get_netdev_idx(struct mlx5_lag *ldev,
+				struct net_device *ndev);
+
+char *mlx5_get_str_port_sel_mode(enum mlx5_lag_mode mode, unsigned long flags);
+void mlx5_infer_tx_enabled(struct lag_tracker *tracker, u8 num_ports,
+			   u8 *ports, int *num_enabled);
+
+void mlx5_ldev_add_debugfs(struct mlx5_core_dev *dev);
+void mlx5_ldev_remove_debugfs(struct dentry *dbg);
+void mlx5_disable_lag(struct mlx5_lag *ldev);
+void mlx5_lag_remove_devices(struct mlx5_lag *ldev);
+int mlx5_deactivate_lag(struct mlx5_lag *ldev);
+void mlx5_lag_add_devices(struct mlx5_lag *ldev);
+
+static inline bool mlx5_lag_is_supported(struct mlx5_core_dev *dev)
+{
+	if (!MLX5_CAP_GEN(dev, vport_group_manager) ||
+	    !MLX5_CAP_GEN(dev, lag_master) ||
+	    MLX5_CAP_GEN(dev, num_lag_ports) < 2 ||
+	    MLX5_CAP_GEN(dev, num_lag_ports) > MLX5_MAX_PORTS)
+		return false;
+	return true;
+}
+
+#endif /* __MLX5_LAG_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/mp.c b/drivers/net/ethernet/mellanox/mlx5/core/lag/mp.c
new file mode 100644
index 0000000000..b1aa494c76
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/mp.c
@@ -0,0 +1,385 @@
+// SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
+/* Copyright (c) 2019 Mellanox Technologies. */
+
+#include <linux/netdevice.h>
+#include <net/nexthop.h>
+#include "lag/lag.h"
+#include "lag/mp.h"
+#include "mlx5_core.h"
+#include "eswitch.h"
+#include "lib/events.h"
+
+static bool __mlx5_lag_is_multipath(struct mlx5_lag *ldev)
+{
+	return ldev->mode == MLX5_LAG_MODE_MULTIPATH;
+}
+
+#define MLX5_LAG_MULTIPATH_OFFLOADS_SUPPORTED_PORTS 2
+static bool mlx5_lag_multipath_check_prereq(struct mlx5_lag *ldev)
+{
+	if (!mlx5_lag_is_ready(ldev))
+		return false;
+
+	if (__mlx5_lag_is_active(ldev) && !__mlx5_lag_is_multipath(ldev))
+		return false;
+
+	if (ldev->ports > MLX5_LAG_MULTIPATH_OFFLOADS_SUPPORTED_PORTS)
+		return false;
+
+	return mlx5_esw_multipath_prereq(ldev->pf[MLX5_LAG_P1].dev,
+					 ldev->pf[MLX5_LAG_P2].dev);
+}
+
+bool mlx5_lag_is_multipath(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev = mlx5_lag_dev(dev);
+
+	return ldev && __mlx5_lag_is_multipath(ldev);
+}
+
+/**
+ * mlx5_lag_set_port_affinity
+ *
+ * @ldev: lag device
+ * @port:
+ *     0 - set normal affinity.
+ *     1 - set affinity to port 1.
+ *     2 - set affinity to port 2.
+ *
+ **/
+static void mlx5_lag_set_port_affinity(struct mlx5_lag *ldev,
+				       enum mlx5_lag_port_affinity port)
+{
+	struct lag_tracker tracker = {};
+
+	if (!__mlx5_lag_is_multipath(ldev))
+		return;
+
+	switch (port) {
+	case MLX5_LAG_NORMAL_AFFINITY:
+		tracker.netdev_state[MLX5_LAG_P1].tx_enabled = true;
+		tracker.netdev_state[MLX5_LAG_P2].tx_enabled = true;
+		tracker.netdev_state[MLX5_LAG_P1].link_up = true;
+		tracker.netdev_state[MLX5_LAG_P2].link_up = true;
+		break;
+	case MLX5_LAG_P1_AFFINITY:
+		tracker.netdev_state[MLX5_LAG_P1].tx_enabled = true;
+		tracker.netdev_state[MLX5_LAG_P1].link_up = true;
+		tracker.netdev_state[MLX5_LAG_P2].tx_enabled = false;
+		tracker.netdev_state[MLX5_LAG_P2].link_up = false;
+		break;
+	case MLX5_LAG_P2_AFFINITY:
+		tracker.netdev_state[MLX5_LAG_P1].tx_enabled = false;
+		tracker.netdev_state[MLX5_LAG_P1].link_up = false;
+		tracker.netdev_state[MLX5_LAG_P2].tx_enabled = true;
+		tracker.netdev_state[MLX5_LAG_P2].link_up = true;
+		break;
+	default:
+		mlx5_core_warn(ldev->pf[MLX5_LAG_P1].dev,
+			       "Invalid affinity port %d", port);
+		return;
+	}
+
+	if (tracker.netdev_state[MLX5_LAG_P1].tx_enabled)
+		mlx5_notifier_call_chain(ldev->pf[MLX5_LAG_P1].dev->priv.events,
+					 MLX5_DEV_EVENT_PORT_AFFINITY,
+					 (void *)0);
+
+	if (tracker.netdev_state[MLX5_LAG_P2].tx_enabled)
+		mlx5_notifier_call_chain(ldev->pf[MLX5_LAG_P2].dev->priv.events,
+					 MLX5_DEV_EVENT_PORT_AFFINITY,
+					 (void *)0);
+
+	mlx5_modify_lag(ldev, &tracker);
+}
+
+static void mlx5_lag_fib_event_flush(struct notifier_block *nb)
+{
+	struct lag_mp *mp = container_of(nb, struct lag_mp, fib_nb);
+
+	flush_workqueue(mp->wq);
+}
+
+static void mlx5_lag_fib_set(struct lag_mp *mp, struct fib_info *fi, u32 dst, int dst_len)
+{
+	mp->fib.mfi = fi;
+	mp->fib.priority = fi->fib_priority;
+	mp->fib.dst = dst;
+	mp->fib.dst_len = dst_len;
+}
+
+struct mlx5_fib_event_work {
+	struct work_struct work;
+	struct mlx5_lag *ldev;
+	unsigned long event;
+	union {
+		struct fib_entry_notifier_info fen_info;
+		struct fib_nh_notifier_info fnh_info;
+	};
+};
+
+static struct net_device*
+mlx5_lag_get_next_fib_dev(struct mlx5_lag *ldev,
+			  struct fib_info *fi,
+			  struct net_device *current_dev)
+{
+	struct net_device *fib_dev;
+	int i, ldev_idx, nhs;
+
+	nhs = fib_info_num_path(fi);
+	i = 0;
+	if (current_dev) {
+		for (; i < nhs; i++) {
+			fib_dev = fib_info_nh(fi, i)->fib_nh_dev;
+			if (fib_dev == current_dev) {
+				i++;
+				break;
+			}
+		}
+	}
+	for (; i < nhs; i++) {
+		fib_dev = fib_info_nh(fi, i)->fib_nh_dev;
+		ldev_idx = mlx5_lag_dev_get_netdev_idx(ldev, fib_dev);
+		if (ldev_idx >= 0)
+			return ldev->pf[ldev_idx].netdev;
+	}
+
+	return NULL;
+}
+
+static void mlx5_lag_fib_route_event(struct mlx5_lag *ldev, unsigned long event,
+				     struct fib_entry_notifier_info *fen_info)
+{
+	struct net_device *nh_dev0, *nh_dev1;
+	struct fib_info *fi = fen_info->fi;
+	struct lag_mp *mp = &ldev->lag_mp;
+
+	/* Handle delete event */
+	if (event == FIB_EVENT_ENTRY_DEL) {
+		/* stop track */
+		if (mp->fib.mfi == fi)
+			mp->fib.mfi = NULL;
+		return;
+	}
+
+	/* Handle multipath entry with lower priority value */
+	if (mp->fib.mfi && mp->fib.mfi != fi &&
+	    (mp->fib.dst != fen_info->dst || mp->fib.dst_len != fen_info->dst_len) &&
+	    fi->fib_priority >= mp->fib.priority)
+		return;
+
+	nh_dev0 = mlx5_lag_get_next_fib_dev(ldev, fi, NULL);
+	nh_dev1 = mlx5_lag_get_next_fib_dev(ldev, fi, nh_dev0);
+
+	/* Handle add/replace event */
+	if (!nh_dev0) {
+		if (mp->fib.dst == fen_info->dst && mp->fib.dst_len == fen_info->dst_len)
+			mp->fib.mfi = NULL;
+		return;
+	}
+
+	if (nh_dev0 == nh_dev1) {
+		mlx5_core_warn(ldev->pf[MLX5_LAG_P1].dev,
+			       "Multipath offload doesn't support routes with multiple nexthops of the same device");
+		return;
+	}
+
+	if (!nh_dev1) {
+		if (__mlx5_lag_is_active(ldev)) {
+			int i = mlx5_lag_dev_get_netdev_idx(ldev, nh_dev0);
+
+			i++;
+			mlx5_lag_set_port_affinity(ldev, i);
+			mlx5_lag_fib_set(mp, fi, fen_info->dst, fen_info->dst_len);
+		}
+
+		return;
+	}
+
+	/* First time we see multipath route */
+	if (!mp->fib.mfi && !__mlx5_lag_is_active(ldev)) {
+		struct lag_tracker tracker;
+
+		tracker = ldev->tracker;
+		mlx5_activate_lag(ldev, &tracker, MLX5_LAG_MODE_MULTIPATH, false);
+	}
+
+	mlx5_lag_set_port_affinity(ldev, MLX5_LAG_NORMAL_AFFINITY);
+	mlx5_lag_fib_set(mp, fi, fen_info->dst, fen_info->dst_len);
+}
+
+static void mlx5_lag_fib_nexthop_event(struct mlx5_lag *ldev,
+				       unsigned long event,
+				       struct fib_nh *fib_nh,
+				       struct fib_info *fi)
+{
+	struct lag_mp *mp = &ldev->lag_mp;
+
+	/* Check the nh event is related to the route */
+	if (!mp->fib.mfi || mp->fib.mfi != fi)
+		return;
+
+	/* nh added/removed */
+	if (event == FIB_EVENT_NH_DEL) {
+		int i = mlx5_lag_dev_get_netdev_idx(ldev, fib_nh->fib_nh_dev);
+
+		if (i >= 0) {
+			i = (i + 1) % 2 + 1; /* peer port */
+			mlx5_lag_set_port_affinity(ldev, i);
+		}
+	} else if (event == FIB_EVENT_NH_ADD &&
+		   fib_info_num_path(fi) == 2) {
+		mlx5_lag_set_port_affinity(ldev, MLX5_LAG_NORMAL_AFFINITY);
+	}
+}
+
+static void mlx5_lag_fib_update(struct work_struct *work)
+{
+	struct mlx5_fib_event_work *fib_work =
+		container_of(work, struct mlx5_fib_event_work, work);
+	struct mlx5_lag *ldev = fib_work->ldev;
+	struct fib_nh *fib_nh;
+
+	/* Protect internal structures from changes */
+	rtnl_lock();
+	switch (fib_work->event) {
+	case FIB_EVENT_ENTRY_REPLACE:
+	case FIB_EVENT_ENTRY_DEL:
+		mlx5_lag_fib_route_event(ldev, fib_work->event,
+					 &fib_work->fen_info);
+		fib_info_put(fib_work->fen_info.fi);
+		break;
+	case FIB_EVENT_NH_ADD:
+	case FIB_EVENT_NH_DEL:
+		fib_nh = fib_work->fnh_info.fib_nh;
+		mlx5_lag_fib_nexthop_event(ldev,
+					   fib_work->event,
+					   fib_work->fnh_info.fib_nh,
+					   fib_nh->nh_parent);
+		fib_info_put(fib_work->fnh_info.fib_nh->nh_parent);
+		break;
+	}
+
+	rtnl_unlock();
+	kfree(fib_work);
+}
+
+static struct mlx5_fib_event_work *
+mlx5_lag_init_fib_work(struct mlx5_lag *ldev, unsigned long event)
+{
+	struct mlx5_fib_event_work *fib_work;
+
+	fib_work = kzalloc(sizeof(*fib_work), GFP_ATOMIC);
+	if (WARN_ON(!fib_work))
+		return NULL;
+
+	INIT_WORK(&fib_work->work, mlx5_lag_fib_update);
+	fib_work->ldev = ldev;
+	fib_work->event = event;
+
+	return fib_work;
+}
+
+static int mlx5_lag_fib_event(struct notifier_block *nb,
+			      unsigned long event,
+			      void *ptr)
+{
+	struct lag_mp *mp = container_of(nb, struct lag_mp, fib_nb);
+	struct mlx5_lag *ldev = container_of(mp, struct mlx5_lag, lag_mp);
+	struct fib_notifier_info *info = ptr;
+	struct mlx5_fib_event_work *fib_work;
+	struct fib_entry_notifier_info *fen_info;
+	struct fib_nh_notifier_info *fnh_info;
+	struct fib_info *fi;
+
+	if (info->family != AF_INET)
+		return NOTIFY_DONE;
+
+	if (!mlx5_lag_multipath_check_prereq(ldev))
+		return NOTIFY_DONE;
+
+	switch (event) {
+	case FIB_EVENT_ENTRY_REPLACE:
+	case FIB_EVENT_ENTRY_DEL:
+		fen_info = container_of(info, struct fib_entry_notifier_info,
+					info);
+		fi = fen_info->fi;
+		if (fi->nh)
+			return NOTIFY_DONE;
+
+		fib_work = mlx5_lag_init_fib_work(ldev, event);
+		if (!fib_work)
+			return NOTIFY_DONE;
+		fib_work->fen_info = *fen_info;
+		/* Take reference on fib_info to prevent it from being
+		 * freed while work is queued. Release it afterwards.
+		 */
+		fib_info_hold(fib_work->fen_info.fi);
+		break;
+	case FIB_EVENT_NH_ADD:
+	case FIB_EVENT_NH_DEL:
+		fnh_info = container_of(info, struct fib_nh_notifier_info,
+					info);
+		fib_work = mlx5_lag_init_fib_work(ldev, event);
+		if (!fib_work)
+			return NOTIFY_DONE;
+		fib_work->fnh_info = *fnh_info;
+		fib_info_hold(fib_work->fnh_info.fib_nh->nh_parent);
+		break;
+	default:
+		return NOTIFY_DONE;
+	}
+
+	queue_work(mp->wq, &fib_work->work);
+
+	return NOTIFY_DONE;
+}
+
+void mlx5_lag_mp_reset(struct mlx5_lag *ldev)
+{
+	/* Clear mfi, as it might become stale when a route delete event
+	 * has been missed, see mlx5_lag_fib_route_event().
+	 */
+	ldev->lag_mp.fib.mfi = NULL;
+}
+
+int mlx5_lag_mp_init(struct mlx5_lag *ldev)
+{
+	struct lag_mp *mp = &ldev->lag_mp;
+	int err;
+
+	/* always clear mfi, as it might become stale when a route delete event
+	 * has been missed
+	 */
+	mp->fib.mfi = NULL;
+
+	if (mp->fib_nb.notifier_call)
+		return 0;
+
+	mp->wq = create_singlethread_workqueue("mlx5_lag_mp");
+	if (!mp->wq)
+		return -ENOMEM;
+
+	mp->fib_nb.notifier_call = mlx5_lag_fib_event;
+	err = register_fib_notifier(&init_net, &mp->fib_nb,
+				    mlx5_lag_fib_event_flush, NULL);
+	if (err) {
+		destroy_workqueue(mp->wq);
+		mp->fib_nb.notifier_call = NULL;
+	}
+
+	return err;
+}
+
+void mlx5_lag_mp_cleanup(struct mlx5_lag *ldev)
+{
+	struct lag_mp *mp = &ldev->lag_mp;
+
+	if (!mp->fib_nb.notifier_call)
+		return;
+
+	unregister_fib_notifier(&init_net, &mp->fib_nb);
+	destroy_workqueue(mp->wq);
+	mp->fib_nb.notifier_call = NULL;
+	mp->fib.mfi = NULL;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/mp.h b/drivers/net/ethernet/mellanox/mlx5/core/lag/mp.h
new file mode 100644
index 0000000000..056a066da6
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/mp.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB */
+/* Copyright (c) 2019 Mellanox Technologies. */
+
+#ifndef __MLX5_LAG_MP_H__
+#define __MLX5_LAG_MP_H__
+
+#include "lag.h"
+#include "mlx5_core.h"
+
+enum mlx5_lag_port_affinity {
+	MLX5_LAG_NORMAL_AFFINITY,
+	MLX5_LAG_P1_AFFINITY,
+	MLX5_LAG_P2_AFFINITY,
+};
+
+struct lag_mp {
+	struct notifier_block     fib_nb;
+	struct {
+		const void        *mfi; /* used in tracking fib events */
+		u32               priority;
+		u32               dst;
+		int               dst_len;
+	} fib;
+	struct workqueue_struct   *wq;
+};
+
+#ifdef CONFIG_MLX5_ESWITCH
+
+void mlx5_lag_mp_reset(struct mlx5_lag *ldev);
+int mlx5_lag_mp_init(struct mlx5_lag *ldev);
+void mlx5_lag_mp_cleanup(struct mlx5_lag *ldev);
+bool mlx5_lag_is_multipath(struct mlx5_core_dev *dev);
+
+#else /* CONFIG_MLX5_ESWITCH */
+
+static inline void mlx5_lag_mp_reset(struct mlx5_lag *ldev) {};
+static inline int mlx5_lag_mp_init(struct mlx5_lag *ldev) { return 0; }
+static inline void mlx5_lag_mp_cleanup(struct mlx5_lag *ldev) {}
+static inline bool mlx5_lag_is_multipath(struct mlx5_core_dev *dev) { return false; }
+
+#endif /* CONFIG_MLX5_ESWITCH */
+#endif /* __MLX5_LAG_MP_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/mpesw.c b/drivers/net/ethernet/mellanox/mlx5/core/lag/mpesw.c
new file mode 100644
index 0000000000..4bf1539152
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/mpesw.c
@@ -0,0 +1,214 @@
+// SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
+/* Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES. All rights reserved. */
+
+#include <linux/netdevice.h>
+#include <net/nexthop.h>
+#include "lag/lag.h"
+#include "eswitch.h"
+#include "esw/acl/ofld.h"
+#include "lib/events.h"
+
+static void mlx5_mpesw_metadata_cleanup(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev;
+	struct mlx5_eswitch *esw;
+	u32 pf_metadata;
+	int i;
+
+	for (i = 0; i < ldev->ports; i++) {
+		dev = ldev->pf[i].dev;
+		esw = dev->priv.eswitch;
+		pf_metadata = ldev->lag_mpesw.pf_metadata[i];
+		if (!pf_metadata)
+			continue;
+		mlx5_esw_acl_ingress_vport_metadata_update(esw, MLX5_VPORT_UPLINK, 0);
+		mlx5_notifier_call_chain(dev->priv.events, MLX5_DEV_EVENT_MULTIPORT_ESW,
+					 (void *)0);
+		mlx5_esw_match_metadata_free(esw, pf_metadata);
+		ldev->lag_mpesw.pf_metadata[i] = 0;
+	}
+}
+
+static int mlx5_mpesw_metadata_set(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev;
+	struct mlx5_eswitch *esw;
+	u32 pf_metadata;
+	int i, err;
+
+	for (i = 0; i < ldev->ports; i++) {
+		dev = ldev->pf[i].dev;
+		esw = dev->priv.eswitch;
+		pf_metadata = mlx5_esw_match_metadata_alloc(esw);
+		if (!pf_metadata) {
+			err = -ENOSPC;
+			goto err_metadata;
+		}
+
+		ldev->lag_mpesw.pf_metadata[i] = pf_metadata;
+		err = mlx5_esw_acl_ingress_vport_metadata_update(esw, MLX5_VPORT_UPLINK,
+								 pf_metadata);
+		if (err)
+			goto err_metadata;
+	}
+
+	for (i = 0; i < ldev->ports; i++) {
+		dev = ldev->pf[i].dev;
+		mlx5_notifier_call_chain(dev->priv.events, MLX5_DEV_EVENT_MULTIPORT_ESW,
+					 (void *)0);
+	}
+
+	return 0;
+
+err_metadata:
+	mlx5_mpesw_metadata_cleanup(ldev);
+	return err;
+}
+
+#define MLX5_LAG_MPESW_OFFLOADS_SUPPORTED_PORTS 2
+static int enable_mpesw(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev0 = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_core_dev *dev1 = ldev->pf[MLX5_LAG_P2].dev;
+	int err;
+
+	if (ldev->mode != MLX5_LAG_MODE_NONE)
+		return -EINVAL;
+
+	if (ldev->ports > MLX5_LAG_MPESW_OFFLOADS_SUPPORTED_PORTS)
+		return -EOPNOTSUPP;
+
+	if (mlx5_eswitch_mode(dev0) != MLX5_ESWITCH_OFFLOADS ||
+	    !MLX5_CAP_PORT_SELECTION(dev0, port_select_flow_table) ||
+	    !MLX5_CAP_GEN(dev0, create_lag_when_not_master_up) ||
+	    !mlx5_lag_check_prereq(ldev))
+		return -EOPNOTSUPP;
+
+	err = mlx5_mpesw_metadata_set(ldev);
+	if (err)
+		return err;
+
+	mlx5_lag_remove_devices(ldev);
+
+	err = mlx5_activate_lag(ldev, NULL, MLX5_LAG_MODE_MPESW, true);
+	if (err) {
+		mlx5_core_warn(dev0, "Failed to create LAG in MPESW mode (%d)\n", err);
+		goto err_add_devices;
+	}
+
+	dev0->priv.flags &= ~MLX5_PRIV_FLAGS_DISABLE_IB_ADEV;
+	mlx5_rescan_drivers_locked(dev0);
+	err = mlx5_eswitch_reload_reps(dev0->priv.eswitch);
+	if (!err)
+		err = mlx5_eswitch_reload_reps(dev1->priv.eswitch);
+	if (err)
+		goto err_rescan_drivers;
+
+	return 0;
+
+err_rescan_drivers:
+	dev0->priv.flags |= MLX5_PRIV_FLAGS_DISABLE_IB_ADEV;
+	mlx5_rescan_drivers_locked(dev0);
+	mlx5_deactivate_lag(ldev);
+err_add_devices:
+	mlx5_lag_add_devices(ldev);
+	mlx5_eswitch_reload_reps(dev0->priv.eswitch);
+	mlx5_eswitch_reload_reps(dev1->priv.eswitch);
+	mlx5_mpesw_metadata_cleanup(ldev);
+	return err;
+}
+
+static void disable_mpesw(struct mlx5_lag *ldev)
+{
+	if (ldev->mode == MLX5_LAG_MODE_MPESW) {
+		mlx5_mpesw_metadata_cleanup(ldev);
+		mlx5_disable_lag(ldev);
+	}
+}
+
+static void mlx5_mpesw_work(struct work_struct *work)
+{
+	struct mlx5_mpesw_work_st *mpesww = container_of(work, struct mlx5_mpesw_work_st, work);
+	struct mlx5_lag *ldev = mpesww->lag;
+
+	mlx5_dev_list_lock();
+	mutex_lock(&ldev->lock);
+	if (ldev->mode_changes_in_progress) {
+		mpesww->result = -EAGAIN;
+		goto unlock;
+	}
+
+	if (mpesww->op == MLX5_MPESW_OP_ENABLE)
+		mpesww->result = enable_mpesw(ldev);
+	else if (mpesww->op == MLX5_MPESW_OP_DISABLE)
+		disable_mpesw(ldev);
+unlock:
+	mutex_unlock(&ldev->lock);
+	mlx5_dev_list_unlock();
+	complete(&mpesww->comp);
+}
+
+static int mlx5_lag_mpesw_queue_work(struct mlx5_core_dev *dev,
+				     enum mpesw_op op)
+{
+	struct mlx5_lag *ldev = mlx5_lag_dev(dev);
+	struct mlx5_mpesw_work_st *work;
+	int err = 0;
+
+	if (!ldev)
+		return 0;
+
+	work = kzalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return -ENOMEM;
+
+	INIT_WORK(&work->work, mlx5_mpesw_work);
+	init_completion(&work->comp);
+	work->op = op;
+	work->lag = ldev;
+
+	if (!queue_work(ldev->wq, &work->work)) {
+		mlx5_core_warn(dev, "failed to queue mpesw work\n");
+		err = -EINVAL;
+		goto out;
+	}
+	wait_for_completion(&work->comp);
+	err = work->result;
+out:
+	kfree(work);
+	return err;
+}
+
+void mlx5_lag_mpesw_disable(struct mlx5_core_dev *dev)
+{
+	mlx5_lag_mpesw_queue_work(dev, MLX5_MPESW_OP_DISABLE);
+}
+
+int mlx5_lag_mpesw_enable(struct mlx5_core_dev *dev)
+{
+	return mlx5_lag_mpesw_queue_work(dev, MLX5_MPESW_OP_ENABLE);
+}
+
+int mlx5_lag_mpesw_do_mirred(struct mlx5_core_dev *mdev,
+			     struct net_device *out_dev,
+			     struct netlink_ext_ack *extack)
+{
+	struct mlx5_lag *ldev = mlx5_lag_dev(mdev);
+
+	if (!netif_is_bond_master(out_dev) || !ldev)
+		return 0;
+
+	if (ldev->mode != MLX5_LAG_MODE_MPESW)
+		return 0;
+
+	NL_SET_ERR_MSG_MOD(extack, "can't forward to bond in mpesw mode");
+	return -EOPNOTSUPP;
+}
+
+bool mlx5_lag_is_mpesw(struct mlx5_core_dev *dev)
+{
+	struct mlx5_lag *ldev = mlx5_lag_dev(dev);
+
+	return ldev && ldev->mode == MLX5_LAG_MODE_MPESW;
+}
+EXPORT_SYMBOL(mlx5_lag_is_mpesw);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/mpesw.h b/drivers/net/ethernet/mellanox/mlx5/core/lag/mpesw.h
new file mode 100644
index 0000000000..02520f27a0
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/mpesw.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB */
+/* Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES. All rights reserved. */
+
+#ifndef __MLX5_LAG_MPESW_H__
+#define __MLX5_LAG_MPESW_H__
+
+#include "lag.h"
+#include "mlx5_core.h"
+
+struct lag_mpesw {
+	struct work_struct mpesw_work;
+	u32 pf_metadata[MLX5_MAX_PORTS];
+};
+
+enum mpesw_op {
+	MLX5_MPESW_OP_ENABLE,
+	MLX5_MPESW_OP_DISABLE,
+};
+
+struct mlx5_mpesw_work_st {
+	struct work_struct work;
+	struct mlx5_lag    *lag;
+	enum mpesw_op      op;
+	struct completion  comp;
+	int result;
+};
+
+int mlx5_lag_mpesw_do_mirred(struct mlx5_core_dev *mdev,
+			     struct net_device *out_dev,
+			     struct netlink_ext_ack *extack);
+bool mlx5_lag_is_mpesw(struct mlx5_core_dev *dev);
+void mlx5_lag_mpesw_disable(struct mlx5_core_dev *dev);
+int mlx5_lag_mpesw_enable(struct mlx5_core_dev *dev);
+
+#endif /* __MLX5_LAG_MPESW_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/port_sel.c b/drivers/net/ethernet/mellanox/mlx5/core/lag/port_sel.c
new file mode 100644
index 0000000000..7d9bbb494d
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/port_sel.c
@@ -0,0 +1,637 @@
+// SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
+/* Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. */
+
+#include <linux/netdevice.h>
+#include "lag.h"
+
+enum {
+	MLX5_LAG_FT_LEVEL_TTC,
+	MLX5_LAG_FT_LEVEL_INNER_TTC,
+	MLX5_LAG_FT_LEVEL_DEFINER,
+};
+
+static struct mlx5_flow_group *
+mlx5_create_hash_flow_group(struct mlx5_flow_table *ft,
+			    struct mlx5_flow_definer *definer,
+			    u8 rules)
+{
+	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
+	struct mlx5_flow_group *fg;
+	u32 *in;
+
+	in = kvzalloc(inlen, GFP_KERNEL);
+	if (!in)
+		return ERR_PTR(-ENOMEM);
+
+	MLX5_SET(create_flow_group_in, in, match_definer_id,
+		 mlx5_get_match_definer_id(definer));
+	MLX5_SET(create_flow_group_in, in, start_flow_index, 0);
+	MLX5_SET(create_flow_group_in, in, end_flow_index, rules - 1);
+	MLX5_SET(create_flow_group_in, in, group_type,
+		 MLX5_CREATE_FLOW_GROUP_IN_GROUP_TYPE_HASH_SPLIT);
+
+	fg = mlx5_create_flow_group(ft, in);
+	kvfree(in);
+	return fg;
+}
+
+static int mlx5_lag_create_port_sel_table(struct mlx5_lag *ldev,
+					  struct mlx5_lag_definer *lag_definer,
+					  u8 *ports)
+{
+	struct mlx5_core_dev *dev = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_flow_table_attr ft_attr = {};
+	struct mlx5_flow_destination dest = {};
+	MLX5_DECLARE_FLOW_ACT(flow_act);
+	struct mlx5_flow_namespace *ns;
+	int err, i;
+	int idx;
+	int j;
+
+	ft_attr.max_fte = ldev->ports * ldev->buckets;
+	ft_attr.level = MLX5_LAG_FT_LEVEL_DEFINER;
+
+	ns = mlx5_get_flow_namespace(dev, MLX5_FLOW_NAMESPACE_PORT_SEL);
+	if (!ns) {
+		mlx5_core_warn(dev, "Failed to get port selection namespace\n");
+		return -EOPNOTSUPP;
+	}
+
+	lag_definer->ft = mlx5_create_flow_table(ns, &ft_attr);
+	if (IS_ERR(lag_definer->ft)) {
+		mlx5_core_warn(dev, "Failed to create port selection table\n");
+		return PTR_ERR(lag_definer->ft);
+	}
+
+	lag_definer->fg = mlx5_create_hash_flow_group(lag_definer->ft,
+						      lag_definer->definer,
+						      ft_attr.max_fte);
+	if (IS_ERR(lag_definer->fg)) {
+		err = PTR_ERR(lag_definer->fg);
+		goto destroy_ft;
+	}
+
+	dest.type = MLX5_FLOW_DESTINATION_TYPE_UPLINK;
+	dest.vport.flags |= MLX5_FLOW_DEST_VPORT_VHCA_ID;
+	flow_act.flags |= FLOW_ACT_NO_APPEND;
+	for (i = 0; i < ldev->ports; i++) {
+		for (j = 0; j < ldev->buckets; j++) {
+			u8 affinity;
+
+			idx = i * ldev->buckets + j;
+			affinity = ports[idx];
+
+			dest.vport.vhca_id = MLX5_CAP_GEN(ldev->pf[affinity - 1].dev,
+							  vhca_id);
+			lag_definer->rules[idx] = mlx5_add_flow_rules(lag_definer->ft,
+								      NULL, &flow_act,
+								      &dest, 1);
+			if (IS_ERR(lag_definer->rules[idx])) {
+				err = PTR_ERR(lag_definer->rules[idx]);
+				while (i--)
+					while (j--)
+						mlx5_del_flow_rules(lag_definer->rules[idx]);
+				goto destroy_fg;
+			}
+		}
+	}
+
+	return 0;
+
+destroy_fg:
+	mlx5_destroy_flow_group(lag_definer->fg);
+destroy_ft:
+	mlx5_destroy_flow_table(lag_definer->ft);
+	return err;
+}
+
+static int mlx5_lag_set_definer_inner(u32 *match_definer_mask,
+				      enum mlx5_traffic_types tt)
+{
+	int format_id;
+	u8 *ipv6;
+
+	switch (tt) {
+	case MLX5_TT_IPV4_UDP:
+	case MLX5_TT_IPV4_TCP:
+		format_id = 23;
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_l4_sport);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_l4_dport);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_ip_src_addr);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_ip_dest_addr);
+		break;
+	case MLX5_TT_IPV4:
+		format_id = 23;
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_l3_type);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_dmac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_dmac_15_0);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_smac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_smac_15_0);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_ip_src_addr);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_ip_dest_addr);
+		break;
+	case MLX5_TT_IPV6_TCP:
+	case MLX5_TT_IPV6_UDP:
+		format_id = 31;
+		MLX5_SET_TO_ONES(match_definer_format_31, match_definer_mask,
+				 inner_l4_sport);
+		MLX5_SET_TO_ONES(match_definer_format_31, match_definer_mask,
+				 inner_l4_dport);
+		ipv6 = MLX5_ADDR_OF(match_definer_format_31, match_definer_mask,
+				    inner_ip_dest_addr);
+		memset(ipv6, 0xff, 16);
+		ipv6 = MLX5_ADDR_OF(match_definer_format_31, match_definer_mask,
+				    inner_ip_src_addr);
+		memset(ipv6, 0xff, 16);
+		break;
+	case MLX5_TT_IPV6:
+		format_id = 32;
+		ipv6 = MLX5_ADDR_OF(match_definer_format_32, match_definer_mask,
+				    inner_ip_dest_addr);
+		memset(ipv6, 0xff, 16);
+		ipv6 = MLX5_ADDR_OF(match_definer_format_32, match_definer_mask,
+				    inner_ip_src_addr);
+		memset(ipv6, 0xff, 16);
+		MLX5_SET_TO_ONES(match_definer_format_32, match_definer_mask,
+				 inner_dmac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_32, match_definer_mask,
+				 inner_dmac_15_0);
+		MLX5_SET_TO_ONES(match_definer_format_32, match_definer_mask,
+				 inner_smac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_32, match_definer_mask,
+				 inner_smac_15_0);
+		break;
+	default:
+		format_id = 23;
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_l3_type);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_dmac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_dmac_15_0);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_smac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_23, match_definer_mask,
+				 inner_smac_15_0);
+		break;
+	}
+
+	return format_id;
+}
+
+static int mlx5_lag_set_definer(u32 *match_definer_mask,
+				enum mlx5_traffic_types tt, bool tunnel,
+				enum netdev_lag_hash hash)
+{
+	int format_id;
+	u8 *ipv6;
+
+	if (tunnel)
+		return mlx5_lag_set_definer_inner(match_definer_mask, tt);
+
+	switch (tt) {
+	case MLX5_TT_IPV4_UDP:
+	case MLX5_TT_IPV4_TCP:
+		format_id = 22;
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_l4_sport);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_l4_dport);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_ip_src_addr);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_ip_dest_addr);
+		break;
+	case MLX5_TT_IPV4:
+		format_id = 22;
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_l3_type);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_dmac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_dmac_15_0);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_smac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_smac_15_0);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_ip_src_addr);
+		MLX5_SET_TO_ONES(match_definer_format_22, match_definer_mask,
+				 outer_ip_dest_addr);
+		break;
+	case MLX5_TT_IPV6_TCP:
+	case MLX5_TT_IPV6_UDP:
+		format_id = 29;
+		MLX5_SET_TO_ONES(match_definer_format_29, match_definer_mask,
+				 outer_l4_sport);
+		MLX5_SET_TO_ONES(match_definer_format_29, match_definer_mask,
+				 outer_l4_dport);
+		ipv6 = MLX5_ADDR_OF(match_definer_format_29, match_definer_mask,
+				    outer_ip_dest_addr);
+		memset(ipv6, 0xff, 16);
+		ipv6 = MLX5_ADDR_OF(match_definer_format_29, match_definer_mask,
+				    outer_ip_src_addr);
+		memset(ipv6, 0xff, 16);
+		break;
+	case MLX5_TT_IPV6:
+		format_id = 30;
+		ipv6 = MLX5_ADDR_OF(match_definer_format_30, match_definer_mask,
+				    outer_ip_dest_addr);
+		memset(ipv6, 0xff, 16);
+		ipv6 = MLX5_ADDR_OF(match_definer_format_30, match_definer_mask,
+				    outer_ip_src_addr);
+		memset(ipv6, 0xff, 16);
+		MLX5_SET_TO_ONES(match_definer_format_30, match_definer_mask,
+				 outer_dmac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_30, match_definer_mask,
+				 outer_dmac_15_0);
+		MLX5_SET_TO_ONES(match_definer_format_30, match_definer_mask,
+				 outer_smac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_30, match_definer_mask,
+				 outer_smac_15_0);
+		break;
+	default:
+		format_id = 0;
+		MLX5_SET_TO_ONES(match_definer_format_0, match_definer_mask,
+				 outer_smac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_0, match_definer_mask,
+				 outer_smac_15_0);
+
+		if (hash == NETDEV_LAG_HASH_VLAN_SRCMAC) {
+			MLX5_SET_TO_ONES(match_definer_format_0,
+					 match_definer_mask,
+					 outer_first_vlan_vid);
+			break;
+		}
+
+		MLX5_SET_TO_ONES(match_definer_format_0, match_definer_mask,
+				 outer_ethertype);
+		MLX5_SET_TO_ONES(match_definer_format_0, match_definer_mask,
+				 outer_dmac_47_16);
+		MLX5_SET_TO_ONES(match_definer_format_0, match_definer_mask,
+				 outer_dmac_15_0);
+		break;
+	}
+
+	return format_id;
+}
+
+static struct mlx5_lag_definer *
+mlx5_lag_create_definer(struct mlx5_lag *ldev, enum netdev_lag_hash hash,
+			enum mlx5_traffic_types tt, bool tunnel, u8 *ports)
+{
+	struct mlx5_core_dev *dev = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_lag_definer *lag_definer;
+	u32 *match_definer_mask;
+	int format_id, err;
+
+	lag_definer = kzalloc(sizeof(*lag_definer), GFP_KERNEL);
+	if (!lag_definer)
+		return ERR_PTR(-ENOMEM);
+
+	match_definer_mask = kvzalloc(MLX5_FLD_SZ_BYTES(match_definer,
+							match_mask),
+				      GFP_KERNEL);
+	if (!match_definer_mask) {
+		err = -ENOMEM;
+		goto free_lag_definer;
+	}
+
+	format_id = mlx5_lag_set_definer(match_definer_mask, tt, tunnel, hash);
+	lag_definer->definer =
+		mlx5_create_match_definer(dev, MLX5_FLOW_NAMESPACE_PORT_SEL,
+					  format_id, match_definer_mask);
+	if (IS_ERR(lag_definer->definer)) {
+		err = PTR_ERR(lag_definer->definer);
+		goto free_mask;
+	}
+
+	err = mlx5_lag_create_port_sel_table(ldev, lag_definer, ports);
+	if (err)
+		goto destroy_match_definer;
+
+	kvfree(match_definer_mask);
+
+	return lag_definer;
+
+destroy_match_definer:
+	mlx5_destroy_match_definer(dev, lag_definer->definer);
+free_mask:
+	kvfree(match_definer_mask);
+free_lag_definer:
+	kfree(lag_definer);
+	return ERR_PTR(err);
+}
+
+static void mlx5_lag_destroy_definer(struct mlx5_lag *ldev,
+				     struct mlx5_lag_definer *lag_definer)
+{
+	struct mlx5_core_dev *dev = ldev->pf[MLX5_LAG_P1].dev;
+	int idx;
+	int i;
+	int j;
+
+	for (i = 0; i < ldev->ports; i++) {
+		for (j = 0; j < ldev->buckets; j++) {
+			idx = i * ldev->buckets + j;
+			mlx5_del_flow_rules(lag_definer->rules[idx]);
+		}
+	}
+	mlx5_destroy_flow_group(lag_definer->fg);
+	mlx5_destroy_flow_table(lag_definer->ft);
+	mlx5_destroy_match_definer(dev, lag_definer->definer);
+	kfree(lag_definer);
+}
+
+static void mlx5_lag_destroy_definers(struct mlx5_lag *ldev)
+{
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	int tt;
+
+	for_each_set_bit(tt, port_sel->tt_map, MLX5_NUM_TT) {
+		if (port_sel->outer.definers[tt])
+			mlx5_lag_destroy_definer(ldev,
+						 port_sel->outer.definers[tt]);
+		if (port_sel->inner.definers[tt])
+			mlx5_lag_destroy_definer(ldev,
+						 port_sel->inner.definers[tt]);
+	}
+}
+
+static int mlx5_lag_create_definers(struct mlx5_lag *ldev,
+				    enum netdev_lag_hash hash_type,
+				    u8 *ports)
+{
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	struct mlx5_lag_definer *lag_definer;
+	int tt, err;
+
+	for_each_set_bit(tt, port_sel->tt_map, MLX5_NUM_TT) {
+		lag_definer = mlx5_lag_create_definer(ldev, hash_type, tt,
+						      false, ports);
+		if (IS_ERR(lag_definer)) {
+			err = PTR_ERR(lag_definer);
+			goto destroy_definers;
+		}
+		port_sel->outer.definers[tt] = lag_definer;
+
+		if (!port_sel->tunnel)
+			continue;
+
+		lag_definer =
+			mlx5_lag_create_definer(ldev, hash_type, tt,
+						true, ports);
+		if (IS_ERR(lag_definer)) {
+			err = PTR_ERR(lag_definer);
+			goto destroy_definers;
+		}
+		port_sel->inner.definers[tt] = lag_definer;
+	}
+
+	return 0;
+
+destroy_definers:
+	mlx5_lag_destroy_definers(ldev);
+	return err;
+}
+
+static void set_tt_map(struct mlx5_lag_port_sel *port_sel,
+		       enum netdev_lag_hash hash)
+{
+	port_sel->tunnel = false;
+
+	switch (hash) {
+	case NETDEV_LAG_HASH_E34:
+		port_sel->tunnel = true;
+		fallthrough;
+	case NETDEV_LAG_HASH_L34:
+		set_bit(MLX5_TT_IPV4_TCP, port_sel->tt_map);
+		set_bit(MLX5_TT_IPV4_UDP, port_sel->tt_map);
+		set_bit(MLX5_TT_IPV6_TCP, port_sel->tt_map);
+		set_bit(MLX5_TT_IPV6_UDP, port_sel->tt_map);
+		set_bit(MLX5_TT_IPV4, port_sel->tt_map);
+		set_bit(MLX5_TT_IPV6, port_sel->tt_map);
+		set_bit(MLX5_TT_ANY, port_sel->tt_map);
+		break;
+	case NETDEV_LAG_HASH_E23:
+		port_sel->tunnel = true;
+		fallthrough;
+	case NETDEV_LAG_HASH_L23:
+		set_bit(MLX5_TT_IPV4, port_sel->tt_map);
+		set_bit(MLX5_TT_IPV6, port_sel->tt_map);
+		set_bit(MLX5_TT_ANY, port_sel->tt_map);
+		break;
+	default:
+		set_bit(MLX5_TT_ANY, port_sel->tt_map);
+		break;
+	}
+}
+
+#define SET_IGNORE_DESTS_BITS(tt_map, dests)				\
+	do {								\
+		int idx;						\
+									\
+		for_each_clear_bit(idx, tt_map, MLX5_NUM_TT)		\
+			set_bit(idx, dests);				\
+	} while (0)
+
+static void mlx5_lag_set_inner_ttc_params(struct mlx5_lag *ldev,
+					  struct ttc_params *ttc_params)
+{
+	struct mlx5_core_dev *dev = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	struct mlx5_flow_table_attr *ft_attr;
+	int tt;
+
+	ttc_params->ns = mlx5_get_flow_namespace(dev,
+						 MLX5_FLOW_NAMESPACE_PORT_SEL);
+	ft_attr = &ttc_params->ft_attr;
+	ft_attr->level = MLX5_LAG_FT_LEVEL_INNER_TTC;
+
+	for_each_set_bit(tt, port_sel->tt_map, MLX5_NUM_TT) {
+		ttc_params->dests[tt].type =
+			MLX5_FLOW_DESTINATION_TYPE_FLOW_TABLE;
+		ttc_params->dests[tt].ft = port_sel->inner.definers[tt]->ft;
+	}
+	SET_IGNORE_DESTS_BITS(port_sel->tt_map, ttc_params->ignore_dests);
+}
+
+static void mlx5_lag_set_outer_ttc_params(struct mlx5_lag *ldev,
+					  struct ttc_params *ttc_params)
+{
+	struct mlx5_core_dev *dev = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	struct mlx5_flow_table_attr *ft_attr;
+	int tt;
+
+	ttc_params->ns = mlx5_get_flow_namespace(dev,
+						 MLX5_FLOW_NAMESPACE_PORT_SEL);
+	ft_attr = &ttc_params->ft_attr;
+	ft_attr->level = MLX5_LAG_FT_LEVEL_TTC;
+
+	for_each_set_bit(tt, port_sel->tt_map, MLX5_NUM_TT) {
+		ttc_params->dests[tt].type =
+			MLX5_FLOW_DESTINATION_TYPE_FLOW_TABLE;
+		ttc_params->dests[tt].ft = port_sel->outer.definers[tt]->ft;
+	}
+	SET_IGNORE_DESTS_BITS(port_sel->tt_map, ttc_params->ignore_dests);
+
+	ttc_params->inner_ttc = port_sel->tunnel;
+	if (!port_sel->tunnel)
+		return;
+
+	for (tt = 0; tt < MLX5_NUM_TUNNEL_TT; tt++) {
+		ttc_params->tunnel_dests[tt].type =
+			MLX5_FLOW_DESTINATION_TYPE_FLOW_TABLE;
+		ttc_params->tunnel_dests[tt].ft =
+			mlx5_get_ttc_flow_table(port_sel->inner.ttc);
+	}
+}
+
+static int mlx5_lag_create_ttc_table(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	struct ttc_params ttc_params = {};
+
+	mlx5_lag_set_outer_ttc_params(ldev, &ttc_params);
+	port_sel->outer.ttc = mlx5_create_ttc_table(dev, &ttc_params);
+	if (IS_ERR(port_sel->outer.ttc))
+		return PTR_ERR(port_sel->outer.ttc);
+
+	return 0;
+}
+
+static int mlx5_lag_create_inner_ttc_table(struct mlx5_lag *ldev)
+{
+	struct mlx5_core_dev *dev = ldev->pf[MLX5_LAG_P1].dev;
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	struct ttc_params ttc_params = {};
+
+	mlx5_lag_set_inner_ttc_params(ldev, &ttc_params);
+	port_sel->inner.ttc = mlx5_create_inner_ttc_table(dev, &ttc_params);
+	if (IS_ERR(port_sel->inner.ttc))
+		return PTR_ERR(port_sel->inner.ttc);
+
+	return 0;
+}
+
+int mlx5_lag_port_sel_create(struct mlx5_lag *ldev,
+			     enum netdev_lag_hash hash_type, u8 *ports)
+{
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	int err;
+
+	set_tt_map(port_sel, hash_type);
+	err = mlx5_lag_create_definers(ldev, hash_type, ports);
+	if (err)
+		return err;
+
+	if (port_sel->tunnel) {
+		err = mlx5_lag_create_inner_ttc_table(ldev);
+		if (err)
+			goto destroy_definers;
+	}
+
+	err = mlx5_lag_create_ttc_table(ldev);
+	if (err)
+		goto destroy_inner;
+
+	return 0;
+
+destroy_inner:
+	if (port_sel->tunnel)
+		mlx5_destroy_ttc_table(port_sel->inner.ttc);
+destroy_definers:
+	mlx5_lag_destroy_definers(ldev);
+	return err;
+}
+
+static int __mlx5_lag_modify_definers_destinations(struct mlx5_lag *ldev,
+						   struct mlx5_lag_definer *def,
+						   u8 *ports)
+{
+	struct mlx5_flow_destination dest = {};
+	int idx;
+	int err;
+	int i;
+	int j;
+
+	dest.type = MLX5_FLOW_DESTINATION_TYPE_UPLINK;
+	dest.vport.flags |= MLX5_FLOW_DEST_VPORT_VHCA_ID;
+
+	for (i = 0; i < ldev->ports; i++) {
+		for (j = 0; j < ldev->buckets; j++) {
+			idx = i * ldev->buckets + j;
+			if (ldev->v2p_map[idx] == ports[idx])
+				continue;
+
+			dest.vport.vhca_id = MLX5_CAP_GEN(ldev->pf[ports[idx] - 1].dev,
+							  vhca_id);
+			err = mlx5_modify_rule_destination(def->rules[idx], &dest, NULL);
+			if (err)
+				return err;
+		}
+	}
+
+	return 0;
+}
+
+static int
+mlx5_lag_modify_definers_destinations(struct mlx5_lag *ldev,
+				      struct mlx5_lag_definer **definers,
+				      u8 *ports)
+{
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	int err;
+	int tt;
+
+	for_each_set_bit(tt, port_sel->tt_map, MLX5_NUM_TT) {
+		err = __mlx5_lag_modify_definers_destinations(ldev, definers[tt], ports);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+int mlx5_lag_port_sel_modify(struct mlx5_lag *ldev, u8 *ports)
+{
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+	int err;
+
+	err = mlx5_lag_modify_definers_destinations(ldev,
+						    port_sel->outer.definers,
+						    ports);
+	if (err)
+		return err;
+
+	if (!port_sel->tunnel)
+		return 0;
+
+	return mlx5_lag_modify_definers_destinations(ldev,
+						     port_sel->inner.definers,
+						     ports);
+}
+
+void mlx5_lag_port_sel_destroy(struct mlx5_lag *ldev)
+{
+	struct mlx5_lag_port_sel *port_sel = &ldev->port_sel;
+
+	mlx5_destroy_ttc_table(port_sel->outer.ttc);
+	if (port_sel->tunnel)
+		mlx5_destroy_ttc_table(port_sel->inner.ttc);
+	mlx5_lag_destroy_definers(ldev);
+	memset(port_sel, 0, sizeof(*port_sel));
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/lag/port_sel.h b/drivers/net/ethernet/mellanox/mlx5/core/lag/port_sel.h
new file mode 100644
index 0000000000..5ec3af2a3e
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/lag/port_sel.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB */
+/* Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. */
+
+#ifndef __MLX5_LAG_FS_H__
+#define __MLX5_LAG_FS_H__
+
+#include "lib/fs_ttc.h"
+
+struct mlx5_lag_definer {
+	struct mlx5_flow_definer *definer;
+	struct mlx5_flow_table *ft;
+	struct mlx5_flow_group *fg;
+	/* Each port has ldev->buckets number of rules and they are arrange in
+	 * [port * buckets .. port * buckets + buckets) locations
+	 */
+	struct mlx5_flow_handle *rules[MLX5_MAX_PORTS * MLX5_LAG_MAX_HASH_BUCKETS];
+};
+
+struct mlx5_lag_ttc {
+	struct mlx5_ttc_table *ttc;
+	struct mlx5_lag_definer *definers[MLX5_NUM_TT];
+};
+
+struct mlx5_lag_port_sel {
+	DECLARE_BITMAP(tt_map, MLX5_NUM_TT);
+	bool   tunnel;
+	struct mlx5_lag_ttc outer;
+	struct mlx5_lag_ttc inner;
+};
+
+#ifdef CONFIG_MLX5_ESWITCH
+
+int mlx5_lag_port_sel_modify(struct mlx5_lag *ldev, u8 *ports);
+void mlx5_lag_port_sel_destroy(struct mlx5_lag *ldev);
+int mlx5_lag_port_sel_create(struct mlx5_lag *ldev,
+			     enum netdev_lag_hash hash_type, u8 *ports);
+
+#else /* CONFIG_MLX5_ESWITCH */
+static inline int mlx5_lag_port_sel_create(struct mlx5_lag *ldev,
+					   enum netdev_lag_hash hash_type,
+					   u8 *ports)
+{
+	return 0;
+}
+
+static inline int mlx5_lag_port_sel_modify(struct mlx5_lag *ldev, u8 *ports)
+{
+	return 0;
+}
+
+static inline void mlx5_lag_port_sel_destroy(struct mlx5_lag *ldev) {}
+#endif /* CONFIG_MLX5_ESWITCH */
+#endif /* __MLX5_LAG_FS_H__ */