~hc/RK356X_SDK_RELEASE.git

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