drivers/net/ethernet/marvell/prestera/prestera_devlink.c

   1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
   2 /* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
   3
   4 #include <net/devlink.h>
   5
   6 #include "prestera_devlink.h"
   7 #include "prestera_hw.h"
   8
   9 /* All driver-specific traps must be documented in
  10  * Documentation/networking/devlink/prestera.rst
  11  */
  12 enum {
  13         DEVLINK_PRESTERA_TRAP_ID_BASE = DEVLINK_TRAP_GENERIC_ID_MAX,
  14         DEVLINK_PRESTERA_TRAP_ID_ARP_BC,
  15         DEVLINK_PRESTERA_TRAP_ID_IS_IS,
  16         DEVLINK_PRESTERA_TRAP_ID_OSPF,
  17         DEVLINK_PRESTERA_TRAP_ID_IP_BC_MAC,
  18         DEVLINK_PRESTERA_TRAP_ID_ROUTER_MC,
  19         DEVLINK_PRESTERA_TRAP_ID_VRRP,
  20         DEVLINK_PRESTERA_TRAP_ID_DHCP,
  21         DEVLINK_PRESTERA_TRAP_ID_MAC_TO_ME,
  22         DEVLINK_PRESTERA_TRAP_ID_IPV4_OPTIONS,
  23         DEVLINK_PRESTERA_TRAP_ID_IP_DEFAULT_ROUTE,
  24         DEVLINK_PRESTERA_TRAP_ID_IP_TO_ME,
  25         DEVLINK_PRESTERA_TRAP_ID_IPV4_ICMP_REDIRECT,
  26         DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_0,
  27         DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_1,
  28         DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_2,
  29         DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_3,
  30         DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_4,
  31         DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_5,
  32         DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_6,
  33         DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_7,
  34         DEVLINK_PRESTERA_TRAP_ID_BGP,
  35         DEVLINK_PRESTERA_TRAP_ID_SSH,
  36         DEVLINK_PRESTERA_TRAP_ID_TELNET,
  37         DEVLINK_PRESTERA_TRAP_ID_ICMP,
  38         DEVLINK_PRESTERA_TRAP_ID_MET_RED,
  39         DEVLINK_PRESTERA_TRAP_ID_IP_SIP_IS_ZERO,
  40         DEVLINK_PRESTERA_TRAP_ID_IP_UC_DIP_DA_MISMATCH,
  41         DEVLINK_PRESTERA_TRAP_ID_ILLEGAL_IPV4_HDR,
  42         DEVLINK_PRESTERA_TRAP_ID_ILLEGAL_IP_ADDR,
  43         DEVLINK_PRESTERA_TRAP_ID_INVALID_SA,
  44         DEVLINK_PRESTERA_TRAP_ID_LOCAL_PORT,
  45         DEVLINK_PRESTERA_TRAP_ID_PORT_NO_VLAN,
  46         DEVLINK_PRESTERA_TRAP_ID_RXDMA_DROP,
  47 };
  48
  49 #define DEVLINK_PRESTERA_TRAP_NAME_ARP_BC \
  50         "arp_bc"
  51 #define DEVLINK_PRESTERA_TRAP_NAME_IS_IS \
  52         "is_is"
  53 #define DEVLINK_PRESTERA_TRAP_NAME_OSPF \
  54         "ospf"
  55 #define DEVLINK_PRESTERA_TRAP_NAME_IP_BC_MAC \
  56         "ip_bc_mac"
  57 #define DEVLINK_PRESTERA_TRAP_NAME_ROUTER_MC \
  58         "router_mc"
  59 #define DEVLINK_PRESTERA_TRAP_NAME_VRRP \
  60         "vrrp"
  61 #define DEVLINK_PRESTERA_TRAP_NAME_DHCP \
  62         "dhcp"
  63 #define DEVLINK_PRESTERA_TRAP_NAME_MAC_TO_ME \
  64         "mac_to_me"
  65 #define DEVLINK_PRESTERA_TRAP_NAME_IPV4_OPTIONS \
  66         "ipv4_options"
  67 #define DEVLINK_PRESTERA_TRAP_NAME_IP_DEFAULT_ROUTE \
  68         "ip_default_route"
  69 #define DEVLINK_PRESTERA_TRAP_NAME_IP_TO_ME \
  70         "ip_to_me"
  71 #define DEVLINK_PRESTERA_TRAP_NAME_IPV4_ICMP_REDIRECT \
  72         "ipv4_icmp_redirect"
  73 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_0 \
  74         "acl_code_0"
  75 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_1 \
  76         "acl_code_1"
  77 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_2 \
  78         "acl_code_2"
  79 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_3 \
  80         "acl_code_3"
  81 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_4 \
  82         "acl_code_4"
  83 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_5 \
  84         "acl_code_5"
  85 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_6 \
  86         "acl_code_6"
  87 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_7 \
  88         "acl_code_7"
  89 #define DEVLINK_PRESTERA_TRAP_NAME_BGP \
  90         "bgp"
  91 #define DEVLINK_PRESTERA_TRAP_NAME_SSH \
  92         "ssh"
  93 #define DEVLINK_PRESTERA_TRAP_NAME_TELNET \
  94         "telnet"
  95 #define DEVLINK_PRESTERA_TRAP_NAME_ICMP \
  96         "icmp"
  97 #define DEVLINK_PRESTERA_TRAP_NAME_RXDMA_DROP \
  98         "rxdma_drop"
  99 #define DEVLINK_PRESTERA_TRAP_NAME_PORT_NO_VLAN \
 100         "port_no_vlan"
 101 #define DEVLINK_PRESTERA_TRAP_NAME_LOCAL_PORT \
 102         "local_port"
 103 #define DEVLINK_PRESTERA_TRAP_NAME_INVALID_SA \
 104         "invalid_sa"
 105 #define DEVLINK_PRESTERA_TRAP_NAME_ILLEGAL_IP_ADDR \
 106         "illegal_ip_addr"
 107 #define DEVLINK_PRESTERA_TRAP_NAME_ILLEGAL_IPV4_HDR \
 108         "illegal_ipv4_hdr"
 109 #define DEVLINK_PRESTERA_TRAP_NAME_IP_UC_DIP_DA_MISMATCH \
 110         "ip_uc_dip_da_mismatch"
 111 #define DEVLINK_PRESTERA_TRAP_NAME_IP_SIP_IS_ZERO \
 112         "ip_sip_is_zero"
 113 #define DEVLINK_PRESTERA_TRAP_NAME_MET_RED \
 114         "met_red"
 115
 116 struct prestera_trap {
 117         struct devlink_trap trap;
 118         u8 cpu_code;
 119 };
 120
 121 struct prestera_trap_item {
 122         enum devlink_trap_action action;
 123         void *trap_ctx;
 124 };
 125
 126 struct prestera_trap_data {
 127         struct prestera_switch *sw;
 128         struct prestera_trap_item *trap_items_arr;
 129         u32 traps_count;
 130 };
 131
 132 #define PRESTERA_TRAP_METADATA DEVLINK_TRAP_METADATA_TYPE_F_IN_PORT
 133
 134 #define PRESTERA_TRAP_CONTROL(_id, _group_id, _action)                        \
 135         DEVLINK_TRAP_GENERIC(CONTROL, _action, _id,                           \
 136                              DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,       \
 137                              PRESTERA_TRAP_METADATA)
 138
 139 #define PRESTERA_TRAP_DRIVER_CONTROL(_id, _group_id)                          \
 140         DEVLINK_TRAP_DRIVER(CONTROL, TRAP, DEVLINK_PRESTERA_TRAP_ID_##_id,    \
 141                             DEVLINK_PRESTERA_TRAP_NAME_##_id,                 \
 142                             DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,        \
 143                             PRESTERA_TRAP_METADATA)
 144
 145 #define PRESTERA_TRAP_EXCEPTION(_id, _group_id)                               \
 146         DEVLINK_TRAP_GENERIC(EXCEPTION, TRAP, _id,                            \
 147                              DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,       \
 148                              PRESTERA_TRAP_METADATA)
 149
 150 #define PRESTERA_TRAP_DRIVER_EXCEPTION(_id, _group_id)                        \
 151         DEVLINK_TRAP_DRIVER(EXCEPTION, TRAP, DEVLINK_PRESTERA_TRAP_ID_##_id,  \
 152                             DEVLINK_PRESTERA_TRAP_NAME_##_id,                 \
 153                             DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,        \
 154                             PRESTERA_TRAP_METADATA)
 155
 156 #define PRESTERA_TRAP_DRIVER_DROP(_id, _group_id)                             \
 157         DEVLINK_TRAP_DRIVER(DROP, DROP, DEVLINK_PRESTERA_TRAP_ID_##_id,       \
 158                             DEVLINK_PRESTERA_TRAP_NAME_##_id,                 \
 159                             DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,        \
 160                             PRESTERA_TRAP_METADATA)
 161
 162 static const struct devlink_trap_group prestera_trap_groups_arr[] = {
 163         /* No policer is associated with following groups (policerid == 0)*/
 164         DEVLINK_TRAP_GROUP_GENERIC(L2_DROPS, 0),
 165         DEVLINK_TRAP_GROUP_GENERIC(L3_DROPS, 0),
 166         DEVLINK_TRAP_GROUP_GENERIC(L3_EXCEPTIONS, 0),
 167         DEVLINK_TRAP_GROUP_GENERIC(NEIGH_DISCOVERY, 0),
 168         DEVLINK_TRAP_GROUP_GENERIC(ACL_TRAP, 0),
 169         DEVLINK_TRAP_GROUP_GENERIC(ACL_DROPS, 0),
 170         DEVLINK_TRAP_GROUP_GENERIC(ACL_SAMPLE, 0),
 171         DEVLINK_TRAP_GROUP_GENERIC(OSPF, 0),
 172         DEVLINK_TRAP_GROUP_GENERIC(STP, 0),
 173         DEVLINK_TRAP_GROUP_GENERIC(LACP, 0),
 174         DEVLINK_TRAP_GROUP_GENERIC(LLDP, 0),
 175         DEVLINK_TRAP_GROUP_GENERIC(VRRP, 0),
 176         DEVLINK_TRAP_GROUP_GENERIC(DHCP, 0),
 177         DEVLINK_TRAP_GROUP_GENERIC(BGP, 0),
 178         DEVLINK_TRAP_GROUP_GENERIC(LOCAL_DELIVERY, 0),
 179         DEVLINK_TRAP_GROUP_GENERIC(BUFFER_DROPS, 0),
 180 };
 181
 182 /* Initialize trap list, as well as associate CPU code with them. */
 183 static struct prestera_trap prestera_trap_items_arr[] = {
 184         {
 185                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ARP_BC, NEIGH_DISCOVERY),
 186                 .cpu_code = 5,
 187         },
 188         {
 189                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(IS_IS, LOCAL_DELIVERY),
 190                 .cpu_code = 13,
 191         },
 192         {
 193                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(OSPF, OSPF),
 194                 .cpu_code = 16,
 195         },
 196         {
 197                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(IP_BC_MAC, LOCAL_DELIVERY),
 198                 .cpu_code = 19,
 199         },
 200         {
 201                 .trap = PRESTERA_TRAP_CONTROL(STP, STP, TRAP),
 202                 .cpu_code = 26,
 203         },
 204         {
 205                 .trap = PRESTERA_TRAP_CONTROL(LACP, LACP, TRAP),
 206                 .cpu_code = 27,
 207         },
 208         {
 209                 .trap = PRESTERA_TRAP_CONTROL(LLDP, LLDP, TRAP),
 210                 .cpu_code = 28,
 211         },
 212         {
 213                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ROUTER_MC, LOCAL_DELIVERY),
 214                 .cpu_code = 29,
 215         },
 216         {
 217                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(VRRP, VRRP),
 218                 .cpu_code = 30,
 219         },
 220         {
 221                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(DHCP, DHCP),
 222                 .cpu_code = 33,
 223         },
 224         {
 225                 .trap = PRESTERA_TRAP_EXCEPTION(MTU_ERROR, L3_EXCEPTIONS),
 226                 .cpu_code = 63,
 227         },
 228         {
 229                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(MAC_TO_ME, LOCAL_DELIVERY),
 230                 .cpu_code = 65,
 231         },
 232         {
 233                 .trap = PRESTERA_TRAP_EXCEPTION(TTL_ERROR, L3_EXCEPTIONS),
 234                 .cpu_code = 133,
 235         },
 236         {
 237                 .trap = PRESTERA_TRAP_DRIVER_EXCEPTION(IPV4_OPTIONS,
 238                                                        L3_EXCEPTIONS),
 239                 .cpu_code = 141,
 240         },
 241         {
 242                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(IP_DEFAULT_ROUTE,
 243                                                      LOCAL_DELIVERY),
 244                 .cpu_code = 160,
 245         },
 246         {
 247                 .trap = PRESTERA_TRAP_CONTROL(LOCAL_ROUTE, LOCAL_DELIVERY,
 248                                               TRAP),
 249                 .cpu_code = 161,
 250         },
 251         {
 252                 .trap = PRESTERA_TRAP_DRIVER_EXCEPTION(IPV4_ICMP_REDIRECT,
 253                                                        L3_EXCEPTIONS),
 254                 .cpu_code = 180,
 255         },
 256         {
 257                 .trap = PRESTERA_TRAP_CONTROL(ARP_RESPONSE, NEIGH_DISCOVERY,
 258                                               TRAP),
 259                 .cpu_code = 188,
 260         },
 261         {
 262                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_0, ACL_TRAP),
 263                 .cpu_code = 192,
 264         },
 265         {
 266                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_1, ACL_TRAP),
 267                 .cpu_code = 193,
 268         },
 269         {
 270                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_2, ACL_TRAP),
 271                 .cpu_code = 194,
 272         },
 273         {
 274                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_3, ACL_TRAP),
 275                 .cpu_code = 195,
 276         },
 277         {
 278                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_4, ACL_TRAP),
 279                 .cpu_code = 196,
 280         },
 281         {
 282                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_5, ACL_TRAP),
 283                 .cpu_code = 197,
 284         },
 285         {
 286                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_6, ACL_TRAP),
 287                 .cpu_code = 198,
 288         },
 289         {
 290                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_7, ACL_TRAP),
 291                 .cpu_code = 199,
 292         },
 293         {
 294                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(BGP, BGP),
 295                 .cpu_code = 206,
 296         },
 297         {
 298                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(SSH, LOCAL_DELIVERY),
 299                 .cpu_code = 207,
 300         },
 301         {
 302                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(TELNET, LOCAL_DELIVERY),
 303                 .cpu_code = 208,
 304         },
 305         {
 306                 .trap = PRESTERA_TRAP_DRIVER_CONTROL(ICMP, LOCAL_DELIVERY),
 307                 .cpu_code = 209,
 308         },
 309         {
 310                 .trap = PRESTERA_TRAP_DRIVER_DROP(RXDMA_DROP, BUFFER_DROPS),
 311                 .cpu_code = 37,
 312         },
 313         {
 314                 .trap = PRESTERA_TRAP_DRIVER_DROP(PORT_NO_VLAN, L2_DROPS),
 315                 .cpu_code = 39,
 316         },
 317         {
 318                 .trap = PRESTERA_TRAP_DRIVER_DROP(LOCAL_PORT, L2_DROPS),
 319                 .cpu_code = 56,
 320         },
 321         {
 322                 .trap = PRESTERA_TRAP_DRIVER_DROP(INVALID_SA, L2_DROPS),
 323                 .cpu_code = 60,
 324         },
 325         {
 326                 .trap = PRESTERA_TRAP_DRIVER_DROP(ILLEGAL_IP_ADDR, L3_DROPS),
 327                 .cpu_code = 136,
 328         },
 329         {
 330                 .trap = PRESTERA_TRAP_DRIVER_DROP(ILLEGAL_IPV4_HDR, L3_DROPS),
 331                 .cpu_code = 137,
 332         },
 333         {
 334                 .trap = PRESTERA_TRAP_DRIVER_DROP(IP_UC_DIP_DA_MISMATCH,
 335                                                   L3_DROPS),
 336                 .cpu_code = 138,
 337         },
 338         {
 339                 .trap = PRESTERA_TRAP_DRIVER_DROP(IP_SIP_IS_ZERO, L3_DROPS),
 340                 .cpu_code = 145,
 341         },
 342         {
 343                 .trap = PRESTERA_TRAP_DRIVER_DROP(MET_RED, BUFFER_DROPS),
 344                 .cpu_code = 185,
 345         },
 346 };
 347
 348 static int prestera_drop_counter_get(struct devlink *devlink,
 349                                      const struct devlink_trap *trap,
 350                                      u64 *p_drops);
 351
 352 static int prestera_dl_info_get(struct devlink *dl,
 353                                 struct devlink_info_req *req,
 354                                 struct netlink_ext_ack *extack)
 355 {
 356         struct prestera_switch *sw = devlink_priv(dl);
 357         char buf[16];
 358         int err;
 359
 360         err = devlink_info_driver_name_put(req, PRESTERA_DRV_NAME);
 361         if (err)
 362                 return err;
 363
 364         snprintf(buf, sizeof(buf), "%d.%d.%d",
 365                  sw->dev->fw_rev.maj,
 366                  sw->dev->fw_rev.min,
 367                  sw->dev->fw_rev.sub);
 368
 369         return devlink_info_version_running_put(req,
 370                                                DEVLINK_INFO_VERSION_GENERIC_FW,
 371                                                buf);
 372 }
 373
 374 static int prestera_trap_init(struct devlink *devlink,
 375                               const struct devlink_trap *trap, void *trap_ctx);
 376
 377 static int prestera_trap_action_set(struct devlink *devlink,
 378                                     const struct devlink_trap *trap,
 379                                     enum devlink_trap_action action,
 380                                     struct netlink_ext_ack *extack);
 381
 382 static const struct devlink_ops prestera_dl_ops = {
 383         .info_get = prestera_dl_info_get,
 384         .trap_init = prestera_trap_init,
 385         .trap_action_set = prestera_trap_action_set,
 386         .trap_drop_counter_get = prestera_drop_counter_get,
 387 };
 388
 389 struct prestera_switch *prestera_devlink_alloc(struct prestera_device *dev)
 390 {
 391         struct devlink *dl;
 392
 393         dl = devlink_alloc(&prestera_dl_ops, sizeof(struct prestera_switch),
 394                            dev->dev);
 395
 396         return devlink_priv(dl);
 397 }
 398
 399 void prestera_devlink_free(struct prestera_switch *sw)
 400 {
 401         struct devlink *dl = priv_to_devlink(sw);
 402
 403         devlink_free(dl);
 404 }
 405
 406 void prestera_devlink_register(struct prestera_switch *sw)
 407 {
 408         struct devlink *dl = priv_to_devlink(sw);
 409
 410         devlink_register(dl);
 411 }
 412
 413 void prestera_devlink_unregister(struct prestera_switch *sw)
 414 {
 415         struct devlink *dl = priv_to_devlink(sw);
 416
 417         devlink_unregister(dl);
 418 }
 419
 420 int prestera_devlink_port_register(struct prestera_port *port)
 421 {
 422         struct prestera_switch *sw = port->sw;
 423         struct devlink *dl = priv_to_devlink(sw);
 424         struct devlink_port_attrs attrs = {};
 425         int err;
 426
 427         attrs.flavour = DEVLINK_PORT_FLAVOUR_PHYSICAL;
 428         attrs.phys.port_number = port->fp_id;
 429         attrs.switch_id.id_len = sizeof(sw->id);
 430         memcpy(attrs.switch_id.id, &sw->id, attrs.switch_id.id_len);
 431
 432         devlink_port_attrs_set(&port->dl_port, &attrs);
 433
 434         err = devlink_port_register(dl, &port->dl_port, port->fp_id);
 435         if (err) {
 436                 dev_err(prestera_dev(sw), "devlink_port_register failed: %d\n", err);
 437                 return err;
 438         }
 439
 440         return 0;
 441 }
 442
 443 void prestera_devlink_port_unregister(struct prestera_port *port)
 444 {
 445         devlink_port_unregister(&port->dl_port);
 446 }
 447
 448 void prestera_devlink_port_set(struct prestera_port *port)
 449 {
 450         devlink_port_type_eth_set(&port->dl_port, port->dev);
 451 }
 452
 453 void prestera_devlink_port_clear(struct prestera_port *port)
 454 {
 455         devlink_port_type_clear(&port->dl_port);
 456 }
 457
 458 struct devlink_port *prestera_devlink_get_port(struct net_device *dev)
 459 {
 460         struct prestera_port *port = netdev_priv(dev);
 461
 462         return &port->dl_port;
 463 }
 464
 465 int prestera_devlink_traps_register(struct prestera_switch *sw)
 466 {
 467         const u32 groups_count = ARRAY_SIZE(prestera_trap_groups_arr);
 468         const u32 traps_count = ARRAY_SIZE(prestera_trap_items_arr);
 469         struct devlink *devlink = priv_to_devlink(sw);
 470         struct prestera_trap_data *trap_data;
 471         struct prestera_trap *prestera_trap;
 472         int err, i;
 473
 474         trap_data = kzalloc(sizeof(*trap_data), GFP_KERNEL);
 475         if (!trap_data)
 476                 return -ENOMEM;
 477
 478         trap_data->trap_items_arr = kcalloc(traps_count,
 479                                             sizeof(struct prestera_trap_item),
 480                                             GFP_KERNEL);
 481         if (!trap_data->trap_items_arr) {
 482                 err = -ENOMEM;
 483                 goto err_trap_items_alloc;
 484         }
 485
 486         trap_data->sw = sw;
 487         trap_data->traps_count = traps_count;
 488         sw->trap_data = trap_data;
 489
 490         err = devlink_trap_groups_register(devlink, prestera_trap_groups_arr,
 491                                            groups_count);
 492         if (err)
 493                 goto err_groups_register;
 494
 495         for (i = 0; i < traps_count; i++) {
 496                 prestera_trap = &prestera_trap_items_arr[i];
 497                 err = devlink_traps_register(devlink, &prestera_trap->trap, 1,
 498                                              sw);
 499                 if (err)
 500                         goto err_trap_register;
 501         }
 502
 503         return 0;
 504
 505 err_trap_register:
 506         for (i--; i >= 0; i--) {
 507                 prestera_trap = &prestera_trap_items_arr[i];
 508                 devlink_traps_unregister(devlink, &prestera_trap->trap, 1);
 509         }
 510         devlink_trap_groups_unregister(devlink, prestera_trap_groups_arr,
 511                                        groups_count);
 512 err_groups_register:
 513         kfree(trap_data->trap_items_arr);
 514 err_trap_items_alloc:
 515         kfree(trap_data);
 516         return err;
 517 }
 518
 519 static struct prestera_trap_item *
 520 prestera_get_trap_item_by_cpu_code(struct prestera_switch *sw, u8 cpu_code)
 521 {
 522         struct prestera_trap_data *trap_data = sw->trap_data;
 523         struct prestera_trap *prestera_trap;
 524         int i;
 525
 526         for (i = 0; i < trap_data->traps_count; i++) {
 527                 prestera_trap = &prestera_trap_items_arr[i];
 528                 if (cpu_code == prestera_trap->cpu_code)
 529                         return &trap_data->trap_items_arr[i];
 530         }
 531
 532         return NULL;
 533 }
 534
 535 void prestera_devlink_trap_report(struct prestera_port *port,
 536                                   struct sk_buff *skb, u8 cpu_code)
 537 {
 538         struct prestera_trap_item *trap_item;
 539         struct devlink *devlink;
 540
 541         devlink = port->dl_port.devlink;
 542
 543         trap_item = prestera_get_trap_item_by_cpu_code(port->sw, cpu_code);
 544         if (unlikely(!trap_item))
 545                 return;
 546
 547         devlink_trap_report(devlink, skb, trap_item->trap_ctx,
 548                             &port->dl_port, NULL);
 549 }
 550
 551 static struct prestera_trap_item *
 552 prestera_devlink_trap_item_lookup(struct prestera_switch *sw, u16 trap_id)
 553 {
 554         struct prestera_trap_data *trap_data = sw->trap_data;
 555         int i;
 556
 557         for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); i++) {
 558                 if (prestera_trap_items_arr[i].trap.id == trap_id)
 559                         return &trap_data->trap_items_arr[i];
 560         }
 561
 562         return NULL;
 563 }
 564
 565 static int prestera_trap_init(struct devlink *devlink,
 566                               const struct devlink_trap *trap, void *trap_ctx)
 567 {
 568         struct prestera_switch *sw = devlink_priv(devlink);
 569         struct prestera_trap_item *trap_item;
 570
 571         trap_item = prestera_devlink_trap_item_lookup(sw, trap->id);
 572         if (WARN_ON(!trap_item))
 573                 return -EINVAL;
 574
 575         trap_item->trap_ctx = trap_ctx;
 576         trap_item->action = trap->init_action;
 577
 578         return 0;
 579 }
 580
 581 static int prestera_trap_action_set(struct devlink *devlink,
 582                                     const struct devlink_trap *trap,
 583                                     enum devlink_trap_action action,
 584                                     struct netlink_ext_ack *extack)
 585 {
 586         /* Currently, driver does not support trap action altering */
 587         return -EOPNOTSUPP;
 588 }
 589
 590 static int prestera_drop_counter_get(struct devlink *devlink,
 591                                      const struct devlink_trap *trap,
 592                                      u64 *p_drops)
 593 {
 594         struct prestera_switch *sw = devlink_priv(devlink);
 595         enum prestera_hw_cpu_code_cnt_t cpu_code_type =
 596                 PRESTERA_HW_CPU_CODE_CNT_TYPE_DROP;
 597         struct prestera_trap *prestera_trap =
 598                 container_of(trap, struct prestera_trap, trap);
 599
 600         return prestera_hw_cpu_code_counters_get(sw, prestera_trap->cpu_code,
 601                                                  cpu_code_type, p_drops);
 602 }
 603
 604 void prestera_devlink_traps_unregister(struct prestera_switch *sw)
 605 {
 606         struct prestera_trap_data *trap_data = sw->trap_data;
 607         struct devlink *dl = priv_to_devlink(sw);
 608         const struct devlink_trap *trap;
 609         int i;
 610
 611         for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); ++i) {
 612                 trap = &prestera_trap_items_arr[i].trap;
 613                 devlink_traps_unregister(dl, trap, 1);
 614         }
 615
 616         devlink_trap_groups_unregister(dl, prestera_trap_groups_arr,
 617                                        ARRAY_SIZE(prestera_trap_groups_arr));
 618         kfree(trap_data->trap_items_arr);
 619         kfree(trap_data);
 620 }