net/devlink/trap.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (c) 2016 Mellanox Technologies. All rights reserved.
   4  * Copyright (c) 2016 Jiri Pirko <jiri@mellanox.com>
   5  */
   6
   7 #include <trace/events/devlink.h>
   8
   9 #include "devl_internal.h"
  10
  11 struct devlink_stats {
  12         u64_stats_t rx_bytes;
  13         u64_stats_t rx_packets;
  14         struct u64_stats_sync syncp;
  15 };
  16
  17 /**
  18  * struct devlink_trap_policer_item - Packet trap policer attributes.
  19  * @policer: Immutable packet trap policer attributes.
  20  * @rate: Rate in packets / sec.
  21  * @burst: Burst size in packets.
  22  * @list: trap_policer_list member.
  23  *
  24  * Describes packet trap policer attributes. Created by devlink during trap
  25  * policer registration.
  26  */
  27 struct devlink_trap_policer_item {
  28         const struct devlink_trap_policer *policer;
  29         u64 rate;
  30         u64 burst;
  31         struct list_head list;
  32 };
  33
  34 /**
  35  * struct devlink_trap_group_item - Packet trap group attributes.
  36  * @group: Immutable packet trap group attributes.
  37  * @policer_item: Associated policer item. Can be NULL.
  38  * @list: trap_group_list member.
  39  * @stats: Trap group statistics.
  40  *
  41  * Describes packet trap group attributes. Created by devlink during trap
  42  * group registration.
  43  */
  44 struct devlink_trap_group_item {
  45         const struct devlink_trap_group *group;
  46         struct devlink_trap_policer_item *policer_item;
  47         struct list_head list;
  48         struct devlink_stats __percpu *stats;
  49 };
  50
  51 /**
  52  * struct devlink_trap_item - Packet trap attributes.
  53  * @trap: Immutable packet trap attributes.
  54  * @group_item: Associated group item.
  55  * @list: trap_list member.
  56  * @action: Trap action.
  57  * @stats: Trap statistics.
  58  * @priv: Driver private information.
  59  *
  60  * Describes both mutable and immutable packet trap attributes. Created by
  61  * devlink during trap registration and used for all trap related operations.
  62  */
  63 struct devlink_trap_item {
  64         const struct devlink_trap *trap;
  65         struct devlink_trap_group_item *group_item;
  66         struct list_head list;
  67         enum devlink_trap_action action;
  68         struct devlink_stats __percpu *stats;
  69         void *priv;
  70 };
  71
  72 static struct devlink_trap_policer_item *
  73 devlink_trap_policer_item_lookup(struct devlink *devlink, u32 id)
  74 {
  75         struct devlink_trap_policer_item *policer_item;
  76
  77         list_for_each_entry(policer_item, &devlink->trap_policer_list, list) {
  78                 if (policer_item->policer->id == id)
  79                         return policer_item;
  80         }
  81
  82         return NULL;
  83 }
  84
  85 static struct devlink_trap_item *
  86 devlink_trap_item_lookup(struct devlink *devlink, const char *name)
  87 {
  88         struct devlink_trap_item *trap_item;
  89
  90         list_for_each_entry(trap_item, &devlink->trap_list, list) {
  91                 if (!strcmp(trap_item->trap->name, name))
  92                         return trap_item;
  93         }
  94
  95         return NULL;
  96 }
  97
  98 static struct devlink_trap_item *
  99 devlink_trap_item_get_from_info(struct devlink *devlink,
 100                                 struct genl_info *info)
 101 {
 102         struct nlattr *attr;
 103
 104         if (!info->attrs[DEVLINK_ATTR_TRAP_NAME])
 105                 return NULL;
 106         attr = info->attrs[DEVLINK_ATTR_TRAP_NAME];
 107
 108         return devlink_trap_item_lookup(devlink, nla_data(attr));
 109 }
 110
 111 static int
 112 devlink_trap_action_get_from_info(struct genl_info *info,
 113                                   enum devlink_trap_action *p_trap_action)
 114 {
 115         u8 val;
 116
 117         val = nla_get_u8(info->attrs[DEVLINK_ATTR_TRAP_ACTION]);
 118         switch (val) {
 119         case DEVLINK_TRAP_ACTION_DROP:
 120         case DEVLINK_TRAP_ACTION_TRAP:
 121         case DEVLINK_TRAP_ACTION_MIRROR:
 122                 *p_trap_action = val;
 123                 break;
 124         default:
 125                 return -EINVAL;
 126         }
 127
 128         return 0;
 129 }
 130
 131 static int devlink_trap_metadata_put(struct sk_buff *msg,
 132                                      const struct devlink_trap *trap)
 133 {
 134         struct nlattr *attr;
 135
 136         attr = nla_nest_start(msg, DEVLINK_ATTR_TRAP_METADATA);
 137         if (!attr)
 138                 return -EMSGSIZE;
 139
 140         if ((trap->metadata_cap & DEVLINK_TRAP_METADATA_TYPE_F_IN_PORT) &&
 141             nla_put_flag(msg, DEVLINK_ATTR_TRAP_METADATA_TYPE_IN_PORT))
 142                 goto nla_put_failure;
 143         if ((trap->metadata_cap & DEVLINK_TRAP_METADATA_TYPE_F_FA_COOKIE) &&
 144             nla_put_flag(msg, DEVLINK_ATTR_TRAP_METADATA_TYPE_FA_COOKIE))
 145                 goto nla_put_failure;
 146
 147         nla_nest_end(msg, attr);
 148
 149         return 0;
 150
 151 nla_put_failure:
 152         nla_nest_cancel(msg, attr);
 153         return -EMSGSIZE;
 154 }
 155
 156 static void devlink_trap_stats_read(struct devlink_stats __percpu *trap_stats,
 157                                     struct devlink_stats *stats)
 158 {
 159         int i;
 160
 161         memset(stats, 0, sizeof(*stats));
 162         for_each_possible_cpu(i) {
 163                 struct devlink_stats *cpu_stats;
 164                 u64 rx_packets, rx_bytes;
 165                 unsigned int start;
 166
 167                 cpu_stats = per_cpu_ptr(trap_stats, i);
 168                 do {
 169                         start = u64_stats_fetch_begin(&cpu_stats->syncp);
 170                         rx_packets = u64_stats_read(&cpu_stats->rx_packets);
 171                         rx_bytes = u64_stats_read(&cpu_stats->rx_bytes);
 172                 } while (u64_stats_fetch_retry(&cpu_stats->syncp, start));
 173
 174                 u64_stats_add(&stats->rx_packets, rx_packets);
 175                 u64_stats_add(&stats->rx_bytes, rx_bytes);
 176         }
 177 }
 178
 179 static int
 180 devlink_trap_group_stats_put(struct sk_buff *msg,
 181                              struct devlink_stats __percpu *trap_stats)
 182 {
 183         struct devlink_stats stats;
 184         struct nlattr *attr;
 185
 186         devlink_trap_stats_read(trap_stats, &stats);
 187
 188         attr = nla_nest_start(msg, DEVLINK_ATTR_STATS);
 189         if (!attr)
 190                 return -EMSGSIZE;
 191
 192         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_STATS_RX_PACKETS,
 193                               u64_stats_read(&stats.rx_packets),
 194                               DEVLINK_ATTR_PAD))
 195                 goto nla_put_failure;
 196
 197         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_STATS_RX_BYTES,
 198                               u64_stats_read(&stats.rx_bytes),
 199                               DEVLINK_ATTR_PAD))
 200                 goto nla_put_failure;
 201
 202         nla_nest_end(msg, attr);
 203
 204         return 0;
 205
 206 nla_put_failure:
 207         nla_nest_cancel(msg, attr);
 208         return -EMSGSIZE;
 209 }
 210
 211 static int devlink_trap_stats_put(struct sk_buff *msg, struct devlink *devlink,
 212                                   const struct devlink_trap_item *trap_item)
 213 {
 214         struct devlink_stats stats;
 215         struct nlattr *attr;
 216         u64 drops = 0;
 217         int err;
 218
 219         if (devlink->ops->trap_drop_counter_get) {
 220                 err = devlink->ops->trap_drop_counter_get(devlink,
 221                                                           trap_item->trap,
 222                                                           &drops);
 223                 if (err)
 224                         return err;
 225         }
 226
 227         devlink_trap_stats_read(trap_item->stats, &stats);
 228
 229         attr = nla_nest_start(msg, DEVLINK_ATTR_STATS);
 230         if (!attr)
 231                 return -EMSGSIZE;
 232
 233         if (devlink->ops->trap_drop_counter_get &&
 234             nla_put_u64_64bit(msg, DEVLINK_ATTR_STATS_RX_DROPPED, drops,
 235                               DEVLINK_ATTR_PAD))
 236                 goto nla_put_failure;
 237
 238         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_STATS_RX_PACKETS,
 239                               u64_stats_read(&stats.rx_packets),
 240                               DEVLINK_ATTR_PAD))
 241                 goto nla_put_failure;
 242
 243         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_STATS_RX_BYTES,
 244                               u64_stats_read(&stats.rx_bytes),
 245                               DEVLINK_ATTR_PAD))
 246                 goto nla_put_failure;
 247
 248         nla_nest_end(msg, attr);
 249
 250         return 0;
 251
 252 nla_put_failure:
 253         nla_nest_cancel(msg, attr);
 254         return -EMSGSIZE;
 255 }
 256
 257 static int devlink_nl_trap_fill(struct sk_buff *msg, struct devlink *devlink,
 258                                 const struct devlink_trap_item *trap_item,
 259                                 enum devlink_command cmd, u32 portid, u32 seq,
 260                                 int flags)
 261 {
 262         struct devlink_trap_group_item *group_item = trap_item->group_item;
 263         void *hdr;
 264         int err;
 265
 266         hdr = genlmsg_put(msg, portid, seq, &devlink_nl_family, flags, cmd);
 267         if (!hdr)
 268                 return -EMSGSIZE;
 269
 270         if (devlink_nl_put_handle(msg, devlink))
 271                 goto nla_put_failure;
 272
 273         if (nla_put_string(msg, DEVLINK_ATTR_TRAP_GROUP_NAME,
 274                            group_item->group->name))
 275                 goto nla_put_failure;
 276
 277         if (nla_put_string(msg, DEVLINK_ATTR_TRAP_NAME, trap_item->trap->name))
 278                 goto nla_put_failure;
 279
 280         if (nla_put_u8(msg, DEVLINK_ATTR_TRAP_TYPE, trap_item->trap->type))
 281                 goto nla_put_failure;
 282
 283         if (trap_item->trap->generic &&
 284             nla_put_flag(msg, DEVLINK_ATTR_TRAP_GENERIC))
 285                 goto nla_put_failure;
 286
 287         if (nla_put_u8(msg, DEVLINK_ATTR_TRAP_ACTION, trap_item->action))
 288                 goto nla_put_failure;
 289
 290         err = devlink_trap_metadata_put(msg, trap_item->trap);
 291         if (err)
 292                 goto nla_put_failure;
 293
 294         err = devlink_trap_stats_put(msg, devlink, trap_item);
 295         if (err)
 296                 goto nla_put_failure;
 297
 298         genlmsg_end(msg, hdr);
 299
 300         return 0;
 301
 302 nla_put_failure:
 303         genlmsg_cancel(msg, hdr);
 304         return -EMSGSIZE;
 305 }
 306
 307 int devlink_nl_trap_get_doit(struct sk_buff *skb, struct genl_info *info)
 308 {
 309         struct netlink_ext_ack *extack = info->extack;
 310         struct devlink *devlink = info->user_ptr[0];
 311         struct devlink_trap_item *trap_item;
 312         struct sk_buff *msg;
 313         int err;
 314
 315         if (list_empty(&devlink->trap_list))
 316                 return -EOPNOTSUPP;
 317
 318         trap_item = devlink_trap_item_get_from_info(devlink, info);
 319         if (!trap_item) {
 320                 NL_SET_ERR_MSG(extack, "Device did not register this trap");
 321                 return -ENOENT;
 322         }
 323
 324         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 325         if (!msg)
 326                 return -ENOMEM;
 327
 328         err = devlink_nl_trap_fill(msg, devlink, trap_item,
 329                                    DEVLINK_CMD_TRAP_NEW, info->snd_portid,
 330                                    info->snd_seq, 0);
 331         if (err)
 332                 goto err_trap_fill;
 333
 334         return genlmsg_reply(msg, info);
 335
 336 err_trap_fill:
 337         nlmsg_free(msg);
 338         return err;
 339 }
 340
 341 static int devlink_nl_trap_get_dump_one(struct sk_buff *msg,
 342                                         struct devlink *devlink,
 343                                         struct netlink_callback *cb, int flags)
 344 {
 345         struct devlink_nl_dump_state *state = devlink_dump_state(cb);
 346         struct devlink_trap_item *trap_item;
 347         int idx = 0;
 348         int err = 0;
 349
 350         list_for_each_entry(trap_item, &devlink->trap_list, list) {
 351                 if (idx < state->idx) {
 352                         idx++;
 353                         continue;
 354                 }
 355                 err = devlink_nl_trap_fill(msg, devlink, trap_item,
 356                                            DEVLINK_CMD_TRAP_NEW,
 357                                            NETLINK_CB(cb->skb).portid,
 358                                            cb->nlh->nlmsg_seq, flags);
 359                 if (err) {
 360                         state->idx = idx;
 361                         break;
 362                 }
 363                 idx++;
 364         }
 365
 366         return err;
 367 }
 368
 369 int devlink_nl_trap_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
 370 {
 371         return devlink_nl_dumpit(skb, cb, devlink_nl_trap_get_dump_one);
 372 }
 373
 374 static int __devlink_trap_action_set(struct devlink *devlink,
 375                                      struct devlink_trap_item *trap_item,
 376                                      enum devlink_trap_action trap_action,
 377                                      struct netlink_ext_ack *extack)
 378 {
 379         int err;
 380
 381         if (trap_item->action != trap_action &&
 382             trap_item->trap->type != DEVLINK_TRAP_TYPE_DROP) {
 383                 NL_SET_ERR_MSG(extack, "Cannot change action of non-drop traps. Skipping");
 384                 return 0;
 385         }
 386
 387         err = devlink->ops->trap_action_set(devlink, trap_item->trap,
 388                                             trap_action, extack);
 389         if (err)
 390                 return err;
 391
 392         trap_item->action = trap_action;
 393
 394         return 0;
 395 }
 396
 397 static int devlink_trap_action_set(struct devlink *devlink,
 398                                    struct devlink_trap_item *trap_item,
 399                                    struct genl_info *info)
 400 {
 401         enum devlink_trap_action trap_action;
 402         int err;
 403
 404         if (!info->attrs[DEVLINK_ATTR_TRAP_ACTION])
 405                 return 0;
 406
 407         err = devlink_trap_action_get_from_info(info, &trap_action);
 408         if (err) {
 409                 NL_SET_ERR_MSG(info->extack, "Invalid trap action");
 410                 return -EINVAL;
 411         }
 412
 413         return __devlink_trap_action_set(devlink, trap_item, trap_action,
 414                                          info->extack);
 415 }
 416
 417 int devlink_nl_cmd_trap_set_doit(struct sk_buff *skb, struct genl_info *info)
 418 {
 419         struct netlink_ext_ack *extack = info->extack;
 420         struct devlink *devlink = info->user_ptr[0];
 421         struct devlink_trap_item *trap_item;
 422
 423         if (list_empty(&devlink->trap_list))
 424                 return -EOPNOTSUPP;
 425
 426         trap_item = devlink_trap_item_get_from_info(devlink, info);
 427         if (!trap_item) {
 428                 NL_SET_ERR_MSG(extack, "Device did not register this trap");
 429                 return -ENOENT;
 430         }
 431
 432         return devlink_trap_action_set(devlink, trap_item, info);
 433 }
 434
 435 static struct devlink_trap_group_item *
 436 devlink_trap_group_item_lookup(struct devlink *devlink, const char *name)
 437 {
 438         struct devlink_trap_group_item *group_item;
 439
 440         list_for_each_entry(group_item, &devlink->trap_group_list, list) {
 441                 if (!strcmp(group_item->group->name, name))
 442                         return group_item;
 443         }
 444
 445         return NULL;
 446 }
 447
 448 static struct devlink_trap_group_item *
 449 devlink_trap_group_item_lookup_by_id(struct devlink *devlink, u16 id)
 450 {
 451         struct devlink_trap_group_item *group_item;
 452
 453         list_for_each_entry(group_item, &devlink->trap_group_list, list) {
 454                 if (group_item->group->id == id)
 455                         return group_item;
 456         }
 457
 458         return NULL;
 459 }
 460
 461 static struct devlink_trap_group_item *
 462 devlink_trap_group_item_get_from_info(struct devlink *devlink,
 463                                       struct genl_info *info)
 464 {
 465         char *name;
 466
 467         if (!info->attrs[DEVLINK_ATTR_TRAP_GROUP_NAME])
 468                 return NULL;
 469         name = nla_data(info->attrs[DEVLINK_ATTR_TRAP_GROUP_NAME]);
 470
 471         return devlink_trap_group_item_lookup(devlink, name);
 472 }
 473
 474 static int
 475 devlink_nl_trap_group_fill(struct sk_buff *msg, struct devlink *devlink,
 476                            const struct devlink_trap_group_item *group_item,
 477                            enum devlink_command cmd, u32 portid, u32 seq,
 478                            int flags)
 479 {
 480         void *hdr;
 481         int err;
 482
 483         hdr = genlmsg_put(msg, portid, seq, &devlink_nl_family, flags, cmd);
 484         if (!hdr)
 485                 return -EMSGSIZE;
 486
 487         if (devlink_nl_put_handle(msg, devlink))
 488                 goto nla_put_failure;
 489
 490         if (nla_put_string(msg, DEVLINK_ATTR_TRAP_GROUP_NAME,
 491                            group_item->group->name))
 492                 goto nla_put_failure;
 493
 494         if (group_item->group->generic &&
 495             nla_put_flag(msg, DEVLINK_ATTR_TRAP_GENERIC))
 496                 goto nla_put_failure;
 497
 498         if (group_item->policer_item &&
 499             nla_put_u32(msg, DEVLINK_ATTR_TRAP_POLICER_ID,
 500                         group_item->policer_item->policer->id))
 501                 goto nla_put_failure;
 502
 503         err = devlink_trap_group_stats_put(msg, group_item->stats);
 504         if (err)
 505                 goto nla_put_failure;
 506
 507         genlmsg_end(msg, hdr);
 508
 509         return 0;
 510
 511 nla_put_failure:
 512         genlmsg_cancel(msg, hdr);
 513         return -EMSGSIZE;
 514 }
 515
 516 int devlink_nl_trap_group_get_doit(struct sk_buff *skb, struct genl_info *info)
 517 {
 518         struct netlink_ext_ack *extack = info->extack;
 519         struct devlink *devlink = info->user_ptr[0];
 520         struct devlink_trap_group_item *group_item;
 521         struct sk_buff *msg;
 522         int err;
 523
 524         if (list_empty(&devlink->trap_group_list))
 525                 return -EOPNOTSUPP;
 526
 527         group_item = devlink_trap_group_item_get_from_info(devlink, info);
 528         if (!group_item) {
 529                 NL_SET_ERR_MSG(extack, "Device did not register this trap group");
 530                 return -ENOENT;
 531         }
 532
 533         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 534         if (!msg)
 535                 return -ENOMEM;
 536
 537         err = devlink_nl_trap_group_fill(msg, devlink, group_item,
 538                                          DEVLINK_CMD_TRAP_GROUP_NEW,
 539                                          info->snd_portid, info->snd_seq, 0);
 540         if (err)
 541                 goto err_trap_group_fill;
 542
 543         return genlmsg_reply(msg, info);
 544
 545 err_trap_group_fill:
 546         nlmsg_free(msg);
 547         return err;
 548 }
 549
 550 static int devlink_nl_trap_group_get_dump_one(struct sk_buff *msg,
 551                                               struct devlink *devlink,
 552                                               struct netlink_callback *cb,
 553                                               int flags)
 554 {
 555         struct devlink_nl_dump_state *state = devlink_dump_state(cb);
 556         struct devlink_trap_group_item *group_item;
 557         int idx = 0;
 558         int err = 0;
 559
 560         list_for_each_entry(group_item, &devlink->trap_group_list, list) {
 561                 if (idx < state->idx) {
 562                         idx++;
 563                         continue;
 564                 }
 565                 err = devlink_nl_trap_group_fill(msg, devlink, group_item,
 566                                                  DEVLINK_CMD_TRAP_GROUP_NEW,
 567                                                  NETLINK_CB(cb->skb).portid,
 568                                                  cb->nlh->nlmsg_seq, flags);
 569                 if (err) {
 570                         state->idx = idx;
 571                         break;
 572                 }
 573                 idx++;
 574         }
 575
 576         return err;
 577 }
 578
 579 int devlink_nl_trap_group_get_dumpit(struct sk_buff *skb,
 580                                      struct netlink_callback *cb)
 581 {
 582         return devlink_nl_dumpit(skb, cb, devlink_nl_trap_group_get_dump_one);
 583 }
 584
 585 static int
 586 __devlink_trap_group_action_set(struct devlink *devlink,
 587                                 struct devlink_trap_group_item *group_item,
 588                                 enum devlink_trap_action trap_action,
 589                                 struct netlink_ext_ack *extack)
 590 {
 591         const char *group_name = group_item->group->name;
 592         struct devlink_trap_item *trap_item;
 593         int err;
 594
 595         if (devlink->ops->trap_group_action_set) {
 596                 err = devlink->ops->trap_group_action_set(devlink, group_item->group,
 597                                                           trap_action, extack);
 598                 if (err)
 599                         return err;
 600
 601                 list_for_each_entry(trap_item, &devlink->trap_list, list) {
 602                         if (strcmp(trap_item->group_item->group->name, group_name))
 603                                 continue;
 604                         if (trap_item->action != trap_action &&
 605                             trap_item->trap->type != DEVLINK_TRAP_TYPE_DROP)
 606                                 continue;
 607                         trap_item->action = trap_action;
 608                 }
 609
 610                 return 0;
 611         }
 612
 613         list_for_each_entry(trap_item, &devlink->trap_list, list) {
 614                 if (strcmp(trap_item->group_item->group->name, group_name))
 615                         continue;
 616                 err = __devlink_trap_action_set(devlink, trap_item,
 617                                                 trap_action, extack);
 618                 if (err)
 619                         return err;
 620         }
 621
 622         return 0;
 623 }
 624
 625 static int
 626 devlink_trap_group_action_set(struct devlink *devlink,
 627                               struct devlink_trap_group_item *group_item,
 628                               struct genl_info *info, bool *p_modified)
 629 {
 630         enum devlink_trap_action trap_action;
 631         int err;
 632
 633         if (!info->attrs[DEVLINK_ATTR_TRAP_ACTION])
 634                 return 0;
 635
 636         err = devlink_trap_action_get_from_info(info, &trap_action);
 637         if (err) {
 638                 NL_SET_ERR_MSG(info->extack, "Invalid trap action");
 639                 return -EINVAL;
 640         }
 641
 642         err = __devlink_trap_group_action_set(devlink, group_item, trap_action,
 643                                               info->extack);
 644         if (err)
 645                 return err;
 646
 647         *p_modified = true;
 648
 649         return 0;
 650 }
 651
 652 static int devlink_trap_group_set(struct devlink *devlink,
 653                                   struct devlink_trap_group_item *group_item,
 654                                   struct genl_info *info)
 655 {
 656         struct devlink_trap_policer_item *policer_item;
 657         struct netlink_ext_ack *extack = info->extack;
 658         const struct devlink_trap_policer *policer;
 659         struct nlattr **attrs = info->attrs;
 660         u32 policer_id;
 661         int err;
 662
 663         if (!attrs[DEVLINK_ATTR_TRAP_POLICER_ID])
 664                 return 0;
 665
 666         if (!devlink->ops->trap_group_set)
 667                 return -EOPNOTSUPP;
 668
 669         policer_id = nla_get_u32(attrs[DEVLINK_ATTR_TRAP_POLICER_ID]);
 670         policer_item = devlink_trap_policer_item_lookup(devlink, policer_id);
 671         if (policer_id && !policer_item) {
 672                 NL_SET_ERR_MSG(extack, "Device did not register this trap policer");
 673                 return -ENOENT;
 674         }
 675         policer = policer_item ? policer_item->policer : NULL;
 676
 677         err = devlink->ops->trap_group_set(devlink, group_item->group, policer,
 678                                            extack);
 679         if (err)
 680                 return err;
 681
 682         group_item->policer_item = policer_item;
 683
 684         return 0;
 685 }
 686
 687 int devlink_nl_cmd_trap_group_set_doit(struct sk_buff *skb,
 688                                        struct genl_info *info)
 689 {
 690         struct netlink_ext_ack *extack = info->extack;
 691         struct devlink *devlink = info->user_ptr[0];
 692         struct devlink_trap_group_item *group_item;
 693         bool modified = false;
 694         int err;
 695
 696         if (list_empty(&devlink->trap_group_list))
 697                 return -EOPNOTSUPP;
 698
 699         group_item = devlink_trap_group_item_get_from_info(devlink, info);
 700         if (!group_item) {
 701                 NL_SET_ERR_MSG(extack, "Device did not register this trap group");
 702                 return -ENOENT;
 703         }
 704
 705         err = devlink_trap_group_action_set(devlink, group_item, info,
 706                                             &modified);
 707         if (err)
 708                 return err;
 709
 710         err = devlink_trap_group_set(devlink, group_item, info);
 711         if (err)
 712                 goto err_trap_group_set;
 713
 714         return 0;
 715
 716 err_trap_group_set:
 717         if (modified)
 718                 NL_SET_ERR_MSG(extack, "Trap group set failed, but some changes were committed already");
 719         return err;
 720 }
 721
 722 static struct devlink_trap_policer_item *
 723 devlink_trap_policer_item_get_from_info(struct devlink *devlink,
 724                                         struct genl_info *info)
 725 {
 726         u32 id;
 727
 728         if (!info->attrs[DEVLINK_ATTR_TRAP_POLICER_ID])
 729                 return NULL;
 730         id = nla_get_u32(info->attrs[DEVLINK_ATTR_TRAP_POLICER_ID]);
 731
 732         return devlink_trap_policer_item_lookup(devlink, id);
 733 }
 734
 735 static int
 736 devlink_trap_policer_stats_put(struct sk_buff *msg, struct devlink *devlink,
 737                                const struct devlink_trap_policer *policer)
 738 {
 739         struct nlattr *attr;
 740         u64 drops;
 741         int err;
 742
 743         if (!devlink->ops->trap_policer_counter_get)
 744                 return 0;
 745
 746         err = devlink->ops->trap_policer_counter_get(devlink, policer, &drops);
 747         if (err)
 748                 return err;
 749
 750         attr = nla_nest_start(msg, DEVLINK_ATTR_STATS);
 751         if (!attr)
 752                 return -EMSGSIZE;
 753
 754         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_STATS_RX_DROPPED, drops,
 755                               DEVLINK_ATTR_PAD))
 756                 goto nla_put_failure;
 757
 758         nla_nest_end(msg, attr);
 759
 760         return 0;
 761
 762 nla_put_failure:
 763         nla_nest_cancel(msg, attr);
 764         return -EMSGSIZE;
 765 }
 766
 767 static int
 768 devlink_nl_trap_policer_fill(struct sk_buff *msg, struct devlink *devlink,
 769                              const struct devlink_trap_policer_item *policer_item,
 770                              enum devlink_command cmd, u32 portid, u32 seq,
 771                              int flags)
 772 {
 773         void *hdr;
 774         int err;
 775
 776         hdr = genlmsg_put(msg, portid, seq, &devlink_nl_family, flags, cmd);
 777         if (!hdr)
 778                 return -EMSGSIZE;
 779
 780         if (devlink_nl_put_handle(msg, devlink))
 781                 goto nla_put_failure;
 782
 783         if (nla_put_u32(msg, DEVLINK_ATTR_TRAP_POLICER_ID,
 784                         policer_item->policer->id))
 785                 goto nla_put_failure;
 786
 787         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_TRAP_POLICER_RATE,
 788                               policer_item->rate, DEVLINK_ATTR_PAD))
 789                 goto nla_put_failure;
 790
 791         if (nla_put_u64_64bit(msg, DEVLINK_ATTR_TRAP_POLICER_BURST,
 792                               policer_item->burst, DEVLINK_ATTR_PAD))
 793                 goto nla_put_failure;
 794
 795         err = devlink_trap_policer_stats_put(msg, devlink,
 796                                              policer_item->policer);
 797         if (err)
 798                 goto nla_put_failure;
 799
 800         genlmsg_end(msg, hdr);
 801
 802         return 0;
 803
 804 nla_put_failure:
 805         genlmsg_cancel(msg, hdr);
 806         return -EMSGSIZE;
 807 }
 808
 809 int devlink_nl_trap_policer_get_doit(struct sk_buff *skb,
 810                                      struct genl_info *info)
 811 {
 812         struct devlink_trap_policer_item *policer_item;
 813         struct netlink_ext_ack *extack = info->extack;
 814         struct devlink *devlink = info->user_ptr[0];
 815         struct sk_buff *msg;
 816         int err;
 817
 818         if (list_empty(&devlink->trap_policer_list))
 819                 return -EOPNOTSUPP;
 820
 821         policer_item = devlink_trap_policer_item_get_from_info(devlink, info);
 822         if (!policer_item) {
 823                 NL_SET_ERR_MSG(extack, "Device did not register this trap policer");
 824                 return -ENOENT;
 825         }
 826
 827         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 828         if (!msg)
 829                 return -ENOMEM;
 830
 831         err = devlink_nl_trap_policer_fill(msg, devlink, policer_item,
 832                                            DEVLINK_CMD_TRAP_POLICER_NEW,
 833                                            info->snd_portid, info->snd_seq, 0);
 834         if (err)
 835                 goto err_trap_policer_fill;
 836
 837         return genlmsg_reply(msg, info);
 838
 839 err_trap_policer_fill:
 840         nlmsg_free(msg);
 841         return err;
 842 }
 843
 844 static int devlink_nl_trap_policer_get_dump_one(struct sk_buff *msg,
 845                                                 struct devlink *devlink,
 846                                                 struct netlink_callback *cb,
 847                                                 int flags)
 848 {
 849         struct devlink_nl_dump_state *state = devlink_dump_state(cb);
 850         struct devlink_trap_policer_item *policer_item;
 851         int idx = 0;
 852         int err = 0;
 853
 854         list_for_each_entry(policer_item, &devlink->trap_policer_list, list) {
 855                 if (idx < state->idx) {
 856                         idx++;
 857                         continue;
 858                 }
 859                 err = devlink_nl_trap_policer_fill(msg, devlink, policer_item,
 860                                                    DEVLINK_CMD_TRAP_POLICER_NEW,
 861                                                    NETLINK_CB(cb->skb).portid,
 862                                                    cb->nlh->nlmsg_seq, flags);
 863                 if (err) {
 864                         state->idx = idx;
 865                         break;
 866                 }
 867                 idx++;
 868         }
 869
 870         return err;
 871 }
 872
 873 int devlink_nl_trap_policer_get_dumpit(struct sk_buff *skb,
 874                                        struct netlink_callback *cb)
 875 {
 876         return devlink_nl_dumpit(skb, cb, devlink_nl_trap_policer_get_dump_one);
 877 }
 878
 879 static int
 880 devlink_trap_policer_set(struct devlink *devlink,
 881                          struct devlink_trap_policer_item *policer_item,
 882                          struct genl_info *info)
 883 {
 884         struct netlink_ext_ack *extack = info->extack;
 885         struct nlattr **attrs = info->attrs;
 886         u64 rate, burst;
 887         int err;
 888
 889         rate = policer_item->rate;
 890         burst = policer_item->burst;
 891
 892         if (attrs[DEVLINK_ATTR_TRAP_POLICER_RATE])
 893                 rate = nla_get_u64(attrs[DEVLINK_ATTR_TRAP_POLICER_RATE]);
 894
 895         if (attrs[DEVLINK_ATTR_TRAP_POLICER_BURST])
 896                 burst = nla_get_u64(attrs[DEVLINK_ATTR_TRAP_POLICER_BURST]);
 897
 898         if (rate < policer_item->policer->min_rate) {
 899                 NL_SET_ERR_MSG(extack, "Policer rate lower than limit");
 900                 return -EINVAL;
 901         }
 902
 903         if (rate > policer_item->policer->max_rate) {
 904                 NL_SET_ERR_MSG(extack, "Policer rate higher than limit");
 905                 return -EINVAL;
 906         }
 907
 908         if (burst < policer_item->policer->min_burst) {
 909                 NL_SET_ERR_MSG(extack, "Policer burst size lower than limit");
 910                 return -EINVAL;
 911         }
 912
 913         if (burst > policer_item->policer->max_burst) {
 914                 NL_SET_ERR_MSG(extack, "Policer burst size higher than limit");
 915                 return -EINVAL;
 916         }
 917
 918         err = devlink->ops->trap_policer_set(devlink, policer_item->policer,
 919                                              rate, burst, info->extack);
 920         if (err)
 921                 return err;
 922
 923         policer_item->rate = rate;
 924         policer_item->burst = burst;
 925
 926         return 0;
 927 }
 928
 929 int devlink_nl_cmd_trap_policer_set_doit(struct sk_buff *skb,
 930                                          struct genl_info *info)
 931 {
 932         struct devlink_trap_policer_item *policer_item;
 933         struct netlink_ext_ack *extack = info->extack;
 934         struct devlink *devlink = info->user_ptr[0];
 935
 936         if (list_empty(&devlink->trap_policer_list))
 937                 return -EOPNOTSUPP;
 938
 939         if (!devlink->ops->trap_policer_set)
 940                 return -EOPNOTSUPP;
 941
 942         policer_item = devlink_trap_policer_item_get_from_info(devlink, info);
 943         if (!policer_item) {
 944                 NL_SET_ERR_MSG(extack, "Device did not register this trap policer");
 945                 return -ENOENT;
 946         }
 947
 948         return devlink_trap_policer_set(devlink, policer_item, info);
 949 }
 950
 951 #define DEVLINK_TRAP(_id, _type)                                              \
 952         {                                                                     \
 953                 .type = DEVLINK_TRAP_TYPE_##_type,                            \
 954                 .id = DEVLINK_TRAP_GENERIC_ID_##_id,                          \
 955                 .name = DEVLINK_TRAP_GENERIC_NAME_##_id,                      \
 956         }
 957
 958 static const struct devlink_trap devlink_trap_generic[] = {
 959         DEVLINK_TRAP(SMAC_MC, DROP),
 960         DEVLINK_TRAP(VLAN_TAG_MISMATCH, DROP),
 961         DEVLINK_TRAP(INGRESS_VLAN_FILTER, DROP),
 962         DEVLINK_TRAP(INGRESS_STP_FILTER, DROP),
 963         DEVLINK_TRAP(EMPTY_TX_LIST, DROP),
 964         DEVLINK_TRAP(PORT_LOOPBACK_FILTER, DROP),
 965         DEVLINK_TRAP(BLACKHOLE_ROUTE, DROP),
 966         DEVLINK_TRAP(TTL_ERROR, EXCEPTION),
 967         DEVLINK_TRAP(TAIL_DROP, DROP),
 968         DEVLINK_TRAP(NON_IP_PACKET, DROP),
 969         DEVLINK_TRAP(UC_DIP_MC_DMAC, DROP),
 970         DEVLINK_TRAP(DIP_LB, DROP),
 971         DEVLINK_TRAP(SIP_MC, DROP),
 972         DEVLINK_TRAP(SIP_LB, DROP),
 973         DEVLINK_TRAP(CORRUPTED_IP_HDR, DROP),
 974         DEVLINK_TRAP(IPV4_SIP_BC, DROP),
 975         DEVLINK_TRAP(IPV6_MC_DIP_RESERVED_SCOPE, DROP),
 976         DEVLINK_TRAP(IPV6_MC_DIP_INTERFACE_LOCAL_SCOPE, DROP),
 977         DEVLINK_TRAP(MTU_ERROR, EXCEPTION),
 978         DEVLINK_TRAP(UNRESOLVED_NEIGH, EXCEPTION),
 979         DEVLINK_TRAP(RPF, EXCEPTION),
 980         DEVLINK_TRAP(REJECT_ROUTE, EXCEPTION),
 981         DEVLINK_TRAP(IPV4_LPM_UNICAST_MISS, EXCEPTION),
 982         DEVLINK_TRAP(IPV6_LPM_UNICAST_MISS, EXCEPTION),
 983         DEVLINK_TRAP(NON_ROUTABLE, DROP),
 984         DEVLINK_TRAP(DECAP_ERROR, EXCEPTION),
 985         DEVLINK_TRAP(OVERLAY_SMAC_MC, DROP),
 986         DEVLINK_TRAP(INGRESS_FLOW_ACTION_DROP, DROP),
 987         DEVLINK_TRAP(EGRESS_FLOW_ACTION_DROP, DROP),
 988         DEVLINK_TRAP(STP, CONTROL),
 989         DEVLINK_TRAP(LACP, CONTROL),
 990         DEVLINK_TRAP(LLDP, CONTROL),
 991         DEVLINK_TRAP(IGMP_QUERY, CONTROL),
 992         DEVLINK_TRAP(IGMP_V1_REPORT, CONTROL),
 993         DEVLINK_TRAP(IGMP_V2_REPORT, CONTROL),
 994         DEVLINK_TRAP(IGMP_V3_REPORT, CONTROL),
 995         DEVLINK_TRAP(IGMP_V2_LEAVE, CONTROL),
 996         DEVLINK_TRAP(MLD_QUERY, CONTROL),
 997         DEVLINK_TRAP(MLD_V1_REPORT, CONTROL),
 998         DEVLINK_TRAP(MLD_V2_REPORT, CONTROL),
 999         DEVLINK_TRAP(MLD_V1_DONE, CONTROL),
1000         DEVLINK_TRAP(IPV4_DHCP, CONTROL),
1001         DEVLINK_TRAP(IPV6_DHCP, CONTROL),
1002         DEVLINK_TRAP(ARP_REQUEST, CONTROL),
1003         DEVLINK_TRAP(ARP_RESPONSE, CONTROL),
1004         DEVLINK_TRAP(ARP_OVERLAY, CONTROL),
1005         DEVLINK_TRAP(IPV6_NEIGH_SOLICIT, CONTROL),
1006         DEVLINK_TRAP(IPV6_NEIGH_ADVERT, CONTROL),
1007         DEVLINK_TRAP(IPV4_BFD, CONTROL),
1008         DEVLINK_TRAP(IPV6_BFD, CONTROL),
1009         DEVLINK_TRAP(IPV4_OSPF, CONTROL),
1010         DEVLINK_TRAP(IPV6_OSPF, CONTROL),
1011         DEVLINK_TRAP(IPV4_BGP, CONTROL),
1012         DEVLINK_TRAP(IPV6_BGP, CONTROL),
1013         DEVLINK_TRAP(IPV4_VRRP, CONTROL),
1014         DEVLINK_TRAP(IPV6_VRRP, CONTROL),
1015         DEVLINK_TRAP(IPV4_PIM, CONTROL),
1016         DEVLINK_TRAP(IPV6_PIM, CONTROL),
1017         DEVLINK_TRAP(UC_LB, CONTROL),
1018         DEVLINK_TRAP(LOCAL_ROUTE, CONTROL),
1019         DEVLINK_TRAP(EXTERNAL_ROUTE, CONTROL),
1020         DEVLINK_TRAP(IPV6_UC_DIP_LINK_LOCAL_SCOPE, CONTROL),
1021         DEVLINK_TRAP(IPV6_DIP_ALL_NODES, CONTROL),
1022         DEVLINK_TRAP(IPV6_DIP_ALL_ROUTERS, CONTROL),
1023         DEVLINK_TRAP(IPV6_ROUTER_SOLICIT, CONTROL),
1024         DEVLINK_TRAP(IPV6_ROUTER_ADVERT, CONTROL),
1025         DEVLINK_TRAP(IPV6_REDIRECT, CONTROL),
1026         DEVLINK_TRAP(IPV4_ROUTER_ALERT, CONTROL),
1027         DEVLINK_TRAP(IPV6_ROUTER_ALERT, CONTROL),
1028         DEVLINK_TRAP(PTP_EVENT, CONTROL),
1029         DEVLINK_TRAP(PTP_GENERAL, CONTROL),
1030         DEVLINK_TRAP(FLOW_ACTION_SAMPLE, CONTROL),
1031         DEVLINK_TRAP(FLOW_ACTION_TRAP, CONTROL),
1032         DEVLINK_TRAP(EARLY_DROP, DROP),
1033         DEVLINK_TRAP(VXLAN_PARSING, DROP),
1034         DEVLINK_TRAP(LLC_SNAP_PARSING, DROP),
1035         DEVLINK_TRAP(VLAN_PARSING, DROP),
1036         DEVLINK_TRAP(PPPOE_PPP_PARSING, DROP),
1037         DEVLINK_TRAP(MPLS_PARSING, DROP),
1038         DEVLINK_TRAP(ARP_PARSING, DROP),
1039         DEVLINK_TRAP(IP_1_PARSING, DROP),
1040         DEVLINK_TRAP(IP_N_PARSING, DROP),
1041         DEVLINK_TRAP(GRE_PARSING, DROP),
1042         DEVLINK_TRAP(UDP_PARSING, DROP),
1043         DEVLINK_TRAP(TCP_PARSING, DROP),
1044         DEVLINK_TRAP(IPSEC_PARSING, DROP),
1045         DEVLINK_TRAP(SCTP_PARSING, DROP),
1046         DEVLINK_TRAP(DCCP_PARSING, DROP),
1047         DEVLINK_TRAP(GTP_PARSING, DROP),
1048         DEVLINK_TRAP(ESP_PARSING, DROP),
1049         DEVLINK_TRAP(BLACKHOLE_NEXTHOP, DROP),
1050         DEVLINK_TRAP(DMAC_FILTER, DROP),
1051         DEVLINK_TRAP(EAPOL, CONTROL),
1052         DEVLINK_TRAP(LOCKED_PORT, DROP),
1053 };
1054
1055 #define DEVLINK_TRAP_GROUP(_id)                                               \
1056         {                                                                     \
1057                 .id = DEVLINK_TRAP_GROUP_GENERIC_ID_##_id,                    \
1058                 .name = DEVLINK_TRAP_GROUP_GENERIC_NAME_##_id,                \
1059         }
1060
1061 static const struct devlink_trap_group devlink_trap_group_generic[] = {
1062         DEVLINK_TRAP_GROUP(L2_DROPS),
1063         DEVLINK_TRAP_GROUP(L3_DROPS),
1064         DEVLINK_TRAP_GROUP(L3_EXCEPTIONS),
1065         DEVLINK_TRAP_GROUP(BUFFER_DROPS),
1066         DEVLINK_TRAP_GROUP(TUNNEL_DROPS),
1067         DEVLINK_TRAP_GROUP(ACL_DROPS),
1068         DEVLINK_TRAP_GROUP(STP),
1069         DEVLINK_TRAP_GROUP(LACP),
1070         DEVLINK_TRAP_GROUP(LLDP),
1071         DEVLINK_TRAP_GROUP(MC_SNOOPING),
1072         DEVLINK_TRAP_GROUP(DHCP),
1073         DEVLINK_TRAP_GROUP(NEIGH_DISCOVERY),
1074         DEVLINK_TRAP_GROUP(BFD),
1075         DEVLINK_TRAP_GROUP(OSPF),
1076         DEVLINK_TRAP_GROUP(BGP),
1077         DEVLINK_TRAP_GROUP(VRRP),
1078         DEVLINK_TRAP_GROUP(PIM),
1079         DEVLINK_TRAP_GROUP(UC_LB),
1080         DEVLINK_TRAP_GROUP(LOCAL_DELIVERY),
1081         DEVLINK_TRAP_GROUP(EXTERNAL_DELIVERY),
1082         DEVLINK_TRAP_GROUP(IPV6),
1083         DEVLINK_TRAP_GROUP(PTP_EVENT),
1084         DEVLINK_TRAP_GROUP(PTP_GENERAL),
1085         DEVLINK_TRAP_GROUP(ACL_SAMPLE),
1086         DEVLINK_TRAP_GROUP(ACL_TRAP),
1087         DEVLINK_TRAP_GROUP(PARSER_ERROR_DROPS),
1088         DEVLINK_TRAP_GROUP(EAPOL),
1089 };
1090
1091 static int devlink_trap_generic_verify(const struct devlink_trap *trap)
1092 {
1093         if (trap->id > DEVLINK_TRAP_GENERIC_ID_MAX)
1094                 return -EINVAL;
1095
1096         if (strcmp(trap->name, devlink_trap_generic[trap->id].name))
1097                 return -EINVAL;
1098
1099         if (trap->type != devlink_trap_generic[trap->id].type)
1100                 return -EINVAL;
1101
1102         return 0;
1103 }
1104
1105 static int devlink_trap_driver_verify(const struct devlink_trap *trap)
1106 {
1107         int i;
1108
1109         if (trap->id <= DEVLINK_TRAP_GENERIC_ID_MAX)
1110                 return -EINVAL;
1111
1112         for (i = 0; i < ARRAY_SIZE(devlink_trap_generic); i++) {
1113                 if (!strcmp(trap->name, devlink_trap_generic[i].name))
1114                         return -EEXIST;
1115         }
1116
1117         return 0;
1118 }
1119
1120 static int devlink_trap_verify(const struct devlink_trap *trap)
1121 {
1122         if (!trap || !trap->name)
1123                 return -EINVAL;
1124
1125         if (trap->generic)
1126                 return devlink_trap_generic_verify(trap);
1127         else
1128                 return devlink_trap_driver_verify(trap);
1129 }
1130
1131 static int
1132 devlink_trap_group_generic_verify(const struct devlink_trap_group *group)
1133 {
1134         if (group->id > DEVLINK_TRAP_GROUP_GENERIC_ID_MAX)
1135                 return -EINVAL;
1136
1137         if (strcmp(group->name, devlink_trap_group_generic[group->id].name))
1138                 return -EINVAL;
1139
1140         return 0;
1141 }
1142
1143 static int
1144 devlink_trap_group_driver_verify(const struct devlink_trap_group *group)
1145 {
1146         int i;
1147
1148         if (group->id <= DEVLINK_TRAP_GROUP_GENERIC_ID_MAX)
1149                 return -EINVAL;
1150
1151         for (i = 0; i < ARRAY_SIZE(devlink_trap_group_generic); i++) {
1152                 if (!strcmp(group->name, devlink_trap_group_generic[i].name))
1153                         return -EEXIST;
1154         }
1155
1156         return 0;
1157 }
1158
1159 static int devlink_trap_group_verify(const struct devlink_trap_group *group)
1160 {
1161         if (group->generic)
1162                 return devlink_trap_group_generic_verify(group);
1163         else
1164                 return devlink_trap_group_driver_verify(group);
1165 }
1166
1167 static void
1168 devlink_trap_group_notify(struct devlink *devlink,
1169                           const struct devlink_trap_group_item *group_item,
1170                           enum devlink_command cmd)
1171 {
1172         struct sk_buff *msg;
1173         int err;
1174
1175         WARN_ON_ONCE(cmd != DEVLINK_CMD_TRAP_GROUP_NEW &&
1176                      cmd != DEVLINK_CMD_TRAP_GROUP_DEL);
1177         if (!xa_get_mark(&devlinks, devlink->index, DEVLINK_REGISTERED))
1178                 return;
1179
1180         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1181         if (!msg)
1182                 return;
1183
1184         err = devlink_nl_trap_group_fill(msg, devlink, group_item, cmd, 0, 0,
1185                                          0);
1186         if (err) {
1187                 nlmsg_free(msg);
1188                 return;
1189         }
1190
1191         genlmsg_multicast_netns(&devlink_nl_family, devlink_net(devlink),
1192                                 msg, 0, DEVLINK_MCGRP_CONFIG, GFP_KERNEL);
1193 }
1194
1195 void devlink_trap_groups_notify_register(struct devlink *devlink)
1196 {
1197         struct devlink_trap_group_item *group_item;
1198
1199         list_for_each_entry(group_item, &devlink->trap_group_list, list)
1200                 devlink_trap_group_notify(devlink, group_item,
1201                                           DEVLINK_CMD_TRAP_GROUP_NEW);
1202 }
1203
1204 void devlink_trap_groups_notify_unregister(struct devlink *devlink)
1205 {
1206         struct devlink_trap_group_item *group_item;
1207
1208         list_for_each_entry_reverse(group_item, &devlink->trap_group_list, list)
1209                 devlink_trap_group_notify(devlink, group_item,
1210                                           DEVLINK_CMD_TRAP_GROUP_DEL);
1211 }
1212
1213 static int
1214 devlink_trap_item_group_link(struct devlink *devlink,
1215                              struct devlink_trap_item *trap_item)
1216 {
1217         u16 group_id = trap_item->trap->init_group_id;
1218         struct devlink_trap_group_item *group_item;
1219
1220         group_item = devlink_trap_group_item_lookup_by_id(devlink, group_id);
1221         if (WARN_ON_ONCE(!group_item))
1222                 return -EINVAL;
1223
1224         trap_item->group_item = group_item;
1225
1226         return 0;
1227 }
1228
1229 static void devlink_trap_notify(struct devlink *devlink,
1230                                 const struct devlink_trap_item *trap_item,
1231                                 enum devlink_command cmd)
1232 {
1233         struct sk_buff *msg;
1234         int err;
1235
1236         WARN_ON_ONCE(cmd != DEVLINK_CMD_TRAP_NEW &&
1237                      cmd != DEVLINK_CMD_TRAP_DEL);
1238         if (!xa_get_mark(&devlinks, devlink->index, DEVLINK_REGISTERED))
1239                 return;
1240
1241         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1242         if (!msg)
1243                 return;
1244
1245         err = devlink_nl_trap_fill(msg, devlink, trap_item, cmd, 0, 0, 0);
1246         if (err) {
1247                 nlmsg_free(msg);
1248                 return;
1249         }
1250
1251         genlmsg_multicast_netns(&devlink_nl_family, devlink_net(devlink),
1252                                 msg, 0, DEVLINK_MCGRP_CONFIG, GFP_KERNEL);
1253 }
1254
1255 void devlink_traps_notify_register(struct devlink *devlink)
1256 {
1257         struct devlink_trap_item *trap_item;
1258
1259         list_for_each_entry(trap_item, &devlink->trap_list, list)
1260                 devlink_trap_notify(devlink, trap_item, DEVLINK_CMD_TRAP_NEW);
1261 }
1262
1263 void devlink_traps_notify_unregister(struct devlink *devlink)
1264 {
1265         struct devlink_trap_item *trap_item;
1266
1267         list_for_each_entry_reverse(trap_item, &devlink->trap_list, list)
1268                 devlink_trap_notify(devlink, trap_item, DEVLINK_CMD_TRAP_DEL);
1269 }
1270
1271 static int
1272 devlink_trap_register(struct devlink *devlink,
1273                       const struct devlink_trap *trap, void *priv)
1274 {
1275         struct devlink_trap_item *trap_item;
1276         int err;
1277
1278         if (devlink_trap_item_lookup(devlink, trap->name))
1279                 return -EEXIST;
1280
1281         trap_item = kzalloc(sizeof(*trap_item), GFP_KERNEL);
1282         if (!trap_item)
1283                 return -ENOMEM;
1284
1285         trap_item->stats = netdev_alloc_pcpu_stats(struct devlink_stats);
1286         if (!trap_item->stats) {
1287                 err = -ENOMEM;
1288                 goto err_stats_alloc;
1289         }
1290
1291         trap_item->trap = trap;
1292         trap_item->action = trap->init_action;
1293         trap_item->priv = priv;
1294
1295         err = devlink_trap_item_group_link(devlink, trap_item);
1296         if (err)
1297                 goto err_group_link;
1298
1299         err = devlink->ops->trap_init(devlink, trap, trap_item);
1300         if (err)
1301                 goto err_trap_init;
1302
1303         list_add_tail(&trap_item->list, &devlink->trap_list);
1304         devlink_trap_notify(devlink, trap_item, DEVLINK_CMD_TRAP_NEW);
1305
1306         return 0;
1307
1308 err_trap_init:
1309 err_group_link:
1310         free_percpu(trap_item->stats);
1311 err_stats_alloc:
1312         kfree(trap_item);
1313         return err;
1314 }
1315
1316 static void devlink_trap_unregister(struct devlink *devlink,
1317                                     const struct devlink_trap *trap)
1318 {
1319         struct devlink_trap_item *trap_item;
1320
1321         trap_item = devlink_trap_item_lookup(devlink, trap->name);
1322         if (WARN_ON_ONCE(!trap_item))
1323                 return;
1324
1325         devlink_trap_notify(devlink, trap_item, DEVLINK_CMD_TRAP_DEL);
1326         list_del(&trap_item->list);
1327         if (devlink->ops->trap_fini)
1328                 devlink->ops->trap_fini(devlink, trap, trap_item);
1329         free_percpu(trap_item->stats);
1330         kfree(trap_item);
1331 }
1332
1333 static void devlink_trap_disable(struct devlink *devlink,
1334                                  const struct devlink_trap *trap)
1335 {
1336         struct devlink_trap_item *trap_item;
1337
1338         trap_item = devlink_trap_item_lookup(devlink, trap->name);
1339         if (WARN_ON_ONCE(!trap_item))
1340                 return;
1341
1342         devlink->ops->trap_action_set(devlink, trap, DEVLINK_TRAP_ACTION_DROP,
1343                                       NULL);
1344         trap_item->action = DEVLINK_TRAP_ACTION_DROP;
1345 }
1346
1347 /**
1348  * devl_traps_register - Register packet traps with devlink.
1349  * @devlink: devlink.
1350  * @traps: Packet traps.
1351  * @traps_count: Count of provided packet traps.
1352  * @priv: Driver private information.
1353  *
1354  * Return: Non-zero value on failure.
1355  */
1356 int devl_traps_register(struct devlink *devlink,
1357                         const struct devlink_trap *traps,
1358                         size_t traps_count, void *priv)
1359 {
1360         int i, err;
1361
1362         if (!devlink->ops->trap_init || !devlink->ops->trap_action_set)
1363                 return -EINVAL;
1364
1365         devl_assert_locked(devlink);
1366         for (i = 0; i < traps_count; i++) {
1367                 const struct devlink_trap *trap = &traps[i];
1368
1369                 err = devlink_trap_verify(trap);
1370                 if (err)
1371                         goto err_trap_verify;
1372
1373                 err = devlink_trap_register(devlink, trap, priv);
1374                 if (err)
1375                         goto err_trap_register;
1376         }
1377
1378         return 0;
1379
1380 err_trap_register:
1381 err_trap_verify:
1382         for (i--; i >= 0; i--)
1383                 devlink_trap_unregister(devlink, &traps[i]);
1384         return err;
1385 }
1386 EXPORT_SYMBOL_GPL(devl_traps_register);
1387
1388 /**
1389  * devlink_traps_register - Register packet traps with devlink.
1390  * @devlink: devlink.
1391  * @traps: Packet traps.
1392  * @traps_count: Count of provided packet traps.
1393  * @priv: Driver private information.
1394  *
1395  * Context: Takes and release devlink->lock <mutex>.
1396  *
1397  * Return: Non-zero value on failure.
1398  */
1399 int devlink_traps_register(struct devlink *devlink,
1400                            const struct devlink_trap *traps,
1401                            size_t traps_count, void *priv)
1402 {
1403         int err;
1404
1405         devl_lock(devlink);
1406         err = devl_traps_register(devlink, traps, traps_count, priv);
1407         devl_unlock(devlink);
1408         return err;
1409 }
1410 EXPORT_SYMBOL_GPL(devlink_traps_register);
1411
1412 /**
1413  * devl_traps_unregister - Unregister packet traps from devlink.
1414  * @devlink: devlink.
1415  * @traps: Packet traps.
1416  * @traps_count: Count of provided packet traps.
1417  */
1418 void devl_traps_unregister(struct devlink *devlink,
1419                            const struct devlink_trap *traps,
1420                            size_t traps_count)
1421 {
1422         int i;
1423
1424         devl_assert_locked(devlink);
1425         /* Make sure we do not have any packets in-flight while unregistering
1426          * traps by disabling all of them and waiting for a grace period.
1427          */
1428         for (i = traps_count - 1; i >= 0; i--)
1429                 devlink_trap_disable(devlink, &traps[i]);
1430         synchronize_rcu();
1431         for (i = traps_count - 1; i >= 0; i--)
1432                 devlink_trap_unregister(devlink, &traps[i]);
1433 }
1434 EXPORT_SYMBOL_GPL(devl_traps_unregister);
1435
1436 /**
1437  * devlink_traps_unregister - Unregister packet traps from devlink.
1438  * @devlink: devlink.
1439  * @traps: Packet traps.
1440  * @traps_count: Count of provided packet traps.
1441  *
1442  * Context: Takes and release devlink->lock <mutex>.
1443  */
1444 void devlink_traps_unregister(struct devlink *devlink,
1445                               const struct devlink_trap *traps,
1446                               size_t traps_count)
1447 {
1448         devl_lock(devlink);
1449         devl_traps_unregister(devlink, traps, traps_count);
1450         devl_unlock(devlink);
1451 }
1452 EXPORT_SYMBOL_GPL(devlink_traps_unregister);
1453
1454 static void
1455 devlink_trap_stats_update(struct devlink_stats __percpu *trap_stats,
1456                           size_t skb_len)
1457 {
1458         struct devlink_stats *stats;
1459
1460         stats = this_cpu_ptr(trap_stats);
1461         u64_stats_update_begin(&stats->syncp);
1462         u64_stats_add(&stats->rx_bytes, skb_len);
1463         u64_stats_inc(&stats->rx_packets);
1464         u64_stats_update_end(&stats->syncp);
1465 }
1466
1467 static void
1468 devlink_trap_report_metadata_set(struct devlink_trap_metadata *metadata,
1469                                  const struct devlink_trap_item *trap_item,
1470                                  struct devlink_port *in_devlink_port,
1471                                  const struct flow_action_cookie *fa_cookie)
1472 {
1473         metadata->trap_name = trap_item->trap->name;
1474         metadata->trap_group_name = trap_item->group_item->group->name;
1475         metadata->fa_cookie = fa_cookie;
1476         metadata->trap_type = trap_item->trap->type;
1477
1478         spin_lock(&in_devlink_port->type_lock);
1479         if (in_devlink_port->type == DEVLINK_PORT_TYPE_ETH)
1480                 metadata->input_dev = in_devlink_port->type_eth.netdev;
1481         spin_unlock(&in_devlink_port->type_lock);
1482 }
1483
1484 /**
1485  * devlink_trap_report - Report trapped packet to drop monitor.
1486  * @devlink: devlink.
1487  * @skb: Trapped packet.
1488  * @trap_ctx: Trap context.
1489  * @in_devlink_port: Input devlink port.
1490  * @fa_cookie: Flow action cookie. Could be NULL.
1491  */
1492 void devlink_trap_report(struct devlink *devlink, struct sk_buff *skb,
1493                          void *trap_ctx, struct devlink_port *in_devlink_port,
1494                          const struct flow_action_cookie *fa_cookie)
1495
1496 {
1497         struct devlink_trap_item *trap_item = trap_ctx;
1498
1499         devlink_trap_stats_update(trap_item->stats, skb->len);
1500         devlink_trap_stats_update(trap_item->group_item->stats, skb->len);
1501
1502         if (tracepoint_enabled(devlink_trap_report)) {
1503                 struct devlink_trap_metadata metadata = {};
1504
1505                 devlink_trap_report_metadata_set(&metadata, trap_item,
1506                                                  in_devlink_port, fa_cookie);
1507                 trace_devlink_trap_report(devlink, skb, &metadata);
1508         }
1509 }
1510 EXPORT_SYMBOL_GPL(devlink_trap_report);
1511
1512 /**
1513  * devlink_trap_ctx_priv - Trap context to driver private information.
1514  * @trap_ctx: Trap context.
1515  *
1516  * Return: Driver private information passed during registration.
1517  */
1518 void *devlink_trap_ctx_priv(void *trap_ctx)
1519 {
1520         struct devlink_trap_item *trap_item = trap_ctx;
1521
1522         return trap_item->priv;
1523 }
1524 EXPORT_SYMBOL_GPL(devlink_trap_ctx_priv);
1525
1526 static int
1527 devlink_trap_group_item_policer_link(struct devlink *devlink,
1528                                      struct devlink_trap_group_item *group_item)
1529 {
1530         u32 policer_id = group_item->group->init_policer_id;
1531         struct devlink_trap_policer_item *policer_item;
1532
1533         if (policer_id == 0)
1534                 return 0;
1535
1536         policer_item = devlink_trap_policer_item_lookup(devlink, policer_id);
1537         if (WARN_ON_ONCE(!policer_item))
1538                 return -EINVAL;
1539
1540         group_item->policer_item = policer_item;
1541
1542         return 0;
1543 }
1544
1545 static int
1546 devlink_trap_group_register(struct devlink *devlink,
1547                             const struct devlink_trap_group *group)
1548 {
1549         struct devlink_trap_group_item *group_item;
1550         int err;
1551
1552         if (devlink_trap_group_item_lookup(devlink, group->name))
1553                 return -EEXIST;
1554
1555         group_item = kzalloc(sizeof(*group_item), GFP_KERNEL);
1556         if (!group_item)
1557                 return -ENOMEM;
1558
1559         group_item->stats = netdev_alloc_pcpu_stats(struct devlink_stats);
1560         if (!group_item->stats) {
1561                 err = -ENOMEM;
1562                 goto err_stats_alloc;
1563         }
1564
1565         group_item->group = group;
1566
1567         err = devlink_trap_group_item_policer_link(devlink, group_item);
1568         if (err)
1569                 goto err_policer_link;
1570
1571         if (devlink->ops->trap_group_init) {
1572                 err = devlink->ops->trap_group_init(devlink, group);
1573                 if (err)
1574                         goto err_group_init;
1575         }
1576
1577         list_add_tail(&group_item->list, &devlink->trap_group_list);
1578         devlink_trap_group_notify(devlink, group_item,
1579                                   DEVLINK_CMD_TRAP_GROUP_NEW);
1580
1581         return 0;
1582
1583 err_group_init:
1584 err_policer_link:
1585         free_percpu(group_item->stats);
1586 err_stats_alloc:
1587         kfree(group_item);
1588         return err;
1589 }
1590
1591 static void
1592 devlink_trap_group_unregister(struct devlink *devlink,
1593                               const struct devlink_trap_group *group)
1594 {
1595         struct devlink_trap_group_item *group_item;
1596
1597         group_item = devlink_trap_group_item_lookup(devlink, group->name);
1598         if (WARN_ON_ONCE(!group_item))
1599                 return;
1600
1601         devlink_trap_group_notify(devlink, group_item,
1602                                   DEVLINK_CMD_TRAP_GROUP_DEL);
1603         list_del(&group_item->list);
1604         free_percpu(group_item->stats);
1605         kfree(group_item);
1606 }
1607
1608 /**
1609  * devl_trap_groups_register - Register packet trap groups with devlink.
1610  * @devlink: devlink.
1611  * @groups: Packet trap groups.
1612  * @groups_count: Count of provided packet trap groups.
1613  *
1614  * Return: Non-zero value on failure.
1615  */
1616 int devl_trap_groups_register(struct devlink *devlink,
1617                               const struct devlink_trap_group *groups,
1618                               size_t groups_count)
1619 {
1620         int i, err;
1621
1622         devl_assert_locked(devlink);
1623         for (i = 0; i < groups_count; i++) {
1624                 const struct devlink_trap_group *group = &groups[i];
1625
1626                 err = devlink_trap_group_verify(group);
1627                 if (err)
1628                         goto err_trap_group_verify;
1629
1630                 err = devlink_trap_group_register(devlink, group);
1631                 if (err)
1632                         goto err_trap_group_register;
1633         }
1634
1635         return 0;
1636
1637 err_trap_group_register:
1638 err_trap_group_verify:
1639         for (i--; i >= 0; i--)
1640                 devlink_trap_group_unregister(devlink, &groups[i]);
1641         return err;
1642 }
1643 EXPORT_SYMBOL_GPL(devl_trap_groups_register);
1644
1645 /**
1646  * devlink_trap_groups_register - Register packet trap groups with devlink.
1647  * @devlink: devlink.
1648  * @groups: Packet trap groups.
1649  * @groups_count: Count of provided packet trap groups.
1650  *
1651  * Context: Takes and release devlink->lock <mutex>.
1652  *
1653  * Return: Non-zero value on failure.
1654  */
1655 int devlink_trap_groups_register(struct devlink *devlink,
1656                                  const struct devlink_trap_group *groups,
1657                                  size_t groups_count)
1658 {
1659         int err;
1660
1661         devl_lock(devlink);
1662         err = devl_trap_groups_register(devlink, groups, groups_count);
1663         devl_unlock(devlink);
1664         return err;
1665 }
1666 EXPORT_SYMBOL_GPL(devlink_trap_groups_register);
1667
1668 /**
1669  * devl_trap_groups_unregister - Unregister packet trap groups from devlink.
1670  * @devlink: devlink.
1671  * @groups: Packet trap groups.
1672  * @groups_count: Count of provided packet trap groups.
1673  */
1674 void devl_trap_groups_unregister(struct devlink *devlink,
1675                                  const struct devlink_trap_group *groups,
1676                                  size_t groups_count)
1677 {
1678         int i;
1679
1680         devl_assert_locked(devlink);
1681         for (i = groups_count - 1; i >= 0; i--)
1682                 devlink_trap_group_unregister(devlink, &groups[i]);
1683 }
1684 EXPORT_SYMBOL_GPL(devl_trap_groups_unregister);
1685
1686 /**
1687  * devlink_trap_groups_unregister - Unregister packet trap groups from devlink.
1688  * @devlink: devlink.
1689  * @groups: Packet trap groups.
1690  * @groups_count: Count of provided packet trap groups.
1691  *
1692  * Context: Takes and release devlink->lock <mutex>.
1693  */
1694 void devlink_trap_groups_unregister(struct devlink *devlink,
1695                                     const struct devlink_trap_group *groups,
1696                                     size_t groups_count)
1697 {
1698         devl_lock(devlink);
1699         devl_trap_groups_unregister(devlink, groups, groups_count);
1700         devl_unlock(devlink);
1701 }
1702 EXPORT_SYMBOL_GPL(devlink_trap_groups_unregister);
1703
1704 static void
1705 devlink_trap_policer_notify(struct devlink *devlink,
1706                             const struct devlink_trap_policer_item *policer_item,
1707                             enum devlink_command cmd)
1708 {
1709         struct sk_buff *msg;
1710         int err;
1711
1712         WARN_ON_ONCE(cmd != DEVLINK_CMD_TRAP_POLICER_NEW &&
1713                      cmd != DEVLINK_CMD_TRAP_POLICER_DEL);
1714         if (!xa_get_mark(&devlinks, devlink->index, DEVLINK_REGISTERED))
1715                 return;
1716
1717         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1718         if (!msg)
1719                 return;
1720
1721         err = devlink_nl_trap_policer_fill(msg, devlink, policer_item, cmd, 0,
1722                                            0, 0);
1723         if (err) {
1724                 nlmsg_free(msg);
1725                 return;
1726         }
1727
1728         genlmsg_multicast_netns(&devlink_nl_family, devlink_net(devlink),
1729                                 msg, 0, DEVLINK_MCGRP_CONFIG, GFP_KERNEL);
1730 }
1731
1732 void devlink_trap_policers_notify_register(struct devlink *devlink)
1733 {
1734         struct devlink_trap_policer_item *policer_item;
1735
1736         list_for_each_entry(policer_item, &devlink->trap_policer_list, list)
1737                 devlink_trap_policer_notify(devlink, policer_item,
1738                                             DEVLINK_CMD_TRAP_POLICER_NEW);
1739 }
1740
1741 void devlink_trap_policers_notify_unregister(struct devlink *devlink)
1742 {
1743         struct devlink_trap_policer_item *policer_item;
1744
1745         list_for_each_entry_reverse(policer_item, &devlink->trap_policer_list,
1746                                     list)
1747                 devlink_trap_policer_notify(devlink, policer_item,
1748                                             DEVLINK_CMD_TRAP_POLICER_DEL);
1749 }
1750
1751 static int
1752 devlink_trap_policer_register(struct devlink *devlink,
1753                               const struct devlink_trap_policer *policer)
1754 {
1755         struct devlink_trap_policer_item *policer_item;
1756         int err;
1757
1758         if (devlink_trap_policer_item_lookup(devlink, policer->id))
1759                 return -EEXIST;
1760
1761         policer_item = kzalloc(sizeof(*policer_item), GFP_KERNEL);
1762         if (!policer_item)
1763                 return -ENOMEM;
1764
1765         policer_item->policer = policer;
1766         policer_item->rate = policer->init_rate;
1767         policer_item->burst = policer->init_burst;
1768
1769         if (devlink->ops->trap_policer_init) {
1770                 err = devlink->ops->trap_policer_init(devlink, policer);
1771                 if (err)
1772                         goto err_policer_init;
1773         }
1774
1775         list_add_tail(&policer_item->list, &devlink->trap_policer_list);
1776         devlink_trap_policer_notify(devlink, policer_item,
1777                                     DEVLINK_CMD_TRAP_POLICER_NEW);
1778
1779         return 0;
1780
1781 err_policer_init:
1782         kfree(policer_item);
1783         return err;
1784 }
1785
1786 static void
1787 devlink_trap_policer_unregister(struct devlink *devlink,
1788                                 const struct devlink_trap_policer *policer)
1789 {
1790         struct devlink_trap_policer_item *policer_item;
1791
1792         policer_item = devlink_trap_policer_item_lookup(devlink, policer->id);
1793         if (WARN_ON_ONCE(!policer_item))
1794                 return;
1795
1796         devlink_trap_policer_notify(devlink, policer_item,
1797                                     DEVLINK_CMD_TRAP_POLICER_DEL);
1798         list_del(&policer_item->list);
1799         if (devlink->ops->trap_policer_fini)
1800                 devlink->ops->trap_policer_fini(devlink, policer);
1801         kfree(policer_item);
1802 }
1803
1804 /**
1805  * devl_trap_policers_register - Register packet trap policers with devlink.
1806  * @devlink: devlink.
1807  * @policers: Packet trap policers.
1808  * @policers_count: Count of provided packet trap policers.
1809  *
1810  * Return: Non-zero value on failure.
1811  */
1812 int
1813 devl_trap_policers_register(struct devlink *devlink,
1814                             const struct devlink_trap_policer *policers,
1815                             size_t policers_count)
1816 {
1817         int i, err;
1818
1819         devl_assert_locked(devlink);
1820         for (i = 0; i < policers_count; i++) {
1821                 const struct devlink_trap_policer *policer = &policers[i];
1822
1823                 if (WARN_ON(policer->id == 0 ||
1824                             policer->max_rate < policer->min_rate ||
1825                             policer->max_burst < policer->min_burst)) {
1826                         err = -EINVAL;
1827                         goto err_trap_policer_verify;
1828                 }
1829
1830                 err = devlink_trap_policer_register(devlink, policer);
1831                 if (err)
1832                         goto err_trap_policer_register;
1833         }
1834         return 0;
1835
1836 err_trap_policer_register:
1837 err_trap_policer_verify:
1838         for (i--; i >= 0; i--)
1839                 devlink_trap_policer_unregister(devlink, &policers[i]);
1840         return err;
1841 }
1842 EXPORT_SYMBOL_GPL(devl_trap_policers_register);
1843
1844 /**
1845  * devl_trap_policers_unregister - Unregister packet trap policers from devlink.
1846  * @devlink: devlink.
1847  * @policers: Packet trap policers.
1848  * @policers_count: Count of provided packet trap policers.
1849  */
1850 void
1851 devl_trap_policers_unregister(struct devlink *devlink,
1852                               const struct devlink_trap_policer *policers,
1853                               size_t policers_count)
1854 {
1855         int i;
1856
1857         devl_assert_locked(devlink);
1858         for (i = policers_count - 1; i >= 0; i--)
1859                 devlink_trap_policer_unregister(devlink, &policers[i]);
1860 }
1861 EXPORT_SYMBOL_GPL(devl_trap_policers_unregister);