marvell/prestera/prestera_devlink.c

0001 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
0002 /* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
0003
0004 #include <net/devlink.h>
0005
0006 #include "prestera_devlink.h"
0007 #include "prestera_hw.h"
0008
0009 /* All driver-specific traps must be documented in
0010  * Documentation/networking/devlink/prestera.rst
0011  */
0012 enum {
0013     DEVLINK_PRESTERA_TRAP_ID_BASE = DEVLINK_TRAP_GENERIC_ID_MAX,
0014     DEVLINK_PRESTERA_TRAP_ID_ARP_BC,
0015     DEVLINK_PRESTERA_TRAP_ID_IS_IS,
0016     DEVLINK_PRESTERA_TRAP_ID_OSPF,
0017     DEVLINK_PRESTERA_TRAP_ID_IP_BC_MAC,
0018     DEVLINK_PRESTERA_TRAP_ID_ROUTER_MC,
0019     DEVLINK_PRESTERA_TRAP_ID_VRRP,
0020     DEVLINK_PRESTERA_TRAP_ID_DHCP,
0021     DEVLINK_PRESTERA_TRAP_ID_MAC_TO_ME,
0022     DEVLINK_PRESTERA_TRAP_ID_IPV4_OPTIONS,
0023     DEVLINK_PRESTERA_TRAP_ID_IP_DEFAULT_ROUTE,
0024     DEVLINK_PRESTERA_TRAP_ID_IP_TO_ME,
0025     DEVLINK_PRESTERA_TRAP_ID_IPV4_ICMP_REDIRECT,
0026     DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_0,
0027     DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_1,
0028     DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_2,
0029     DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_3,
0030     DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_4,
0031     DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_5,
0032     DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_6,
0033     DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_7,
0034     DEVLINK_PRESTERA_TRAP_ID_BGP,
0035     DEVLINK_PRESTERA_TRAP_ID_SSH,
0036     DEVLINK_PRESTERA_TRAP_ID_TELNET,
0037     DEVLINK_PRESTERA_TRAP_ID_ICMP,
0038     DEVLINK_PRESTERA_TRAP_ID_MET_RED,
0039     DEVLINK_PRESTERA_TRAP_ID_IP_SIP_IS_ZERO,
0040     DEVLINK_PRESTERA_TRAP_ID_IP_UC_DIP_DA_MISMATCH,
0041     DEVLINK_PRESTERA_TRAP_ID_ILLEGAL_IPV4_HDR,
0042     DEVLINK_PRESTERA_TRAP_ID_ILLEGAL_IP_ADDR,
0043     DEVLINK_PRESTERA_TRAP_ID_INVALID_SA,
0044     DEVLINK_PRESTERA_TRAP_ID_LOCAL_PORT,
0045     DEVLINK_PRESTERA_TRAP_ID_PORT_NO_VLAN,
0046     DEVLINK_PRESTERA_TRAP_ID_RXDMA_DROP,
0047 };
0048
0049 #define DEVLINK_PRESTERA_TRAP_NAME_ARP_BC \
0050     "arp_bc"
0051 #define DEVLINK_PRESTERA_TRAP_NAME_IS_IS \
0052     "is_is"
0053 #define DEVLINK_PRESTERA_TRAP_NAME_OSPF \
0054     "ospf"
0055 #define DEVLINK_PRESTERA_TRAP_NAME_IP_BC_MAC \
0056     "ip_bc_mac"
0057 #define DEVLINK_PRESTERA_TRAP_NAME_ROUTER_MC \
0058     "router_mc"
0059 #define DEVLINK_PRESTERA_TRAP_NAME_VRRP \
0060     "vrrp"
0061 #define DEVLINK_PRESTERA_TRAP_NAME_DHCP \
0062     "dhcp"
0063 #define DEVLINK_PRESTERA_TRAP_NAME_MAC_TO_ME \
0064     "mac_to_me"
0065 #define DEVLINK_PRESTERA_TRAP_NAME_IPV4_OPTIONS \
0066     "ipv4_options"
0067 #define DEVLINK_PRESTERA_TRAP_NAME_IP_DEFAULT_ROUTE \
0068     "ip_default_route"
0069 #define DEVLINK_PRESTERA_TRAP_NAME_IP_TO_ME \
0070     "ip_to_me"
0071 #define DEVLINK_PRESTERA_TRAP_NAME_IPV4_ICMP_REDIRECT \
0072     "ipv4_icmp_redirect"
0073 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_0 \
0074     "acl_code_0"
0075 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_1 \
0076     "acl_code_1"
0077 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_2 \
0078     "acl_code_2"
0079 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_3 \
0080     "acl_code_3"
0081 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_4 \
0082     "acl_code_4"
0083 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_5 \
0084     "acl_code_5"
0085 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_6 \
0086     "acl_code_6"
0087 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_7 \
0088     "acl_code_7"
0089 #define DEVLINK_PRESTERA_TRAP_NAME_BGP \
0090     "bgp"
0091 #define DEVLINK_PRESTERA_TRAP_NAME_SSH \
0092     "ssh"
0093 #define DEVLINK_PRESTERA_TRAP_NAME_TELNET \
0094     "telnet"
0095 #define DEVLINK_PRESTERA_TRAP_NAME_ICMP \
0096     "icmp"
0097 #define DEVLINK_PRESTERA_TRAP_NAME_RXDMA_DROP \
0098     "rxdma_drop"
0099 #define DEVLINK_PRESTERA_TRAP_NAME_PORT_NO_VLAN \
0100     "port_no_vlan"
0101 #define DEVLINK_PRESTERA_TRAP_NAME_LOCAL_PORT \
0102     "local_port"
0103 #define DEVLINK_PRESTERA_TRAP_NAME_INVALID_SA \
0104     "invalid_sa"
0105 #define DEVLINK_PRESTERA_TRAP_NAME_ILLEGAL_IP_ADDR \
0106     "illegal_ip_addr"
0107 #define DEVLINK_PRESTERA_TRAP_NAME_ILLEGAL_IPV4_HDR \
0108     "illegal_ipv4_hdr"
0109 #define DEVLINK_PRESTERA_TRAP_NAME_IP_UC_DIP_DA_MISMATCH \
0110     "ip_uc_dip_da_mismatch"
0111 #define DEVLINK_PRESTERA_TRAP_NAME_IP_SIP_IS_ZERO \
0112     "ip_sip_is_zero"
0113 #define DEVLINK_PRESTERA_TRAP_NAME_MET_RED \
0114     "met_red"
0115
0116 struct prestera_trap {
0117     struct devlink_trap trap;
0118     u8 cpu_code;
0119 };
0120
0121 struct prestera_trap_item {
0122     enum devlink_trap_action action;
0123     void *trap_ctx;
0124 };
0125
0126 struct prestera_trap_data {
0127     struct prestera_switch *sw;
0128     struct prestera_trap_item *trap_items_arr;
0129     u32 traps_count;
0130 };
0131
0132 #define PRESTERA_TRAP_METADATA DEVLINK_TRAP_METADATA_TYPE_F_IN_PORT
0133
0134 #define PRESTERA_TRAP_CONTROL(_id, _group_id, _action)                \
0135     DEVLINK_TRAP_GENERIC(CONTROL, _action, _id,               \
0136                  DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,       \
0137                  PRESTERA_TRAP_METADATA)
0138
0139 #define PRESTERA_TRAP_DRIVER_CONTROL(_id, _group_id)                  \
0140     DEVLINK_TRAP_DRIVER(CONTROL, TRAP, DEVLINK_PRESTERA_TRAP_ID_##_id,    \
0141                 DEVLINK_PRESTERA_TRAP_NAME_##_id,             \
0142                 DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,        \
0143                 PRESTERA_TRAP_METADATA)
0144
0145 #define PRESTERA_TRAP_EXCEPTION(_id, _group_id)                   \
0146     DEVLINK_TRAP_GENERIC(EXCEPTION, TRAP, _id,                \
0147                  DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,       \
0148                  PRESTERA_TRAP_METADATA)
0149
0150 #define PRESTERA_TRAP_DRIVER_EXCEPTION(_id, _group_id)                \
0151     DEVLINK_TRAP_DRIVER(EXCEPTION, TRAP, DEVLINK_PRESTERA_TRAP_ID_##_id,  \
0152                 DEVLINK_PRESTERA_TRAP_NAME_##_id,             \
0153                 DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,        \
0154                 PRESTERA_TRAP_METADATA)
0155
0156 #define PRESTERA_TRAP_DRIVER_DROP(_id, _group_id)                 \
0157     DEVLINK_TRAP_DRIVER(DROP, DROP, DEVLINK_PRESTERA_TRAP_ID_##_id,       \
0158                 DEVLINK_PRESTERA_TRAP_NAME_##_id,             \
0159                 DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,        \
0160                 PRESTERA_TRAP_METADATA)
0161
0162 static const struct devlink_trap_group prestera_trap_groups_arr[] = {
0163     /* No policer is associated with following groups (policerid == 0)*/
0164     DEVLINK_TRAP_GROUP_GENERIC(L2_DROPS, 0),
0165     DEVLINK_TRAP_GROUP_GENERIC(L3_DROPS, 0),
0166     DEVLINK_TRAP_GROUP_GENERIC(L3_EXCEPTIONS, 0),
0167     DEVLINK_TRAP_GROUP_GENERIC(NEIGH_DISCOVERY, 0),
0168     DEVLINK_TRAP_GROUP_GENERIC(ACL_TRAP, 0),
0169     DEVLINK_TRAP_GROUP_GENERIC(ACL_DROPS, 0),
0170     DEVLINK_TRAP_GROUP_GENERIC(ACL_SAMPLE, 0),
0171     DEVLINK_TRAP_GROUP_GENERIC(OSPF, 0),
0172     DEVLINK_TRAP_GROUP_GENERIC(STP, 0),
0173     DEVLINK_TRAP_GROUP_GENERIC(LACP, 0),
0174     DEVLINK_TRAP_GROUP_GENERIC(LLDP, 0),
0175     DEVLINK_TRAP_GROUP_GENERIC(VRRP, 0),
0176     DEVLINK_TRAP_GROUP_GENERIC(DHCP, 0),
0177     DEVLINK_TRAP_GROUP_GENERIC(BGP, 0),
0178     DEVLINK_TRAP_GROUP_GENERIC(LOCAL_DELIVERY, 0),
0179     DEVLINK_TRAP_GROUP_GENERIC(BUFFER_DROPS, 0),
0180 };
0181
0182 /* Initialize trap list, as well as associate CPU code with them. */
0183 static struct prestera_trap prestera_trap_items_arr[] = {
0184     {
0185         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ARP_BC, NEIGH_DISCOVERY),
0186         .cpu_code = 5,
0187     },
0188     {
0189         .trap = PRESTERA_TRAP_DRIVER_CONTROL(IS_IS, LOCAL_DELIVERY),
0190         .cpu_code = 13,
0191     },
0192     {
0193         .trap = PRESTERA_TRAP_DRIVER_CONTROL(OSPF, OSPF),
0194         .cpu_code = 16,
0195     },
0196     {
0197         .trap = PRESTERA_TRAP_DRIVER_CONTROL(IP_BC_MAC, LOCAL_DELIVERY),
0198         .cpu_code = 19,
0199     },
0200     {
0201         .trap = PRESTERA_TRAP_CONTROL(STP, STP, TRAP),
0202         .cpu_code = 26,
0203     },
0204     {
0205         .trap = PRESTERA_TRAP_CONTROL(LACP, LACP, TRAP),
0206         .cpu_code = 27,
0207     },
0208     {
0209         .trap = PRESTERA_TRAP_CONTROL(LLDP, LLDP, TRAP),
0210         .cpu_code = 28,
0211     },
0212     {
0213         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ROUTER_MC, LOCAL_DELIVERY),
0214         .cpu_code = 29,
0215     },
0216     {
0217         .trap = PRESTERA_TRAP_DRIVER_CONTROL(VRRP, VRRP),
0218         .cpu_code = 30,
0219     },
0220     {
0221         .trap = PRESTERA_TRAP_DRIVER_CONTROL(DHCP, DHCP),
0222         .cpu_code = 33,
0223     },
0224     {
0225         .trap = PRESTERA_TRAP_EXCEPTION(MTU_ERROR, L3_EXCEPTIONS),
0226         .cpu_code = 63,
0227     },
0228     {
0229         .trap = PRESTERA_TRAP_DRIVER_CONTROL(MAC_TO_ME, LOCAL_DELIVERY),
0230         .cpu_code = 65,
0231     },
0232     {
0233         .trap = PRESTERA_TRAP_EXCEPTION(TTL_ERROR, L3_EXCEPTIONS),
0234         .cpu_code = 133,
0235     },
0236     {
0237         .trap = PRESTERA_TRAP_DRIVER_EXCEPTION(IPV4_OPTIONS,
0238                                L3_EXCEPTIONS),
0239         .cpu_code = 141,
0240     },
0241     {
0242         .trap = PRESTERA_TRAP_DRIVER_CONTROL(IP_DEFAULT_ROUTE,
0243                              LOCAL_DELIVERY),
0244         .cpu_code = 160,
0245     },
0246     {
0247         .trap = PRESTERA_TRAP_CONTROL(LOCAL_ROUTE, LOCAL_DELIVERY,
0248                           TRAP),
0249         .cpu_code = 161,
0250     },
0251     {
0252         .trap = PRESTERA_TRAP_DRIVER_EXCEPTION(IPV4_ICMP_REDIRECT,
0253                                L3_EXCEPTIONS),
0254         .cpu_code = 180,
0255     },
0256     {
0257         .trap = PRESTERA_TRAP_CONTROL(ARP_RESPONSE, NEIGH_DISCOVERY,
0258                           TRAP),
0259         .cpu_code = 188,
0260     },
0261     {
0262         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_0, ACL_TRAP),
0263         .cpu_code = 192,
0264     },
0265     {
0266         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_1, ACL_TRAP),
0267         .cpu_code = 193,
0268     },
0269     {
0270         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_2, ACL_TRAP),
0271         .cpu_code = 194,
0272     },
0273     {
0274         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_3, ACL_TRAP),
0275         .cpu_code = 195,
0276     },
0277     {
0278         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_4, ACL_TRAP),
0279         .cpu_code = 196,
0280     },
0281     {
0282         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_5, ACL_TRAP),
0283         .cpu_code = 197,
0284     },
0285     {
0286         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_6, ACL_TRAP),
0287         .cpu_code = 198,
0288     },
0289     {
0290         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_7, ACL_TRAP),
0291         .cpu_code = 199,
0292     },
0293     {
0294         .trap = PRESTERA_TRAP_DRIVER_CONTROL(BGP, BGP),
0295         .cpu_code = 206,
0296     },
0297     {
0298         .trap = PRESTERA_TRAP_DRIVER_CONTROL(SSH, LOCAL_DELIVERY),
0299         .cpu_code = 207,
0300     },
0301     {
0302         .trap = PRESTERA_TRAP_DRIVER_CONTROL(TELNET, LOCAL_DELIVERY),
0303         .cpu_code = 208,
0304     },
0305     {
0306         .trap = PRESTERA_TRAP_DRIVER_CONTROL(ICMP, LOCAL_DELIVERY),
0307         .cpu_code = 209,
0308     },
0309     {
0310         .trap = PRESTERA_TRAP_DRIVER_DROP(RXDMA_DROP, BUFFER_DROPS),
0311         .cpu_code = 37,
0312     },
0313     {
0314         .trap = PRESTERA_TRAP_DRIVER_DROP(PORT_NO_VLAN, L2_DROPS),
0315         .cpu_code = 39,
0316     },
0317     {
0318         .trap = PRESTERA_TRAP_DRIVER_DROP(LOCAL_PORT, L2_DROPS),
0319         .cpu_code = 56,
0320     },
0321     {
0322         .trap = PRESTERA_TRAP_DRIVER_DROP(INVALID_SA, L2_DROPS),
0323         .cpu_code = 60,
0324     },
0325     {
0326         .trap = PRESTERA_TRAP_DRIVER_DROP(ILLEGAL_IP_ADDR, L3_DROPS),
0327         .cpu_code = 136,
0328     },
0329     {
0330         .trap = PRESTERA_TRAP_DRIVER_DROP(ILLEGAL_IPV4_HDR, L3_DROPS),
0331         .cpu_code = 137,
0332     },
0333     {
0334         .trap = PRESTERA_TRAP_DRIVER_DROP(IP_UC_DIP_DA_MISMATCH,
0335                           L3_DROPS),
0336         .cpu_code = 138,
0337     },
0338     {
0339         .trap = PRESTERA_TRAP_DRIVER_DROP(IP_SIP_IS_ZERO, L3_DROPS),
0340         .cpu_code = 145,
0341     },
0342     {
0343         .trap = PRESTERA_TRAP_DRIVER_DROP(MET_RED, BUFFER_DROPS),
0344         .cpu_code = 185,
0345     },
0346 };
0347
0348 static int prestera_drop_counter_get(struct devlink *devlink,
0349                      const struct devlink_trap *trap,
0350                      u64 *p_drops);
0351
0352 static int prestera_dl_info_get(struct devlink *dl,
0353                 struct devlink_info_req *req,
0354                 struct netlink_ext_ack *extack)
0355 {
0356     struct prestera_switch *sw = devlink_priv(dl);
0357     char buf[16];
0358     int err;
0359
0360     err = devlink_info_driver_name_put(req, PRESTERA_DRV_NAME);
0361     if (err)
0362         return err;
0363
0364     snprintf(buf, sizeof(buf), "%d.%d.%d",
0365          sw->dev->fw_rev.maj,
0366          sw->dev->fw_rev.min,
0367          sw->dev->fw_rev.sub);
0368
0369     return devlink_info_version_running_put(req,
0370                            DEVLINK_INFO_VERSION_GENERIC_FW,
0371                            buf);
0372 }
0373
0374 static int prestera_trap_init(struct devlink *devlink,
0375                   const struct devlink_trap *trap, void *trap_ctx);
0376
0377 static int prestera_trap_action_set(struct devlink *devlink,
0378                     const struct devlink_trap *trap,
0379                     enum devlink_trap_action action,
0380                     struct netlink_ext_ack *extack);
0381
0382 static const struct devlink_ops prestera_dl_ops = {
0383     .info_get = prestera_dl_info_get,
0384     .trap_init = prestera_trap_init,
0385     .trap_action_set = prestera_trap_action_set,
0386     .trap_drop_counter_get = prestera_drop_counter_get,
0387 };
0388
0389 struct prestera_switch *prestera_devlink_alloc(struct prestera_device *dev)
0390 {
0391     struct devlink *dl;
0392
0393     dl = devlink_alloc(&prestera_dl_ops, sizeof(struct prestera_switch),
0394                dev->dev);
0395
0396     return devlink_priv(dl);
0397 }
0398
0399 void prestera_devlink_free(struct prestera_switch *sw)
0400 {
0401     struct devlink *dl = priv_to_devlink(sw);
0402
0403     devlink_free(dl);
0404 }
0405
0406 void prestera_devlink_register(struct prestera_switch *sw)
0407 {
0408     struct devlink *dl = priv_to_devlink(sw);
0409
0410     devlink_register(dl);
0411 }
0412
0413 void prestera_devlink_unregister(struct prestera_switch *sw)
0414 {
0415     struct devlink *dl = priv_to_devlink(sw);
0416
0417     devlink_unregister(dl);
0418 }
0419
0420 int prestera_devlink_port_register(struct prestera_port *port)
0421 {
0422     struct prestera_switch *sw = port->sw;
0423     struct devlink *dl = priv_to_devlink(sw);
0424     struct devlink_port_attrs attrs = {};
0425     int err;
0426
0427     attrs.flavour = DEVLINK_PORT_FLAVOUR_PHYSICAL;
0428     attrs.phys.port_number = port->fp_id;
0429     attrs.switch_id.id_len = sizeof(sw->id);
0430     memcpy(attrs.switch_id.id, &sw->id, attrs.switch_id.id_len);
0431
0432     devlink_port_attrs_set(&port->dl_port, &attrs);
0433
0434     err = devlink_port_register(dl, &port->dl_port, port->fp_id);
0435     if (err) {
0436         dev_err(prestera_dev(sw), "devlink_port_register failed: %d\n", err);
0437         return err;
0438     }
0439
0440     return 0;
0441 }
0442
0443 void prestera_devlink_port_unregister(struct prestera_port *port)
0444 {
0445     devlink_port_unregister(&port->dl_port);
0446 }
0447
0448 void prestera_devlink_port_set(struct prestera_port *port)
0449 {
0450     devlink_port_type_eth_set(&port->dl_port, port->dev);
0451 }
0452
0453 void prestera_devlink_port_clear(struct prestera_port *port)
0454 {
0455     devlink_port_type_clear(&port->dl_port);
0456 }
0457
0458 struct devlink_port *prestera_devlink_get_port(struct net_device *dev)
0459 {
0460     struct prestera_port *port = netdev_priv(dev);
0461
0462     return &port->dl_port;
0463 }
0464
0465 int prestera_devlink_traps_register(struct prestera_switch *sw)
0466 {
0467     const u32 groups_count = ARRAY_SIZE(prestera_trap_groups_arr);
0468     const u32 traps_count = ARRAY_SIZE(prestera_trap_items_arr);
0469     struct devlink *devlink = priv_to_devlink(sw);
0470     struct prestera_trap_data *trap_data;
0471     struct prestera_trap *prestera_trap;
0472     int err, i;
0473
0474     trap_data = kzalloc(sizeof(*trap_data), GFP_KERNEL);
0475     if (!trap_data)
0476         return -ENOMEM;
0477
0478     trap_data->trap_items_arr = kcalloc(traps_count,
0479                         sizeof(struct prestera_trap_item),
0480                         GFP_KERNEL);
0481     if (!trap_data->trap_items_arr) {
0482         err = -ENOMEM;
0483         goto err_trap_items_alloc;
0484     }
0485
0486     trap_data->sw = sw;
0487     trap_data->traps_count = traps_count;
0488     sw->trap_data = trap_data;
0489
0490     err = devlink_trap_groups_register(devlink, prestera_trap_groups_arr,
0491                        groups_count);
0492     if (err)
0493         goto err_groups_register;
0494
0495     for (i = 0; i < traps_count; i++) {
0496         prestera_trap = &prestera_trap_items_arr[i];
0497         err = devlink_traps_register(devlink, &prestera_trap->trap, 1,
0498                          sw);
0499         if (err)
0500             goto err_trap_register;
0501     }
0502
0503     return 0;
0504
0505 err_trap_register:
0506     for (i--; i >= 0; i--) {
0507         prestera_trap = &prestera_trap_items_arr[i];
0508         devlink_traps_unregister(devlink, &prestera_trap->trap, 1);
0509     }
0510     devlink_trap_groups_unregister(devlink, prestera_trap_groups_arr,
0511                        groups_count);
0512 err_groups_register:
0513     kfree(trap_data->trap_items_arr);
0514 err_trap_items_alloc:
0515     kfree(trap_data);
0516     return err;
0517 }
0518
0519 static struct prestera_trap_item *
0520 prestera_get_trap_item_by_cpu_code(struct prestera_switch *sw, u8 cpu_code)
0521 {
0522     struct prestera_trap_data *trap_data = sw->trap_data;
0523     struct prestera_trap *prestera_trap;
0524     int i;
0525
0526     for (i = 0; i < trap_data->traps_count; i++) {
0527         prestera_trap = &prestera_trap_items_arr[i];
0528         if (cpu_code == prestera_trap->cpu_code)
0529             return &trap_data->trap_items_arr[i];
0530     }
0531
0532     return NULL;
0533 }
0534
0535 void prestera_devlink_trap_report(struct prestera_port *port,
0536                   struct sk_buff *skb, u8 cpu_code)
0537 {
0538     struct prestera_trap_item *trap_item;
0539     struct devlink *devlink;
0540
0541     devlink = port->dl_port.devlink;
0542
0543     trap_item = prestera_get_trap_item_by_cpu_code(port->sw, cpu_code);
0544     if (unlikely(!trap_item))
0545         return;
0546
0547     devlink_trap_report(devlink, skb, trap_item->trap_ctx,
0548                 &port->dl_port, NULL);
0549 }
0550
0551 static struct prestera_trap_item *
0552 prestera_devlink_trap_item_lookup(struct prestera_switch *sw, u16 trap_id)
0553 {
0554     struct prestera_trap_data *trap_data = sw->trap_data;
0555     int i;
0556
0557     for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); i++) {
0558         if (prestera_trap_items_arr[i].trap.id == trap_id)
0559             return &trap_data->trap_items_arr[i];
0560     }
0561
0562     return NULL;
0563 }
0564
0565 static int prestera_trap_init(struct devlink *devlink,
0566                   const struct devlink_trap *trap, void *trap_ctx)
0567 {
0568     struct prestera_switch *sw = devlink_priv(devlink);
0569     struct prestera_trap_item *trap_item;
0570
0571     trap_item = prestera_devlink_trap_item_lookup(sw, trap->id);
0572     if (WARN_ON(!trap_item))
0573         return -EINVAL;
0574
0575     trap_item->trap_ctx = trap_ctx;
0576     trap_item->action = trap->init_action;
0577
0578     return 0;
0579 }
0580
0581 static int prestera_trap_action_set(struct devlink *devlink,
0582                     const struct devlink_trap *trap,
0583                     enum devlink_trap_action action,
0584                     struct netlink_ext_ack *extack)
0585 {
0586     /* Currently, driver does not support trap action altering */
0587     return -EOPNOTSUPP;
0588 }
0589
0590 static int prestera_drop_counter_get(struct devlink *devlink,
0591                      const struct devlink_trap *trap,
0592                      u64 *p_drops)
0593 {
0594     struct prestera_switch *sw = devlink_priv(devlink);
0595     enum prestera_hw_cpu_code_cnt_t cpu_code_type =
0596         PRESTERA_HW_CPU_CODE_CNT_TYPE_DROP;
0597     struct prestera_trap *prestera_trap =
0598         container_of(trap, struct prestera_trap, trap);
0599
0600     return prestera_hw_cpu_code_counters_get(sw, prestera_trap->cpu_code,
0601                          cpu_code_type, p_drops);
0602 }
0603
0604 void prestera_devlink_traps_unregister(struct prestera_switch *sw)
0605 {
0606     struct prestera_trap_data *trap_data = sw->trap_data;
0607     struct devlink *dl = priv_to_devlink(sw);
0608     const struct devlink_trap *trap;
0609     int i;
0610
0611     for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); ++i) {
0612         trap = &prestera_trap_items_arr[i].trap;
0613         devlink_traps_unregister(dl, trap, 1);
0614     }
0615
0616     devlink_trap_groups_unregister(dl, prestera_trap_groups_arr,
0617                        ARRAY_SIZE(prestera_trap_groups_arr));
0618     kfree(trap_data->trap_items_arr);
0619     kfree(trap_data);
0620 }