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 void prestera_devlink_traps_fini(struct prestera_switch *sw);
 349
 350 static int prestera_drop_counter_get(struct devlink *devlink,
 351                                      const struct devlink_trap *trap,
 352                                      u64 *p_drops);
 353
 354 static int prestera_dl_info_get(struct devlink *dl,
 355                                 struct devlink_info_req *req,
 356                                 struct netlink_ext_ack *extack)
 357 {
 358         struct prestera_switch *sw = devlink_priv(dl);
 359         char buf[16];
 360         int err;
 361
 362         err = devlink_info_driver_name_put(req, PRESTERA_DRV_NAME);
 363         if (err)
 364                 return err;
 365
 366         snprintf(buf, sizeof(buf), "%d.%d.%d",
 367                  sw->dev->fw_rev.maj,
 368                  sw->dev->fw_rev.min,
 369                  sw->dev->fw_rev.sub);
 370
 371         return devlink_info_version_running_put(req,
 372                                                DEVLINK_INFO_VERSION_GENERIC_FW,
 373                                                buf);
 374 }
 375
 376 static int prestera_trap_init(struct devlink *devlink,
 377                               const struct devlink_trap *trap, void *trap_ctx);
 378
 379 static int prestera_trap_action_set(struct devlink *devlink,
 380                                     const struct devlink_trap *trap,
 381                                     enum devlink_trap_action action,
 382                                     struct netlink_ext_ack *extack);
 383
 384 static int prestera_devlink_traps_register(struct prestera_switch *sw);
 385
 386 static const struct devlink_ops prestera_dl_ops = {
 387         .info_get = prestera_dl_info_get,
 388         .trap_init = prestera_trap_init,
 389         .trap_action_set = prestera_trap_action_set,
 390         .trap_drop_counter_get = prestera_drop_counter_get,
 391 };
 392
 393 struct prestera_switch *prestera_devlink_alloc(void)
 394 {
 395         struct devlink *dl;
 396
 397         dl = devlink_alloc(&prestera_dl_ops, sizeof(struct prestera_switch));
 398
 399         return devlink_priv(dl);
 400 }
 401
 402 void prestera_devlink_free(struct prestera_switch *sw)
 403 {
 404         struct devlink *dl = priv_to_devlink(sw);
 405
 406         devlink_free(dl);
 407 }
 408
 409 int prestera_devlink_register(struct prestera_switch *sw)
 410 {
 411         struct devlink *dl = priv_to_devlink(sw);
 412         int err;
 413
 414         err = devlink_register(dl, sw->dev->dev);
 415         if (err) {
 416                 dev_err(prestera_dev(sw), "devlink_register failed: %d\n", err);
 417                 return err;
 418         }
 419
 420         err = prestera_devlink_traps_register(sw);
 421         if (err) {
 422                 devlink_unregister(dl);
 423                 dev_err(sw->dev->dev, "devlink_traps_register failed: %d\n",
 424                         err);
 425                 return err;
 426         }
 427
 428         return 0;
 429 }
 430
 431 void prestera_devlink_unregister(struct prestera_switch *sw)
 432 {
 433         struct prestera_trap_data *trap_data = sw->trap_data;
 434         struct devlink *dl = priv_to_devlink(sw);
 435
 436         prestera_devlink_traps_fini(sw);
 437         devlink_unregister(dl);
 438
 439         kfree(trap_data->trap_items_arr);
 440         kfree(trap_data);
 441 }
 442
 443 int prestera_devlink_port_register(struct prestera_port *port)
 444 {
 445         struct prestera_switch *sw = port->sw;
 446         struct devlink *dl = priv_to_devlink(sw);
 447         struct devlink_port_attrs attrs = {};
 448         int err;
 449
 450         attrs.flavour = DEVLINK_PORT_FLAVOUR_PHYSICAL;
 451         attrs.phys.port_number = port->fp_id;
 452         attrs.switch_id.id_len = sizeof(sw->id);
 453         memcpy(attrs.switch_id.id, &sw->id, attrs.switch_id.id_len);
 454
 455         devlink_port_attrs_set(&port->dl_port, &attrs);
 456
 457         err = devlink_port_register(dl, &port->dl_port, port->fp_id);
 458         if (err) {
 459                 dev_err(prestera_dev(sw), "devlink_port_register failed: %d\n", err);
 460                 return err;
 461         }
 462
 463         return 0;
 464 }
 465
 466 void prestera_devlink_port_unregister(struct prestera_port *port)
 467 {
 468         devlink_port_unregister(&port->dl_port);
 469 }
 470
 471 void prestera_devlink_port_set(struct prestera_port *port)
 472 {
 473         devlink_port_type_eth_set(&port->dl_port, port->dev);
 474 }
 475
 476 void prestera_devlink_port_clear(struct prestera_port *port)
 477 {
 478         devlink_port_type_clear(&port->dl_port);
 479 }
 480
 481 struct devlink_port *prestera_devlink_get_port(struct net_device *dev)
 482 {
 483         struct prestera_port *port = netdev_priv(dev);
 484
 485         return &port->dl_port;
 486 }
 487
 488 static int prestera_devlink_traps_register(struct prestera_switch *sw)
 489 {
 490         const u32 groups_count = ARRAY_SIZE(prestera_trap_groups_arr);
 491         const u32 traps_count = ARRAY_SIZE(prestera_trap_items_arr);
 492         struct devlink *devlink = priv_to_devlink(sw);
 493         struct prestera_trap_data *trap_data;
 494         struct prestera_trap *prestera_trap;
 495         int err, i;
 496
 497         trap_data = kzalloc(sizeof(*trap_data), GFP_KERNEL);
 498         if (!trap_data)
 499                 return -ENOMEM;
 500
 501         trap_data->trap_items_arr = kcalloc(traps_count,
 502                                             sizeof(struct prestera_trap_item),
 503                                             GFP_KERNEL);
 504         if (!trap_data->trap_items_arr) {
 505                 err = -ENOMEM;
 506                 goto err_trap_items_alloc;
 507         }
 508
 509         trap_data->sw = sw;
 510         trap_data->traps_count = traps_count;
 511         sw->trap_data = trap_data;
 512
 513         err = devlink_trap_groups_register(devlink, prestera_trap_groups_arr,
 514                                            groups_count);
 515         if (err)
 516                 goto err_groups_register;
 517
 518         for (i = 0; i < traps_count; i++) {
 519                 prestera_trap = &prestera_trap_items_arr[i];
 520                 err = devlink_traps_register(devlink, &prestera_trap->trap, 1,
 521                                              sw);
 522                 if (err)
 523                         goto err_trap_register;
 524         }
 525
 526         return 0;
 527
 528 err_trap_register:
 529         for (i--; i >= 0; i--) {
 530                 prestera_trap = &prestera_trap_items_arr[i];
 531                 devlink_traps_unregister(devlink, &prestera_trap->trap, 1);
 532         }
 533 err_groups_register:
 534         kfree(trap_data->trap_items_arr);
 535 err_trap_items_alloc:
 536         kfree(trap_data);
 537         return err;
 538 }
 539
 540 static struct prestera_trap_item *
 541 prestera_get_trap_item_by_cpu_code(struct prestera_switch *sw, u8 cpu_code)
 542 {
 543         struct prestera_trap_data *trap_data = sw->trap_data;
 544         struct prestera_trap *prestera_trap;
 545         int i;
 546
 547         for (i = 0; i < trap_data->traps_count; i++) {
 548                 prestera_trap = &prestera_trap_items_arr[i];
 549                 if (cpu_code == prestera_trap->cpu_code)
 550                         return &trap_data->trap_items_arr[i];
 551         }
 552
 553         return NULL;
 554 }
 555
 556 void prestera_devlink_trap_report(struct prestera_port *port,
 557                                   struct sk_buff *skb, u8 cpu_code)
 558 {
 559         struct prestera_trap_item *trap_item;
 560         struct devlink *devlink;
 561
 562         devlink = port->dl_port.devlink;
 563
 564         trap_item = prestera_get_trap_item_by_cpu_code(port->sw, cpu_code);
 565         if (unlikely(!trap_item))
 566                 return;
 567
 568         devlink_trap_report(devlink, skb, trap_item->trap_ctx,
 569                             &port->dl_port, NULL);
 570 }
 571
 572 static struct prestera_trap_item *
 573 prestera_devlink_trap_item_lookup(struct prestera_switch *sw, u16 trap_id)
 574 {
 575         struct prestera_trap_data *trap_data = sw->trap_data;
 576         int i;
 577
 578         for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); i++) {
 579                 if (prestera_trap_items_arr[i].trap.id == trap_id)
 580                         return &trap_data->trap_items_arr[i];
 581         }
 582
 583         return NULL;
 584 }
 585
 586 static int prestera_trap_init(struct devlink *devlink,
 587                               const struct devlink_trap *trap, void *trap_ctx)
 588 {
 589         struct prestera_switch *sw = devlink_priv(devlink);
 590         struct prestera_trap_item *trap_item;
 591
 592         trap_item = prestera_devlink_trap_item_lookup(sw, trap->id);
 593         if (WARN_ON(!trap_item))
 594                 return -EINVAL;
 595
 596         trap_item->trap_ctx = trap_ctx;
 597         trap_item->action = trap->init_action;
 598
 599         return 0;
 600 }
 601
 602 static int prestera_trap_action_set(struct devlink *devlink,
 603                                     const struct devlink_trap *trap,
 604                                     enum devlink_trap_action action,
 605                                     struct netlink_ext_ack *extack)
 606 {
 607         /* Currently, driver does not support trap action altering */
 608         return -EOPNOTSUPP;
 609 }
 610
 611 static int prestera_drop_counter_get(struct devlink *devlink,
 612                                      const struct devlink_trap *trap,
 613                                      u64 *p_drops)
 614 {
 615         struct prestera_switch *sw = devlink_priv(devlink);
 616         enum prestera_hw_cpu_code_cnt_t cpu_code_type =
 617                 PRESTERA_HW_CPU_CODE_CNT_TYPE_DROP;
 618         struct prestera_trap *prestera_trap =
 619                 container_of(trap, struct prestera_trap, trap);
 620
 621         return prestera_hw_cpu_code_counters_get(sw, prestera_trap->cpu_code,
 622                                                  cpu_code_type, p_drops);
 623 }
 624
 625 static void prestera_devlink_traps_fini(struct prestera_switch *sw)
 626 {
 627         struct devlink *dl = priv_to_devlink(sw);
 628         const struct devlink_trap *trap;
 629         int i;
 630
 631         for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); ++i) {
 632                 trap = &prestera_trap_items_arr[i].trap;
 633                 devlink_traps_unregister(dl, trap, 1);
 634         }
 635
 636         devlink_trap_groups_unregister(dl, prestera_trap_groups_arr,
 637                                        ARRAY_SIZE(prestera_trap_groups_arr));
 638 }