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

   1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
   2 /* Copyright (c) 2019 Mellanox Technologies. All rights reserved */
   3
   4 #include <linux/bitops.h>
   5 #include <linux/kernel.h>
   6 #include <linux/netlink.h>
   7 #include <net/devlink.h>
   8 #include <uapi/linux/devlink.h>
   9
  10 #include "core.h"
  11 #include "reg.h"
  12 #include "spectrum.h"
  13 #include "spectrum_trap.h"
  14
  15 /* All driver-specific traps must be documented in
  16  * Documentation/networking/devlink/mlxsw.rst
  17  */
  18 enum {
  19         DEVLINK_MLXSW_TRAP_ID_BASE = DEVLINK_TRAP_GENERIC_ID_MAX,
  20         DEVLINK_MLXSW_TRAP_ID_IRIF_DISABLED,
  21         DEVLINK_MLXSW_TRAP_ID_ERIF_DISABLED,
  22 };
  23
  24 #define DEVLINK_MLXSW_TRAP_NAME_IRIF_DISABLED \
  25         "irif_disabled"
  26 #define DEVLINK_MLXSW_TRAP_NAME_ERIF_DISABLED \
  27         "erif_disabled"
  28
  29 #define MLXSW_SP_TRAP_METADATA DEVLINK_TRAP_METADATA_TYPE_F_IN_PORT
  30
  31 static int mlxsw_sp_rx_listener(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb,
  32                                 u8 local_port,
  33                                 struct mlxsw_sp_port *mlxsw_sp_port)
  34 {
  35         struct mlxsw_sp_port_pcpu_stats *pcpu_stats;
  36
  37         if (unlikely(!mlxsw_sp_port)) {
  38                 dev_warn_ratelimited(mlxsw_sp->bus_info->dev, "Port %d: skb received for non-existent port\n",
  39                                      local_port);
  40                 kfree_skb(skb);
  41                 return -EINVAL;
  42         }
  43
  44         skb->dev = mlxsw_sp_port->dev;
  45
  46         pcpu_stats = this_cpu_ptr(mlxsw_sp_port->pcpu_stats);
  47         u64_stats_update_begin(&pcpu_stats->syncp);
  48         pcpu_stats->rx_packets++;
  49         pcpu_stats->rx_bytes += skb->len;
  50         u64_stats_update_end(&pcpu_stats->syncp);
  51
  52         skb->protocol = eth_type_trans(skb, skb->dev);
  53
  54         return 0;
  55 }
  56
  57 static void mlxsw_sp_rx_drop_listener(struct sk_buff *skb, u8 local_port,
  58                                       void *trap_ctx)
  59 {
  60         struct devlink_port *in_devlink_port;
  61         struct mlxsw_sp_port *mlxsw_sp_port;
  62         struct mlxsw_sp *mlxsw_sp;
  63         struct devlink *devlink;
  64         int err;
  65
  66         mlxsw_sp = devlink_trap_ctx_priv(trap_ctx);
  67         mlxsw_sp_port = mlxsw_sp->ports[local_port];
  68
  69         err = mlxsw_sp_rx_listener(mlxsw_sp, skb, local_port, mlxsw_sp_port);
  70         if (err)
  71                 return;
  72
  73         devlink = priv_to_devlink(mlxsw_sp->core);
  74         in_devlink_port = mlxsw_core_port_devlink_port_get(mlxsw_sp->core,
  75                                                            local_port);
  76         skb_push(skb, ETH_HLEN);
  77         devlink_trap_report(devlink, skb, trap_ctx, in_devlink_port, NULL);
  78         consume_skb(skb);
  79 }
  80
  81 static void mlxsw_sp_rx_acl_drop_listener(struct sk_buff *skb, u8 local_port,
  82                                           void *trap_ctx)
  83 {
  84         u32 cookie_index = mlxsw_skb_cb(skb)->cookie_index;
  85         const struct flow_action_cookie *fa_cookie;
  86         struct devlink_port *in_devlink_port;
  87         struct mlxsw_sp_port *mlxsw_sp_port;
  88         struct mlxsw_sp *mlxsw_sp;
  89         struct devlink *devlink;
  90         int err;
  91
  92         mlxsw_sp = devlink_trap_ctx_priv(trap_ctx);
  93         mlxsw_sp_port = mlxsw_sp->ports[local_port];
  94
  95         err = mlxsw_sp_rx_listener(mlxsw_sp, skb, local_port, mlxsw_sp_port);
  96         if (err)
  97                 return;
  98
  99         devlink = priv_to_devlink(mlxsw_sp->core);
 100         in_devlink_port = mlxsw_core_port_devlink_port_get(mlxsw_sp->core,
 101                                                            local_port);
 102         skb_push(skb, ETH_HLEN);
 103         rcu_read_lock();
 104         fa_cookie = mlxsw_sp_acl_act_cookie_lookup(mlxsw_sp, cookie_index);
 105         devlink_trap_report(devlink, skb, trap_ctx, in_devlink_port, fa_cookie);
 106         rcu_read_unlock();
 107         consume_skb(skb);
 108 }
 109
 110 static void mlxsw_sp_rx_exception_listener(struct sk_buff *skb, u8 local_port,
 111                                            void *trap_ctx)
 112 {
 113         struct devlink_port *in_devlink_port;
 114         struct mlxsw_sp_port *mlxsw_sp_port;
 115         struct mlxsw_sp *mlxsw_sp;
 116         struct devlink *devlink;
 117         int err;
 118
 119         mlxsw_sp = devlink_trap_ctx_priv(trap_ctx);
 120         mlxsw_sp_port = mlxsw_sp->ports[local_port];
 121
 122         err = mlxsw_sp_rx_listener(mlxsw_sp, skb, local_port, mlxsw_sp_port);
 123         if (err)
 124                 return;
 125
 126         devlink = priv_to_devlink(mlxsw_sp->core);
 127         in_devlink_port = mlxsw_core_port_devlink_port_get(mlxsw_sp->core,
 128                                                            local_port);
 129         skb_push(skb, ETH_HLEN);
 130         devlink_trap_report(devlink, skb, trap_ctx, in_devlink_port, NULL);
 131         skb_pull(skb, ETH_HLEN);
 132         skb->offload_fwd_mark = 1;
 133         netif_receive_skb(skb);
 134 }
 135
 136 #define MLXSW_SP_TRAP_DROP(_id, _group_id)                                    \
 137         DEVLINK_TRAP_GENERIC(DROP, DROP, _id,                                 \
 138                              DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,       \
 139                              MLXSW_SP_TRAP_METADATA)
 140
 141 #define MLXSW_SP_TRAP_DROP_EXT(_id, _group_id, _metadata)                     \
 142         DEVLINK_TRAP_GENERIC(DROP, DROP, _id,                                 \
 143                              DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,       \
 144                              MLXSW_SP_TRAP_METADATA | (_metadata))
 145
 146 #define MLXSW_SP_TRAP_DRIVER_DROP(_id, _group_id)                             \
 147         DEVLINK_TRAP_DRIVER(DROP, DROP, DEVLINK_MLXSW_TRAP_ID_##_id,          \
 148                             DEVLINK_MLXSW_TRAP_NAME_##_id,                    \
 149                             DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,        \
 150                             MLXSW_SP_TRAP_METADATA)
 151
 152 #define MLXSW_SP_TRAP_EXCEPTION(_id, _group_id)               \
 153         DEVLINK_TRAP_GENERIC(EXCEPTION, TRAP, _id,                            \
 154                              DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,       \
 155                              MLXSW_SP_TRAP_METADATA)
 156
 157 #define MLXSW_SP_RXL_DISCARD(_id, _group_id)                                  \
 158         MLXSW_RXL_DIS(mlxsw_sp_rx_drop_listener, DISCARD_##_id,               \
 159                       TRAP_EXCEPTION_TO_CPU, false, SP_##_group_id,           \
 160                       SET_FW_DEFAULT, SP_##_group_id)
 161
 162 #define MLXSW_SP_RXL_ACL_DISCARD(_id, _en_group_id, _dis_group_id)            \
 163         MLXSW_RXL_DIS(mlxsw_sp_rx_acl_drop_listener, DISCARD_##_id,           \
 164                       TRAP_EXCEPTION_TO_CPU, false, SP_##_en_group_id,        \
 165                       SET_FW_DEFAULT, SP_##_dis_group_id)
 166
 167 #define MLXSW_SP_RXL_EXCEPTION(_id, _group_id, _action)                       \
 168         MLXSW_RXL(mlxsw_sp_rx_exception_listener, _id,                        \
 169                    _action, false, SP_##_group_id, SET_FW_DEFAULT)
 170
 171 #define MLXSW_SP_TRAP_POLICER(_id, _rate, _burst)                             \
 172         DEVLINK_TRAP_POLICER(_id, _rate, _burst,                              \
 173                              MLXSW_REG_QPCR_HIGHEST_CIR,                      \
 174                              MLXSW_REG_QPCR_LOWEST_CIR,                       \
 175                              1 << MLXSW_REG_QPCR_HIGHEST_CBS,                 \
 176                              1 << MLXSW_REG_QPCR_LOWEST_CBS)
 177
 178 /* Ordered by policer identifier */
 179 static const struct devlink_trap_policer mlxsw_sp_trap_policers_arr[] = {
 180         MLXSW_SP_TRAP_POLICER(1, 10 * 1024, 128),
 181 };
 182
 183 static const struct devlink_trap_group mlxsw_sp_trap_groups_arr[] = {
 184         DEVLINK_TRAP_GROUP_GENERIC(L2_DROPS, 1),
 185         DEVLINK_TRAP_GROUP_GENERIC(L3_DROPS, 1),
 186         DEVLINK_TRAP_GROUP_GENERIC(TUNNEL_DROPS, 1),
 187         DEVLINK_TRAP_GROUP_GENERIC(ACL_DROPS, 1),
 188 };
 189
 190 static const struct devlink_trap mlxsw_sp_traps_arr[] = {
 191         MLXSW_SP_TRAP_DROP(SMAC_MC, L2_DROPS),
 192         MLXSW_SP_TRAP_DROP(VLAN_TAG_MISMATCH, L2_DROPS),
 193         MLXSW_SP_TRAP_DROP(INGRESS_VLAN_FILTER, L2_DROPS),
 194         MLXSW_SP_TRAP_DROP(INGRESS_STP_FILTER, L2_DROPS),
 195         MLXSW_SP_TRAP_DROP(EMPTY_TX_LIST, L2_DROPS),
 196         MLXSW_SP_TRAP_DROP(PORT_LOOPBACK_FILTER, L2_DROPS),
 197         MLXSW_SP_TRAP_DROP(BLACKHOLE_ROUTE, L3_DROPS),
 198         MLXSW_SP_TRAP_DROP(NON_IP_PACKET, L3_DROPS),
 199         MLXSW_SP_TRAP_DROP(UC_DIP_MC_DMAC, L3_DROPS),
 200         MLXSW_SP_TRAP_DROP(DIP_LB, L3_DROPS),
 201         MLXSW_SP_TRAP_DROP(SIP_MC, L3_DROPS),
 202         MLXSW_SP_TRAP_DROP(SIP_LB, L3_DROPS),
 203         MLXSW_SP_TRAP_DROP(CORRUPTED_IP_HDR, L3_DROPS),
 204         MLXSW_SP_TRAP_DROP(IPV4_SIP_BC, L3_DROPS),
 205         MLXSW_SP_TRAP_DROP(IPV6_MC_DIP_RESERVED_SCOPE, L3_DROPS),
 206         MLXSW_SP_TRAP_DROP(IPV6_MC_DIP_INTERFACE_LOCAL_SCOPE, L3_DROPS),
 207         MLXSW_SP_TRAP_EXCEPTION(MTU_ERROR, L3_DROPS),
 208         MLXSW_SP_TRAP_EXCEPTION(TTL_ERROR, L3_DROPS),
 209         MLXSW_SP_TRAP_EXCEPTION(RPF, L3_DROPS),
 210         MLXSW_SP_TRAP_EXCEPTION(REJECT_ROUTE, L3_DROPS),
 211         MLXSW_SP_TRAP_EXCEPTION(UNRESOLVED_NEIGH, L3_DROPS),
 212         MLXSW_SP_TRAP_EXCEPTION(IPV4_LPM_UNICAST_MISS, L3_DROPS),
 213         MLXSW_SP_TRAP_EXCEPTION(IPV6_LPM_UNICAST_MISS, L3_DROPS),
 214         MLXSW_SP_TRAP_DRIVER_DROP(IRIF_DISABLED, L3_DROPS),
 215         MLXSW_SP_TRAP_DRIVER_DROP(ERIF_DISABLED, L3_DROPS),
 216         MLXSW_SP_TRAP_DROP(NON_ROUTABLE, L3_DROPS),
 217         MLXSW_SP_TRAP_EXCEPTION(DECAP_ERROR, TUNNEL_DROPS),
 218         MLXSW_SP_TRAP_DROP(OVERLAY_SMAC_MC, TUNNEL_DROPS),
 219         MLXSW_SP_TRAP_DROP_EXT(INGRESS_FLOW_ACTION_DROP, ACL_DROPS,
 220                                DEVLINK_TRAP_METADATA_TYPE_F_FA_COOKIE),
 221         MLXSW_SP_TRAP_DROP_EXT(EGRESS_FLOW_ACTION_DROP, ACL_DROPS,
 222                                DEVLINK_TRAP_METADATA_TYPE_F_FA_COOKIE),
 223 };
 224
 225 static const struct mlxsw_listener mlxsw_sp_listeners_arr[] = {
 226         MLXSW_SP_RXL_DISCARD(ING_PACKET_SMAC_MC, L2_DISCARDS),
 227         MLXSW_SP_RXL_DISCARD(ING_SWITCH_VTAG_ALLOW, L2_DISCARDS),
 228         MLXSW_SP_RXL_DISCARD(ING_SWITCH_VLAN, L2_DISCARDS),
 229         MLXSW_SP_RXL_DISCARD(ING_SWITCH_STP, L2_DISCARDS),
 230         MLXSW_SP_RXL_DISCARD(LOOKUP_SWITCH_UC, L2_DISCARDS),
 231         MLXSW_SP_RXL_DISCARD(LOOKUP_SWITCH_MC_NULL, L2_DISCARDS),
 232         MLXSW_SP_RXL_DISCARD(LOOKUP_SWITCH_LB, L2_DISCARDS),
 233         MLXSW_SP_RXL_DISCARD(ROUTER2, L3_DISCARDS),
 234         MLXSW_SP_RXL_DISCARD(ING_ROUTER_NON_IP_PACKET, L3_DISCARDS),
 235         MLXSW_SP_RXL_DISCARD(ING_ROUTER_UC_DIP_MC_DMAC, L3_DISCARDS),
 236         MLXSW_SP_RXL_DISCARD(ING_ROUTER_DIP_LB, L3_DISCARDS),
 237         MLXSW_SP_RXL_DISCARD(ING_ROUTER_SIP_MC, L3_DISCARDS),
 238         MLXSW_SP_RXL_DISCARD(ING_ROUTER_SIP_LB, L3_DISCARDS),
 239         MLXSW_SP_RXL_DISCARD(ING_ROUTER_CORRUPTED_IP_HDR, L3_DISCARDS),
 240         MLXSW_SP_RXL_DISCARD(ING_ROUTER_IPV4_SIP_BC, L3_DISCARDS),
 241         MLXSW_SP_RXL_DISCARD(IPV6_MC_DIP_RESERVED_SCOPE, L3_DISCARDS),
 242         MLXSW_SP_RXL_DISCARD(IPV6_MC_DIP_INTERFACE_LOCAL_SCOPE, L3_DISCARDS),
 243         MLXSW_SP_RXL_EXCEPTION(MTUERROR, L3_DISCARDS, TRAP_TO_CPU),
 244         MLXSW_SP_RXL_EXCEPTION(TTLERROR, L3_DISCARDS, TRAP_TO_CPU),
 245         MLXSW_SP_RXL_EXCEPTION(RPF, L3_DISCARDS, TRAP_TO_CPU),
 246         MLXSW_SP_RXL_EXCEPTION(RTR_INGRESS1, L3_DISCARDS, TRAP_TO_CPU),
 247         MLXSW_SP_RXL_EXCEPTION(HOST_MISS_IPV4, L3_DISCARDS, TRAP_TO_CPU),
 248         MLXSW_SP_RXL_EXCEPTION(HOST_MISS_IPV6, L3_DISCARDS, TRAP_TO_CPU),
 249         MLXSW_SP_RXL_EXCEPTION(DISCARD_ROUTER3, L3_DISCARDS,
 250                                TRAP_EXCEPTION_TO_CPU),
 251         MLXSW_SP_RXL_EXCEPTION(DISCARD_ROUTER_LPM4, L3_DISCARDS,
 252                                TRAP_EXCEPTION_TO_CPU),
 253         MLXSW_SP_RXL_EXCEPTION(DISCARD_ROUTER_LPM6, L3_DISCARDS,
 254                                TRAP_EXCEPTION_TO_CPU),
 255         MLXSW_SP_RXL_DISCARD(ROUTER_IRIF_EN, L3_DISCARDS),
 256         MLXSW_SP_RXL_DISCARD(ROUTER_ERIF_EN, L3_DISCARDS),
 257         MLXSW_SP_RXL_DISCARD(NON_ROUTABLE, L3_DISCARDS),
 258         MLXSW_SP_RXL_EXCEPTION(DECAP_ECN0, TUNNEL_DISCARDS,
 259                                TRAP_EXCEPTION_TO_CPU),
 260         MLXSW_SP_RXL_EXCEPTION(IPIP_DECAP_ERROR, TUNNEL_DISCARDS,
 261                                TRAP_EXCEPTION_TO_CPU),
 262         MLXSW_SP_RXL_EXCEPTION(DISCARD_DEC_PKT, TUNNEL_DISCARDS,
 263                                TRAP_EXCEPTION_TO_CPU),
 264         MLXSW_SP_RXL_DISCARD(OVERLAY_SMAC_MC, TUNNEL_DISCARDS),
 265         MLXSW_SP_RXL_ACL_DISCARD(INGRESS_ACL, ACL_DISCARDS, DUMMY),
 266         MLXSW_SP_RXL_ACL_DISCARD(EGRESS_ACL, ACL_DISCARDS, DUMMY),
 267 };
 268
 269 /* Mapping between hardware trap and devlink trap. Multiple hardware traps can
 270  * be mapped to the same devlink trap. Order is according to
 271  * 'mlxsw_sp_listeners_arr'.
 272  */
 273 static const u16 mlxsw_sp_listener_devlink_map[] = {
 274         DEVLINK_TRAP_GENERIC_ID_SMAC_MC,
 275         DEVLINK_TRAP_GENERIC_ID_VLAN_TAG_MISMATCH,
 276         DEVLINK_TRAP_GENERIC_ID_INGRESS_VLAN_FILTER,
 277         DEVLINK_TRAP_GENERIC_ID_INGRESS_STP_FILTER,
 278         DEVLINK_TRAP_GENERIC_ID_EMPTY_TX_LIST,
 279         DEVLINK_TRAP_GENERIC_ID_EMPTY_TX_LIST,
 280         DEVLINK_TRAP_GENERIC_ID_PORT_LOOPBACK_FILTER,
 281         DEVLINK_TRAP_GENERIC_ID_BLACKHOLE_ROUTE,
 282         DEVLINK_TRAP_GENERIC_ID_NON_IP_PACKET,
 283         DEVLINK_TRAP_GENERIC_ID_UC_DIP_MC_DMAC,
 284         DEVLINK_TRAP_GENERIC_ID_DIP_LB,
 285         DEVLINK_TRAP_GENERIC_ID_SIP_MC,
 286         DEVLINK_TRAP_GENERIC_ID_SIP_LB,
 287         DEVLINK_TRAP_GENERIC_ID_CORRUPTED_IP_HDR,
 288         DEVLINK_TRAP_GENERIC_ID_IPV4_SIP_BC,
 289         DEVLINK_TRAP_GENERIC_ID_IPV6_MC_DIP_RESERVED_SCOPE,
 290         DEVLINK_TRAP_GENERIC_ID_IPV6_MC_DIP_INTERFACE_LOCAL_SCOPE,
 291         DEVLINK_TRAP_GENERIC_ID_MTU_ERROR,
 292         DEVLINK_TRAP_GENERIC_ID_TTL_ERROR,
 293         DEVLINK_TRAP_GENERIC_ID_RPF,
 294         DEVLINK_TRAP_GENERIC_ID_REJECT_ROUTE,
 295         DEVLINK_TRAP_GENERIC_ID_UNRESOLVED_NEIGH,
 296         DEVLINK_TRAP_GENERIC_ID_UNRESOLVED_NEIGH,
 297         DEVLINK_TRAP_GENERIC_ID_UNRESOLVED_NEIGH,
 298         DEVLINK_TRAP_GENERIC_ID_IPV4_LPM_UNICAST_MISS,
 299         DEVLINK_TRAP_GENERIC_ID_IPV6_LPM_UNICAST_MISS,
 300         DEVLINK_MLXSW_TRAP_ID_IRIF_DISABLED,
 301         DEVLINK_MLXSW_TRAP_ID_ERIF_DISABLED,
 302         DEVLINK_TRAP_GENERIC_ID_NON_ROUTABLE,
 303         DEVLINK_TRAP_GENERIC_ID_DECAP_ERROR,
 304         DEVLINK_TRAP_GENERIC_ID_DECAP_ERROR,
 305         DEVLINK_TRAP_GENERIC_ID_DECAP_ERROR,
 306         DEVLINK_TRAP_GENERIC_ID_OVERLAY_SMAC_MC,
 307         DEVLINK_TRAP_GENERIC_ID_INGRESS_FLOW_ACTION_DROP,
 308         DEVLINK_TRAP_GENERIC_ID_EGRESS_FLOW_ACTION_DROP,
 309 };
 310
 311 #define MLXSW_SP_THIN_POLICER_ID        (MLXSW_REG_HTGT_TRAP_GROUP_MAX + 1)
 312
 313 static struct mlxsw_sp_trap_policer_item *
 314 mlxsw_sp_trap_policer_item_lookup(struct mlxsw_sp *mlxsw_sp, u32 id)
 315 {
 316         struct mlxsw_sp_trap_policer_item *policer_item;
 317         struct mlxsw_sp_trap *trap = mlxsw_sp->trap;
 318
 319         list_for_each_entry(policer_item, &trap->policer_item_list, list) {
 320                 if (policer_item->id == id)
 321                         return policer_item;
 322         }
 323
 324         return NULL;
 325 }
 326
 327 static int mlxsw_sp_trap_cpu_policers_set(struct mlxsw_sp *mlxsw_sp)
 328 {
 329         char qpcr_pl[MLXSW_REG_QPCR_LEN];
 330
 331         /* The purpose of "thin" policer is to drop as many packets
 332          * as possible. The dummy group is using it.
 333          */
 334         __set_bit(MLXSW_SP_THIN_POLICER_ID, mlxsw_sp->trap->policers_usage);
 335         mlxsw_reg_qpcr_pack(qpcr_pl, MLXSW_SP_THIN_POLICER_ID,
 336                             MLXSW_REG_QPCR_IR_UNITS_M, false, 1, 4);
 337         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpcr), qpcr_pl);
 338 }
 339
 340 static int mlxsw_sp_trap_dummy_group_init(struct mlxsw_sp *mlxsw_sp)
 341 {
 342         char htgt_pl[MLXSW_REG_HTGT_LEN];
 343
 344         mlxsw_reg_htgt_pack(htgt_pl, MLXSW_REG_HTGT_TRAP_GROUP_SP_DUMMY,
 345                             MLXSW_SP_THIN_POLICER_ID, 0, 1);
 346         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(htgt), htgt_pl);
 347 }
 348
 349 static int mlxsw_sp_trap_policers_init(struct mlxsw_sp *mlxsw_sp)
 350 {
 351         struct devlink *devlink = priv_to_devlink(mlxsw_sp->core);
 352         struct mlxsw_sp_trap *trap = mlxsw_sp->trap;
 353         u64 free_policers = 0;
 354         u32 last_id = 0;
 355         int err, i;
 356
 357         for_each_clear_bit(i, trap->policers_usage, trap->max_policers)
 358                 free_policers++;
 359
 360         if (ARRAY_SIZE(mlxsw_sp_trap_policers_arr) > free_policers) {
 361                 dev_err(mlxsw_sp->bus_info->dev, "Exceeded number of supported packet trap policers\n");
 362                 return -ENOBUFS;
 363         }
 364
 365         trap->policers_arr = kcalloc(free_policers,
 366                                      sizeof(struct devlink_trap_policer),
 367                                      GFP_KERNEL);
 368         if (!trap->policers_arr)
 369                 return -ENOMEM;
 370
 371         trap->policers_count = free_policers;
 372
 373         for (i = 0; i < free_policers; i++) {
 374                 const struct devlink_trap_policer *policer;
 375
 376                 if (i < ARRAY_SIZE(mlxsw_sp_trap_policers_arr)) {
 377                         policer = &mlxsw_sp_trap_policers_arr[i];
 378                         trap->policers_arr[i] = *policer;
 379                         last_id = policer->id;
 380                 } else {
 381                         /* Use parameters set for first policer and override
 382                          * relevant ones.
 383                          */
 384                         policer = &mlxsw_sp_trap_policers_arr[0];
 385                         trap->policers_arr[i] = *policer;
 386                         trap->policers_arr[i].id = ++last_id;
 387                         trap->policers_arr[i].init_rate = 1;
 388                         trap->policers_arr[i].init_burst = 16;
 389                 }
 390         }
 391
 392         INIT_LIST_HEAD(&trap->policer_item_list);
 393
 394         err = devlink_trap_policers_register(devlink, trap->policers_arr,
 395                                              trap->policers_count);
 396         if (err)
 397                 goto err_trap_policers_register;
 398
 399         return 0;
 400
 401 err_trap_policers_register:
 402         kfree(trap->policers_arr);
 403         return err;
 404 }
 405
 406 static void mlxsw_sp_trap_policers_fini(struct mlxsw_sp *mlxsw_sp)
 407 {
 408         struct devlink *devlink = priv_to_devlink(mlxsw_sp->core);
 409         struct mlxsw_sp_trap *trap = mlxsw_sp->trap;
 410
 411         devlink_trap_policers_unregister(devlink, trap->policers_arr,
 412                                          trap->policers_count);
 413         WARN_ON(!list_empty(&trap->policer_item_list));
 414         kfree(trap->policers_arr);
 415 }
 416
 417 int mlxsw_sp_devlink_traps_init(struct mlxsw_sp *mlxsw_sp)
 418 {
 419         size_t groups_count = ARRAY_SIZE(mlxsw_sp_trap_groups_arr);
 420         struct devlink *devlink = priv_to_devlink(mlxsw_sp->core);
 421         int err;
 422
 423         err = mlxsw_sp_trap_cpu_policers_set(mlxsw_sp);
 424         if (err)
 425                 return err;
 426
 427         err = mlxsw_sp_trap_dummy_group_init(mlxsw_sp);
 428         if (err)
 429                 return err;
 430
 431         if (WARN_ON(ARRAY_SIZE(mlxsw_sp_listener_devlink_map) !=
 432                     ARRAY_SIZE(mlxsw_sp_listeners_arr)))
 433                 return -EINVAL;
 434
 435         err = mlxsw_sp_trap_policers_init(mlxsw_sp);
 436         if (err)
 437                 return err;
 438
 439         err = devlink_trap_groups_register(devlink, mlxsw_sp_trap_groups_arr,
 440                                            groups_count);
 441         if (err)
 442                 goto err_trap_groups_register;
 443
 444         err = devlink_traps_register(devlink, mlxsw_sp_traps_arr,
 445                                      ARRAY_SIZE(mlxsw_sp_traps_arr), mlxsw_sp);
 446         if (err)
 447                 goto err_traps_register;
 448
 449         return 0;
 450
 451 err_traps_register:
 452         devlink_trap_groups_unregister(devlink, mlxsw_sp_trap_groups_arr,
 453                                        groups_count);
 454 err_trap_groups_register:
 455         mlxsw_sp_trap_policers_fini(mlxsw_sp);
 456         return err;
 457 }
 458
 459 void mlxsw_sp_devlink_traps_fini(struct mlxsw_sp *mlxsw_sp)
 460 {
 461         size_t groups_count = ARRAY_SIZE(mlxsw_sp_trap_groups_arr);
 462         struct devlink *devlink = priv_to_devlink(mlxsw_sp->core);
 463
 464         devlink_traps_unregister(devlink, mlxsw_sp_traps_arr,
 465                                  ARRAY_SIZE(mlxsw_sp_traps_arr));
 466         devlink_trap_groups_unregister(devlink, mlxsw_sp_trap_groups_arr,
 467                                        groups_count);
 468         mlxsw_sp_trap_policers_fini(mlxsw_sp);
 469 }
 470
 471 int mlxsw_sp_trap_init(struct mlxsw_core *mlxsw_core,
 472                        const struct devlink_trap *trap, void *trap_ctx)
 473 {
 474         int i;
 475
 476         for (i = 0; i < ARRAY_SIZE(mlxsw_sp_listener_devlink_map); i++) {
 477                 const struct mlxsw_listener *listener;
 478                 int err;
 479
 480                 if (mlxsw_sp_listener_devlink_map[i] != trap->id)
 481                         continue;
 482                 listener = &mlxsw_sp_listeners_arr[i];
 483
 484                 err = mlxsw_core_trap_register(mlxsw_core, listener, trap_ctx);
 485                 if (err)
 486                         return err;
 487         }
 488
 489         return 0;
 490 }
 491
 492 void mlxsw_sp_trap_fini(struct mlxsw_core *mlxsw_core,
 493                         const struct devlink_trap *trap, void *trap_ctx)
 494 {
 495         int i;
 496
 497         for (i = 0; i < ARRAY_SIZE(mlxsw_sp_listener_devlink_map); i++) {
 498                 const struct mlxsw_listener *listener;
 499
 500                 if (mlxsw_sp_listener_devlink_map[i] != trap->id)
 501                         continue;
 502                 listener = &mlxsw_sp_listeners_arr[i];
 503
 504                 mlxsw_core_trap_unregister(mlxsw_core, listener, trap_ctx);
 505         }
 506 }
 507
 508 int mlxsw_sp_trap_action_set(struct mlxsw_core *mlxsw_core,
 509                              const struct devlink_trap *trap,
 510                              enum devlink_trap_action action)
 511 {
 512         int i;
 513
 514         for (i = 0; i < ARRAY_SIZE(mlxsw_sp_listener_devlink_map); i++) {
 515                 const struct mlxsw_listener *listener;
 516                 bool enabled;
 517                 int err;
 518
 519                 if (mlxsw_sp_listener_devlink_map[i] != trap->id)
 520                         continue;
 521                 listener = &mlxsw_sp_listeners_arr[i];
 522                 switch (action) {
 523                 case DEVLINK_TRAP_ACTION_DROP:
 524                         enabled = false;
 525                         break;
 526                 case DEVLINK_TRAP_ACTION_TRAP:
 527                         enabled = true;
 528                         break;
 529                 default:
 530                         return -EINVAL;
 531                 }
 532                 err = mlxsw_core_trap_state_set(mlxsw_core, listener, enabled);
 533                 if (err)
 534                         return err;
 535         }
 536
 537         return 0;
 538 }
 539
 540 int mlxsw_sp_trap_group_init(struct mlxsw_core *mlxsw_core,
 541                              const struct devlink_trap_group *group)
 542 {
 543         struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
 544         u16 hw_policer_id = MLXSW_REG_HTGT_INVALID_POLICER;
 545         char htgt_pl[MLXSW_REG_HTGT_LEN];
 546         u8 priority, tc, group_id;
 547
 548         switch (group->id) {
 549         case DEVLINK_TRAP_GROUP_GENERIC_ID_L2_DROPS:
 550                 group_id = MLXSW_REG_HTGT_TRAP_GROUP_SP_L2_DISCARDS;
 551                 priority = 0;
 552                 tc = 1;
 553                 break;
 554         case DEVLINK_TRAP_GROUP_GENERIC_ID_L3_DROPS:
 555                 group_id = MLXSW_REG_HTGT_TRAP_GROUP_SP_L3_DISCARDS;
 556                 priority = 0;
 557                 tc = 1;
 558                 break;
 559         case DEVLINK_TRAP_GROUP_GENERIC_ID_TUNNEL_DROPS:
 560                 group_id = MLXSW_REG_HTGT_TRAP_GROUP_SP_TUNNEL_DISCARDS;
 561                 priority = 0;
 562                 tc = 1;
 563                 break;
 564         case DEVLINK_TRAP_GROUP_GENERIC_ID_ACL_DROPS:
 565                 group_id = MLXSW_REG_HTGT_TRAP_GROUP_SP_ACL_DISCARDS;
 566                 priority = 0;
 567                 tc = 1;
 568                 break;
 569         default:
 570                 return -EINVAL;
 571         }
 572
 573         if (group->init_policer_id) {
 574                 struct mlxsw_sp_trap_policer_item *policer_item;
 575                 u32 id = group->init_policer_id;
 576
 577                 policer_item = mlxsw_sp_trap_policer_item_lookup(mlxsw_sp, id);
 578                 if (WARN_ON(!policer_item))
 579                         return -EINVAL;
 580                 hw_policer_id = policer_item->hw_id;
 581         }
 582
 583         mlxsw_reg_htgt_pack(htgt_pl, group_id, hw_policer_id, priority, tc);
 584         return mlxsw_reg_write(mlxsw_core, MLXSW_REG(htgt), htgt_pl);
 585 }
 586
 587 static struct mlxsw_sp_trap_policer_item *
 588 mlxsw_sp_trap_policer_item_init(struct mlxsw_sp *mlxsw_sp, u32 id)
 589 {
 590         struct mlxsw_sp_trap_policer_item *policer_item;
 591         struct mlxsw_sp_trap *trap = mlxsw_sp->trap;
 592         u16 hw_id;
 593
 594         /* We should be able to allocate a policer because the number of
 595          * policers we registered with devlink is in according with the number
 596          * of available policers.
 597          */
 598         hw_id = find_first_zero_bit(trap->policers_usage, trap->max_policers);
 599         if (WARN_ON(hw_id == trap->max_policers))
 600                 return ERR_PTR(-ENOBUFS);
 601
 602         policer_item = kzalloc(sizeof(*policer_item), GFP_KERNEL);
 603         if (!policer_item)
 604                 return ERR_PTR(-ENOMEM);
 605
 606         __set_bit(hw_id, trap->policers_usage);
 607         policer_item->hw_id = hw_id;
 608         policer_item->id = id;
 609         list_add_tail(&policer_item->list, &trap->policer_item_list);
 610
 611         return policer_item;
 612 }
 613
 614 static void
 615 mlxsw_sp_trap_policer_item_fini(struct mlxsw_sp *mlxsw_sp,
 616                                 struct mlxsw_sp_trap_policer_item *policer_item)
 617 {
 618         list_del(&policer_item->list);
 619         __clear_bit(policer_item->hw_id, mlxsw_sp->trap->policers_usage);
 620         kfree(policer_item);
 621 }
 622
 623 static int mlxsw_sp_trap_policer_bs(u64 burst, u8 *p_burst_size,
 624                                     struct netlink_ext_ack *extack)
 625 {
 626         int bs = fls64(burst) - 1;
 627
 628         if (burst != (1 << bs)) {
 629                 NL_SET_ERR_MSG_MOD(extack, "Policer burst size is not power of two");
 630                 return -EINVAL;
 631         }
 632
 633         *p_burst_size = bs;
 634
 635         return 0;
 636 }
 637
 638 static int __mlxsw_sp_trap_policer_set(struct mlxsw_sp *mlxsw_sp, u16 hw_id,
 639                                        u64 rate, u64 burst, bool clear_counter,
 640                                        struct netlink_ext_ack *extack)
 641 {
 642         char qpcr_pl[MLXSW_REG_QPCR_LEN];
 643         u8 burst_size;
 644         int err;
 645
 646         err = mlxsw_sp_trap_policer_bs(burst, &burst_size, extack);
 647         if (err)
 648                 return err;
 649
 650         mlxsw_reg_qpcr_pack(qpcr_pl, hw_id, MLXSW_REG_QPCR_IR_UNITS_M, false,
 651                             rate, burst_size);
 652         mlxsw_reg_qpcr_clear_counter_set(qpcr_pl, clear_counter);
 653         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpcr), qpcr_pl);
 654 }
 655
 656 int mlxsw_sp_trap_policer_init(struct mlxsw_core *mlxsw_core,
 657                                const struct devlink_trap_policer *policer)
 658 {
 659         struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
 660         struct mlxsw_sp_trap_policer_item *policer_item;
 661         int err;
 662
 663         policer_item = mlxsw_sp_trap_policer_item_init(mlxsw_sp, policer->id);
 664         if (IS_ERR(policer_item))
 665                 return PTR_ERR(policer_item);
 666
 667         err = __mlxsw_sp_trap_policer_set(mlxsw_sp, policer_item->hw_id,
 668                                           policer->init_rate,
 669                                           policer->init_burst, true, NULL);
 670         if (err)
 671                 goto err_trap_policer_set;
 672
 673         return 0;
 674
 675 err_trap_policer_set:
 676         mlxsw_sp_trap_policer_item_fini(mlxsw_sp, policer_item);
 677         return err;
 678 }
 679
 680 void mlxsw_sp_trap_policer_fini(struct mlxsw_core *mlxsw_core,
 681                                 const struct devlink_trap_policer *policer)
 682 {
 683         struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
 684         struct mlxsw_sp_trap_policer_item *policer_item;
 685
 686         policer_item = mlxsw_sp_trap_policer_item_lookup(mlxsw_sp, policer->id);
 687         if (WARN_ON(!policer_item))
 688                 return;
 689
 690         mlxsw_sp_trap_policer_item_fini(mlxsw_sp, policer_item);
 691 }
 692
 693 int mlxsw_sp_trap_policer_set(struct mlxsw_core *mlxsw_core,
 694                               const struct devlink_trap_policer *policer,
 695                               u64 rate, u64 burst,
 696                               struct netlink_ext_ack *extack)
 697 {
 698         struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
 699         struct mlxsw_sp_trap_policer_item *policer_item;
 700
 701         policer_item = mlxsw_sp_trap_policer_item_lookup(mlxsw_sp, policer->id);
 702         if (WARN_ON(!policer_item))
 703                 return -EINVAL;
 704
 705         return __mlxsw_sp_trap_policer_set(mlxsw_sp, policer_item->hw_id,
 706                                            rate, burst, false, extack);
 707 }
 708
 709 int
 710 mlxsw_sp_trap_policer_counter_get(struct mlxsw_core *mlxsw_core,
 711                                   const struct devlink_trap_policer *policer,
 712                                   u64 *p_drops)
 713 {
 714         struct mlxsw_sp *mlxsw_sp = mlxsw_core_driver_priv(mlxsw_core);
 715         struct mlxsw_sp_trap_policer_item *policer_item;
 716         char qpcr_pl[MLXSW_REG_QPCR_LEN];
 717         int err;
 718
 719         policer_item = mlxsw_sp_trap_policer_item_lookup(mlxsw_sp, policer->id);
 720         if (WARN_ON(!policer_item))
 721                 return -EINVAL;
 722
 723         mlxsw_reg_qpcr_pack(qpcr_pl, policer_item->hw_id,
 724                             MLXSW_REG_QPCR_IR_UNITS_M, false, 0, 0);
 725         err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(qpcr), qpcr_pl);
 726         if (err)
 727                 return err;
 728
 729         *p_drops = mlxsw_reg_qpcr_violate_count_get(qpcr_pl);
 730
 731         return 0;
 732 }