2 files changed, 77 insertions, 1 deletions

diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.c
index 6a77bf236c22..7f10e9cd7870 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.c
@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
 /* Copyright (c) 2019 Mellanox Technologies. All rights reserved */
 
+#include <linux/bitops.h>
 #include <linux/kernel.h>
 #include <net/devlink.h>
 #include <uapi/linux/devlink.h>
@@ -166,6 +167,18 @@ static void mlxsw_sp_rx_exception_listener(struct sk_buff *skb, u8 local_port,
 	MLXSW_RXL(mlxsw_sp_rx_exception_listener, _id,			      \
 		   _action, false, SP_##_group_id, SET_FW_DEFAULT)
 
+#define MLXSW_SP_TRAP_POLICER(_id, _rate, _burst)			      \
+	DEVLINK_TRAP_POLICER(_id, _rate, _burst,			      \
+			     MLXSW_REG_QPCR_HIGHEST_CIR,		      \
+			     MLXSW_REG_QPCR_LOWEST_CIR,			      \
+			     1 << MLXSW_REG_QPCR_HIGHEST_CBS,		      \
+			     1 << MLXSW_REG_QPCR_LOWEST_CBS)
+
+/* Ordered by policer identifier */
+static const struct devlink_trap_policer mlxsw_sp_trap_policers_arr[] = {
+	MLXSW_SP_TRAP_POLICER(1, 10 * 1024, 128),
+};
+
 static const struct devlink_trap_group mlxsw_sp_trap_groups_arr[] = {
 	DEVLINK_TRAP_GROUP_GENERIC(L2_DROPS, 0),
 	DEVLINK_TRAP_GROUP_GENERIC(L3_DROPS, 0),
@@ -325,6 +338,58 @@ static int mlxsw_sp_trap_dummy_group_init(struct mlxsw_sp *mlxsw_sp)
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(htgt), htgt_pl);
 }
 
+static int mlxsw_sp_trap_policers_init(struct mlxsw_sp *mlxsw_sp)
+{
+	struct mlxsw_sp_trap *trap = mlxsw_sp->trap;
+	u64 free_policers = 0;
+	u32 last_id = 0;
+	int i;
+
+	for_each_clear_bit(i, trap->policers_usage, trap->max_policers)
+		free_policers++;
+
+	if (ARRAY_SIZE(mlxsw_sp_trap_policers_arr) > free_policers) {
+		dev_err(mlxsw_sp->bus_info->dev, "Exceeded number of supported packet trap policers\n");
+		return -ENOBUFS;
+	}
+
+	trap->policers_arr = kcalloc(free_policers,
+				     sizeof(struct devlink_trap_policer),
+				     GFP_KERNEL);
+	if (!trap->policers_arr)
+		return -ENOMEM;
+
+	trap->policers_count = free_policers;
+
+	for (i = 0; i < free_policers; i++) {
+		const struct devlink_trap_policer *policer;
+
+		if (i < ARRAY_SIZE(mlxsw_sp_trap_policers_arr)) {
+			policer = &mlxsw_sp_trap_policers_arr[i];
+			trap->policers_arr[i] = *policer;
+			last_id = policer->id;
+		} else {
+			/* Use parameters set for first policer and override
+			 * relevant ones.
+			 */
+			policer = &mlxsw_sp_trap_policers_arr[0];
+			trap->policers_arr[i] = *policer;
+			trap->policers_arr[i].id = ++last_id;
+			trap->policers_arr[i].init_rate = 1;
+			trap->policers_arr[i].init_burst = 16;
+		}
+	}
+
+	return 0;
+}
+
+static void mlxsw_sp_trap_policers_fini(struct mlxsw_sp *mlxsw_sp)
+{
+	struct mlxsw_sp_trap *trap = mlxsw_sp->trap;
+
+	kfree(trap->policers_arr);
+}
+
 int mlxsw_sp_devlink_traps_init(struct mlxsw_sp *mlxsw_sp)
 {
 	size_t groups_count = ARRAY_SIZE(mlxsw_sp_trap_groups_arr);
@@ -343,10 +408,14 @@ int mlxsw_sp_devlink_traps_init(struct mlxsw_sp *mlxsw_sp)
 		    ARRAY_SIZE(mlxsw_sp_listeners_arr)))
 		return -EINVAL;
 
+	err = mlxsw_sp_trap_policers_init(mlxsw_sp);
+	if (err)
+		return err;
+
 	err = devlink_trap_groups_register(devlink, mlxsw_sp_trap_groups_arr,
 					   groups_count);
 	if (err)
-		return err;
+		goto err_trap_groups_register;
 
 	err = devlink_traps_register(devlink, mlxsw_sp_traps_arr,
 				     ARRAY_SIZE(mlxsw_sp_traps_arr), mlxsw_sp);
@@ -358,6 +427,8 @@ int mlxsw_sp_devlink_traps_init(struct mlxsw_sp *mlxsw_sp)
 err_traps_register:
 	devlink_trap_groups_unregister(devlink, mlxsw_sp_trap_groups_arr,
 				       groups_count);
+err_trap_groups_register:
+	mlxsw_sp_trap_policers_fini(mlxsw_sp);
 	return err;
 }
 
@@ -370,6 +441,7 @@ void mlxsw_sp_devlink_traps_fini(struct mlxsw_sp *mlxsw_sp)
 				 ARRAY_SIZE(mlxsw_sp_traps_arr));
 	devlink_trap_groups_unregister(devlink, mlxsw_sp_trap_groups_arr,
 				       groups_count);
+	mlxsw_sp_trap_policers_fini(mlxsw_sp);
 }
 
 int mlxsw_sp_trap_init(struct mlxsw_core *mlxsw_core,
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.h
index 12c5641b2165..05bb652b1a76 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.h
@@ -4,7 +4,11 @@
 #ifndef _MLXSW_SPECTRUM_TRAP_H
 #define _MLXSW_SPECTRUM_TRAP_H
 
+#include <net/devlink.h>
+
 struct mlxsw_sp_trap {
+	struct devlink_trap_policer *policers_arr; /* Registered policers */
+	u64 policers_count; /* Number of registered policers */
 	u64 max_policers;
 	unsigned long policers_usage[]; /* Usage bitmap */
 };