mlxsw: spectrum_trap: Prepare policers for registration with devlink

Prepare an array of policer IDs to register with devlink and their
associated parameters.

The array is composed from both policers that are currently bound to
exposed trap groups and policers that are not bound to any trap group.

v2:
* Provide max/min rate/burst size when registering policers
Signed-off-by: Ido Schimmel <idosch@mellanox.com>
Reviewed-by: Jiri Pirko <jiri@mellanox.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/ethernet/mellanox/mlxsw/spectrum_trap.c

 // 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,

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 */
 };