net/core/drop_monitor.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Monitoring code for network dropped packet alerts
   4  *
   5  * Copyright (C) 2009 Neil Horman <nhorman@tuxdriver.com>
   6  */
   7
   8 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   9
  10 #include <linux/netdevice.h>
  11 #include <linux/etherdevice.h>
  12 #include <linux/string.h>
  13 #include <linux/if_arp.h>
  14 #include <linux/inetdevice.h>
  15 #include <linux/inet.h>
  16 #include <linux/interrupt.h>
  17 #include <linux/netpoll.h>
  18 #include <linux/sched.h>
  19 #include <linux/delay.h>
  20 #include <linux/types.h>
  21 #include <linux/workqueue.h>
  22 #include <linux/netlink.h>
  23 #include <linux/net_dropmon.h>
  24 #include <linux/percpu.h>
  25 #include <linux/timer.h>
  26 #include <linux/bitops.h>
  27 #include <linux/slab.h>
  28 #include <linux/module.h>
  29 #include <net/genetlink.h>
  30 #include <net/netevent.h>
  31 #include <net/flow_offload.h>
  32 #include <net/devlink.h>
  33
  34 #include <trace/events/skb.h>
  35 #include <trace/events/napi.h>
  36 #include <trace/events/devlink.h>
  37
  38 #include <asm/unaligned.h>
  39
  40 #define TRACE_ON 1
  41 #define TRACE_OFF 0
  42
  43 /*
  44  * Globals, our netlink socket pointer
  45  * and the work handle that will send up
  46  * netlink alerts
  47  */
  48 static int trace_state = TRACE_OFF;
  49 static bool monitor_hw;
  50
  51 /* net_dm_mutex
  52  *
  53  * An overall lock guarding every operation coming from userspace.
  54  */
  55 static DEFINE_MUTEX(net_dm_mutex);
  56
  57 struct net_dm_stats {
  58         u64_stats_t dropped;
  59         struct u64_stats_sync syncp;
  60 };
  61
  62 #define NET_DM_MAX_HW_TRAP_NAME_LEN 40
  63
  64 struct net_dm_hw_entry {
  65         char trap_name[NET_DM_MAX_HW_TRAP_NAME_LEN];
  66         u32 count;
  67 };
  68
  69 struct net_dm_hw_entries {
  70         u32 num_entries;
  71         struct net_dm_hw_entry entries[];
  72 };
  73
  74 struct per_cpu_dm_data {
  75         spinlock_t              lock;   /* Protects 'skb', 'hw_entries' and
  76                                          * 'send_timer'
  77                                          */
  78         union {
  79                 struct sk_buff                  *skb;
  80                 struct net_dm_hw_entries        *hw_entries;
  81         };
  82         struct sk_buff_head     drop_queue;
  83         struct work_struct      dm_alert_work;
  84         struct timer_list       send_timer;
  85         struct net_dm_stats     stats;
  86 };
  87
  88 struct dm_hw_stat_delta {
  89         unsigned long last_rx;
  90         unsigned long last_drop_val;
  91         struct rcu_head rcu;
  92 };
  93
  94 static struct genl_family net_drop_monitor_family;
  95
  96 static DEFINE_PER_CPU(struct per_cpu_dm_data, dm_cpu_data);
  97 static DEFINE_PER_CPU(struct per_cpu_dm_data, dm_hw_cpu_data);
  98
  99 static int dm_hit_limit = 64;
 100 static int dm_delay = 1;
 101 static unsigned long dm_hw_check_delta = 2*HZ;
 102
 103 static enum net_dm_alert_mode net_dm_alert_mode = NET_DM_ALERT_MODE_SUMMARY;
 104 static u32 net_dm_trunc_len;
 105 static u32 net_dm_queue_len = 1000;
 106
 107 struct net_dm_alert_ops {
 108         void (*kfree_skb_probe)(void *ignore, struct sk_buff *skb,
 109                                 void *location,
 110                                 enum skb_drop_reason reason);
 111         void (*napi_poll_probe)(void *ignore, struct napi_struct *napi,
 112                                 int work, int budget);
 113         void (*work_item_func)(struct work_struct *work);
 114         void (*hw_work_item_func)(struct work_struct *work);
 115         void (*hw_trap_probe)(void *ignore, const struct devlink *devlink,
 116                               struct sk_buff *skb,
 117                               const struct devlink_trap_metadata *metadata);
 118 };
 119
 120 struct net_dm_skb_cb {
 121         union {
 122                 struct devlink_trap_metadata *hw_metadata;
 123                 void *pc;
 124         };
 125         enum skb_drop_reason reason;
 126 };
 127
 128 #define NET_DM_SKB_CB(__skb) ((struct net_dm_skb_cb *)&((__skb)->cb[0]))
 129
 130 static struct sk_buff *reset_per_cpu_data(struct per_cpu_dm_data *data)
 131 {
 132         size_t al;
 133         struct net_dm_alert_msg *msg;
 134         struct nlattr *nla;
 135         struct sk_buff *skb;
 136         unsigned long flags;
 137         void *msg_header;
 138
 139         al = sizeof(struct net_dm_alert_msg);
 140         al += dm_hit_limit * sizeof(struct net_dm_drop_point);
 141         al += sizeof(struct nlattr);
 142
 143         skb = genlmsg_new(al, GFP_KERNEL);
 144
 145         if (!skb)
 146                 goto err;
 147
 148         msg_header = genlmsg_put(skb, 0, 0, &net_drop_monitor_family,
 149                                  0, NET_DM_CMD_ALERT);
 150         if (!msg_header) {
 151                 nlmsg_free(skb);
 152                 skb = NULL;
 153                 goto err;
 154         }
 155         nla = nla_reserve(skb, NLA_UNSPEC,
 156                           sizeof(struct net_dm_alert_msg));
 157         if (!nla) {
 158                 nlmsg_free(skb);
 159                 skb = NULL;
 160                 goto err;
 161         }
 162         msg = nla_data(nla);
 163         memset(msg, 0, al);
 164         goto out;
 165
 166 err:
 167         mod_timer(&data->send_timer, jiffies + HZ / 10);
 168 out:
 169         spin_lock_irqsave(&data->lock, flags);
 170         swap(data->skb, skb);
 171         spin_unlock_irqrestore(&data->lock, flags);
 172
 173         if (skb) {
 174                 struct nlmsghdr *nlh = (struct nlmsghdr *)skb->data;
 175                 struct genlmsghdr *gnlh = (struct genlmsghdr *)nlmsg_data(nlh);
 176
 177                 genlmsg_end(skb, genlmsg_data(gnlh));
 178         }
 179
 180         return skb;
 181 }
 182
 183 static const struct genl_multicast_group dropmon_mcgrps[] = {
 184         { .name = "events", },
 185 };
 186
 187 static void send_dm_alert(struct work_struct *work)
 188 {
 189         struct sk_buff *skb;
 190         struct per_cpu_dm_data *data;
 191
 192         data = container_of(work, struct per_cpu_dm_data, dm_alert_work);
 193
 194         skb = reset_per_cpu_data(data);
 195
 196         if (skb)
 197                 genlmsg_multicast(&net_drop_monitor_family, skb, 0,
 198                                   0, GFP_KERNEL);
 199 }
 200
 201 /*
 202  * This is the timer function to delay the sending of an alert
 203  * in the event that more drops will arrive during the
 204  * hysteresis period.
 205  */
 206 static void sched_send_work(struct timer_list *t)
 207 {
 208         struct per_cpu_dm_data *data = from_timer(data, t, send_timer);
 209
 210         schedule_work(&data->dm_alert_work);
 211 }
 212
 213 static void trace_drop_common(struct sk_buff *skb, void *location)
 214 {
 215         struct net_dm_alert_msg *msg;
 216         struct net_dm_drop_point *point;
 217         struct nlmsghdr *nlh;
 218         struct nlattr *nla;
 219         int i;
 220         struct sk_buff *dskb;
 221         struct per_cpu_dm_data *data;
 222         unsigned long flags;
 223
 224         local_irq_save(flags);
 225         data = this_cpu_ptr(&dm_cpu_data);
 226         spin_lock(&data->lock);
 227         dskb = data->skb;
 228
 229         if (!dskb)
 230                 goto out;
 231
 232         nlh = (struct nlmsghdr *)dskb->data;
 233         nla = genlmsg_data(nlmsg_data(nlh));
 234         msg = nla_data(nla);
 235         point = msg->points;
 236         for (i = 0; i < msg->entries; i++) {
 237                 if (!memcmp(&location, &point->pc, sizeof(void *))) {
 238                         point->count++;
 239                         goto out;
 240                 }
 241                 point++;
 242         }
 243         if (msg->entries == dm_hit_limit)
 244                 goto out;
 245         /*
 246          * We need to create a new entry
 247          */
 248         __nla_reserve_nohdr(dskb, sizeof(struct net_dm_drop_point));
 249         nla->nla_len += NLA_ALIGN(sizeof(struct net_dm_drop_point));
 250         memcpy(point->pc, &location, sizeof(void *));
 251         point->count = 1;
 252         msg->entries++;
 253
 254         if (!timer_pending(&data->send_timer)) {
 255                 data->send_timer.expires = jiffies + dm_delay * HZ;
 256                 add_timer(&data->send_timer);
 257         }
 258
 259 out:
 260         spin_unlock_irqrestore(&data->lock, flags);
 261 }
 262
 263 static void trace_kfree_skb_hit(void *ignore, struct sk_buff *skb,
 264                                 void *location,
 265                                 enum skb_drop_reason reason)
 266 {
 267         trace_drop_common(skb, location);
 268 }
 269
 270 static void trace_napi_poll_hit(void *ignore, struct napi_struct *napi,
 271                                 int work, int budget)
 272 {
 273         struct net_device *dev = napi->dev;
 274         struct dm_hw_stat_delta *stat;
 275         /*
 276          * Don't check napi structures with no associated device
 277          */
 278         if (!dev)
 279                 return;
 280
 281         rcu_read_lock();
 282         stat = rcu_dereference(dev->dm_private);
 283         if (stat) {
 284                 /*
 285                  * only add a note to our monitor buffer if:
 286                  * 1) its after the last_rx delta
 287                  * 2) our rx_dropped count has gone up
 288                  */
 289                 if (time_after(jiffies, stat->last_rx + dm_hw_check_delta) &&
 290                     (dev->stats.rx_dropped != stat->last_drop_val)) {
 291                         trace_drop_common(NULL, NULL);
 292                         stat->last_drop_val = dev->stats.rx_dropped;
 293                         stat->last_rx = jiffies;
 294                 }
 295         }
 296         rcu_read_unlock();
 297 }
 298
 299 static struct net_dm_hw_entries *
 300 net_dm_hw_reset_per_cpu_data(struct per_cpu_dm_data *hw_data)
 301 {
 302         struct net_dm_hw_entries *hw_entries;
 303         unsigned long flags;
 304
 305         hw_entries = kzalloc(struct_size(hw_entries, entries, dm_hit_limit),
 306                              GFP_KERNEL);
 307         if (!hw_entries) {
 308                 /* If the memory allocation failed, we try to perform another
 309                  * allocation in 1/10 second. Otherwise, the probe function
 310                  * will constantly bail out.
 311                  */
 312                 mod_timer(&hw_data->send_timer, jiffies + HZ / 10);
 313         }
 314
 315         spin_lock_irqsave(&hw_data->lock, flags);
 316         swap(hw_data->hw_entries, hw_entries);
 317         spin_unlock_irqrestore(&hw_data->lock, flags);
 318
 319         return hw_entries;
 320 }
 321
 322 static int net_dm_hw_entry_put(struct sk_buff *msg,
 323                                const struct net_dm_hw_entry *hw_entry)
 324 {
 325         struct nlattr *attr;
 326
 327         attr = nla_nest_start(msg, NET_DM_ATTR_HW_ENTRY);
 328         if (!attr)
 329                 return -EMSGSIZE;
 330
 331         if (nla_put_string(msg, NET_DM_ATTR_HW_TRAP_NAME, hw_entry->trap_name))
 332                 goto nla_put_failure;
 333
 334         if (nla_put_u32(msg, NET_DM_ATTR_HW_TRAP_COUNT, hw_entry->count))
 335                 goto nla_put_failure;
 336
 337         nla_nest_end(msg, attr);
 338
 339         return 0;
 340
 341 nla_put_failure:
 342         nla_nest_cancel(msg, attr);
 343         return -EMSGSIZE;
 344 }
 345
 346 static int net_dm_hw_entries_put(struct sk_buff *msg,
 347                                  const struct net_dm_hw_entries *hw_entries)
 348 {
 349         struct nlattr *attr;
 350         int i;
 351
 352         attr = nla_nest_start(msg, NET_DM_ATTR_HW_ENTRIES);
 353         if (!attr)
 354                 return -EMSGSIZE;
 355
 356         for (i = 0; i < hw_entries->num_entries; i++) {
 357                 int rc;
 358
 359                 rc = net_dm_hw_entry_put(msg, &hw_entries->entries[i]);
 360                 if (rc)
 361                         goto nla_put_failure;
 362         }
 363
 364         nla_nest_end(msg, attr);
 365
 366         return 0;
 367
 368 nla_put_failure:
 369         nla_nest_cancel(msg, attr);
 370         return -EMSGSIZE;
 371 }
 372
 373 static int
 374 net_dm_hw_summary_report_fill(struct sk_buff *msg,
 375                               const struct net_dm_hw_entries *hw_entries)
 376 {
 377         struct net_dm_alert_msg anc_hdr = { 0 };
 378         void *hdr;
 379         int rc;
 380
 381         hdr = genlmsg_put(msg, 0, 0, &net_drop_monitor_family, 0,
 382                           NET_DM_CMD_ALERT);
 383         if (!hdr)
 384                 return -EMSGSIZE;
 385
 386         /* We need to put the ancillary header in order not to break user
 387          * space.
 388          */
 389         if (nla_put(msg, NLA_UNSPEC, sizeof(anc_hdr), &anc_hdr))
 390                 goto nla_put_failure;
 391
 392         rc = net_dm_hw_entries_put(msg, hw_entries);
 393         if (rc)
 394                 goto nla_put_failure;
 395
 396         genlmsg_end(msg, hdr);
 397
 398         return 0;
 399
 400 nla_put_failure:
 401         genlmsg_cancel(msg, hdr);
 402         return -EMSGSIZE;
 403 }
 404
 405 static void net_dm_hw_summary_work(struct work_struct *work)
 406 {
 407         struct net_dm_hw_entries *hw_entries;
 408         struct per_cpu_dm_data *hw_data;
 409         struct sk_buff *msg;
 410         int rc;
 411
 412         hw_data = container_of(work, struct per_cpu_dm_data, dm_alert_work);
 413
 414         hw_entries = net_dm_hw_reset_per_cpu_data(hw_data);
 415         if (!hw_entries)
 416                 return;
 417
 418         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 419         if (!msg)
 420                 goto out;
 421
 422         rc = net_dm_hw_summary_report_fill(msg, hw_entries);
 423         if (rc) {
 424                 nlmsg_free(msg);
 425                 goto out;
 426         }
 427
 428         genlmsg_multicast(&net_drop_monitor_family, msg, 0, 0, GFP_KERNEL);
 429
 430 out:
 431         kfree(hw_entries);
 432 }
 433
 434 static void
 435 net_dm_hw_trap_summary_probe(void *ignore, const struct devlink *devlink,
 436                              struct sk_buff *skb,
 437                              const struct devlink_trap_metadata *metadata)
 438 {
 439         struct net_dm_hw_entries *hw_entries;
 440         struct net_dm_hw_entry *hw_entry;
 441         struct per_cpu_dm_data *hw_data;
 442         unsigned long flags;
 443         int i;
 444
 445         if (metadata->trap_type == DEVLINK_TRAP_TYPE_CONTROL)
 446                 return;
 447
 448         hw_data = this_cpu_ptr(&dm_hw_cpu_data);
 449         spin_lock_irqsave(&hw_data->lock, flags);
 450         hw_entries = hw_data->hw_entries;
 451
 452         if (!hw_entries)
 453                 goto out;
 454
 455         for (i = 0; i < hw_entries->num_entries; i++) {
 456                 hw_entry = &hw_entries->entries[i];
 457                 if (!strncmp(hw_entry->trap_name, metadata->trap_name,
 458                              NET_DM_MAX_HW_TRAP_NAME_LEN - 1)) {
 459                         hw_entry->count++;
 460                         goto out;
 461                 }
 462         }
 463         if (WARN_ON_ONCE(hw_entries->num_entries == dm_hit_limit))
 464                 goto out;
 465
 466         hw_entry = &hw_entries->entries[hw_entries->num_entries];
 467         strlcpy(hw_entry->trap_name, metadata->trap_name,
 468                 NET_DM_MAX_HW_TRAP_NAME_LEN - 1);
 469         hw_entry->count = 1;
 470         hw_entries->num_entries++;
 471
 472         if (!timer_pending(&hw_data->send_timer)) {
 473                 hw_data->send_timer.expires = jiffies + dm_delay * HZ;
 474                 add_timer(&hw_data->send_timer);
 475         }
 476
 477 out:
 478         spin_unlock_irqrestore(&hw_data->lock, flags);
 479 }
 480
 481 static const struct net_dm_alert_ops net_dm_alert_summary_ops = {
 482         .kfree_skb_probe        = trace_kfree_skb_hit,
 483         .napi_poll_probe        = trace_napi_poll_hit,
 484         .work_item_func         = send_dm_alert,
 485         .hw_work_item_func      = net_dm_hw_summary_work,
 486         .hw_trap_probe          = net_dm_hw_trap_summary_probe,
 487 };
 488
 489 static void net_dm_packet_trace_kfree_skb_hit(void *ignore,
 490                                               struct sk_buff *skb,
 491                                               void *location,
 492                                               enum skb_drop_reason reason)
 493 {
 494         ktime_t tstamp = ktime_get_real();
 495         struct per_cpu_dm_data *data;
 496         struct net_dm_skb_cb *cb;
 497         struct sk_buff *nskb;
 498         unsigned long flags;
 499
 500         if (!skb_mac_header_was_set(skb))
 501                 return;
 502
 503         nskb = skb_clone(skb, GFP_ATOMIC);
 504         if (!nskb)
 505                 return;
 506
 507         if (unlikely(reason >= SKB_DROP_REASON_MAX || reason <= 0))
 508                 reason = SKB_DROP_REASON_NOT_SPECIFIED;
 509         cb = NET_DM_SKB_CB(nskb);
 510         cb->reason = reason;
 511         cb->pc = location;
 512         /* Override the timestamp because we care about the time when the
 513          * packet was dropped.
 514          */
 515         nskb->tstamp = tstamp;
 516
 517         data = this_cpu_ptr(&dm_cpu_data);
 518
 519         spin_lock_irqsave(&data->drop_queue.lock, flags);
 520         if (skb_queue_len(&data->drop_queue) < net_dm_queue_len)
 521                 __skb_queue_tail(&data->drop_queue, nskb);
 522         else
 523                 goto unlock_free;
 524         spin_unlock_irqrestore(&data->drop_queue.lock, flags);
 525
 526         schedule_work(&data->dm_alert_work);
 527
 528         return;
 529
 530 unlock_free:
 531         spin_unlock_irqrestore(&data->drop_queue.lock, flags);
 532         u64_stats_update_begin(&data->stats.syncp);
 533         u64_stats_inc(&data->stats.dropped);
 534         u64_stats_update_end(&data->stats.syncp);
 535         consume_skb(nskb);
 536 }
 537
 538 static void net_dm_packet_trace_napi_poll_hit(void *ignore,
 539                                               struct napi_struct *napi,
 540                                               int work, int budget)
 541 {
 542 }
 543
 544 static size_t net_dm_in_port_size(void)
 545 {
 546                /* NET_DM_ATTR_IN_PORT nest */
 547         return nla_total_size(0) +
 548                /* NET_DM_ATTR_PORT_NETDEV_IFINDEX */
 549                nla_total_size(sizeof(u32)) +
 550                /* NET_DM_ATTR_PORT_NETDEV_NAME */
 551                nla_total_size(IFNAMSIZ + 1);
 552 }
 553
 554 #define NET_DM_MAX_SYMBOL_LEN 40
 555
 556 static size_t net_dm_packet_report_size(size_t payload_len,
 557                                         enum skb_drop_reason reason)
 558 {
 559         size_t size;
 560
 561         size = nlmsg_msg_size(GENL_HDRLEN + net_drop_monitor_family.hdrsize);
 562
 563         return NLMSG_ALIGN(size) +
 564                /* NET_DM_ATTR_ORIGIN */
 565                nla_total_size(sizeof(u16)) +
 566                /* NET_DM_ATTR_PC */
 567                nla_total_size(sizeof(u64)) +
 568                /* NET_DM_ATTR_SYMBOL */
 569                nla_total_size(NET_DM_MAX_SYMBOL_LEN + 1) +
 570                /* NET_DM_ATTR_IN_PORT */
 571                net_dm_in_port_size() +
 572                /* NET_DM_ATTR_TIMESTAMP */
 573                nla_total_size(sizeof(u64)) +
 574                /* NET_DM_ATTR_ORIG_LEN */
 575                nla_total_size(sizeof(u32)) +
 576                /* NET_DM_ATTR_PROTO */
 577                nla_total_size(sizeof(u16)) +
 578                /* NET_DM_ATTR_REASON */
 579                nla_total_size(strlen(drop_reasons[reason]) + 1) +
 580                /* NET_DM_ATTR_PAYLOAD */
 581                nla_total_size(payload_len);
 582 }
 583
 584 static int net_dm_packet_report_in_port_put(struct sk_buff *msg, int ifindex,
 585                                             const char *name)
 586 {
 587         struct nlattr *attr;
 588
 589         attr = nla_nest_start(msg, NET_DM_ATTR_IN_PORT);
 590         if (!attr)
 591                 return -EMSGSIZE;
 592
 593         if (ifindex &&
 594             nla_put_u32(msg, NET_DM_ATTR_PORT_NETDEV_IFINDEX, ifindex))
 595                 goto nla_put_failure;
 596
 597         if (name && nla_put_string(msg, NET_DM_ATTR_PORT_NETDEV_NAME, name))
 598                 goto nla_put_failure;
 599
 600         nla_nest_end(msg, attr);
 601
 602         return 0;
 603
 604 nla_put_failure:
 605         nla_nest_cancel(msg, attr);
 606         return -EMSGSIZE;
 607 }
 608
 609 static int net_dm_packet_report_fill(struct sk_buff *msg, struct sk_buff *skb,
 610                                      size_t payload_len)
 611 {
 612         struct net_dm_skb_cb *cb = NET_DM_SKB_CB(skb);
 613         char buf[NET_DM_MAX_SYMBOL_LEN];
 614         struct nlattr *attr;
 615         void *hdr;
 616         int rc;
 617
 618         hdr = genlmsg_put(msg, 0, 0, &net_drop_monitor_family, 0,
 619                           NET_DM_CMD_PACKET_ALERT);
 620         if (!hdr)
 621                 return -EMSGSIZE;
 622
 623         if (nla_put_u16(msg, NET_DM_ATTR_ORIGIN, NET_DM_ORIGIN_SW))
 624                 goto nla_put_failure;
 625
 626         if (nla_put_u64_64bit(msg, NET_DM_ATTR_PC, (u64)(uintptr_t)cb->pc,
 627                               NET_DM_ATTR_PAD))
 628                 goto nla_put_failure;
 629
 630         if (nla_put_string(msg, NET_DM_ATTR_REASON,
 631                            drop_reasons[cb->reason]))
 632                 goto nla_put_failure;
 633
 634         snprintf(buf, sizeof(buf), "%pS", cb->pc);
 635         if (nla_put_string(msg, NET_DM_ATTR_SYMBOL, buf))
 636                 goto nla_put_failure;
 637
 638         rc = net_dm_packet_report_in_port_put(msg, skb->skb_iif, NULL);
 639         if (rc)
 640                 goto nla_put_failure;
 641
 642         if (nla_put_u64_64bit(msg, NET_DM_ATTR_TIMESTAMP,
 643                               ktime_to_ns(skb->tstamp), NET_DM_ATTR_PAD))
 644                 goto nla_put_failure;
 645
 646         if (nla_put_u32(msg, NET_DM_ATTR_ORIG_LEN, skb->len))
 647                 goto nla_put_failure;
 648
 649         if (!payload_len)
 650                 goto out;
 651
 652         if (nla_put_u16(msg, NET_DM_ATTR_PROTO, be16_to_cpu(skb->protocol)))
 653                 goto nla_put_failure;
 654
 655         attr = skb_put(msg, nla_total_size(payload_len));
 656         attr->nla_type = NET_DM_ATTR_PAYLOAD;
 657         attr->nla_len = nla_attr_size(payload_len);
 658         if (skb_copy_bits(skb, 0, nla_data(attr), payload_len))
 659                 goto nla_put_failure;
 660
 661 out:
 662         genlmsg_end(msg, hdr);
 663
 664         return 0;
 665
 666 nla_put_failure:
 667         genlmsg_cancel(msg, hdr);
 668         return -EMSGSIZE;
 669 }
 670
 671 #define NET_DM_MAX_PACKET_SIZE (0xffff - NLA_HDRLEN - NLA_ALIGNTO)
 672
 673 static void net_dm_packet_report(struct sk_buff *skb)
 674 {
 675         struct sk_buff *msg;
 676         size_t payload_len;
 677         int rc;
 678
 679         /* Make sure we start copying the packet from the MAC header */
 680         if (skb->data > skb_mac_header(skb))
 681                 skb_push(skb, skb->data - skb_mac_header(skb));
 682         else
 683                 skb_pull(skb, skb_mac_header(skb) - skb->data);
 684
 685         /* Ensure packet fits inside a single netlink attribute */
 686         payload_len = min_t(size_t, skb->len, NET_DM_MAX_PACKET_SIZE);
 687         if (net_dm_trunc_len)
 688                 payload_len = min_t(size_t, net_dm_trunc_len, payload_len);
 689
 690         msg = nlmsg_new(net_dm_packet_report_size(payload_len,
 691                                                   NET_DM_SKB_CB(skb)->reason),
 692                         GFP_KERNEL);
 693         if (!msg)
 694                 goto out;
 695
 696         rc = net_dm_packet_report_fill(msg, skb, payload_len);
 697         if (rc) {
 698                 nlmsg_free(msg);
 699                 goto out;
 700         }
 701
 702         genlmsg_multicast(&net_drop_monitor_family, msg, 0, 0, GFP_KERNEL);
 703
 704 out:
 705         consume_skb(skb);
 706 }
 707
 708 static void net_dm_packet_work(struct work_struct *work)
 709 {
 710         struct per_cpu_dm_data *data;
 711         struct sk_buff_head list;
 712         struct sk_buff *skb;
 713         unsigned long flags;
 714
 715         data = container_of(work, struct per_cpu_dm_data, dm_alert_work);
 716
 717         __skb_queue_head_init(&list);
 718
 719         spin_lock_irqsave(&data->drop_queue.lock, flags);
 720         skb_queue_splice_tail_init(&data->drop_queue, &list);
 721         spin_unlock_irqrestore(&data->drop_queue.lock, flags);
 722
 723         while ((skb = __skb_dequeue(&list)))
 724                 net_dm_packet_report(skb);
 725 }
 726
 727 static size_t
 728 net_dm_flow_action_cookie_size(const struct devlink_trap_metadata *hw_metadata)
 729 {
 730         return hw_metadata->fa_cookie ?
 731                nla_total_size(hw_metadata->fa_cookie->cookie_len) : 0;
 732 }
 733
 734 static size_t
 735 net_dm_hw_packet_report_size(size_t payload_len,
 736                              const struct devlink_trap_metadata *hw_metadata)
 737 {
 738         size_t size;
 739
 740         size = nlmsg_msg_size(GENL_HDRLEN + net_drop_monitor_family.hdrsize);
 741
 742         return NLMSG_ALIGN(size) +
 743                /* NET_DM_ATTR_ORIGIN */
 744                nla_total_size(sizeof(u16)) +
 745                /* NET_DM_ATTR_HW_TRAP_GROUP_NAME */
 746                nla_total_size(strlen(hw_metadata->trap_group_name) + 1) +
 747                /* NET_DM_ATTR_HW_TRAP_NAME */
 748                nla_total_size(strlen(hw_metadata->trap_name) + 1) +
 749                /* NET_DM_ATTR_IN_PORT */
 750                net_dm_in_port_size() +
 751                /* NET_DM_ATTR_FLOW_ACTION_COOKIE */
 752                net_dm_flow_action_cookie_size(hw_metadata) +
 753                /* NET_DM_ATTR_TIMESTAMP */
 754                nla_total_size(sizeof(u64)) +
 755                /* NET_DM_ATTR_ORIG_LEN */
 756                nla_total_size(sizeof(u32)) +
 757                /* NET_DM_ATTR_PROTO */
 758                nla_total_size(sizeof(u16)) +
 759                /* NET_DM_ATTR_PAYLOAD */
 760                nla_total_size(payload_len);
 761 }
 762
 763 static int net_dm_hw_packet_report_fill(struct sk_buff *msg,
 764                                         struct sk_buff *skb, size_t payload_len)
 765 {
 766         struct devlink_trap_metadata *hw_metadata;
 767         struct nlattr *attr;
 768         void *hdr;
 769
 770         hw_metadata = NET_DM_SKB_CB(skb)->hw_metadata;
 771
 772         hdr = genlmsg_put(msg, 0, 0, &net_drop_monitor_family, 0,
 773                           NET_DM_CMD_PACKET_ALERT);
 774         if (!hdr)
 775                 return -EMSGSIZE;
 776
 777         if (nla_put_u16(msg, NET_DM_ATTR_ORIGIN, NET_DM_ORIGIN_HW))
 778                 goto nla_put_failure;
 779
 780         if (nla_put_string(msg, NET_DM_ATTR_HW_TRAP_GROUP_NAME,
 781                            hw_metadata->trap_group_name))
 782                 goto nla_put_failure;
 783
 784         if (nla_put_string(msg, NET_DM_ATTR_HW_TRAP_NAME,
 785                            hw_metadata->trap_name))
 786                 goto nla_put_failure;
 787
 788         if (hw_metadata->input_dev) {
 789                 struct net_device *dev = hw_metadata->input_dev;
 790                 int rc;
 791
 792                 rc = net_dm_packet_report_in_port_put(msg, dev->ifindex,
 793                                                       dev->name);
 794                 if (rc)
 795                         goto nla_put_failure;
 796         }
 797
 798         if (hw_metadata->fa_cookie &&
 799             nla_put(msg, NET_DM_ATTR_FLOW_ACTION_COOKIE,
 800                     hw_metadata->fa_cookie->cookie_len,
 801                     hw_metadata->fa_cookie->cookie))
 802                 goto nla_put_failure;
 803
 804         if (nla_put_u64_64bit(msg, NET_DM_ATTR_TIMESTAMP,
 805                               ktime_to_ns(skb->tstamp), NET_DM_ATTR_PAD))
 806                 goto nla_put_failure;
 807
 808         if (nla_put_u32(msg, NET_DM_ATTR_ORIG_LEN, skb->len))
 809                 goto nla_put_failure;
 810
 811         if (!payload_len)
 812                 goto out;
 813
 814         if (nla_put_u16(msg, NET_DM_ATTR_PROTO, be16_to_cpu(skb->protocol)))
 815                 goto nla_put_failure;
 816
 817         attr = skb_put(msg, nla_total_size(payload_len));
 818         attr->nla_type = NET_DM_ATTR_PAYLOAD;
 819         attr->nla_len = nla_attr_size(payload_len);
 820         if (skb_copy_bits(skb, 0, nla_data(attr), payload_len))
 821                 goto nla_put_failure;
 822
 823 out:
 824         genlmsg_end(msg, hdr);
 825
 826         return 0;
 827
 828 nla_put_failure:
 829         genlmsg_cancel(msg, hdr);
 830         return -EMSGSIZE;
 831 }
 832
 833 static struct devlink_trap_metadata *
 834 net_dm_hw_metadata_copy(const struct devlink_trap_metadata *metadata)
 835 {
 836         const struct flow_action_cookie *fa_cookie;
 837         struct devlink_trap_metadata *hw_metadata;
 838         const char *trap_group_name;
 839         const char *trap_name;
 840
 841         hw_metadata = kzalloc(sizeof(*hw_metadata), GFP_ATOMIC);
 842         if (!hw_metadata)
 843                 return NULL;
 844
 845         trap_group_name = kstrdup(metadata->trap_group_name, GFP_ATOMIC);
 846         if (!trap_group_name)
 847                 goto free_hw_metadata;
 848         hw_metadata->trap_group_name = trap_group_name;
 849
 850         trap_name = kstrdup(metadata->trap_name, GFP_ATOMIC);
 851         if (!trap_name)
 852                 goto free_trap_group;
 853         hw_metadata->trap_name = trap_name;
 854
 855         if (metadata->fa_cookie) {
 856                 size_t cookie_size = sizeof(*fa_cookie) +
 857                                      metadata->fa_cookie->cookie_len;
 858
 859                 fa_cookie = kmemdup(metadata->fa_cookie, cookie_size,
 860                                     GFP_ATOMIC);
 861                 if (!fa_cookie)
 862                         goto free_trap_name;
 863                 hw_metadata->fa_cookie = fa_cookie;
 864         }
 865
 866         hw_metadata->input_dev = metadata->input_dev;
 867         netdev_hold(hw_metadata->input_dev, &hw_metadata->dev_tracker,
 868                     GFP_ATOMIC);
 869
 870         return hw_metadata;
 871
 872 free_trap_name:
 873         kfree(trap_name);
 874 free_trap_group:
 875         kfree(trap_group_name);
 876 free_hw_metadata:
 877         kfree(hw_metadata);
 878         return NULL;
 879 }
 880
 881 static void
 882 net_dm_hw_metadata_free(struct devlink_trap_metadata *hw_metadata)
 883 {
 884         netdev_put(hw_metadata->input_dev, &hw_metadata->dev_tracker);
 885         kfree(hw_metadata->fa_cookie);
 886         kfree(hw_metadata->trap_name);
 887         kfree(hw_metadata->trap_group_name);
 888         kfree(hw_metadata);
 889 }
 890
 891 static void net_dm_hw_packet_report(struct sk_buff *skb)
 892 {
 893         struct devlink_trap_metadata *hw_metadata;
 894         struct sk_buff *msg;
 895         size_t payload_len;
 896         int rc;
 897
 898         if (skb->data > skb_mac_header(skb))
 899                 skb_push(skb, skb->data - skb_mac_header(skb));
 900         else
 901                 skb_pull(skb, skb_mac_header(skb) - skb->data);
 902
 903         payload_len = min_t(size_t, skb->len, NET_DM_MAX_PACKET_SIZE);
 904         if (net_dm_trunc_len)
 905                 payload_len = min_t(size_t, net_dm_trunc_len, payload_len);
 906
 907         hw_metadata = NET_DM_SKB_CB(skb)->hw_metadata;
 908         msg = nlmsg_new(net_dm_hw_packet_report_size(payload_len, hw_metadata),
 909                         GFP_KERNEL);
 910         if (!msg)
 911                 goto out;
 912
 913         rc = net_dm_hw_packet_report_fill(msg, skb, payload_len);
 914         if (rc) {
 915                 nlmsg_free(msg);
 916                 goto out;
 917         }
 918
 919         genlmsg_multicast(&net_drop_monitor_family, msg, 0, 0, GFP_KERNEL);
 920
 921 out:
 922         net_dm_hw_metadata_free(NET_DM_SKB_CB(skb)->hw_metadata);
 923         consume_skb(skb);
 924 }
 925
 926 static void net_dm_hw_packet_work(struct work_struct *work)
 927 {
 928         struct per_cpu_dm_data *hw_data;
 929         struct sk_buff_head list;
 930         struct sk_buff *skb;
 931         unsigned long flags;
 932
 933         hw_data = container_of(work, struct per_cpu_dm_data, dm_alert_work);
 934
 935         __skb_queue_head_init(&list);
 936
 937         spin_lock_irqsave(&hw_data->drop_queue.lock, flags);
 938         skb_queue_splice_tail_init(&hw_data->drop_queue, &list);
 939         spin_unlock_irqrestore(&hw_data->drop_queue.lock, flags);
 940
 941         while ((skb = __skb_dequeue(&list)))
 942                 net_dm_hw_packet_report(skb);
 943 }
 944
 945 static void
 946 net_dm_hw_trap_packet_probe(void *ignore, const struct devlink *devlink,
 947                             struct sk_buff *skb,
 948                             const struct devlink_trap_metadata *metadata)
 949 {
 950         struct devlink_trap_metadata *n_hw_metadata;
 951         ktime_t tstamp = ktime_get_real();
 952         struct per_cpu_dm_data *hw_data;
 953         struct sk_buff *nskb;
 954         unsigned long flags;
 955
 956         if (metadata->trap_type == DEVLINK_TRAP_TYPE_CONTROL)
 957                 return;
 958
 959         if (!skb_mac_header_was_set(skb))
 960                 return;
 961
 962         nskb = skb_clone(skb, GFP_ATOMIC);
 963         if (!nskb)
 964                 return;
 965
 966         n_hw_metadata = net_dm_hw_metadata_copy(metadata);
 967         if (!n_hw_metadata)
 968                 goto free;
 969
 970         NET_DM_SKB_CB(nskb)->hw_metadata = n_hw_metadata;
 971         nskb->tstamp = tstamp;
 972
 973         hw_data = this_cpu_ptr(&dm_hw_cpu_data);
 974
 975         spin_lock_irqsave(&hw_data->drop_queue.lock, flags);
 976         if (skb_queue_len(&hw_data->drop_queue) < net_dm_queue_len)
 977                 __skb_queue_tail(&hw_data->drop_queue, nskb);
 978         else
 979                 goto unlock_free;
 980         spin_unlock_irqrestore(&hw_data->drop_queue.lock, flags);
 981
 982         schedule_work(&hw_data->dm_alert_work);
 983
 984         return;
 985
 986 unlock_free:
 987         spin_unlock_irqrestore(&hw_data->drop_queue.lock, flags);
 988         u64_stats_update_begin(&hw_data->stats.syncp);
 989         u64_stats_inc(&hw_data->stats.dropped);
 990         u64_stats_update_end(&hw_data->stats.syncp);
 991         net_dm_hw_metadata_free(n_hw_metadata);
 992 free:
 993         consume_skb(nskb);
 994 }
 995
 996 static const struct net_dm_alert_ops net_dm_alert_packet_ops = {
 997         .kfree_skb_probe        = net_dm_packet_trace_kfree_skb_hit,
 998         .napi_poll_probe        = net_dm_packet_trace_napi_poll_hit,
 999         .work_item_func         = net_dm_packet_work,
1000         .hw_work_item_func      = net_dm_hw_packet_work,
1001         .hw_trap_probe          = net_dm_hw_trap_packet_probe,
1002 };
1003
1004 static const struct net_dm_alert_ops *net_dm_alert_ops_arr[] = {
1005         [NET_DM_ALERT_MODE_SUMMARY]     = &net_dm_alert_summary_ops,
1006         [NET_DM_ALERT_MODE_PACKET]      = &net_dm_alert_packet_ops,
1007 };
1008
1009 #if IS_ENABLED(CONFIG_NET_DEVLINK)
1010 static int net_dm_hw_probe_register(const struct net_dm_alert_ops *ops)
1011 {
1012         return register_trace_devlink_trap_report(ops->hw_trap_probe, NULL);
1013 }
1014
1015 static void net_dm_hw_probe_unregister(const struct net_dm_alert_ops *ops)
1016 {
1017         unregister_trace_devlink_trap_report(ops->hw_trap_probe, NULL);
1018         tracepoint_synchronize_unregister();
1019 }
1020 #else
1021 static int net_dm_hw_probe_register(const struct net_dm_alert_ops *ops)
1022 {
1023         return -EOPNOTSUPP;
1024 }
1025
1026 static void net_dm_hw_probe_unregister(const struct net_dm_alert_ops *ops)
1027 {
1028 }
1029 #endif
1030
1031 static int net_dm_hw_monitor_start(struct netlink_ext_ack *extack)
1032 {
1033         const struct net_dm_alert_ops *ops;
1034         int cpu, rc;
1035
1036         if (monitor_hw) {
1037                 NL_SET_ERR_MSG_MOD(extack, "Hardware monitoring already enabled");
1038                 return -EAGAIN;
1039         }
1040
1041         ops = net_dm_alert_ops_arr[net_dm_alert_mode];
1042
1043         if (!try_module_get(THIS_MODULE)) {
1044                 NL_SET_ERR_MSG_MOD(extack, "Failed to take reference on module");
1045                 return -ENODEV;
1046         }
1047
1048         for_each_possible_cpu(cpu) {
1049                 struct per_cpu_dm_data *hw_data = &per_cpu(dm_hw_cpu_data, cpu);
1050                 struct net_dm_hw_entries *hw_entries;
1051
1052                 INIT_WORK(&hw_data->dm_alert_work, ops->hw_work_item_func);
1053                 timer_setup(&hw_data->send_timer, sched_send_work, 0);
1054                 hw_entries = net_dm_hw_reset_per_cpu_data(hw_data);
1055                 kfree(hw_entries);
1056         }
1057
1058         rc = net_dm_hw_probe_register(ops);
1059         if (rc) {
1060                 NL_SET_ERR_MSG_MOD(extack, "Failed to connect probe to devlink_trap_probe() tracepoint");
1061                 goto err_module_put;
1062         }
1063
1064         monitor_hw = true;
1065
1066         return 0;
1067
1068 err_module_put:
1069         for_each_possible_cpu(cpu) {
1070                 struct per_cpu_dm_data *hw_data = &per_cpu(dm_hw_cpu_data, cpu);
1071                 struct sk_buff *skb;
1072
1073                 del_timer_sync(&hw_data->send_timer);
1074                 cancel_work_sync(&hw_data->dm_alert_work);
1075                 while ((skb = __skb_dequeue(&hw_data->drop_queue))) {
1076                         struct devlink_trap_metadata *hw_metadata;
1077
1078                         hw_metadata = NET_DM_SKB_CB(skb)->hw_metadata;
1079                         net_dm_hw_metadata_free(hw_metadata);
1080                         consume_skb(skb);
1081                 }
1082         }
1083         module_put(THIS_MODULE);
1084         return rc;
1085 }
1086
1087 static void net_dm_hw_monitor_stop(struct netlink_ext_ack *extack)
1088 {
1089         const struct net_dm_alert_ops *ops;
1090         int cpu;
1091
1092         if (!monitor_hw) {
1093                 NL_SET_ERR_MSG_MOD(extack, "Hardware monitoring already disabled");
1094                 return;
1095         }
1096
1097         ops = net_dm_alert_ops_arr[net_dm_alert_mode];
1098
1099         monitor_hw = false;
1100
1101         net_dm_hw_probe_unregister(ops);
1102
1103         for_each_possible_cpu(cpu) {
1104                 struct per_cpu_dm_data *hw_data = &per_cpu(dm_hw_cpu_data, cpu);
1105                 struct sk_buff *skb;
1106
1107                 del_timer_sync(&hw_data->send_timer);
1108                 cancel_work_sync(&hw_data->dm_alert_work);
1109                 while ((skb = __skb_dequeue(&hw_data->drop_queue))) {
1110                         struct devlink_trap_metadata *hw_metadata;
1111
1112                         hw_metadata = NET_DM_SKB_CB(skb)->hw_metadata;
1113                         net_dm_hw_metadata_free(hw_metadata);
1114                         consume_skb(skb);
1115                 }
1116         }
1117
1118         module_put(THIS_MODULE);
1119 }
1120
1121 static int net_dm_trace_on_set(struct netlink_ext_ack *extack)
1122 {
1123         const struct net_dm_alert_ops *ops;
1124         int cpu, rc;
1125
1126         ops = net_dm_alert_ops_arr[net_dm_alert_mode];
1127
1128         if (!try_module_get(THIS_MODULE)) {
1129                 NL_SET_ERR_MSG_MOD(extack, "Failed to take reference on module");
1130                 return -ENODEV;
1131         }
1132
1133         for_each_possible_cpu(cpu) {
1134                 struct per_cpu_dm_data *data = &per_cpu(dm_cpu_data, cpu);
1135                 struct sk_buff *skb;
1136
1137                 INIT_WORK(&data->dm_alert_work, ops->work_item_func);
1138                 timer_setup(&data->send_timer, sched_send_work, 0);
1139                 /* Allocate a new per-CPU skb for the summary alert message and
1140                  * free the old one which might contain stale data from
1141                  * previous tracing.
1142                  */
1143                 skb = reset_per_cpu_data(data);
1144                 consume_skb(skb);
1145         }
1146
1147         rc = register_trace_kfree_skb(ops->kfree_skb_probe, NULL);
1148         if (rc) {
1149                 NL_SET_ERR_MSG_MOD(extack, "Failed to connect probe to kfree_skb() tracepoint");
1150                 goto err_module_put;
1151         }
1152
1153         rc = register_trace_napi_poll(ops->napi_poll_probe, NULL);
1154         if (rc) {
1155                 NL_SET_ERR_MSG_MOD(extack, "Failed to connect probe to napi_poll() tracepoint");
1156                 goto err_unregister_trace;
1157         }
1158
1159         return 0;
1160
1161 err_unregister_trace:
1162         unregister_trace_kfree_skb(ops->kfree_skb_probe, NULL);
1163 err_module_put:
1164         for_each_possible_cpu(cpu) {
1165                 struct per_cpu_dm_data *data = &per_cpu(dm_cpu_data, cpu);
1166                 struct sk_buff *skb;
1167
1168                 del_timer_sync(&data->send_timer);
1169                 cancel_work_sync(&data->dm_alert_work);
1170                 while ((skb = __skb_dequeue(&data->drop_queue)))
1171                         consume_skb(skb);
1172         }
1173         module_put(THIS_MODULE);
1174         return rc;
1175 }
1176
1177 static void net_dm_trace_off_set(void)
1178 {
1179         const struct net_dm_alert_ops *ops;
1180         int cpu;
1181
1182         ops = net_dm_alert_ops_arr[net_dm_alert_mode];
1183
1184         unregister_trace_napi_poll(ops->napi_poll_probe, NULL);
1185         unregister_trace_kfree_skb(ops->kfree_skb_probe, NULL);
1186
1187         tracepoint_synchronize_unregister();
1188
1189         /* Make sure we do not send notifications to user space after request
1190          * to stop tracing returns.
1191          */
1192         for_each_possible_cpu(cpu) {
1193                 struct per_cpu_dm_data *data = &per_cpu(dm_cpu_data, cpu);
1194                 struct sk_buff *skb;
1195
1196                 del_timer_sync(&data->send_timer);
1197                 cancel_work_sync(&data->dm_alert_work);
1198                 while ((skb = __skb_dequeue(&data->drop_queue)))
1199                         consume_skb(skb);
1200         }
1201
1202         module_put(THIS_MODULE);
1203 }
1204
1205 static int set_all_monitor_traces(int state, struct netlink_ext_ack *extack)
1206 {
1207         int rc = 0;
1208
1209         if (state == trace_state) {
1210                 NL_SET_ERR_MSG_MOD(extack, "Trace state already set to requested state");
1211                 return -EAGAIN;
1212         }
1213
1214         switch (state) {
1215         case TRACE_ON:
1216                 rc = net_dm_trace_on_set(extack);
1217                 break;
1218         case TRACE_OFF:
1219                 net_dm_trace_off_set();
1220                 break;
1221         default:
1222                 rc = 1;
1223                 break;
1224         }
1225
1226         if (!rc)
1227                 trace_state = state;
1228         else
1229                 rc = -EINPROGRESS;
1230
1231         return rc;
1232 }
1233
1234 static bool net_dm_is_monitoring(void)
1235 {
1236         return trace_state == TRACE_ON || monitor_hw;
1237 }
1238
1239 static int net_dm_alert_mode_get_from_info(struct genl_info *info,
1240                                            enum net_dm_alert_mode *p_alert_mode)
1241 {
1242         u8 val;
1243
1244         val = nla_get_u8(info->attrs[NET_DM_ATTR_ALERT_MODE]);
1245
1246         switch (val) {
1247         case NET_DM_ALERT_MODE_SUMMARY:
1248         case NET_DM_ALERT_MODE_PACKET:
1249                 *p_alert_mode = val;
1250                 break;
1251         default:
1252                 return -EINVAL;
1253         }
1254
1255         return 0;
1256 }
1257
1258 static int net_dm_alert_mode_set(struct genl_info *info)
1259 {
1260         struct netlink_ext_ack *extack = info->extack;
1261         enum net_dm_alert_mode alert_mode;
1262         int rc;
1263
1264         if (!info->attrs[NET_DM_ATTR_ALERT_MODE])
1265                 return 0;
1266
1267         rc = net_dm_alert_mode_get_from_info(info, &alert_mode);
1268         if (rc) {
1269                 NL_SET_ERR_MSG_MOD(extack, "Invalid alert mode");
1270                 return -EINVAL;
1271         }
1272
1273         net_dm_alert_mode = alert_mode;
1274
1275         return 0;
1276 }
1277
1278 static void net_dm_trunc_len_set(struct genl_info *info)
1279 {
1280         if (!info->attrs[NET_DM_ATTR_TRUNC_LEN])
1281                 return;
1282
1283         net_dm_trunc_len = nla_get_u32(info->attrs[NET_DM_ATTR_TRUNC_LEN]);
1284 }
1285
1286 static void net_dm_queue_len_set(struct genl_info *info)
1287 {
1288         if (!info->attrs[NET_DM_ATTR_QUEUE_LEN])
1289                 return;
1290
1291         net_dm_queue_len = nla_get_u32(info->attrs[NET_DM_ATTR_QUEUE_LEN]);
1292 }
1293
1294 static int net_dm_cmd_config(struct sk_buff *skb,
1295                         struct genl_info *info)
1296 {
1297         struct netlink_ext_ack *extack = info->extack;
1298         int rc;
1299
1300         if (net_dm_is_monitoring()) {
1301                 NL_SET_ERR_MSG_MOD(extack, "Cannot configure drop monitor during monitoring");
1302                 return -EBUSY;
1303         }
1304
1305         rc = net_dm_alert_mode_set(info);
1306         if (rc)
1307                 return rc;
1308
1309         net_dm_trunc_len_set(info);
1310
1311         net_dm_queue_len_set(info);
1312
1313         return 0;
1314 }
1315
1316 static int net_dm_monitor_start(bool set_sw, bool set_hw,
1317                                 struct netlink_ext_ack *extack)
1318 {
1319         bool sw_set = false;
1320         int rc;
1321
1322         if (set_sw) {
1323                 rc = set_all_monitor_traces(TRACE_ON, extack);
1324                 if (rc)
1325                         return rc;
1326                 sw_set = true;
1327         }
1328
1329         if (set_hw) {
1330                 rc = net_dm_hw_monitor_start(extack);
1331                 if (rc)
1332                         goto err_monitor_hw;
1333         }
1334
1335         return 0;
1336
1337 err_monitor_hw:
1338         if (sw_set)
1339                 set_all_monitor_traces(TRACE_OFF, extack);
1340         return rc;
1341 }
1342
1343 static void net_dm_monitor_stop(bool set_sw, bool set_hw,
1344                                 struct netlink_ext_ack *extack)
1345 {
1346         if (set_hw)
1347                 net_dm_hw_monitor_stop(extack);
1348         if (set_sw)
1349                 set_all_monitor_traces(TRACE_OFF, extack);
1350 }
1351
1352 static int net_dm_cmd_trace(struct sk_buff *skb,
1353                         struct genl_info *info)
1354 {
1355         bool set_sw = !!info->attrs[NET_DM_ATTR_SW_DROPS];
1356         bool set_hw = !!info->attrs[NET_DM_ATTR_HW_DROPS];
1357         struct netlink_ext_ack *extack = info->extack;
1358
1359         /* To maintain backward compatibility, we start / stop monitoring of
1360          * software drops if no flag is specified.
1361          */
1362         if (!set_sw && !set_hw)
1363                 set_sw = true;
1364
1365         switch (info->genlhdr->cmd) {
1366         case NET_DM_CMD_START:
1367                 return net_dm_monitor_start(set_sw, set_hw, extack);
1368         case NET_DM_CMD_STOP:
1369                 net_dm_monitor_stop(set_sw, set_hw, extack);
1370                 return 0;
1371         }
1372
1373         return -EOPNOTSUPP;
1374 }
1375
1376 static int net_dm_config_fill(struct sk_buff *msg, struct genl_info *info)
1377 {
1378         void *hdr;
1379
1380         hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq,
1381                           &net_drop_monitor_family, 0, NET_DM_CMD_CONFIG_NEW);
1382         if (!hdr)
1383                 return -EMSGSIZE;
1384
1385         if (nla_put_u8(msg, NET_DM_ATTR_ALERT_MODE, net_dm_alert_mode))
1386                 goto nla_put_failure;
1387
1388         if (nla_put_u32(msg, NET_DM_ATTR_TRUNC_LEN, net_dm_trunc_len))
1389                 goto nla_put_failure;
1390
1391         if (nla_put_u32(msg, NET_DM_ATTR_QUEUE_LEN, net_dm_queue_len))
1392                 goto nla_put_failure;
1393
1394         genlmsg_end(msg, hdr);
1395
1396         return 0;
1397
1398 nla_put_failure:
1399         genlmsg_cancel(msg, hdr);
1400         return -EMSGSIZE;
1401 }
1402
1403 static int net_dm_cmd_config_get(struct sk_buff *skb, struct genl_info *info)
1404 {
1405         struct sk_buff *msg;
1406         int rc;
1407
1408         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1409         if (!msg)
1410                 return -ENOMEM;
1411
1412         rc = net_dm_config_fill(msg, info);
1413         if (rc)
1414                 goto free_msg;
1415
1416         return genlmsg_reply(msg, info);
1417
1418 free_msg:
1419         nlmsg_free(msg);
1420         return rc;
1421 }
1422
1423 static void net_dm_stats_read(struct net_dm_stats *stats)
1424 {
1425         int cpu;
1426
1427         memset(stats, 0, sizeof(*stats));
1428         for_each_possible_cpu(cpu) {
1429                 struct per_cpu_dm_data *data = &per_cpu(dm_cpu_data, cpu);
1430                 struct net_dm_stats *cpu_stats = &data->stats;
1431                 unsigned int start;
1432                 u64 dropped;
1433
1434                 do {
1435                         start = u64_stats_fetch_begin_irq(&cpu_stats->syncp);
1436                         dropped = u64_stats_read(&cpu_stats->dropped);
1437                 } while (u64_stats_fetch_retry_irq(&cpu_stats->syncp, start));
1438
1439                 u64_stats_add(&stats->dropped, dropped);
1440         }
1441 }
1442
1443 static int net_dm_stats_put(struct sk_buff *msg)
1444 {
1445         struct net_dm_stats stats;
1446         struct nlattr *attr;
1447
1448         net_dm_stats_read(&stats);
1449
1450         attr = nla_nest_start(msg, NET_DM_ATTR_STATS);
1451         if (!attr)
1452                 return -EMSGSIZE;
1453
1454         if (nla_put_u64_64bit(msg, NET_DM_ATTR_STATS_DROPPED,
1455                               u64_stats_read(&stats.dropped), NET_DM_ATTR_PAD))
1456                 goto nla_put_failure;
1457
1458         nla_nest_end(msg, attr);
1459
1460         return 0;
1461
1462 nla_put_failure:
1463         nla_nest_cancel(msg, attr);
1464         return -EMSGSIZE;
1465 }
1466
1467 static void net_dm_hw_stats_read(struct net_dm_stats *stats)
1468 {
1469         int cpu;
1470
1471         memset(stats, 0, sizeof(*stats));
1472         for_each_possible_cpu(cpu) {
1473                 struct per_cpu_dm_data *hw_data = &per_cpu(dm_hw_cpu_data, cpu);
1474                 struct net_dm_stats *cpu_stats = &hw_data->stats;
1475                 unsigned int start;
1476                 u64 dropped;
1477
1478                 do {
1479                         start = u64_stats_fetch_begin_irq(&cpu_stats->syncp);
1480                         dropped = u64_stats_read(&cpu_stats->dropped);
1481                 } while (u64_stats_fetch_retry_irq(&cpu_stats->syncp, start));
1482
1483                 u64_stats_add(&stats->dropped, dropped);
1484         }
1485 }
1486
1487 static int net_dm_hw_stats_put(struct sk_buff *msg)
1488 {
1489         struct net_dm_stats stats;
1490         struct nlattr *attr;
1491
1492         net_dm_hw_stats_read(&stats);
1493
1494         attr = nla_nest_start(msg, NET_DM_ATTR_HW_STATS);
1495         if (!attr)
1496                 return -EMSGSIZE;
1497
1498         if (nla_put_u64_64bit(msg, NET_DM_ATTR_STATS_DROPPED,
1499                               u64_stats_read(&stats.dropped), NET_DM_ATTR_PAD))
1500                 goto nla_put_failure;
1501
1502         nla_nest_end(msg, attr);
1503
1504         return 0;
1505
1506 nla_put_failure:
1507         nla_nest_cancel(msg, attr);
1508         return -EMSGSIZE;
1509 }
1510
1511 static int net_dm_stats_fill(struct sk_buff *msg, struct genl_info *info)
1512 {
1513         void *hdr;
1514         int rc;
1515
1516         hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq,
1517                           &net_drop_monitor_family, 0, NET_DM_CMD_STATS_NEW);
1518         if (!hdr)
1519                 return -EMSGSIZE;
1520
1521         rc = net_dm_stats_put(msg);
1522         if (rc)
1523                 goto nla_put_failure;
1524
1525         rc = net_dm_hw_stats_put(msg);
1526         if (rc)
1527                 goto nla_put_failure;
1528
1529         genlmsg_end(msg, hdr);
1530
1531         return 0;
1532
1533 nla_put_failure:
1534         genlmsg_cancel(msg, hdr);
1535         return -EMSGSIZE;
1536 }
1537
1538 static int net_dm_cmd_stats_get(struct sk_buff *skb, struct genl_info *info)
1539 {
1540         struct sk_buff *msg;
1541         int rc;
1542
1543         msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
1544         if (!msg)
1545                 return -ENOMEM;
1546
1547         rc = net_dm_stats_fill(msg, info);
1548         if (rc)
1549                 goto free_msg;
1550
1551         return genlmsg_reply(msg, info);
1552
1553 free_msg:
1554         nlmsg_free(msg);
1555         return rc;
1556 }
1557
1558 static int dropmon_net_event(struct notifier_block *ev_block,
1559                              unsigned long event, void *ptr)
1560 {
1561         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1562         struct dm_hw_stat_delta *stat;
1563
1564         switch (event) {
1565         case NETDEV_REGISTER:
1566                 if (WARN_ON_ONCE(rtnl_dereference(dev->dm_private)))
1567                         break;
1568                 stat = kzalloc(sizeof(*stat), GFP_KERNEL);
1569                 if (!stat)
1570                         break;
1571
1572                 stat->last_rx = jiffies;
1573                 rcu_assign_pointer(dev->dm_private, stat);
1574
1575                 break;
1576         case NETDEV_UNREGISTER:
1577                 stat = rtnl_dereference(dev->dm_private);
1578                 if (stat) {
1579                         rcu_assign_pointer(dev->dm_private, NULL);
1580                         kfree_rcu(stat, rcu);
1581                 }
1582                 break;
1583         }
1584         return NOTIFY_DONE;
1585 }
1586
1587 static const struct nla_policy net_dm_nl_policy[NET_DM_ATTR_MAX + 1] = {
1588         [NET_DM_ATTR_UNSPEC] = { .strict_start_type = NET_DM_ATTR_UNSPEC + 1 },
1589         [NET_DM_ATTR_ALERT_MODE] = { .type = NLA_U8 },
1590         [NET_DM_ATTR_TRUNC_LEN] = { .type = NLA_U32 },
1591         [NET_DM_ATTR_QUEUE_LEN] = { .type = NLA_U32 },
1592         [NET_DM_ATTR_SW_DROPS]  = {. type = NLA_FLAG },
1593         [NET_DM_ATTR_HW_DROPS]  = {. type = NLA_FLAG },
1594 };
1595
1596 static const struct genl_small_ops dropmon_ops[] = {
1597         {
1598                 .cmd = NET_DM_CMD_CONFIG,
1599                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
1600                 .doit = net_dm_cmd_config,
1601                 .flags = GENL_ADMIN_PERM,
1602         },
1603         {
1604                 .cmd = NET_DM_CMD_START,
1605                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
1606                 .doit = net_dm_cmd_trace,
1607         },
1608         {
1609                 .cmd = NET_DM_CMD_STOP,
1610                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
1611                 .doit = net_dm_cmd_trace,
1612         },
1613         {
1614                 .cmd = NET_DM_CMD_CONFIG_GET,
1615                 .doit = net_dm_cmd_config_get,
1616         },
1617         {
1618                 .cmd = NET_DM_CMD_STATS_GET,
1619                 .doit = net_dm_cmd_stats_get,
1620         },
1621 };
1622
1623 static int net_dm_nl_pre_doit(const struct genl_ops *ops,
1624                               struct sk_buff *skb, struct genl_info *info)
1625 {
1626         mutex_lock(&net_dm_mutex);
1627
1628         return 0;
1629 }
1630
1631 static void net_dm_nl_post_doit(const struct genl_ops *ops,
1632                                 struct sk_buff *skb, struct genl_info *info)
1633 {
1634         mutex_unlock(&net_dm_mutex);
1635 }
1636
1637 static struct genl_family net_drop_monitor_family __ro_after_init = {
1638         .hdrsize        = 0,
1639         .name           = "NET_DM",
1640         .version        = 2,
1641         .maxattr        = NET_DM_ATTR_MAX,
1642         .policy         = net_dm_nl_policy,
1643         .pre_doit       = net_dm_nl_pre_doit,
1644         .post_doit      = net_dm_nl_post_doit,
1645         .module         = THIS_MODULE,
1646         .small_ops      = dropmon_ops,
1647         .n_small_ops    = ARRAY_SIZE(dropmon_ops),
1648         .mcgrps         = dropmon_mcgrps,
1649         .n_mcgrps       = ARRAY_SIZE(dropmon_mcgrps),
1650 };
1651
1652 static struct notifier_block dropmon_net_notifier = {
1653         .notifier_call = dropmon_net_event
1654 };
1655
1656 static void __net_dm_cpu_data_init(struct per_cpu_dm_data *data)
1657 {
1658         spin_lock_init(&data->lock);
1659         skb_queue_head_init(&data->drop_queue);
1660         u64_stats_init(&data->stats.syncp);
1661 }
1662
1663 static void __net_dm_cpu_data_fini(struct per_cpu_dm_data *data)
1664 {
1665         WARN_ON(!skb_queue_empty(&data->drop_queue));
1666 }
1667
1668 static void net_dm_cpu_data_init(int cpu)
1669 {
1670         struct per_cpu_dm_data *data;
1671
1672         data = &per_cpu(dm_cpu_data, cpu);
1673         __net_dm_cpu_data_init(data);
1674 }
1675
1676 static void net_dm_cpu_data_fini(int cpu)
1677 {
1678         struct per_cpu_dm_data *data;
1679
1680         data = &per_cpu(dm_cpu_data, cpu);
1681         /* At this point, we should have exclusive access
1682          * to this struct and can free the skb inside it.
1683          */
1684         consume_skb(data->skb);
1685         __net_dm_cpu_data_fini(data);
1686 }
1687
1688 static void net_dm_hw_cpu_data_init(int cpu)
1689 {
1690         struct per_cpu_dm_data *hw_data;
1691
1692         hw_data = &per_cpu(dm_hw_cpu_data, cpu);
1693         __net_dm_cpu_data_init(hw_data);
1694 }
1695
1696 static void net_dm_hw_cpu_data_fini(int cpu)
1697 {
1698         struct per_cpu_dm_data *hw_data;
1699
1700         hw_data = &per_cpu(dm_hw_cpu_data, cpu);
1701         kfree(hw_data->hw_entries);
1702         __net_dm_cpu_data_fini(hw_data);
1703 }
1704
1705 static int __init init_net_drop_monitor(void)
1706 {
1707         int cpu, rc;
1708
1709         pr_info("Initializing network drop monitor service\n");
1710
1711         if (sizeof(void *) > 8) {
1712                 pr_err("Unable to store program counters on this arch, Drop monitor failed\n");
1713                 return -ENOSPC;
1714         }
1715
1716         rc = genl_register_family(&net_drop_monitor_family);
1717         if (rc) {
1718                 pr_err("Could not create drop monitor netlink family\n");
1719                 return rc;
1720         }
1721         WARN_ON(net_drop_monitor_family.mcgrp_offset != NET_DM_GRP_ALERT);
1722
1723         rc = register_netdevice_notifier(&dropmon_net_notifier);
1724         if (rc < 0) {
1725                 pr_crit("Failed to register netdevice notifier\n");
1726                 goto out_unreg;
1727         }
1728
1729         rc = 0;
1730
1731         for_each_possible_cpu(cpu) {
1732                 net_dm_cpu_data_init(cpu);
1733                 net_dm_hw_cpu_data_init(cpu);
1734         }
1735
1736         goto out;
1737
1738 out_unreg:
1739         genl_unregister_family(&net_drop_monitor_family);
1740 out:
1741         return rc;
1742 }
1743
1744 static void exit_net_drop_monitor(void)
1745 {
1746         int cpu;
1747
1748         BUG_ON(unregister_netdevice_notifier(&dropmon_net_notifier));
1749
1750         /*
1751          * Because of the module_get/put we do in the trace state change path
1752          * we are guaranteed not to have any current users when we get here
1753          */
1754
1755         for_each_possible_cpu(cpu) {
1756                 net_dm_hw_cpu_data_fini(cpu);
1757                 net_dm_cpu_data_fini(cpu);
1758         }
1759
1760         BUG_ON(genl_unregister_family(&net_drop_monitor_family));
1761 }
1762
1763 module_init(init_net_drop_monitor);
1764 module_exit(exit_net_drop_monitor);
1765
1766 MODULE_LICENSE("GPL v2");
1767 MODULE_AUTHOR("Neil Horman <nhorman@tuxdriver.com>");
1768 MODULE_ALIAS_GENL_FAMILY("NET_DM");
1769 MODULE_DESCRIPTION("Monitoring code for network dropped packet alerts");